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())