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; +}