From 0d8bee547218cb7e98f2dc98ccef75f802de75a9 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 13 Nov 2026 05:24:31 +0000 Subject: [PATCH 1/4] Fill the DEVELOPER Status fields and update results Each behavior's priority weight, pieces, create CPU and update summary come from HelmReport's full behavior lists. Status also gains the warning and completed counts, the helm's CPU load, and each iteration's update results (IVPHELM_UPDATE_RESULT). A scenario test covers these and the objective functions, and the README gets an outline for a helm library section. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VLcYK4e29B5FiPbDDJy5fp --- README.md | 9 ++++ src/lib/helm/helm.cpp | 85 +++++++++++++++++++++++++++++++------ src/test/helm/test_helm.cpp | 67 +++++++++++++++++++++++++++++ 3 files changed, 149 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 625ce35..03fa53e 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,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.* diff --git a/src/lib/helm/helm.cpp b/src/lib/helm/helm.cpp index 1e3b157..22676d9 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); @@ -1163,31 +1186,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 +1311,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..d2e0331 100644 --- a/src/test/helm/test_helm.cpp +++ b/src/test/helm/test_helm.cpp @@ -847,6 +847,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() @@ -866,5 +932,6 @@ int main() allstop(); malconfig(); deferred_initialize(); + developer_status(); return errors == 0 ? 0 : 1; } From ae9dd1e10dd3de45e3581b1afaf66df0538c43e6 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 13 Nov 2026 06:29:09 +0000 Subject: [PATCH 2/4] alpha: Liaison's map in place of pMarineViewer goby_liaison now runs with the alpha launch: its Map tab shows the vehicle and its track, Scope the helm's messages, and Commander publishes a Variable or Command to the helm in place of pMarineViewer's buttons. set_datum_warp.sh opens the map at the datum. viewer.launch, alpha.moos and get_images.sh go, and init.sh installs goby3-gui instead of MOOSDB, MOOS-IvP's apps and goby3-moos. The goby_moos_gateway plugin still builds, for MOOS communities. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VLcYK4e29B5FiPbDDJy5fp --- README.md | 2 +- init.sh | 7 +- launch/goby/alpha/.gitignore | 1 - launch/goby/alpha/README.md | 25 ++---- launch/goby/alpha/alpha.launch | 4 +- launch/goby/alpha/alpha.moos | 87 -------------------- launch/goby/alpha/config/liaison.pb.cfg | 15 ++++ launch/goby/alpha/config/moos_gateway.pb.cfg | 22 ----- launch/goby/alpha/get_images.sh | 14 ---- launch/goby/alpha/set_datum_warp.sh | 30 +------ launch/goby/alpha/viewer.launch | 7 -- 11 files changed, 34 insertions(+), 180 deletions(-) delete mode 100644 launch/goby/alpha/.gitignore delete mode 100644 launch/goby/alpha/alpha.moos delete mode 100644 launch/goby/alpha/config/moos_gateway.pb.cfg delete mode 100755 launch/goby/alpha/get_images.sh delete mode 100755 launch/goby/alpha/viewer.launch diff --git a/README.md b/README.md index 00e9467..4a9b6ae 100644 --- a/README.md +++ b/README.md @@ -57,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..ef1fff3 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 @@ -36,16 +32,13 @@ goby_launch alpha.launch As in MOOS-IvP's alpha, the helm parks when the survey is done (pHelmIvP's `park_on_allstop`); deploy again to return. -## 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 +46,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/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 From e8931e7ca4819c67f34a466f8fc0b284ac630105 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 13 Nov 2026 06:48:18 +0000 Subject: [PATCH 3/4] Command zero speed on every iteration of an allstop When alpha's return completed (DEPLOY=false), the helm went to NothingToDo and output no solution again: pHelmIvP posts an allstop's zero DESIRED_* only when it changes, and after a no-decision allstop's iteration of grace not at all, relying on pMarinePID to stop on stale input. A Goby frontseat holds its last DesiredCourse, so the vehicle ran on at 2 m/s. The helm now outputs the allstop's zero solution on every iteration of it, after the grace. replay still compares only what pHelmIvP posts. The s1_alpha closed loop now runs to the end of the mission: the vehicle must stop within 10 m of the return point and stay there. test_helm's mission_end checks the zero solutions directly. deploy.sh loads IvPico's own build/usr/lib/libivpico_interface.so, so GOBY_TOOL_LOAD_SHARED_LIBRARY needn't be set. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VLcYK4e29B5FiPbDDJy5fp --- launch/goby/alpha/README.md | 5 +- launch/goby/alpha/deploy.sh | 5 +- src/lib/helm/helm.cpp | 52 ++++++++-------- src/test/helm/test_helm.cpp | 76 ++++++++++++++++++++++-- src/test/missions/goby/s1_alpha/test.yml | 32 +++++++++- src/test/missions/replay.cpp | 9 ++- 6 files changed, 143 insertions(+), 36 deletions(-) diff --git a/launch/goby/alpha/README.md b/launch/goby/alpha/README.md index ef1fff3..8deacfe 100644 --- a/launch/goby/alpha/README.md +++ b/launch/goby/alpha/README.md @@ -22,15 +22,14 @@ 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, with `GOBY_TOOL_LOAD_SHARED_LIBRARY` set to IvPico's `build/usr/lib/libivpico_interface.so`. -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 Liaison diff --git a/launch/goby/alpha/deploy.sh b/launch/goby/alpha/deploy.sh index 75931b1..0f2ded4 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 +lib=$(dirname "$0")/../../../build/usr/lib/libivpico_interface.so +[ -f "${lib}" ] || lib=libivpico_interface.so +export GOBY_TOOL_LOAD_SHARED_LIBRARY=${GOBY_TOOL_LOAD_SHARED_LIBRARY:-${lib}} 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' diff --git a/src/lib/helm/helm.cpp b/src/lib/helm/helm.cpp index 22676d9..120fdbc 100644 --- a/src/lib/helm/helm.cpp +++ b/src/lib/helm/helm.cpp @@ -1122,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; } diff --git a/src/test/helm/test_helm.cpp b/src/test/helm/test_helm.cpp index d2e0331..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() @@ -930,6 +997,7 @@ int main() restart(true); ok_skew(); allstop(); + mission_end(); malconfig(); deferred_initialize(); developer_status(); 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()) From 66fbcb18293fde727022510d34ddc93169ff6d8e Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 13 Nov 2026 06:53:41 +0000 Subject: [PATCH 4/4] deploy.sh: give the interface library with -l A GOBY_TOOL_LOAD_SHARED_LIBRARY already set for another project took the place of IvPico's library, and the publish failed with "Unknown type ivp.interface.Variable". Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VLcYK4e29B5FiPbDDJy5fp --- launch/goby/alpha/README.md | 2 +- launch/goby/alpha/deploy.sh | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/goby/alpha/README.md b/launch/goby/alpha/README.md index 8deacfe..7360b91 100644 --- a/launch/goby/alpha/README.md +++ b/launch/goby/alpha/README.md @@ -27,7 +27,7 @@ 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, with `GOBY_TOOL_LOAD_SHARED_LIBRARY` set to IvPico's `build/usr/lib/libivpico_interface.so`. +`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 vehicle surveys, returns, and stops at the return point; deploy again to repeat. diff --git a/launch/goby/alpha/deploy.sh b/launch/goby/alpha/deploy.sh index 0f2ded4..877b717 100755 --- a/launch/goby/alpha/deploy.sh +++ b/launch/goby/alpha/deploy.sh @@ -3,10 +3,10 @@ set -euo pipefail export GOBY_INTERPROCESS='platform: "alpha"' -# IvPico's build (build.sh), or else an installed 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 -export GOBY_TOOL_LOAD_SHARED_LIBRARY=${GOBY_TOOL_LOAD_SHARED_LIBRARY:-${lib}} -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'