From cbea49d7d0024f2cb2403b7c807c5aa23974374b Mon Sep 17 00:00:00 2001 From: Claude Date: Tue, 27 Oct 2026 06:39:13 +0000 Subject: [PATCH] Add libivpico_helm: pHelmIvP's helm loop without MOOS ivpico::helm::Helm ports HelmIvP's startup, mail handling and Iterate() onto the interface messages. HelmEngine, BehaviorSet, the modes and the behaviors are MOOS-IvP's own, unchanged. - Inputs (Navigation, Variable, Command) are queued and applied at the start of the next iterate(t), as pHelmIvP applies its mail. - What pHelmIvP would have posted to the MOOSDB on one iteration is fed back on the next, for the variables the helm registered, as the MOOSDB echoes it. Behaviors that update or spawn each other depend on this. - "initialize_" (deferred) variables are posted after defer_window if nothing else has set them. - Each iteration returns an Output: the Solution, Status, Postings, BehaviorEvents and LifeEvents, and ObjectiveFunctions if asked for. - interface/config.proto adds HelmConfig, pHelmIvP's configuration. - Contacts from Navigation (self: false) or NODE_REPORT variables go into a ContactLedger and LedgerSnap; alerts come later. src/test/helm/test_helm.cpp runs scenarios with a fake clock: the s1_alpha pattern (with and without park_on_allstop), an update between behaviors through the echo, spawning, allstop reasons, MALCONFIG, and deferred initialize. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VLcYK4e29B5FiPbDDJy5fp --- src/lib/CMakeLists.txt | 9 +- src/lib/helm/conversions.cpp | 115 ++++ src/lib/helm/conversions.h | 33 + src/lib/helm/helm.cpp | 1057 ++++++++++++++++++++++++++++++++ src/lib/helm/helm.h | 66 ++ src/lib/interface/config.proto | 80 +++ src/test/CMakeLists.txt | 4 + src/test/helm/test_helm.cpp | 466 ++++++++++++++ 8 files changed, 1829 insertions(+), 1 deletion(-) create mode 100644 src/lib/helm/conversions.cpp create mode 100644 src/lib/helm/conversions.h create mode 100644 src/lib/helm/helm.cpp create mode 100644 src/lib/helm/helm.h create mode 100644 src/lib/interface/config.proto create mode 100644 src/test/helm/test_helm.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f5440cb..7f6af65 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -5,8 +5,15 @@ add_library(ivpico_interface) protobuf_generate( TARGET ivpico_interface LANGUAGE cpp - PROTOS interface/options.proto interface/ivp.proto + PROTOS interface/options.proto interface/ivp.proto interface/config.proto IMPORT_DIRS ${CMAKE_CURRENT_SOURCE_DIR} ${openocean_messages_PROTO_DIR} PROTOC_OUT_DIR ${CMAKE_CURRENT_BINARY_DIR}) target_include_directories(ivpico_interface PUBLIC $) target_link_libraries(ivpico_interface PUBLIC openocean_messages::openocean_messages) + +add_library(ivpico_helm helm/helm.cpp helm/conversions.cpp) +target_include_directories(ivpico_helm PUBLIC $) +target_link_libraries(ivpico_helm + PUBLIC ivpico_interface + PRIVATE IVP::helmivp IVP::geodaid IVP::behaviors IVP::contacts IVP::logic IVP::ivpbuild + IVP::ivpsolve IVP::ivpcore IVP::mbutil) diff --git a/src/lib/helm/conversions.cpp b/src/lib/helm/conversions.cpp new file mode 100644 index 0000000..98e1c89 --- /dev/null +++ b/src/lib/helm/conversions.cpp @@ -0,0 +1,115 @@ +#include "conversions.h" + +#include + +#include "MBUtils.h" + +namespace ivpico +{ +namespace helm +{ +namespace +{ +constexpr double degrees(double radians) { return radians * 180.0 / M_PI; } +constexpr double radians(double degrees) { return degrees * M_PI / 180.0; } +} // namespace + +double heading_degrees(double radians) +{ + const double h = std::fmod(degrees(radians), 360.0); + return h < 0 ? h + 360.0 : h; +} + +std::optional select_speed(const openocean::Navigation& nav, openocean::SpeedMode mode) +{ + std::optional first; + for (const auto& speed : nav.speed()) + { + if (!speed.has_value()) + continue; + if (speed.mode() == mode) + return speed.value(); + if (!first) + first = speed.value(); + } + return first; +} + +NodeRecord node_record(const ivp::interface::Navigation& nav, openocean::SpeedMode speed_mode) +{ + const auto& n = nav.nav(); + NodeRecord record; + record.setName(n.vehicle().name()); + record.setTimeStamp(static_cast(n.time()) * 1e-6); + if (n.has_geodetic() && n.geodetic().has_latitude() && n.geodetic().has_longitude()) + { + record.setLat(n.geodetic().latitude()); + record.setLon(n.geodetic().longitude()); + } + if (n.has_enu() && n.enu().has_e() && n.enu().has_n()) + { + record.setX(n.enu().e()); + record.setY(n.enu().n()); + } + if (n.has_attitude() && n.attitude().has_heading()) + record.setHeading(heading_degrees(n.attitude().heading())); + if (auto speed = select_speed(n, speed_mode)) + record.setSpeed(*speed); + if (n.has_geodetic() && n.geodetic().has_depth()) + record.setDepth(n.geodetic().depth()); + if (n.has_altitude()) + record.setAltitude(n.altitude()); + if (n.has_course_over_ground()) + record.setCourseOG(heading_degrees(n.course_over_ground())); + + record.setType(nav.type()); + record.setGroup(nav.group()); + record.setColor(nav.color()); + record.setVSource(nav.vsource()); + if (nav.has_length()) + record.setLength(nav.length()); + if (nav.has_beam()) + record.setBeam(nav.beam()); + return record; +} + +std::string able_filter_string(const ivp::interface::Command::AbleFilter& filter) +{ + using AbleFilter = ivp::interface::Command::AbleFilter; + std::string s = "action=" + tolower(AbleFilter::Action_Name(filter.action())); + if (!filter.contact().empty()) + s += ",contact=" + filter.contact(); + if (!filter.behavior_type().empty()) + s += ",bhv_type=" + filter.behavior_type(); + if (!filter.vsource().empty()) + s += ",vsource=" + filter.vsource(); + if (!filter.obstacle_id().empty()) + s += ",obstacle_id=" + filter.obstacle_id(); + return s; +} + +void set_desired(openocean::ControlSetpoint& desired, const std::string& domain_var, double value) +{ + const std::string var = tolower(domain_var); + if (var == "course" || var == "heading") + desired.mutable_attitude()->set_heading(radians(value)); + else if (var == "speed") + desired.mutable_speed()->set_value(value); + else if (var == "depth") + desired.set_depth(value); + else if (var == "altitude") + desired.set_altitude(value); + else if (var == "pitch") + desired.mutable_attitude()->set_pitch(radians(value)); + else if (var == "roll") + desired.mutable_attitude()->set_roll(radians(value)); + else + { + auto& custom = *desired.add_custom(); + custom.set_name("DESIRED_" + toupper(domain_var)); + custom.set_value(value); + } +} + +} // namespace helm +} // namespace ivpico diff --git a/src/lib/helm/conversions.h b/src/lib/helm/conversions.h new file mode 100644 index 0000000..938cfe4 --- /dev/null +++ b/src/lib/helm/conversions.h @@ -0,0 +1,33 @@ +#ifndef IVPICO_HELM_CONVERSIONS_H +#define IVPICO_HELM_CONVERSIONS_H + +#include +#include + +#include "NodeRecord.h" + +#include "interface/ivp.pb.h" + +namespace ivpico +{ +namespace helm +{ +// Radians clockwise from north to MOOS-IvP's degrees, from 0 up to 360 +double heading_degrees(double radians); + +// The speed entry with the given mode, or else the first entry +std::optional select_speed(const openocean::Navigation& nav, openocean::SpeedMode mode); + +// A contact as MOOS-IvP's contact ledger takes it (the equivalent of a NODE_REPORT) +NodeRecord node_record(const ivp::interface::Navigation& nav, openocean::SpeedMode speed_mode); + +// The BHV_ABLE_FILTER string +std::string able_filter_string(const ivp::interface::Command::AbleFilter& filter); + +// Sets the setpoint field for the decision on a helm domain variable +void set_desired(openocean::ControlSetpoint& desired, const std::string& domain_var, double value); + +} // namespace helm +} // namespace ivpico + +#endif diff --git a/src/lib/helm/helm.cpp b/src/lib/helm/helm.cpp new file mode 100644 index 0000000..706af5a --- /dev/null +++ b/src/lib/helm/helm.cpp @@ -0,0 +1,1057 @@ +#include "helm.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "BehaviorSet.h" +#include "BuildUtils.h" +#include "ContactLedger.h" +#include "HelmEngine.h" +#include "HelmReport.h" +#include "InfoBuffer.h" +#include "IvPBehavior.h" +#include "IvPDomain.h" +#include "LedgerSnap.h" +#include "LifeEvent.h" +#include "MBUtils.h" +#include "NodeRecord.h" +#include "NodeRecordUtils.h" +#include "PlatModelGenerator.h" +#include "Populator_BehaviorSet.h" +#include "VarDataPair.h" + +#include "conversions.h" + +using ivp::interface::AllStop; +using ivp::interface::BehaviorEvent; +using ivp::interface::Command; +using ivp::interface::HelmConfig; +using ivp::interface::Navigation; +using ivp::interface::Posting; +using ivp::interface::Status; +using ivp::interface::Variable; + +namespace ivpico +{ +namespace helm +{ +namespace +{ +struct Value +{ + bool is_string{false}; + double d{0}; + std::string s; +}; + +Value make_value(const VarDataPair& pair) +{ + Value v; + v.is_string = pair.is_string(); + v.d = pair.get_ddata(); + v.s = pair.get_sdata(); + return v; +} + +void set_value(Posting& posting, const Value& v) +{ + if (v.is_string) + posting.set_sval(v.s); + else + posting.set_dval(v.d); +} + +std::optional behavior_event_kind(const std::string& var, + const std::string& key) +{ + if (var == "BHV_WARNING") + return strEnds(key, "retract") ? BehaviorEvent::WARNING_RETRACTED : BehaviorEvent::WARNING; + if (var == "BHV_ERROR") + return BehaviorEvent::ERROR; + if (var == "BHV_EVENT") + return BehaviorEvent::EVENT; + if (var == "BHV_CONFIG_WARNING") + return BehaviorEvent::CONFIG_WARNING; + if (var == "BHV_BAD_CONFIG") + return BehaviorEvent::BAD_CONFIG; + if (var == "BHV_STATUS") + return BehaviorEvent::STATUS; + if (var == "BHV_SETTINGS") + return BehaviorEvent::SETTINGS; + return std::nullopt; +} + +AllStop allstop_enum(const std::string& msg) +{ + if (msg == "clear") + return ivp::interface::CLEAR; + if (msg == "ManualOverride") + return ivp::interface::MANUAL_OVERRIDE; + if (msg == "BehaviorError") + return ivp::interface::BEHAVIOR_ERROR; + if (msg == "NothingToDo") + return ivp::interface::NOTHING_TO_DO; + if (msg == "NoGoalBehavior") + return ivp::interface::NO_GOAL_BEHAVIOR; + if (strBegins(msg, "MissingDecVars")) + return ivp::interface::MISSING_DECISION_VARIABLES; + if (msg == "DisabledByStandbyHelm") + return ivp::interface::DISABLED_BY_STANDBY_HELM; + return ivp::interface::ALLSTOP_UNKNOWN; +} + +std::string desired_variable(const std::string& domain_var) +{ + std::string var = "DESIRED_" + toupper(domain_var); + return var == "DESIRED_COURSE" ? "DESIRED_HEADING" : var; +} + +int64_t micros(double seconds) { return static_cast(std::llround(seconds * 1e6)); } + +double seconds(int64_t micros, double fallback) +{ + return micros == 0 ? fallback : static_cast(micros) * 1e-6; +} + +} // namespace + +class Helm::Impl +{ + public: + Impl(const HelmConfig& cfg, std::optional geodesy); + ~Impl() { delete bhv_set_; } + + using Input = std::variant; + void queue(Input input) { pending_.push_back(std::move(input)); } + const Output& iterate(double time); + + Status::State state() const { return status_; } + const std::vector& config_warnings() const { return config_warnings_; } + + private: + void configure(); + void start(double time); + + // mail + void apply(const Navigation& nav, double time); + void apply(const Command& command, double time); + void apply_variable(const std::string& var, const Value& value, double msg_time); + void set_manual_override(bool on); + bool process_node_record(NodeRecord record); + void process_node_report_local(const NodeRecord& record); + void register_variable(const std::string& var); + void register_variables(); + + // iterate + void update_status(); + bool enabled() const { return status_ == Status::PARK || status_ == Status::DRIVE; } + bool hold_for_nav_solution(); + void post_initial_variables(bool deferred); + void post_helm_start_messages(); + void update_ledger_snap(); + void update_plat_model(); + void post_mode_messages(); + void post_behavior_messages(); + void post_life_events(); + void post_allstop(const std::string& msg); + void post_decisions(); + void fill_status(); + + bool detect_change(const std::string& key, const std::string& value); + bool detect_change(const std::string& key, double value); + void notify(const std::string& var, const Value& value); + void notify(const std::string& var, double d) { notify(var, Value{false, d, {}}); } + void notify(const std::string& var, const std::string& s) { notify(var, Value{true, 0, s}); } + Posting& add_posting(Posting::Origin origin, const std::string& behavior, + const std::string& var, const Value& value); + void add_behavior_event(BehaviorEvent::Kind kind, const std::string& behavior, + const std::string& text); + + HelmConfig cfg_; + std::optional geodesy_; + std::vector config_warnings_; + + IvPDomain domain_; + std::set optional_vars_; + InfoBuffer info_buffer_; + LedgerSnap ledger_snap_; + ContactLedger ledger_; + PlatModelGenerator pmgen_; + std::unique_ptr engine_; + BehaviorSet* bhv_set_{nullptr}; + HelmReport report_; + + std::string ownship_; + Status::State status_{Status::PARK}; + bool has_control_{false}; + bool allow_park_{true}; + bool park_on_allstop_{true}; + double nav_grace_{5}; + double defer_window_{1}; + + bool started_{false}; + double curr_time_{0}; + double start_time_{0}; + bool nav_started_{false}; + bool init_vars_done_{false}; + bool helm_start_posted_{false}; + bool refresh_pending_{false}; + double refresh_time_{0}; + const double refresh_interval_{2}; + + std::string allstop_msg_; + std::string allstop_reason_; + unsigned no_decisions_{0}; + unsigned no_goal_decisions_{0}; + std::string comms_policy_{"open"}; + unsigned iteration_{0}; + std::vector missing_decision_vars_; + + std::map outgoing_strings_; + std::map outgoing_doubles_; + std::set registered_; + + // What pHelmIvP would have posted to the MOOSDB this iteration, which the + // MOOSDB delivers back on the next one for the variables the helm registered + std::vector> posted_; + double posted_time_{0}; + std::vector pending_; + Output output_; +}; + +Helm::Impl::Impl(const HelmConfig& cfg, std::optional geodesy) + : cfg_(cfg), geodesy_(std::move(geodesy)) +{ + configure(); +} + +void Helm::Impl::configure() +{ + ownship_ = cfg_.ownship(); + has_control_ = cfg_.start_in_drive(); + if (cfg_.has_allow_park()) + allow_park_ = cfg_.allow_park(); + if (cfg_.has_park_on_allstop()) + park_on_allstop_ = cfg_.park_on_allstop(); + if (cfg_.has_nav_grace()) + nav_grace_ = cfg_.nav_grace(); + if (cfg_.has_defer_window()) + defer_window_ = cfg_.defer_window(); + + for (const auto& d : cfg_.domain()) + { + unsigned points = d.points(); + const double range = d.high() - d.low(); + if (d.resolution_case() == HelmConfig::Domain::kDelta && d.delta() > 0 && range > 0 && + d.delta() <= range) + points = static_cast(range / d.delta() + 1); + if (d.high() < d.low() || (range == 0 && points != 1) || + !domain_.addDomain(d.name().c_str(), d.low(), d.high(), points)) + config_warnings_.push_back("Bad domain: " + d.ShortDebugString()); + if (d.optional()) + optional_vars_.insert(d.name()); + } + + if (geodesy_) + ledger_.setGeodesy(geodesy_->to_local, geodesy_->to_global); + double contact_max_age = cfg_.has_contact_max_age() ? cfg_.contact_max_age() : 45; + if (contact_max_age < 10 || contact_max_age > 1800) + config_warnings_.push_back("contact_max_age must be from 10 to 1800 seconds"); + ledger_.setStaleThresh(vclip(contact_max_age, 10, 1800)); + if (cfg_.has_contact_max_count()) + ledger_.setMaxContacts(cfg_.contact_max_count()); + + const auto& pm = cfg_.platform_model(); + std::string pmgen = + pm.algorithm() == HelmConfig::PlatformModel::DUBINS ? "alg=dubins" : "alg=holo"; + if (pm.has_radius()) + pmgen += "#radius=" + doubleToStringX(pm.radius()); + if (pm.has_spoke_degs()) + pmgen += "#spoke_degs=" + doubleToStringX(pm.spoke_degs()); + if (!pmgen_.setParams(pmgen)) + config_warnings_.push_back("Bad platform_model: " + pm.ShortDebugString()); + + if (!cfg_.has_seed_random() || cfg_.seed_random()) + srand(static_cast(time(nullptr) * 31 + getpid())); + + if (!config_warnings_.empty()) + return; + + engine_ = std::make_unique(domain_, &info_buffer_, &ledger_snap_); + Populator_BehaviorSet populator(domain_, &info_buffer_, &ledger_snap_); + populator.setBHVDirNotFoundOK(cfg_.behavior_dir_not_found_ok()); + populator.setOwnship(ownship_); + for (const auto& dir : cfg_.behavior_dir()) populator.addBehaviorDir(dir); + bhv_set_ = populator.populate( + std::set(cfg_.behavior_file().begin(), cfg_.behavior_file().end())); + for (const auto& w : populator.getConfigWarnings()) config_warnings_.push_back(w); + + if (!bhv_set_) + { + if (config_warnings_.empty()) + config_warnings_.push_back("No behaviors from the behavior files"); + return; + } + if (!config_warnings_.empty()) + return; + + bhv_set_->setReportIPF(cfg_.report_objective_functions()); + engine_->setBehaviorSet(bhv_set_); + for (unsigned i = 0; i < bhv_set_->size(); ++i) + { + // pHelmIvP sets "us" itself, since only it knows the community name + bhv_set_->getBehavior(i)->IvPBehavior::setParam("us", ownship_); + bhv_set_->getBehavior(i)->onSetParamComplete(); + } +} + +void Helm::Impl::start(double time) +{ + started_ = true; + start_time_ = time; + info_buffer_.setCurrTime(time); + info_buffer_.setStartTime(time); + ledger_.setCurrTimeUTC(time); + + if (!config_warnings_.empty()) + { + status_ = Status::MALCONFIG; + has_control_ = false; + return; + } + + post_initial_variables(false); + post_helm_start_messages(); + register_variables(); + + notify("IVPHELM_DOMAIN", domainToString(domain_)); + notify("IVPHELM_MODESET", bhv_set_->getModeSetDefinition()); + if (!cfg_.helm_alias().empty()) + notify("IVPHELM_ALIAS", cfg_.helm_alias()); +} + +const Output& Helm::Impl::iterate(double time) +{ + output_ = Output(); + curr_time_ = time; + + // the previous iteration's posts reach the helm first, as they reached the MOOSDB first + auto echo = std::move(posted_); + posted_.clear(); + const double echo_time = posted_time_ == 0 ? time : posted_time_; + posted_time_ = time; + + if (!started_) + start(time); + + info_buffer_.setCurrTime(time); + ledger_.setCurrTimeUTC(time); + if (status_ != Status::MALCONFIG) + { + for (const auto& [var, value] : echo) + if (registered_.count(var)) + apply_variable(var, value, echo_time); + for (const auto& input : pending_) + { + if (const auto* nav = std::get_if(&input)) + apply(*nav, time); + else if (const auto* var = std::get_if(&input)) + { + Value value; + value.is_string = var->value_case() == Variable::kSval; + value.d = var->dval(); + value.s = var->sval(); + apply_variable(var->key(), value, seconds(var->time(), time)); + } + else + apply(std::get(input), time); + } + info_buffer_.setValue("COMMS_POLICY", comms_policy_, time); + } + pending_.clear(); + + update_status(); + notify("IVPHELM_STATE", Status::State_Name(status_)); + fill_status(); + if (!enabled()) + { + info_buffer_.clearDeltaVectors(); + return output_; + } + + if (hold_for_nav_solution()) + return output_; + + if (!init_vars_done_ && time - start_time_ >= defer_window_) + post_initial_variables(true); + + std::vector keep_vnames = bhv_set_->getContactNames(); + ledger_.clearStaleNodes(keep_vnames, 2); + ledger_.extrapolate(); + update_ledger_snap(); + + if (!has_control_) + { + post_allstop("ManualOverride"); + fill_status(); + info_buffer_.clearDeltaVectors(); + return output_; + } + + // no decision for the first second, to give inputs time to arrive + if (time - start_time_ < 1) + return output_; + + update_plat_model(); + report_ = engine_->determineNextDecision(bhv_set_, time); + iteration_ = report_.getIteration(); + + if (refresh_pending_ && time - refresh_time_ > refresh_interval_) + { + outgoing_strings_.clear(); + outgoing_doubles_.clear(); + refresh_time_ = time; + refresh_pending_ = false; + } + + for (const auto& var : bhv_set_->getNewInfoVars()) register_variable(var); + post_mode_messages(); + post_behavior_messages(); + post_life_events(); + post_decisions(); + + if (allstop_msg_ != "clear" && allow_park_ && park_on_allstop_) + has_control_ = false; + + fill_status(); + info_buffer_.clearDeltaVectors(); + return output_; +} + +void Helm::Impl::apply(const Navigation& nav, double time) +{ + const auto& n = nav.nav(); + const double t = seconds(n.time(), time); + + if (!nav.self()) + { + NodeRecord record = node_record(nav, cfg_.speed_mode()); + if (record.getName().empty()) + return; + process_node_record(record); + info_buffer_.setValue("NODE_REPORT", record.getSpec(), t); + return; + } + + std::optional> xy, latlon; + if (n.has_enu() && n.enu().has_e() && n.enu().has_n()) + xy = std::make_pair(n.enu().e(), n.enu().n()); + if (n.has_geodetic() && n.geodetic().has_latitude() && n.geodetic().has_longitude()) + latlon = std::make_pair(n.geodetic().latitude(), n.geodetic().longitude()); + if (!xy && latlon && geodesy_) + { + double x, y; + geodesy_->to_local(latlon->first, latlon->second, x, y); + xy = std::make_pair(x, y); + } + if (!latlon && xy && geodesy_) + { + double lat, lon; + geodesy_->to_global(xy->first, xy->second, lat, lon); + latlon = std::make_pair(lat, lon); + } + + if (xy) + { + info_buffer_.setValue("NAV_X", xy->first, t); + info_buffer_.setValue("NAV_Y", xy->second, t); + } + if (latlon) + { + info_buffer_.setValue("NAV_LAT", latlon->first, t); + info_buffer_.setValue("NAV_LONG", latlon->second, t); + } + if (n.has_attitude() && n.attitude().has_heading()) + info_buffer_.setValue("NAV_HEADING", heading_degrees(n.attitude().heading()), t); + if (auto speed = select_speed(n, cfg_.speed_mode())) + info_buffer_.setValue("NAV_SPEED", *speed, t); + if (n.has_geodetic() && n.geodetic().has_depth()) + info_buffer_.setValue("NAV_DEPTH", n.geodetic().depth(), t); + if (n.has_altitude()) + info_buffer_.setValue("NAV_ALTITUDE", n.altitude(), t); + + NodeRecord local; + local.setName(n.vehicle().name().empty() ? ownship_ : n.vehicle().name()); + local.setGroup(nav.group()); + local.setType(nav.type()); + local.setColor(nav.color()); + if (nav.has_length()) + local.setLength(nav.length()); + process_node_report_local(local); +} + +void Helm::Impl::apply(const Command& command, double time) +{ + switch (command.command_case()) + { + case Command::kManualOverride: set_manual_override(command.manual_override()); break; + case Command::kRefresh: + if (command.refresh()) + refresh_pending_ = true; + break; + case Command::kAbleFilter: + apply_variable("BHV_ABLE_FILTER", + Value{true, 0, able_filter_string(command.able_filter())}, time); + break; + case Command::kCommsPolicy: + comms_policy_ = tolower(Command::CommsPolicy_Name(command.comms_policy())); + break; + default: break; + } +} + +// HelmIvP::OnNewMail for one message +void Helm::Impl::apply_variable(const std::string& var, const Value& value, double msg_time) +{ + const std::string sval = value.s; + if (var == "MOOS_MANUAL_OVERRIDE" || var == "MOOS_MANUAL_OVERIDE") + { + if (toupper(sval) == "FALSE") + set_manual_override(false); + else if (toupper(sval) == "TRUE") + set_manual_override(true); + info_buffer_.setValue(var, sval, msg_time); + } + // Status is always complete, so there's nothing to rejournal + else if (var == "IVPHELM_STATE" || var == "DB_CLIENTS" || var == "RESTART_HELM" || + var == "IVPHELM_REJOURNAL") + { + } + else if (var == "BHV_ABLE_FILTER") + engine_->addAbleFilterMsg(sval); + else if (var == "NODE_REPORT_LOCAL") + process_node_report_local(string2NodeRecord(sval)); + else if (var == "NODE_REPORT" || var == "AIS_REPORT" || var == "AIS_REPORT_LOCAL") + { + std::string whynot; + const std::string vname = ledger_.processNodeReport(sval, whynot); + if (vname.empty()) + add_behavior_event(BehaviorEvent::WARNING, "", "Bad node report: " + whynot); + else + { + info_buffer_.setValue(toupper(vname) + "_NAV_GROUP", ledger_.getGroup(vname)); + info_buffer_.setValue(toupper(vname) + "_NAV_TYPE", ledger_.getType(vname)); + } + info_buffer_.setValue(var, sval, msg_time); + } + else if (var == "HELM_MAP_CLEAR") + refresh_pending_ = true; + else if (value.is_string) + info_buffer_.setValue(var, sval, msg_time); + else + info_buffer_.setValue(var, value.d, msg_time); +} + +void Helm::Impl::set_manual_override(bool on) +{ + if (!on) + has_control_ = true; + else if (allow_park_) + has_control_ = false; + update_status(); +} + +bool Helm::Impl::process_node_record(NodeRecord record) +{ + std::string whynot; + const std::string vname = ledger_.processNodeRecord(record, whynot); + if (vname.empty()) + { + add_behavior_event(BehaviorEvent::WARNING, "", "Bad node report: " + whynot); + return false; + } + info_buffer_.setValue(toupper(vname) + "_NAV_GROUP", ledger_.getGroup(vname)); + info_buffer_.setValue(toupper(vname) + "_NAV_TYPE", ledger_.getType(vname)); + return true; +} + +void Helm::Impl::process_node_report_local(const NodeRecord& record) +{ + const std::string vname = toupper(record.getName()); + if (vname.empty()) + return; + if (!record.getGroup().empty()) + info_buffer_.setValue(vname + "_NAV_GROUP", record.getGroup()); + if (!record.getType().empty()) + info_buffer_.setValue(vname + "_NAV_TYPE", record.getType()); + if (!record.getColor().empty()) + info_buffer_.setValue(vname + "_NAV_COLOR", record.getColor()); + if (record.getLength() != 0) + info_buffer_.setValue(vname + "_NAV_LENGTH", record.getLength()); +} + +void Helm::Impl::register_variable(const std::string& var) +{ + if (registered_.insert(var).second) + notify("IVPHELM_REGISTER", var); +} + +void Helm::Impl::register_variables() +{ + for (const char* var : + {"MOOS_MANUAL_OVERIDE", "MOOS_MANUAL_OVERRIDE", "RESTART_HELM", "IVPHELM_REJOURNAL", + "BHV_ABLE_FILTER", "NAV_X", "NAV_Y", "NAV_SPEED", "NAV_HEADING", "NAV_DEPTH", + "HELM_MAP_CLEAR", "AIS_REPORT", "NODE_REPORT", "AIS_REPORT_LOCAL", "NODE_REPORT_LOCAL", + "IVPHELM_STATE"}) + register_variable(var); + for (const auto& var : bhv_set_->getInfoVars()) register_variable(var); + for (const auto& var : bhv_set_->getSpecUpdateVars()) register_variable(var); +} + +void Helm::Impl::update_status() +{ + if (status_ == Status::PARK && has_control_) + status_ = Status::DRIVE; + else if (status_ == Status::DRIVE && !has_control_) + status_ = Status::PARK; +} + +bool Helm::Impl::hold_for_nav_solution() +{ + if (nav_started_) + return false; + + std::vector vars{"NAV_X", "NAV_Y", "NAV_HEADING", "NAV_SPEED"}; + if (domain_.hasDomain("depth")) + vars.push_back("NAV_DEPTH"); + std::string missing; + for (const auto& var : vars) + if (!info_buffer_.isKnown(var)) + missing += (missing.empty() ? "" : ",") + var; + + if (missing.empty()) + { + nav_started_ = true; + return false; + } + if (curr_time_ - start_time_ > nav_grace_) + add_behavior_event(BehaviorEvent::WARNING, "", "Waiting for helm nav solution: " + missing); + return true; +} + +// pHelmIvP's handleInitialVarsPhase1 (deferred = false) and Phase2 (deferred = true) +void Helm::Impl::post_initial_variables(bool deferred) +{ + if (deferred) + init_vars_done_ = true; + for (const VarDataPair& pair : bhv_set_->getInitialVariables()) + { + const std::string var = stripBlankEnds(pair.get_var()); + const std::string key = tolower(pair.get_key()); + if (strContainsWhite(var)) + continue; + if (!deferred && key == "defer") + register_variable(var); + if ((!deferred && key == "post") || + (deferred && key == "defer" && !info_buffer_.isKnown(var))) + { + Value value = make_value(pair); + value.s = stripBlankEnds(value.s); + value.is_string = !value.s.empty(); + if (value.is_string) + info_buffer_.setValue(var, value.s); + else + info_buffer_.setValue(var, value.d); + // the MOOSDB echoes these too, but pHelmIvP ignores that echo (HELM_VAR_INIT) + add_posting(Posting::INITIALIZE, "", var, value); + } + } +} + +void Helm::Impl::post_helm_start_messages() +{ + if (helm_start_posted_) + return; + helm_start_posted_ = true; + for (const VarDataPair& pair : bhv_set_->getHelmStartMessages()) + { + const std::string var = stripBlankEnds(pair.get_var()); + if (strContainsWhite(var)) + continue; + Value value = make_value(pair); + value.s = stripBlankEnds(value.s); + value.is_string = !value.s.empty(); + if (value.is_string) + info_buffer_.setValue(var, value.s); + else + info_buffer_.setValue(var, value.d); + add_posting(Posting::HELM_START, "", var, value); + notify(var, value); + } +} + +void Helm::Impl::update_ledger_snap() +{ + ledger_snap_.clear(); + for (const auto& v : ledger_.getVNames()) + { + ledger_snap_.setX(v, ledger_.getX(v)); + ledger_snap_.setY(v, ledger_.getY(v)); + ledger_snap_.setHdg(v, ledger_.getHeading(v)); + ledger_snap_.setSpd(v, ledger_.getSpeed(v)); + ledger_snap_.setDep(v, ledger_.getDepth(v)); + ledger_snap_.setLat(v, ledger_.getLat(v)); + ledger_snap_.setLon(v, ledger_.getLon(v)); + ledger_snap_.setUTC(v, ledger_.getUTC(v)); + ledger_snap_.setUTCAge(v, ledger_.getUTCAge(v)); + ledger_snap_.setUTCReceived(v, ledger_.getUTCReceived(v)); + ledger_snap_.setUTCAgeReceived(v, ledger_.getUTCAgeReceived(v)); + ledger_snap_.setGroup(v, ledger_.getGroup(v)); + ledger_snap_.setType(v, ledger_.getType(v)); + ledger_snap_.setVSource(v, ledger_.getVSource(v)); + } + ledger_snap_.setCurrTimeUTC(curr_time_); +} + +void Helm::Impl::update_plat_model() +{ + bool ok1, ok2, ok3, ok4; + const double osx = info_buffer_.dQuery("NAV_X", ok1); + const double osy = info_buffer_.dQuery("NAV_Y", ok2); + const double osh = info_buffer_.dQuery("NAV_HEADING", ok3); + const double osv = info_buffer_.dQuery("NAV_SPEED", ok4); + if (!ok1 || !ok2 || !ok3 || !ok4) + return; + + pmgen_.setCurrTime(info_buffer_.getCurrTime()); + engine_->setPlatModel(pmgen_.generate(osx, osy, osh, osv)); + const double turn_rate = pmgen_.getTurnRate(); + output_.status.set_turn_rate(turn_rate); + notify("TURN_RATE", turn_rate); +} + +void Helm::Impl::post_mode_messages() +{ + for (const VarDataPair& pair : bhv_set_->getModeVarDataPairs()) + { + const Value value = make_value(pair); + const bool changed = value.is_string ? detect_change(pair.get_key(), value.s) + : detect_change(pair.get_key(), value.d); + if (changed) + { + add_posting(Posting::MODE, "", pair.get_var(), value); + notify(pair.get_var(), value); + } + } +} + +void Helm::Impl::post_behavior_messages() +{ + for (const auto& warning : bhv_set_->getWarnings()) + add_behavior_event(BehaviorEvent::WARNING, "", warning); + bhv_set_->clearWarnings(); + + notify("IVPHELM_ITER", iteration_); + for (unsigned i = 0; i < bhv_set_->size(); ++i) + { + const std::string bhv = bhv_set_->getDescriptor(i); + for (const VarDataPair& msg : bhv_set_->getMessages(i)) + { + const std::string var = msg.get_var(); + const std::string key = msg.get_key(); + const Value value = make_value(msg); + + if (var == "BHV_IPF") + { + auto& of = output_.objective_functions.emplace_back(); + of.set_time(micros(curr_time_)); + of.set_behavior(bhv); + of.set_encoding(value.s); + of.set_iteration(iteration_); + continue; + } + + // a string with an empty value is compared as a number, as pHelmIvP does + const bool changed = + value.s.empty() ? detect_change(key, value.d) : detect_change(key, value.s); + + if (value.is_string) + info_buffer_.setValue(var, value.s); + else + info_buffer_.setValue(var, value.d); + + if (!changed && key != "repeatable") + continue; + + if (auto kind = behavior_event_kind(var, key)) + add_behavior_event(*kind, bhv, + value.is_string ? value.s : doubleToStringX(value.d)); + else + add_posting(Posting::BEHAVIOR, bhv, var, value); + notify(var, value); + } + } + + bhv_set_->updateStateSpaceVars(); + bhv_set_->removeCompletedBehaviors(); +} + +void Helm::Impl::post_life_events() +{ + for (const LifeEvent& event : bhv_set_->getLifeEvents()) + { + auto& out = output_.life_events.emplace_back(); + out.set_time(micros(curr_time_)); + const std::string type = event.getEventType(); + out.set_type(type == "spawn" ? ivp::interface::LifeEvent::SPAWN + : type == "death" ? ivp::interface::LifeEvent::DEATH + : type == "abort" ? ivp::interface::LifeEvent::ABORT + : ivp::interface::LifeEvent::TYPE_UNKNOWN); + out.set_behavior(event.getBehaviorName()); + out.set_behavior_type(event.getBehaviorType()); + out.set_spawn_string(event.getSpawnString()); + out.set_post_mortem(event.getPostMortem()); + out.set_iteration(iteration_); + notify("IVPHELM_LIFE_EVENT", + event.getSpec(curr_time_ - start_time_) + ",iter=" + uintToString(iteration_)); + } + bhv_set_->clearLifeEvents(); +} + +void Helm::Impl::post_decisions() +{ + std::string allstop_msg = "clear"; + if (report_.getHalted()) + allstop_msg = "BehaviorError"; + else if (report_.getOFNUM() == 0) + allstop_msg = "NothingToDo"; + else if (cfg_.goals_mandatory() && !report_.getActiveGoal()) + allstop_msg = "NoGoalBehavior"; + + missing_decision_vars_.clear(); + if (allstop_msg == "clear") + { + for (unsigned i = 0; i < domain_.size(); ++i) + { + const std::string var = domain_.getVarName(i); + if (!report_.hasDecision(var) && !optional_vars_.count(var)) + missing_decision_vars_.push_back(var); + } + if (!missing_decision_vars_.empty()) + { + allstop_msg = "MissingDecVars:"; + for (std::size_t i = 0; i < missing_decision_vars_.size(); ++i) + allstop_msg += (i ? "," : "") + missing_decision_vars_[i]; + add_behavior_event(BehaviorEvent::ERROR, "", allstop_msg); + notify("BHV_ERROR", allstop_msg); + } + } + + post_allstop(allstop_msg); + if (allstop_msg != "clear") + return; + + ivp::interface::Solution solution; + solution.set_allstop(ivp::interface::CLEAR); + solution.set_iteration(iteration_); + auto& desired = *solution.mutable_desired(); + desired.set_time(micros(curr_time_)); + for (unsigned i = 0; i < domain_.size(); ++i) + { + const std::string var = domain_.getVarName(i); + if (!report_.hasDecision(var)) + continue; + const double value = report_.getDecision(var); + set_desired(desired, var, value); + notify(desired_variable(var), value); + } + output_.solution = solution; +} + +// HelmIvP::postAllStop +void Helm::Impl::post_allstop(const std::string& msg) +{ + if (!enabled() || msg == allstop_msg_) + return; + + allstop_msg_ = msg; + allstop_reason_ = msg; + no_decisions_ = + (msg == "NothingToDo" || strBegins(msg, "MissingDecVars")) ? no_decisions_ + 1 : 0; + no_goal_decisions_ = msg == "NoGoalBehavior" ? no_goal_decisions_ + 1 : 0; + + notify("IVPHELM_ALLSTOP", msg); + if (msg == "clear") + return; + + // one iteration's grace to let the helm move between modes + if (no_decisions_ == 1) + { + allstop_msg_ = "IncompleteOrEmptyDecision"; + return; + } + if (no_goal_decisions_ == 1) + { + allstop_msg_ = "NoActiveGoalBehavior"; + return; + } + + ivp::interface::Solution solution; + solution.set_allstop(allstop_enum(msg)); + solution.set_iteration(iteration_); + auto& desired = *solution.mutable_desired(); + desired.set_time(micros(curr_time_)); + for (unsigned i = 0; i < domain_.size(); ++i) + { + const std::string var = domain_.getVarName(i); + set_desired(desired, var, 0); + notify(desired_variable(var), 0.0); + } + output_.solution = solution; +} + +void Helm::Impl::fill_status() +{ + Status& s = output_.status; + s.set_time(micros(curr_time_)); + s.set_state(status_); + s.set_allstop(allstop_enum(allstop_reason_)); + s.set_iteration(iteration_); + s.clear_missing_decision_variables(); + for (const auto& var : missing_decision_vars_) s.add_missing_decision_variables(var); + s.set_halt_message(report_.getHalted() ? report_.getHaltMsg() : ""); + + s.clear_mode(); + s.clear_behavior(); + s.clear_contact(); + if (!bhv_set_) + return; + + for (const VarDataPair& pair : bhv_set_->getModeVarDataPairs()) + { + auto& mode = *s.add_mode(); + mode.set_variable(pair.get_var()); + mode.set_value(pair.is_string() ? pair.get_sdata() : doubleToStringX(pair.get_ddata())); + } + + std::map run_states; + auto add_states = [&](const std::string& list, Status::Behavior::RunState state) + { + for (const auto& name : parseString(list, ',')) + if (!name.empty()) + run_states[name] = state; + }; + add_states(report_.getIdleBehaviors(false), Status::Behavior::IDLE); + add_states(report_.getRunningBehaviors(false), Status::Behavior::RUNNING); + add_states(report_.getActiveBehaviors(false), Status::Behavior::ACTIVE); + add_states(report_.getDisabledBehaviors(false), Status::Behavior::DISABLED); + add_states(report_.getCompletedBehaviors(false), Status::Behavior::COMPLETED); + + for (unsigned i = 0; i < bhv_set_->size(); ++i) + { + auto& b = *s.add_behavior(); + b.set_name(bhv_set_->getDescriptor(i)); + b.set_type(bhv_set_->getBehavior(i)->getBehaviorType()); + auto it = run_states.find(b.name()); + b.set_run_state(it == run_states.end() ? Status::Behavior::RUN_STATE_UNKNOWN : it->second); + b.set_state_time_entered(micros(bhv_set_->getStateTimeEntered(i))); + b.set_mode(bhv_set_->getBehaviorMode(i)); + b.set_submode(bhv_set_->getBehaviorSubMode(i)); + } + s.set_behavior_count(bhv_set_->size()); + s.set_behavior_count_ever(bhv_set_->getTCount()); + s.set_completed_pending(bhv_set_->getCompletedPending()); + + for (const auto& vname : ledger_.getVNames()) s.add_contact(vname); + + s.set_objective_function_count(report_.getOFNUM()); + s.set_total_pieces_formed(report_.getTotalPcsFormed()); + s.set_total_pieces_cached(report_.getTotalPcsCached()); + s.set_create_cpu(report_.getCreateTime()); + s.set_solve_cpu(report_.getSolveTime()); + s.set_loop_cpu(report_.getLoopTime()); + s.set_max_create_cpu(report_.getMaxCreateTime()); + s.set_max_solve_cpu(report_.getMaxSolveTime()); + s.set_max_loop_cpu(report_.getMaxLoopTime()); +} + +bool Helm::Impl::detect_change(const std::string& key, const std::string& value) +{ + if (key.empty()) + return true; + auto [it, inserted] = outgoing_strings_.emplace(key, value); + if (inserted) + return true; + if (it->second == value) + return false; + it->second = value; + return true; +} + +bool Helm::Impl::detect_change(const std::string& key, double value) +{ + if (key.empty()) + return true; + auto [it, inserted] = outgoing_doubles_.emplace(key, value); + if (inserted) + return true; + if (it->second == value) + return false; + it->second = value; + return true; +} + +void Helm::Impl::notify(const std::string& var, const Value& value) +{ + posted_.emplace_back(var, value); +} + +Posting& Helm::Impl::add_posting(Posting::Origin origin, const std::string& behavior, + const std::string& var, const Value& value) +{ + Posting& posting = output_.postings.emplace_back(); + posting.set_time(micros(curr_time_)); + posting.set_origin(origin); + posting.set_behavior(behavior); + posting.set_key(var); + set_value(posting, value); + posting.set_iteration(iteration_); + return posting; +} + +void Helm::Impl::add_behavior_event(BehaviorEvent::Kind kind, const std::string& behavior, + const std::string& text) +{ + BehaviorEvent& event = output_.behavior_events.emplace_back(); + event.set_time(micros(curr_time_)); + event.set_behavior(behavior); + event.set_kind(kind); + event.set_text(text); + event.set_iteration(iteration_); +} + +Helm::Helm(const HelmConfig& cfg, std::optional geodesy) + : impl_(std::make_unique(cfg, std::move(geodesy))) +{ +} + +Helm::~Helm() = default; + +void Helm::handle(const Navigation& navigation) { impl_->queue(navigation); } +void Helm::handle(const Variable& variable) { impl_->queue(variable); } +void Helm::handle(const Command& command) { impl_->queue(command); } + +const Output& Helm::iterate(double time) { return impl_->iterate(time); } + +Status::State Helm::state() const { return impl_->state(); } + +const std::vector& Helm::config_warnings() const { return impl_->config_warnings(); } + +} // namespace helm +} // namespace ivpico diff --git a/src/lib/helm/helm.h b/src/lib/helm/helm.h new file mode 100644 index 0000000..1e50684 --- /dev/null +++ b/src/lib/helm/helm.h @@ -0,0 +1,66 @@ +#ifndef IVPICO_HELM_HELM_H +#define IVPICO_HELM_HELM_H + +#include +#include +#include +#include +#include + +#include "interface/config.pb.h" +#include "interface/ivp.pb.h" + +namespace ivpico +{ +namespace helm +{ +// Conversions between latitude/longitude (degrees) and the local x/y grid (meters) +struct Geodesy +{ + std::function to_local; + std::function to_global; +}; + +// Everything the helm published on one iteration +struct Output +{ + std::optional solution; + ivp::interface::Status status; + std::vector postings; + std::vector behavior_events; + std::vector visualizations; + std::vector offboard_messages; + std::vector life_events; + std::vector objective_functions; + std::optional configuration; +}; + +// pHelmIvP without MOOS: inputs are queued as they arrive and applied at the +// start of the next iterate(), as pHelmIvP applies its mail. +class Helm +{ + public: + explicit Helm(const ivp::interface::HelmConfig& cfg, + std::optional geodesy = std::nullopt); + ~Helm(); + + void handle(const ivp::interface::Navigation& navigation); + void handle(const ivp::interface::Variable& variable); + void handle(const ivp::interface::Command& command); + + // time: seconds since the UNIX epoch (warped, if the caller warps time) + const Output& iterate(double time); + + ivp::interface::Status::State state() const; + // configuration and .bhv problems; any of them leaves the helm in MALCONFIG + const std::vector& config_warnings() const; + + private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace helm +} // namespace ivpico + +#endif diff --git a/src/lib/interface/config.proto b/src/lib/interface/config.proto new file mode 100644 index 0000000..17258ab --- /dev/null +++ b/src/lib/interface/config.proto @@ -0,0 +1,80 @@ +syntax = "proto3"; + +package ivp.interface; + +import "interface/options.proto"; +import "openocean/messages/common.proto"; + +// The equivalent of pHelmIvP's configuration block. Unset optional fields +// take pHelmIvP's defaults. +message HelmConfig +{ + // .bhv files ("behaviors"), read in sorted order + repeated string behavior_file = 1; + + message Domain + { + string name = 1; + double low = 2; + double high = 3; + oneof resolution + { + uint32 points = 4; + double delta = 5; + } + // a decision isn't required for this variable + bool optional = 6 [(ivp.field).level = ADVANCED]; + } + repeated Domain domain = 2; + + // what behaviors know as "us" (pHelmIvP uses the MOOS community) + string ownship = 3; + + bool start_in_drive = 4; + // default true + optional bool allow_park = 5; + // default true + optional bool park_on_allstop = 6; + // seconds before warning that NAV_X, NAV_Y, etc. are still missing; default 5 + optional double nav_grace = 7; + + bool goals_mandatory = 100 [(ivp.field).level = ADVANCED]; + string helm_alias = 101 [(ivp.field).level = ADVANCED]; + // default true + optional bool seed_random = 102 [(ivp.field).level = ADVANCED]; + // seconds, 10 to 1800; default 45 + optional double contact_max_age = 103 [(ivp.field).level = ADVANCED]; + // default 500 + optional uint32 contact_max_count = 104 [(ivp.field).level = ADVANCED]; + + message PlatformModel + { + enum Algorithm + { + HOLONOMIC = 0; + DUBINS = 1; + } + Algorithm algorithm = 1; + optional double radius = 2; + optional double spoke_degs = 3; + } + PlatformModel platform_model = 105 [(ivp.field).level = ADVANCED]; + + // directories of libBHV_*.so behaviors + repeated string behavior_dir = 106 [(ivp.field).level = ADVANCED]; + bool behavior_dir_not_found_ok = 107 [(ivp.field).level = ADVANCED]; + + // seconds an input may lag the helm's clock before it's ignored; negative + // accepts any; default 60 + optional double ok_skew = 108 [(ivp.field).level = ADVANCED]; + // seconds to wait for a value from elsewhere before posting an + // "initialize_" (deferred) variable; default 1 + optional double defer_window = 109 [(ivp.field).level = ADVANCED]; + // which Navigation.speed entry becomes NAV_SPEED, if there's one + // with this mode + openocean.SpeedMode speed_mode = 110 [(ivp.field).level = ADVANCED]; + + bool report_objective_functions = 200 [(ivp.field).level = DEVELOPER]; + // default true; false leaves alerts to an outside contact manager + optional bool contact_alerts = 201 [(ivp.field).level = DEVELOPER]; +} diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 6631915..b780187 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -1,3 +1,7 @@ add_executable(check_helm_libs check_helm_libs.cpp) target_link_libraries(check_helm_libs PRIVATE IVP::helmivp IVP::ivpbuild IVP::ivpsolve IVP::ivpcore) add_test(NAME helm_libs COMMAND check_helm_libs) + +add_executable(test_helm helm/test_helm.cpp) +target_link_libraries(test_helm PRIVATE ivpico_helm) +add_test(NAME helm COMMAND test_helm) diff --git a/src/test/helm/test_helm.cpp b/src/test/helm/test_helm.cpp new file mode 100644 index 0000000..b5da4f2 --- /dev/null +++ b/src/test/helm/test_helm.cpp @@ -0,0 +1,466 @@ +// Scenario tests for ivpico::helm::Helm, the MOOS-free port of pHelmIvP. +// +// Each scenario writes a small .bhv file, configures a helm with a course and +// speed domain, and drives it with a fake clock: inputs go in with +// Helm::handle(), then Helm::iterate(t) runs one helm iteration at time t, as +// pHelmIvP handles its mail and then iterates. Where a scenario needs the +// vehicle to move, a simple kinematic model turns the helm's decisions into the +// next Navigation input. +// +// Prints one line per failed check and exits non-zero if there were any. + +#include +#include +#include +#include +#include +#include + +#include "helm/helm.h" + +using ivp::interface::Command; +using ivp::interface::HelmConfig; +using ivp::interface::Navigation; +using ivp::interface::Posting; +using ivp::interface::Solution; +using ivp::interface::Status; +using ivp::interface::Variable; +using ivpico::helm::Helm; +using ivpico::helm::Output; + +namespace +{ +int errors = 0; + +void check(bool ok, const std::string& scenario, const std::string& what) +{ + if (!ok) + { + std::cerr << "failed: " << scenario << ": " << what << "\n"; + ++errors; + } +} + +constexpr double t0 = 1.7e9; +constexpr double dt = 0.25; + +std::string write_bhv(const std::string& name, const std::string& contents) +{ + const auto path = std::filesystem::temp_directory_path() / ("ivpico_test_" + name + ".bhv"); + std::ofstream(path) << contents; + return path.string(); +} + +HelmConfig config(const std::string& bhv_file) +{ + HelmConfig cfg; + cfg.add_behavior_file(bhv_file); + cfg.set_ownship("alpha"); + auto* course = cfg.add_domain(); + course->set_name("course"); + course->set_low(0); + course->set_high(359); + course->set_points(360); + auto* speed = cfg.add_domain(); + speed->set_name("speed"); + speed->set_low(0); + speed->set_high(4); + speed->set_points(41); + return cfg; +} + +struct Vehicle +{ + double x{0}, y{0}, heading{0}, speed{0}; + + Navigation navigation(double t) const + { + Navigation nav; + nav.set_self(true); + auto& n = *nav.mutable_nav(); + n.set_time(static_cast(t * 1e6)); + n.mutable_enu()->set_e(x); + n.mutable_enu()->set_n(y); + n.mutable_attitude()->set_heading(heading * M_PI / 180); + n.add_speed()->set_value(speed); + return nav; + } + + // Turn and change speed at once, then move for dt + void follow(const Solution& solution) + { + const auto& desired = solution.desired(); + if (desired.has_attitude() && desired.attitude().has_heading()) + heading = desired.attitude().heading() * 180 / M_PI; + if (desired.has_speed() && desired.speed().has_value()) + speed = desired.speed().value(); + x += speed * dt * std::sin(heading * M_PI / 180); + y += speed * dt * std::cos(heading * M_PI / 180); + } +}; + +Variable variable(const std::string& key, const std::string& value) +{ + Variable v; + v.set_key(key); + v.set_sval(value); + return v; +} + +Command manual_override(bool on) +{ + Command c; + c.set_manual_override(on); + return c; +} + +double desired_course(const Solution& s) { return s.desired().attitude().heading() * 180 / M_PI; } + +bool has_posting(const Output& out, const std::string& key, const std::string& value, + Posting::Origin origin) +{ + for (const auto& p : out.postings) + if (p.key() == key && p.sval() == value && p.origin() == origin) + return true; + return false; +} + +std::string mode(const Output& out) +{ + for (const auto& m : out.status.mode()) + if (m.variable() == "MODE") + return m.value(); + return ""; +} + +// The s1_alpha pattern: initialize DEPLOY and RETURN, two modes, a survey +// waypoint whose endflag sets RETURN, and a return waypoint. Checks: +// - "initialize" postings on the first iteration; +// - no decision before navigation arrives, and PARK until the override is lifted; +// - after DEPLOY and the override, DRIVE in mode ACTIVE:SURVEYING, heading north; +// - reaching the waypoint posts RETURN=true. +// On the iteration the survey completes, no behavior produces an objective +// function, so the helm declares NothingToDo. With park_on_allstop (the +// default) it then parks, as pHelmIvP does at the end of s1_alpha's survey. +// Without it, RETURN=true switches to ACTIVE:RETURNING and turns the vehicle +// around. +void waypoint_mission(bool park_on_allstop) +{ + const std::string scenario = + std::string("waypoint_mission (park_on_allstop: ") + (park_on_allstop ? "true)" : "false)"); + auto cfg = config(write_bhv("waypoint_mission", R"( +initialize DEPLOY = false +initialize RETURN = false + +set MODE = ACTIVE { + DEPLOY = true +} INACTIVE + +set MODE = SURVEYING { + MODE = ACTIVE + RETURN != true +} RETURNING + +Behavior = BHV_Waypoint +{ + name = waypt_survey + pwt = 100 + condition = MODE == SURVEYING + endflag = RETURN = true + perpetual = true + speed = 2 + capture_radius = 5 + points = 0,40 +} + +Behavior = BHV_Waypoint +{ + name = waypt_return + pwt = 100 + condition = MODE == RETURNING + speed = 2 + capture_radius = 5 + points = 0,-40 +} +)")); + cfg.set_park_on_allstop(park_on_allstop); + Helm helm(cfg); + check(helm.config_warnings().empty(), scenario, "config warnings"); + + double t = t0; + const Output& first = helm.iterate(t); + check(has_posting(first, "DEPLOY", "false", Posting::INITIALIZE), scenario, + "no initialize DEPLOY=false"); + check(!first.solution, scenario, "solution before navigation"); + check(first.status.state() == Status::PARK, scenario, "not PARK at start"); + + Vehicle vehicle; + helm.handle(vehicle.navigation(t)); + helm.handle(variable("DEPLOY", "true")); + helm.handle(manual_override(false)); + + bool surveying = false, returned = false, turned = false, parked = false; + for (int i = 0; i < 400; ++i) + { + t += dt; + const Output& out = helm.iterate(t); + if (out.solution && out.solution->allstop() == ivp::interface::CLEAR) + { + if (mode(out) == "ACTIVE:SURVEYING" && std::fabs(desired_course(*out.solution)) < 2) + surveying = true; + if (mode(out) == "ACTIVE:RETURNING" && + std::fabs(desired_course(*out.solution) - 180) < 2) + turned = true; + vehicle.follow(*out.solution); + } + for (const auto& p : out.postings) + if (p.key() == "RETURN" && p.sval() == "true" && p.behavior() == "waypt_survey") + returned = true; + if (returned && out.status.state() == Status::PARK) + parked = true; + helm.handle(vehicle.navigation(t)); + } + check(surveying, scenario, "never surveyed north in DRIVE"); + check(returned, scenario, "waypt_survey never posted its endflag RETURN=true"); + if (park_on_allstop) + check(parked && !turned, scenario, "didn't park when the survey completed"); + else + check(turned && !parked, scenario, "never turned south in mode ACTIVE:RETURNING"); +} + +// One behavior posts an update for another: a timer's endflag posts +// CH_UPDATES = heading=180 to a constant-heading behavior that started at 90. +// pHelmIvP only sees that update when the MOOSDB echoes the timer's post +// back, on the next iteration, and the helm library emulates that echo. +// Checks that the course is 90 until the update is posted, still 90 on the +// iteration that posts it, and 180 on the next. +void update_through_echo() +{ + const std::string scenario = "update_through_echo"; + auto cfg = config(write_bhv(scenario, R"( +Behavior = BHV_ConstantHeading +{ + name = ch + heading = 90 + duration = no-time-limit + updates = CH_UPDATES +} + +Behavior = BHV_Timer +{ + name = timer + duration = 3 + endflag = CH_UPDATES = heading=180 +} +)")); + cfg.mutable_domain(1)->set_optional(true); + cfg.set_start_in_drive(true); + Helm helm(cfg); + check(helm.config_warnings().empty(), scenario, "config warnings"); + + Vehicle vehicle; + double t = t0; + int posted_at = -1, turned_at = -1; + for (int i = 0; i < 40; ++i) + { + helm.handle(vehicle.navigation(t)); + const Output& out = helm.iterate(t); + for (const auto& p : out.postings) + if (p.key() == "CH_UPDATES" && posted_at < 0) + posted_at = i; + if (out.solution && out.solution->allstop() == ivp::interface::CLEAR && turned_at < 0 && + std::fabs(desired_course(*out.solution) - 180) < 1) + turned_at = i; + if (out.solution && posted_at >= 0 && i == posted_at) + check(std::fabs(desired_course(*out.solution) - 90) < 1, scenario, + "course changed on the iteration that posted the update"); + t += dt; + } + check(posted_at >= 0, scenario, "timer never posted CH_UPDATES"); + check(turned_at == posted_at + 1, scenario, + "course 180 on iteration " + std::to_string(turned_at) + ", not one after the post (" + + std::to_string(posted_at) + ")"); +} + +// A template behavior spawns when its updates variable names a new instance. +// Checks for a SPAWN life event and the spawned behavior's decision. +void spawn() +{ + const std::string scenario = "spawn"; + auto cfg = config(write_bhv(scenario, R"( +Behavior = BHV_ConstantHeading +{ + name = ch_ + templating = spawn + updates = SPAWN_CH + duration = no-time-limit + heading = 0 +} +)")); + cfg.mutable_domain(1)->set_optional(true); + cfg.set_start_in_drive(true); + cfg.set_park_on_allstop(false); + Helm helm(cfg); + check(helm.config_warnings().empty(), scenario, "config warnings"); + + Vehicle vehicle; + double t = t0; + bool spawned = false, decided = false; + for (int i = 0; i < 20; ++i) + { + helm.handle(vehicle.navigation(t)); + if (i == 8) + helm.handle(variable("SPAWN_CH", "name=one # heading=45")); + const Output& out = helm.iterate(t); + for (const auto& e : out.life_events) + if (e.type() == ivp::interface::LifeEvent::SPAWN && e.behavior() == "ch_one") + spawned = true; + if (out.solution && out.solution->allstop() == ivp::interface::CLEAR && + std::fabs(desired_course(*out.solution) - 45) < 1) + decided = true; + t += dt; + } + check(spawned, scenario, "no SPAWN life event for ch_one"); + check(decided, scenario, "the spawned behavior never decided course 45"); +} + +// Allstop reasons and park-on-allstop: +// - no speed decision with a required speed domain gives +// MISSING_DECISION_VARIABLES and PARK, since park_on_allstop defaults to +// true. pHelmIvP parks during the one iteration's grace it gives a missing +// decision, so the zero solution comes with MANUAL_OVERRIDE; +// - with speed optional, the same behaviors give a CLEAR solution; +// - manual override in DRIVE gives MANUAL_OVERRIDE and PARK. +void allstop() +{ + const std::string scenario = "allstop"; + const std::string bhv = write_bhv(scenario, R"( +Behavior = BHV_ConstantHeading +{ + name = ch + heading = 90 + duration = no-time-limit +} +)"); + { + auto cfg = config(bhv); + cfg.set_start_in_drive(true); + Helm helm(cfg); + Vehicle vehicle; + double t = t0; + bool missing = false, zeroed = false; + for (int i = 0; i < 12; ++i) + { + helm.handle(vehicle.navigation(t)); + const Output& out = helm.iterate(t); + if (out.status.allstop() == ivp::interface::MISSING_DECISION_VARIABLES && + out.status.missing_decision_variables_size() == 1 && + out.status.missing_decision_variables(0) == "speed") + missing = true; + if (out.solution && out.solution->allstop() != ivp::interface::CLEAR && + out.solution->desired().speed().value() == 0) + zeroed = true; + t += dt; + } + check(missing, scenario, "no MISSING_DECISION_VARIABLES for speed"); + check(zeroed, scenario, "no zero solution for the missing speed"); + check(helm.state() == Status::PARK, scenario, "didn't park on allstop"); + } + { + auto cfg = config(bhv); + cfg.set_start_in_drive(true); + cfg.mutable_domain(1)->set_optional(true); + Helm helm(cfg); + Vehicle vehicle; + double t = t0; + bool clear = false, overridden = false; + for (int i = 0; i < 16; ++i) + { + helm.handle(vehicle.navigation(t)); + if (i == 12) + helm.handle(manual_override(true)); + const Output& out = helm.iterate(t); + if (i < 12 && out.solution && out.solution->allstop() == ivp::interface::CLEAR) + clear = true; + if (i >= 12 && out.status.allstop() == ivp::interface::MANUAL_OVERRIDE && + out.status.state() == Status::PARK) + overridden = true; + t += dt; + } + check(clear, scenario, "no CLEAR solution with speed optional"); + check(overridden, scenario, "manual override didn't PARK with MANUAL_OVERRIDE"); + } +} + +// A .bhv file with an unknown behavior parameter leaves the helm in MALCONFIG, +// with a config warning and no solution. +void malconfig() +{ + const std::string scenario = "malconfig"; + auto cfg = config(write_bhv(scenario, R"( +Behavior = BHV_ConstantHeading +{ + name = ch + heading = 90 + no_such_parameter = 1 +} +)")); + cfg.set_start_in_drive(true); + Helm helm(cfg); + check(!helm.config_warnings().empty(), scenario, "no config warning"); + Vehicle vehicle; + helm.handle(vehicle.navigation(t0)); + const Output& out = helm.iterate(t0); + check(out.status.state() == Status::MALCONFIG, scenario, "not MALCONFIG"); + check(!out.solution, scenario, "solution while MALCONFIG"); +} + +// "initialize_" (deferred) variables are posted only if nothing else has +// set them within the defer window: FOO is set from outside first and isn't +// posted; BAR isn't, and is posted. +void deferred_initialize() +{ + const std::string scenario = "deferred_initialize"; + auto cfg = config(write_bhv(scenario, R"( +initialize_ FOO = helm +initialize_ BAR = helm + +Behavior = BHV_ConstantHeading +{ + name = ch + heading = 90 + duration = no-time-limit +} +)")); + cfg.mutable_domain(1)->set_optional(true); + Helm helm(cfg); + Vehicle vehicle; + double t = t0; + bool foo = false, bar = false; + for (int i = 0; i < 12; ++i) + { + helm.handle(vehicle.navigation(t)); + if (i == 1) + helm.handle(variable("FOO", "elsewhere")); + const Output& out = helm.iterate(t); + foo = foo || has_posting(out, "FOO", "helm", Posting::INITIALIZE); + bar = bar || has_posting(out, "BAR", "helm", Posting::INITIALIZE); + t += dt; + } + check(!foo, scenario, "posted FOO, which was already set"); + check(bar, scenario, "never posted BAR"); +} + +} // namespace + +int main() +{ + waypoint_mission(true); + waypoint_mission(false); + update_through_echo(); + spawn(); + allstop(); + malconfig(); + deferred_initialize(); + return errors == 0 ? 0 : 1; +}