diff --git a/README.md b/README.md index 0b99a42..4a9b6ae 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,15 @@ ctest --test-dir build `build.sh` passes any arguments on to IvPico's CMake, and `BUILD_DIR` overrides the build directory. Add `build/usr/bin` to your `PATH` to run the installed apps. +## Helm library + +*Outline, to be written:* + +- `ivpico::helm::Helm`: construct from a `HelmConfig`, `handle()` the inputs, `iterate()` at the helm's rate. +- Each iteration's `Output` (solution, status, postings, behavior events, visualizations, offboard messages, life events, objective functions, configuration) and its pHelmIvP equivalent. +- Field levels (CORE, ADVANCED, DEVELOPER) and what each adds to `Status` and `HelmConfig`. +- Differences from pHelmIvP: the MOOSDB echo, rejournal, standby helm. + ## Goby app *This section was written by Claude.* @@ -48,7 +57,7 @@ ctest --test-dir build Contacts come in as `Navigation` with `self: false`, or as `NODE_REPORT` variables. The helm raises pContactMgrV20's alerts itself (configured in `helm.contact_manager`), so contact behaviors such as collision avoidance spawn without a separate contact manager. -`launch/goby/alpha` runs the alpha mission against Goby's basic frontseat simulator, with OpenCPN as a chart display: see its [README](launch/goby/alpha/README.md). +`launch/goby/alpha` runs the alpha mission against Goby's basic frontseat simulator, shown on Liaison's map: see its [README](launch/goby/alpha/README.md). ## License diff --git a/init.sh b/init.sh index aeca427..71575fb 100755 --- a/init.sh +++ b/init.sh @@ -43,10 +43,9 @@ apt_install "${packages[@]}" packages=(libopenocean-messages-dev libivp-helm-dev) if ${goby}; then # libgoby3-dev only recommends libais-dev and dccl5-compiler, but goby's CMake package - # links libais.so, and goby_add_app() runs protoc-gen-dccl. MOOS-IvP's apps - # (pMarineViewer for the Goby launches) need libmoos-ivp but don't depend on it - packages+=(libgoby3-dev goby3-apps libais-dev dccl5-compiler - libgoby3-moos-dev goby3-moos moos10-mit-db moos-ivp-apps moos-ivp-gui libmoos-ivp) + # links libais.so, and goby_add_app() runs protoc-gen-dccl. goby3-gui has goby_liaison, + # for the Goby launches' map + packages+=(libgoby3-dev goby3-apps goby3-gui libais-dev dccl5-compiler libgoby3-moos-dev) fi # Adds an apt.oceansoft.org channel to openocean_sources, checking its key's fingerprint diff --git a/launch/goby/alpha/.gitignore b/launch/goby/alpha/.gitignore deleted file mode 100644 index 47241b6..0000000 --- a/launch/goby/alpha/.gitignore +++ /dev/null @@ -1 +0,0 @@ -images/ diff --git a/launch/goby/alpha/README.md b/launch/goby/alpha/README.md index 0bc01b3..7360b91 100644 --- a/launch/goby/alpha/README.md +++ b/launch/goby/alpha/README.md @@ -11,13 +11,9 @@ flowchart LR fs -- "NodeStatus" --> helm fs -- "NodeStatus" --> ocpn["goby_opencpn_interface"] ocpn -- "AIS, TCP 54000" --> opencpn["OpenCPN"] - fs -- "NodeStatus" --> gw["goby_moos_gateway"] - helm -- "Posting, Status" --> gw - gw -- "buttons (Variable)" --> helm - subgraph moos["MOOS (viewer.launch)"] - gw <--> db["MOOSDB"] <--> pnr["pNodeReporter"] - db <--> pmv["pMarineViewer"] - end + fs -- "NodeStatus" --> liaison["goby_liaison
(http://localhost:50001)"] + helm -- "Posting, Status, ..." --> liaison + liaison -- "Variable, Command" --> helm ``` ## Running @@ -26,26 +22,22 @@ From the top of IvPico, after `./build.sh`: ``` export PATH=$PWD/build/usr/bin:$PATH -export GOBY_TOOL_LOAD_SHARED_LIBRARY=$PWD/build/usr/lib/libivpico_interface.so cd launch/goby/alpha goby_launch alpha.launch ./deploy.sh ``` -`deploy.sh` publishes DEPLOY=true and lifts the manual override with `goby zeromq publish`, which can publish any other `Variable` or `Command` the same way. +`deploy.sh` publishes DEPLOY=true and lifts the manual override with `goby zeromq publish`, which can publish any other `Variable` or `Command` the same way, given IvPico's `build/usr/lib/libivpico_interface.so` with `-l`. -As in MOOS-IvP's alpha, the helm parks when the survey is done (pHelmIvP's `park_on_allstop`); deploy again to return. +As in MOOS-IvP's alpha, the vehicle surveys, returns, and stops at the return point; deploy again to repeat. -## Viewing in pMarineViewer +## Viewing in Liaison -`viewer.launch` shows the vehicle in pMarineViewer as in MOOS-IvP's alpha mission, with its buttons (DEPLOY, RETURN, ...) working. Start it after `alpha.launch`, having fetched MOOS-IvP's background images once with `get_images.sh`: +`goby_liaison` (`config/liaison.pb.cfg`) serves http://localhost:50001: -``` -./get_images.sh -goby_launch viewer.launch -``` - -It runs a MOOS community (`alpha.moos`), which `goby_moos_gateway` with IvPico's plugin (`src/bin/goby/moos_gateway`) gives what pHelmIvP and its navigation would post: `NAV_*`, the helm's postings (`VIEW_*`, `WPT_STAT`, ...) and `IVPHELM_*`. The MOOS variables listed in `config/moos_gateway.pb.cfg` go back to the helm as `Variable`s. +- Map: the vehicle and its track. The helm's `Visualization`s (waypoints, ...) aren't drawn yet. +- Scope: the helm's messages (`Status`, `Posting`, ...). +- Commander: publishes a `Variable` (e.g. RETURN=true) or `Command` to the helm, as `deploy.sh` does. ## Changing the datum and time warp @@ -53,7 +45,7 @@ It runs a MOOS community (`alpha.moos`), which `goby_moos_gateway` with IvPico's ./set_datum_warp.sh --datum 42.358456 -71.087589 --warp 5 ``` -sets them in every configuration (and `alpha.moos`), starts the vehicle at the datum, and chooses the MOOS-IvP image covering it for pMarineViewer. With no options it prints the current settings. +sets them in every configuration, starts the vehicle at the datum, and opens Liaison's map there. With no options it prints the current settings. ## Viewing in OpenCPN diff --git a/launch/goby/alpha/alpha.launch b/launch/goby/alpha/alpha.launch index 625900e..90cc08e 100755 --- a/launch/goby/alpha/alpha.launch +++ b/launch/goby/alpha/alpha.launch @@ -9,5 +9,5 @@ goby_frontseat_interface_basic_simulator config/frontseat.pb.cfg goby_helm_ivp config/helm_ivp.pb.cfg # the vehicle as an AIS target for OpenCPN (see README.md) goby_opencpn_interface config/opencpn.pb.cfg -# goby_liaison (from the goby3-gui package) shows and publishes messages at http://localhost:50001 -# goby_liaison config/liaison.pb.cfg +# the map, and the helm's messages, at http://localhost:50001 (see README.md) +goby_liaison config/liaison.pb.cfg diff --git a/launch/goby/alpha/alpha.moos b/launch/goby/alpha/alpha.moos deleted file mode 100644 index ceeb734..0000000 --- a/launch/goby/alpha/alpha.moos +++ /dev/null @@ -1,87 +0,0 @@ -// pMarineViewer for the Goby alpha mission (viewer.launch), as in MOOS-IvP's -// s1_alpha mission. goby_moos_gateway posts NAV_*, the helm's postings and -// IVPHELM_* here, and gives pMarineViewer's buttons back to goby_helm_ivp. - -ServerHost = localhost -ServerPort = 9000 -Community = alpha - -MOOSTimeWarp = 10 - -LatOrigin = 43.825300 -LongOrigin = -70.330400 - -ProcessConfig = pNodeReporter -{ - AppTick = 2 - CommsTick = 2 - - platform_type = kayak - platform_color = yellow - platform_length = 4 -} - -ProcessConfig = pMarineViewer -{ - AppTick = 4 - CommsTick = 4 - - tiff_file = forrest19.tif - //tiff_file = MIT_SP.tif - - set_pan_x = -90 - set_pan_y = -280 - zoom = 0.65 - vehicle_shape_scale = 1.5 - hash_delta = 50 - hash_shade = 0.22 - hash_viewable = true - - trails_point_size = 1 - - //op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa - //op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa - //op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa - //op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa - - // Appcast configuration - appcast_height = 75 - appcast_width = 30 - appcast_viewable = true - appcast_color_scheme = indigo - nodes_font_size = xlarge - procs_font_size = xlarge - appcast_font_size = large - - // datum_viewable = true - // datum_size = 18 - // gui_size = small - - // left_context[survey-point] = DEPLOY=true - // left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false - // left_context[survey-point] = RETURN=false - - right_context[return] = DEPLOY=true - right_context[return] = MOOS_MANUAL_OVERRIDE=false - right_context[return] = RETURN=false - - scope = SUMMARY - scope = RETURN - scope = WPT_STAT - scope = VIEW_SEGLIST - scope = VIEW_POINT - scope = VIEW_POLYGON - scope = MVIEWER_LCLICK - scope = MVIEWER_RCLICK - - button_one = DEPLOY # DEPLOY=true - button_one = MOOS_MANUAL_OVERRIDE=false # RETURN=false - button_two = RETURN # RETURN=true - button_three = SLOWER # WPT_UPDATE=speed=1.5 - button_four = FASTER # WPT_UPDATE=speed=3.5 - - action = MENU_KEY=deploy # DEPLOY = true # RETURN = false - action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false - action = RETURN=true - action = UPDATES_RETURN=speed=1.4 -} diff --git a/launch/goby/alpha/config/liaison.pb.cfg b/launch/goby/alpha/config/liaison.pb.cfg index f5022dd..0e3b00b 100644 --- a/launch/goby/alpha/config/liaison.pb.cfg +++ b/launch/goby/alpha/config/liaison.pb.cfg @@ -13,8 +13,23 @@ interprocess { http_address: "localhost" http_port: 50001 +load_shared_library: "../../../build/usr/lib/libivpico_interface.so" + pb_commander_config { sqlite3_database: "/tmp/liaison_commander_autosave.db" + load_protobuf { + name: "ivp.interface.Variable" + publish_to { group: "ivpico::helm::variable" } + } + load_protobuf { + name: "ivp.interface.Command" + publish_to { group: "ivpico::helm::command" } + } } pb_scope_config { } +pb_map_config { + initial_lat: 43.825300 + initial_lon: -70.330400 + initial_zoom: 16 +} diff --git a/launch/goby/alpha/config/moos_gateway.pb.cfg b/launch/goby/alpha/config/moos_gateway.pb.cfg deleted file mode 100644 index 435c408..0000000 --- a/launch/goby/alpha/config/moos_gateway.pb.cfg +++ /dev/null @@ -1,22 +0,0 @@ -app { - simulation { - time { - use_sim_time: true - warp_factor: 10 - } - } -} - -interprocess { - platform: "alpha" -} - -moos { - server: "localhost" - port: 9000 -} - -[ivpico.goby.protobuf.ivpico] { - # what pMarineViewer's buttons and actions post (alpha.moos) - to_helm: ["DEPLOY", "MOOS_MANUAL_OVERRIDE", "RETURN", "WPT_UPDATE", "UPDATES_RETURN"] -} diff --git a/launch/goby/alpha/deploy.sh b/launch/goby/alpha/deploy.sh index 75931b1..877b717 100755 --- a/launch/goby/alpha/deploy.sh +++ b/launch/goby/alpha/deploy.sh @@ -3,7 +3,10 @@ set -euo pipefail export GOBY_INTERPROCESS='platform: "alpha"' -export GOBY_TOOL_LOAD_SHARED_LIBRARY=${GOBY_TOOL_LOAD_SHARED_LIBRARY:-libivpico_interface.so} +# IvPico's build (build.sh), or else an installed libivpico_interface.so. Given with -l, as +# GOBY_TOOL_LOAD_SHARED_LIBRARY may be set for another project +lib=$(dirname "$0")/../../../build/usr/lib/libivpico_interface.so +[ -f "${lib}" ] || lib=libivpico_interface.so -goby zeromq publish ivpico::helm::variable PROTOBUF/ivp.interface.Variable 'key: "DEPLOY" sval: "true"' -goby zeromq publish ivpico::helm::command PROTOBUF/ivp.interface.Command 'manual_override: false' +goby zeromq publish -l "${lib}" ivpico::helm::variable PROTOBUF/ivp.interface.Variable 'key: "DEPLOY" sval: "true"' +goby zeromq publish -l "${lib}" ivpico::helm::command PROTOBUF/ivp.interface.Command 'manual_override: false' diff --git a/launch/goby/alpha/get_images.sh b/launch/goby/alpha/get_images.sh deleted file mode 100755 index 4793b74..0000000 --- a/launch/goby/alpha/get_images.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env bash -# Downloads the MOOS-IvP background images that set_datum_warp.sh can choose between (those with -# lat/lon bounds) into images/, for pMarineViewer (viewer.launch). -set -euo pipefail - -cd "$(dirname "$0")" -mkdir -p images -url=https://raw.githubusercontent.com/GobySoft/moos-ivp/ivp-without-moos/ivp/data -for image in forrest19 MIT_SP; do - for ext in tif info; do - [ -s "images/${image}.${ext}" ] || curl -fsSL -o "images/${image}.${ext}" "${url}/${image}.${ext}" - done -done -ls images diff --git a/launch/goby/alpha/set_datum_warp.sh b/launch/goby/alpha/set_datum_warp.sh index 28faad8..38b1c9c 100755 --- a/launch/goby/alpha/set_datum_warp.sh +++ b/launch/goby/alpha/set_datum_warp.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash -# Sets the alpha mission's datum (also the simulator's start position) and time warp in its -# Goby configuration and alpha.moos, choosing the MOOS-IvP image that covers the datum. +# Sets the alpha mission's datum (also the simulator's start position and where Liaison's +# map opens) and time warp in its configuration. # # Usage: set_datum_warp.sh [--datum ] [--warp ] # With no options, prints the current settings. @@ -38,37 +38,15 @@ while [ $# -gt 0 ]; do done goby_cfgs=(config/*.pb.cfg) -image_dir=${IVP_IMAGE_DIRS:-images} - -# the smallest image whose .info bounds contain the datum -image_for() { - for info in "${image_dir}"/*.info; do - awk -v lat="$1" -v lon="$2" -v tif="$(basename "${info%.info}").tif" -F= ' - { gsub(/[ \t]/, ""); b[$1] = $2 } - END { - if (!("lat_north" in b) || !("lon_east" in b)) exit - if (lat <= b["lat_north"] && lat >= b["lat_south"] && lon <= b["lon_east"] && lon >= b["lon_west"]) - print (b["lat_north"] - b["lat_south"]) * (b["lon_east"] - b["lon_west"]), tif - }' "${info}" - done | sort -g | head -1 | cut -d' ' -f2 -} if [ -n "${lat}" ]; then sed -i -E "s/(lat_origin: ).*/\1${lat}/; s/(lon_origin: ).*/\1${lon}/" "${goby_cfgs[@]}" sed -i -E "s/^(\s*lat: )[-0-9.]+/\1${lat}/; s/^(\s*lon: )[-0-9.]+/\1${lon}/" config/frontseat.pb.cfg - sed -i -E "s/^(LatOrigin\s*=\s*).*/\1${lat}/; s/^(LongOrigin\s*=\s*).*/\1${lon}/" alpha.moos - image=$(image_for "${lat}" "${lon}") - if [ -n "${image}" ]; then - sed -i -E "s/^(\s*tiff_file\s*=\s*).*/\1${image}/" alpha.moos - else - echo "No image in ${image_dir} covers ${lat}, ${lon}: set tiff_file in alpha.moos" >&2 - fi + sed -i -E "s/(initial_lat: ).*/\1${lat}/; s/(initial_lon: ).*/\1${lon}/" config/liaison.pb.cfg fi if [ -n "${warp}" ]; then sed -i -E "s/(warp_factor: ).*/\1${warp}/" "${goby_cfgs[@]}" - sed -i -E "s/^(MOOSTimeWarp\s*=\s*).*/\1${warp}/" alpha.moos fi -grep -HE "lat_origin|lon_origin|warp_factor|^\s*(lat|lon): " "${goby_cfgs[@]}" | sed -E 's/\s+/ /g' -grep -HE "^(LatOrigin|LongOrigin|MOOSTimeWarp)|^\s*tiff_file" alpha.moos | sed -E 's/\s+/ /g' +grep -HE "lat_origin|lon_origin|initial_lat|initial_lon|warp_factor|^\s*(lat|lon): " "${goby_cfgs[@]}" | sed -E 's/\s+/ /g' diff --git a/launch/goby/alpha/viewer.launch b/launch/goby/alpha/viewer.launch deleted file mode 100755 index 6c69f65..0000000 --- a/launch/goby/alpha/viewer.launch +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env goby_launch - -# pMarineViewer, as in MOOS-IvP's alpha mission (see README.md): start after alpha.launch -MOOSDB alpha.moos -pNodeReporter alpha.moos -[env=IVP_IMAGE_DIRS=${IVP_IMAGE_DIRS:-$PWD/images}] pMarineViewer alpha.moos -[env=GOBY_MOOS_GATEWAY_PLUGINS=${GOBY_MOOS_GATEWAY_PLUGINS:-$PWD/../../../build/usr/lib/libivpico_moos_gateway_plugin.so}] goby_moos_gateway config/moos_gateway.pb.cfg diff --git a/src/lib/helm/helm.cpp b/src/lib/helm/helm.cpp index 1e3b157..120fdbc 100644 --- a/src/lib/helm/helm.cpp +++ b/src/lib/helm/helm.cpp @@ -19,6 +19,7 @@ #include "helm.h" +#include #include #include #include @@ -224,6 +225,12 @@ class Helm::Impl double nav_grace_{5}; double defer_window_{1}; double ok_skew_{60}; + unsigned warning_count_{0}; + // percent of a CPU the helm used between iterations + double cpu_load_{0}; + std::clock_t last_cpu_{0}; + std::chrono::steady_clock::time_point last_wall_; + void update_cpu_load(); bool restart_post_pending_{false}; std::string configuration_; @@ -474,10 +481,26 @@ bool Helm::Impl::skewed(const std::string& var, double msg_time) return true; } +void Helm::Impl::update_cpu_load() +{ + const std::clock_t cpu = std::clock(); + const auto wall = std::chrono::steady_clock::now(); + if (last_cpu_ != 0) + { + const double wall_seconds = std::chrono::duration(wall - last_wall_).count(); + if (wall_seconds > 0) + cpu_load_ = + 100.0 * static_cast(cpu - last_cpu_) / CLOCKS_PER_SEC / wall_seconds; + } + last_cpu_ = cpu; + last_wall_ = wall; +} + const Output& Helm::Impl::iterate(double time) { output_ = Output(); curr_time_ = time; + update_cpu_load(); info_buffer_.setCurrTime(time); ledger_.setCurrTimeUTC(time); @@ -1099,43 +1122,47 @@ void Helm::Impl::post_decisions() output_.solution = solution; } -// HelmIvP::postAllStop +// HelmIvP::postAllStop, but the zero solution goes out every iteration of an allstop: +// a frontseat holds its last command, where pMarinePID stops on stale DESIRED_* void Helm::Impl::post_allstop(const std::string& msg) { - if (!enabled() || msg == allstop_msg_) + if (!enabled()) return; - allstop_msg_ = msg; - allstop_reason_ = msg; - no_decisions_ = - (msg == "NothingToDo" || msg == "NoGoalBehavior" || strBegins(msg, "MissingDecVars")) - ? no_decisions_ + 1 - : 0; + if (msg != allstop_msg_) + { + allstop_msg_ = msg; + allstop_reason_ = msg; + no_decisions_ = + (msg == "NothingToDo" || msg == "NoGoalBehavior" || strBegins(msg, "MissingDecVars")) + ? no_decisions_ + 1 + : 0; + + notify("IVPHELM_ALLSTOP", msg); + if (msg == "clear") + return; - notify("IVPHELM_ALLSTOP", msg); - if (msg == "clear") - return; + // one iteration's grace to let the helm move between modes + if (no_decisions_ == 1) + return; - // one iteration's grace to let the helm move between modes - if (no_decisions_ == 1) - return; + // other allstops keep control, and the helm decides again once they clear + if (allow_park_ && park_on_allstop_ && + (msg == "BehaviorError" || msg == "DisabledByStandbyHelm")) + has_control_ = false; - // other allstops keep control, and the helm decides again once they clear - if (allow_park_ && park_on_allstop_ && - (msg == "BehaviorError" || msg == "DisabledByStandbyHelm")) - has_control_ = false; + for (unsigned i = 0; i < domain_.size(); ++i) + notify(desired_variable(domain_.getVarName(i)), 0.0); + } + else if (msg == "clear") + 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); - } + for (unsigned i = 0; i < domain_.size(); ++i) set_desired(desired, domain_.getVarName(i), 0); output_.solution = solution; } @@ -1163,31 +1190,67 @@ void Helm::Impl::fill_status() mode.set_value(pair.is_string() ? pair.get_sdata() : doubleToStringX(pair.get_ddata())); } - std::map run_states; + // HelmReport's full lists: "name$time$updates" entries separated by ':', and + // for active behaviors "name$time$pwt$pieces$cpu$updates$ipfs$mode$submode" + std::map details; auto add_states = [&](const std::string& list, Status::Behavior::RunState state) { - for (const auto& name : parseString(list, ',')) - if (!name.empty()) - run_states[name] = state; + for (const auto& entry : parseString(list, ':')) + { + const auto fields = parseString(entry, '$'); + if (fields.empty() || fields[0].empty()) + continue; + Status::Behavior& b = details[fields[0]]; + b.Clear(); + b.set_run_state(state); + if (state == Status::Behavior::ACTIVE && fields.size() >= 6) + { + b.set_priority_weight(atof(fields[2].c_str())); + b.set_pieces(static_cast(atoi(fields[3].c_str()))); + b.set_create_cpu(atof(fields[4].c_str())); + b.set_update_summary(fields[5]); + } + else if (fields.size() >= 3) + b.set_update_summary(fields[2]); + } }; // first, since a respawned behavior keeps the name of one that completed - add_states(report_.getCompletedBehaviors(false), Status::Behavior::COMPLETED); - 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(true), Status::Behavior::COMPLETED); + add_states(report_.getIdleBehaviors(true), Status::Behavior::IDLE); + add_states(report_.getRunningBehaviors(true), Status::Behavior::RUNNING); + add_states(report_.getActiveBehaviors(true), Status::Behavior::ACTIVE); + add_states(report_.getDisabledBehaviors(true), Status::Behavior::DISABLED); for (unsigned i = 0; i < bhv_set_->size(); ++i) { auto& b = *s.add_behavior(); - b.set_name(bhv_set_->getDescriptor(i)); + const std::string name = bhv_set_->getDescriptor(i); + auto it = details.find(name); + if (it != details.end()) + b = it->second; + b.set_name(name); 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_completed_count(report_.getCompletedCnt()); + s.set_warning_count(warning_count_); + s.set_cpu_load(cpu_load_); + + s.clear_update_result(); + for (const auto& result : report_.getUpdateResults()) + { + auto& u = *s.add_update_result(); + u.set_behavior(tokStringParse(result, "bhv", ',', '=')); + u.set_variable(tokStringParse(result, "var", ',', '=')); + const std::string r = tokStringParse(result, "result", ',', '='); + u.set_result(r == "success" ? Status::UpdateResult::SUCCESS + : r == "duplicate" ? Status::UpdateResult::DUPLICATE + : r == "name_mismatch" ? Status::UpdateResult::NAME_MISMATCH + : r == "param_error" ? Status::UpdateResult::PARAM_ERROR + : Status::UpdateResult::RESULT_UNKNOWN); + } s.set_behavior_count(bhv_set_->size()); s.set_behavior_count_ever(bhv_set_->getTCount()); s.set_completed_pending(bhv_set_->getCompletedPending()); @@ -1252,6 +1315,8 @@ Posting& Helm::Impl::add_posting(Posting::Origin origin, const std::string& beha void Helm::Impl::add_behavior_event(BehaviorEvent::Kind kind, const std::string& behavior, const std::string& text) { + if (kind == BehaviorEvent::WARNING) + ++warning_count_; BehaviorEvent& event = output_.behavior_events.emplace_back(); event.set_time(micros(curr_time_)); event.set_behavior(behavior); diff --git a/src/test/helm/test_helm.cpp b/src/test/helm/test_helm.cpp index d93abb3..903fb9e 100644 --- a/src/test/helm/test_helm.cpp +++ b/src/test/helm/test_helm.cpp @@ -745,7 +745,9 @@ Behavior = BHV_ConstantHeading Helm helm(cfg); Vehicle vehicle; double t = t0; - bool missing = false, solution = false; + // After one iteration's grace, every iteration of the allstop has a zero solution + bool missing = false, clear = false, held = false; + int missing_iterations = 0; for (int i = 0; i < 12; ++i) { helm.handle(vehicle.navigation(t)); @@ -753,13 +755,21 @@ Behavior = BHV_ConstantHeading 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) - solution = true; + if (++missing_iterations > 1 && + !(out.solution && + out.solution->allstop() == ivp::interface::MISSING_DECISION_VARIABLES && + out.solution->desired().speed().value() == 0)) + held = true; + } + if (out.solution && out.solution->allstop() == ivp::interface::CLEAR) + clear = true; t += dt; } check(missing, scenario, "no MISSING_DECISION_VARIABLES for speed"); - check(!solution, scenario, "a solution without a speed decision"); + check(!clear, scenario, "a CLEAR solution without a speed decision"); + check(!held, scenario, "an iteration of the allstop without a zero solution"); check(helm.state() == Status::DRIVE, scenario, "parked on a missing decision"); } { @@ -788,6 +798,63 @@ Behavior = BHV_ConstantHeading } } +// The mission ends as alpha's does: the only behavior completes, leaving +// NothingToDo. pHelmIvP posts no DESIRED_* from then on, relying on pMarinePID +// to stop on stale input; a frontseat holds its last command instead, so the +// helm must keep commanding zero speed. After one iteration's grace, every +// iteration should have a NOTHING_TO_DO solution with speed 0. +void mission_end() +{ + const std::string scenario = "mission_end"; + auto cfg = config(write_bhv(scenario, R"( +Behavior = BHV_ConstantSpeed +{ + name = cs + speed = 2 + duration = 2 +} + +Behavior = BHV_ConstantHeading +{ + name = ch + heading = 90 + duration = 2 +} +)")); + cfg.set_start_in_drive(true); + Helm helm(cfg); + check(helm.config_warnings().empty(), scenario, "config warnings"); + + Vehicle vehicle; + double t = t0; + bool moving = false; + int nothing_iterations = 0, stopped_iterations = 0; + for (int i = 0; i < 40; ++i) + { + helm.handle(vehicle.navigation(t)); + const Output& out = helm.iterate(t); + if (out.solution && out.solution->allstop() == ivp::interface::CLEAR && + out.solution->desired().speed().value() > 1) + moving = true; + if (out.status.allstop() == ivp::interface::NOTHING_TO_DO) + { + ++nothing_iterations; + if (out.solution && out.solution->allstop() == ivp::interface::NOTHING_TO_DO && + out.solution->desired().speed().value() == 0) + ++stopped_iterations; + } + if (out.solution) + vehicle.follow(*out.solution); + t += dt; + } + check(moving, scenario, "never moved before the behaviors completed"); + check(nothing_iterations > 10, scenario, "never NOTHING_TO_DO after the behaviors completed"); + check(stopped_iterations == nothing_iterations - 1, scenario, + std::to_string(stopped_iterations) + " zero solutions in " + + std::to_string(nothing_iterations) + " iterations of NOTHING_TO_DO"); + check(vehicle.speed == 0, scenario, "still moving at the end"); +} + // A .bhv file with an unknown behavior parameter leaves the helm in MALCONFIG, // with a config warning and no solution. void malconfig() @@ -847,6 +914,72 @@ Behavior = BHV_ConstantHeading check(bar, scenario, "never posted BAR"); } +// The DEVELOPER fields of Status and the objective functions. A timer sends ch +// a good update and then a bad one (an unknown parameter), then completes, so +// the status should show each update's result, the completed count, and ch's +// priority weight, pieces and update summary while it is active. +void developer_status() +{ + const std::string scenario = "developer_status"; + auto cfg = config(write_bhv(scenario, R"( +Behavior = BHV_ConstantHeading +{ + name = ch + pwt = 75 + heading = 90 + duration = no-time-limit + updates = CH_UPDATES +} + +Behavior = BHV_Timer +{ + name = timer + duration = 2 + endflag = CH_UPDATES = heading=180 + endflag = CH_UPDATES = nonsense=1 +} +)")); + cfg.mutable_domain(1)->set_optional(true); + cfg.set_start_in_drive(true); + cfg.set_report_objective_functions(true); + Helm helm(cfg); + check(helm.config_warnings().empty(), scenario, "config warnings"); + + Vehicle vehicle; + double t = t0; + bool ch_details = false, success = false, param_error = false, completed = false, ipf = false; + std::string summary; + for (int i = 0; i < 40; ++i) + { + helm.handle(vehicle.navigation(t)); + const Output& out = helm.iterate(t); + for (const auto& b : out.status.behavior()) + if (b.name() == "ch" && b.run_state() == Status::Behavior::ACTIVE) + { + ch_details = ch_details || (std::fabs(b.priority_weight() - 75) < 1e-6 && + b.pieces() >= 1 && b.has_create_cpu()); + summary = b.update_summary(); + } + for (const auto& u : out.status.update_result()) + { + if (u.behavior() != "ch" || u.variable() != "CH_UPDATES") + continue; + success = success || u.result() == Status::UpdateResult::SUCCESS; + param_error = param_error || u.result() == Status::UpdateResult::PARAM_ERROR; + } + completed = completed || out.status.completed_count() == 1; + for (const auto& of : out.objective_functions) + ipf = ipf || (of.behavior() == "ch" && !of.encoding().empty()); + t += dt; + } + check(ch_details, scenario, "no priority weight 75, pieces and create cpu for active ch"); + check(success, scenario, "no SUCCESS update result for heading=180"); + check(param_error, scenario, "no PARAM_ERROR update result for nonsense=1"); + check(completed, scenario, "completed_count never 1 after the timer completed"); + check(summary != "0/0", scenario, "ch's update summary never counted the updates"); + check(ipf, scenario, "no objective function from ch"); +} + } // namespace int main() @@ -864,7 +997,9 @@ int main() restart(true); ok_skew(); allstop(); + mission_end(); malconfig(); deferred_initialize(); + developer_status(); return errors == 0 ? 0 : 1; } diff --git a/src/test/missions/goby/s1_alpha/test.yml b/src/test/missions/goby/s1_alpha/test.yml index ba30b82..dbc9175 100644 --- a/src/test/missions/goby/s1_alpha/test.yml +++ b/src/test/missions/goby/s1_alpha/test.yml @@ -1,4 +1,4 @@ -# s1_alpha: survey, then return. +# s1_alpha: survey, return, then stop at the return point. # Times are real seconds: a tenth of mission time. include: [../closed_loop.yml] @@ -24,15 +24,41 @@ test: type: ivp.interface.Posting match: 'key: "RETURN" sval: "true"' timeout: 46 + - name: returns home and stops + expect: + # recorded at 459 s of mission time + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "MISSION" sval: "complete"' + timeout: 54 + # waypt_return's point is 0,-20; the vehicle starts at 0,0 and + # never stops on the survey + - group: goby::middleware::frontseat::node_status + type: goby.middleware.frontseat.protobuf.NodeStatus + condition: + - { field: speed.over_ground, op: LT, value: "0.1" } + - { field: local_fix.x, op: GT, value: "-10" } + - { field: local_fix.x, op: LT, value: "10" } + - { field: local_fix.y, op: GT, value: "-30" } + - { field: local_fix.y, op: LT, value: "-10" } + timeout: 56 + - name: stays home + expect: + # the survey and the return stay east of x = 0 + - group: goby::middleware::frontseat::node_status + type: goby.middleware.frontseat.protobuf.NodeStatus + condition: [{ field: local_fix.x, op: LT, value: "-30" }] + count: 0 + timeout: 62 - name: no errors expect: - group: ivpico::helm::status type: ivp.interface.Status condition: [{ field: state, op: EQ, value: MALCONFIG }] count: 0 - timeout: 48 + timeout: 62 - group: ivpico::helm::status type: ivp.interface.Status condition: [{ field: allstop, op: EQ, value: BEHAVIOR_ERROR }] count: 0 - timeout: 48 + timeout: 62 diff --git a/src/test/missions/replay.cpp b/src/test/missions/replay.cpp index 48440c5..46044fc 100644 --- a/src/test/missions/replay.cpp +++ b/src/test/missions/replay.cpp @@ -736,6 +736,7 @@ int main(int argc, char* argv[]) reports.push_back(&e); size_t next_input = 0, next_report = 0; + auto previous_allstop = ivp::interface::ALLSTOP_UNKNOWN; for (size_t k = 0; k < ticks.size(); ++k) { for (; next_input < alog.entries.size() && @@ -755,6 +756,8 @@ int main(int argc, char* argv[]) const Output& out = helm.iterate(alog.logstart + ticks[k]); const int tick = static_cast(k); + const bool allstop_changed = out.status.allstop() != previous_allstop; + previous_allstop = out.status.allstop(); add_change(ivpico, "IVPHELM_STATE", tick, Status::State_Name(out.status.state())); if (out.status.allstop() != ivp::interface::ALLSTOP_UNKNOWN) add_change(ivpico, "IVPHELM_ALLSTOP", tick, ivpico::helm::allstop_string(out.status)); @@ -813,7 +816,11 @@ int main(int argc, char* argv[]) const std::string type = lower(ivp::interface::LifeEvent::Type_Name(e.type())); ivpico["IVPHELM_LIFE_EVENT"].push_back({tick, type + " " + e.behavior()}); } - if (out.solution) + // pHelmIvP posts an allstop's zero DESIRED_* only on the iteration the allstop + // changes (and, after an iteration's grace, never for one with no decisions), + // relying on pMarinePID to stop when they go stale. The helm repeats its zero + // solution every iteration of an allstop, so only the change is compared. + if (out.solution && (out.solution->allstop() == ivp::interface::CLEAR || allstop_changed)) { const auto& d = out.solution->desired(); if (d.has_attitude() && d.attitude().has_heading())