diff --git a/.github/workflows/record-missions.yml b/.github/workflows/record-missions.yml index fd87b35..228af75 100644 --- a/.github/workflows/record-missions.yml +++ b/.github/workflows/record-missions.yml @@ -30,7 +30,7 @@ jobs: - name: Init run: | git config --global --add safe.directory '*' - ./init.sh + ./init.sh --no-goby # all of MOOS-IvP, not just the helm libraries apt-get install -y --no-install-recommends make patch diff --git a/CLAUDE.md b/CLAUDE.md index 61184c1..0690848 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -17,3 +17,7 @@ Guidance for Claude when working in this repository. - When in doubt leave the documentation unwritten, but provide an outline for the human developers to use to fill out the rest of the document. - Prefer figures to text when an idea can be better explained graphically. +## Branches + +- Name a feature branch with a short description of the change: `add-goby-test-tool`, `fix-udpm-reassembly`. Name a bug fix branch with the issue it closes after the description: `fix-bad-bug/13` + diff --git a/CMakeLists.txt b/CMakeLists.txt index 61f9a2d..ed1c740 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,15 +11,22 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/") option(BUILD_SHARED_LIBS "Build shared libraries" ON) +include(GNUInstallDirs) +# installed programs find the libraries installed beside them, e.g. in build.sh's build/usr +set(CMAKE_INSTALL_RPATH "$ORIGIN/../${CMAKE_INSTALL_LIBDIR}") + find_package(Protobuf REQUIRED) find_package(openocean_messages 0.1 REQUIRED COMPONENTS cpp) # MOOS-IvP's helm libraries (IVP_BUILD_HELM_LIBS_ONLY) find_package(IVP CONFIG REQUIRED) -find_package(GOBY 3.0 COMPONENTS zeromq) +find_package(GOBY 3.0 COMPONENTS zeromq OPTIONAL_COMPONENTS moos) if(NOT GOBY_FOUND) message(STATUS "Goby3 not found: not building the Goby app") +elseif(GOBY_moos_FOUND) + # goby_moos links MOOS::MOOS + find_package(MOOS 10 QUIET) endif() include(CTest) diff --git a/README.md b/README.md index a14cc87..e6c3ea3 100644 --- a/README.md +++ b/README.md @@ -26,10 +26,10 @@ One way to think of this project is as a generator for the equivalent of the MOO *This section was written by Claude.* -`init.sh` checks out the submodules and installs the Ubuntu packages needed to build them and IvPico. These include Goby3 from [packages.gobysoft.org](http://packages.gobysoft.org), for the Goby app; without Goby3, `build.sh` skips the Goby app. `build.sh` then builds, all in `build/`: +`init.sh` checks out the submodules and installs the Ubuntu packages needed to build them and IvPico. These include Goby3, for the Goby app, adding [packages.gobysoft.org](http://packages.gobysoft.org)'s continuous builds if apt doesn't already have it; `init.sh --no-goby` leaves Goby3 out, and `build.sh` then skips the Goby app. `build.sh` then builds, all in `build/`, and installs each into `build/usr`: - openocean-messages, into `build/external/openocean-messages`; -- MOOS-IvP's helm libraries (`IVP_BUILD_HELM_LIBS_ONLY`, as shared libraries, without MOOS or its apps), into `build/external/moos-ivp`, installed into `build/external/install` for `find_package(IVP)`; +- MOOS-IvP's helm libraries (`IVP_BUILD_HELM_LIBS_ONLY`, as shared libraries, without MOOS or its apps), into `build/external/moos-ivp`, installed first so that IvPico can `find_package(IVP)`; - IvPico, against both. ``` @@ -38,7 +38,7 @@ One way to think of this project is as a generator for the equivalent of the MOO ctest --test-dir build ``` -`build.sh` passes any arguments on to IvPico's CMake, and `BUILD_DIR` overrides the build directory. +`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. To update a submodule, check out the commit you want in it and commit the new pointer, e.g. `git -C external/openocean-messages pull origin main && git add external/openocean-messages`. @@ -48,15 +48,5 @@ To update a submodule, check out the commit you want in it and commit the new po `goby_helm_ivp` runs the helm in Goby3. It publishes and subscribes the `ivp.interface` messages on the `ivpico::helm::*` groups (`src/bin/goby/groups.h`), and can stand in for the MOOS helm behind `goby_frontseat_interface` (`frontseat { enable: true }`). `goby_helm_ivp -e` prints an example configuration. -To run the alpha mission against Goby's basic frontseat simulator: - -``` -export PATH=$PWD/build/src/bin/goby/helm_ivp:$PATH -export GOBY_TOOL_LOAD_SHARED_LIBRARY=$PWD/build/src/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. +`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). diff --git a/build.sh b/build.sh index bfbe048..3076bf5 100755 --- a/build.sh +++ b/build.sh @@ -1,7 +1,7 @@ #!/usr/bin/env bash # Builds the dependencies from their submodules (openocean-messages, and MOOS-IvP's helm libraries), -# then IvPico against them, all in BUILD_DIR (default build). Arguments are passed on to IvPico's -# cmake. +# then IvPico against them, all in BUILD_DIR (default build), and installs them all into +# BUILD_DIR/usr (e.g. add build/usr/bin to PATH). Arguments are passed on to IvPico's cmake. set -euo pipefail cd "$(dirname "$0")" @@ -14,10 +14,10 @@ done BUILD_DIR=$(realpath -m "${BUILD_DIR:-build}") OPENOCEAN_BUILD_DIR=${BUILD_DIR}/external/openocean-messages IVP_BUILD_DIR=${BUILD_DIR}/external/moos-ivp -# find_package(IVP) only works from an installation -IVP_INSTALL_DIR=${BUILD_DIR}/external/install +PREFIX=${BUILD_DIR}/usr BUILD_DIR=${OPENOCEAN_BUILD_DIR} external/openocean-messages/build.sh -DBUILD_TESTING=OFF +cmake --install "${OPENOCEAN_BUILD_DIR}" --prefix "${PREFIX}" > /dev/null cmake -S external/moos-ivp/ivp/src -B "${IVP_BUILD_DIR}" -G Ninja \ -DIVP_BUILD_HELM_LIBS_ONLY=ON -DBUILD_SHARED_LIBS=ON \ @@ -25,10 +25,13 @@ cmake -S external/moos-ivp/ivp/src -B "${IVP_BUILD_DIR}" -G Ninja \ -DIVP_INC_DIRECTORY="${IVP_BUILD_DIR}/include" \ -DIVP_BIN_DIRECTORY="${IVP_BUILD_DIR}/bin" ninja -C "${IVP_BUILD_DIR}" -cmake --install "${IVP_BUILD_DIR}" --prefix "${IVP_INSTALL_DIR}" > /dev/null +# find_package(IVP) only works from an installation +cmake --install "${IVP_BUILD_DIR}" --prefix "${PREFIX}" > /dev/null cmake -S . -B "${BUILD_DIR}" -G Ninja \ -Dopenocean_messages_DIR="${OPENOCEAN_BUILD_DIR}" \ - -DIVP_DIR="${IVP_INSTALL_DIR}/lib/cmake/IVP" \ + -DIVP_DIR="${PREFIX}/lib/cmake/IVP" \ + -DCMAKE_INSTALL_PREFIX="${PREFIX}" \ "$@" ninja -C "${BUILD_DIR}" +cmake --install "${BUILD_DIR}" > /dev/null diff --git a/init.sh b/init.sh index d2db034..7eec28b 100755 --- a/init.sh +++ b/init.sh @@ -1,22 +1,28 @@ #!/usr/bin/env bash # Checks out the submodules and installs the Ubuntu packages needed to build them and IvPico. # -# Usage: init.sh +# Usage: init.sh [--no-goby] +# --no-goby don't install Goby3 (build.sh then skips the Goby app) set -euo pipefail cd "$(dirname "$0")" -case "${1:-}" in - "") ;; - -h | --help) - sed -n '4p' "$0" | sed 's/^# //' - exit 0 - ;; - *) - sed -n '4p' "$0" | sed 's/^# //' >&2 - exit 1 - ;; -esac +usage() { sed -n '4,5p' "$0" | sed 's/^# //'; } + +goby=true +for arg in "$@"; do + case "${arg}" in + --no-goby) goby=false ;; + -h | --help) + usage + exit 0 + ;; + *) + usage >&2 + exit 1 + ;; + esac +done git submodule update --init --recommend-shallow @@ -31,22 +37,32 @@ apt_install() { # moos-ivp's helm libraries (IVP_BUILD_HELM_LIBS_ONLY) need only CMake and a compiler # xz-utils: the replay tests' recorded missions are compressed -packages=(cmake ninja-build g++ git ca-certificates protobuf-compiler libprotobuf-dev curl gpg xz-utils) +packages=(cmake ninja-build g++ git ca-certificates protobuf-compiler libprotobuf-dev xz-utils) $SUDO apt-get update apt_install "${packages[@]}" -# Goby3, for src/bin/goby, from packages.gobysoft.org -keyring=/usr/share/keyrings/gobysoft-archive-keyring.gpg -if [ ! -f "${keyring}" ]; then - curl -fsSL "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0x19478082E2F8D3FE" | - gpg --dearmor | $SUDO tee "${keyring}" > /dev/null +if ${goby}; then + # Goby3, for src/bin/goby. If apt doesn't have it yet, add packages.gobysoft.org's + # continuous builds, which have 'goby test' (newer than 3.6.1) + goby_policy=$(apt-cache policy libgoby3-dev) + if ! grep -q "Candidate: [^(]" <<< "${goby_policy}"; then + apt_install curl gpg + keyring=/usr/share/keyrings/gobysoft-archive-keyring.gpg + if [ ! -f "${keyring}" ]; then + curl -fsSL "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0x19478082E2F8D3FE" | + gpg --dearmor | $SUDO tee "${keyring}" > /dev/null + fi + . /etc/os-release + echo "deb [signed-by=${keyring}] http://packages.gobysoft.org/${ID}/continuous/ ${VERSION_CODENAME}/" | + $SUDO tee /etc/apt/sources.list.d/gobysoft_continuous.list > /dev/null + $SUDO apt-get update + fi + # libgoby3-dev only recommends these, but goby's CMake package links libais.so, + # and goby_add_app() runs protoc-gen-dccl + apt_install libgoby3-dev goby3-apps libais-dev dccl5-compiler + # pMarineViewer for the Goby launches (viewer.launch), through goby_moos_gateway; + # MOOS-IvP's apps need libmoos-ivp but don't depend on it + apt_install libgoby3-moos-dev goby3-moos moosdb10 moos-ivp-apps moos-ivp-gui libmoos-ivp fi -. /etc/os-release -echo "deb [signed-by=${keyring}] http://packages.gobysoft.org/${ID}/release/ ${VERSION_CODENAME}/" | - $SUDO tee /etc/apt/sources.list.d/gobysoft_release.list > /dev/null -$SUDO apt-get update -# libgoby3-dev only recommends these, but goby's CMake package links libais.so, -# and goby_add_app() runs protoc-gen-dccl -apt_install libgoby3-dev goby3-apps libais-dev dccl5-compiler external/openocean-messages/init.sh --cxx diff --git a/launch/goby/alpha/README.md b/launch/goby/alpha/README.md new file mode 100644 index 0000000..f19539a --- /dev/null +++ b/launch/goby/alpha/README.md @@ -0,0 +1,65 @@ +# alpha + +*This section was written by Claude.* + +MOOS-IvP's alpha mission (a survey, then a return) with `goby_helm_ivp` driving Goby's basic frontseat simulator, at time warp 10. + +```mermaid +flowchart LR + helm["goby_helm_ivp"] -- "DesiredCourse" --> fs["goby_frontseat_interface"] + fs <--> sim["goby_basic_frontseat_simulator"] + 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 +``` + +## Running + +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. + +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 + +`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`: + +``` +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. + +## Changing the datum and time warp + +``` +./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. + +## Viewing in OpenCPN + +`goby_opencpn_interface` serves the vehicle's position as AIS on TCP port 54000 (`config/opencpn.pb.cfg`). + +1. Install OpenCPN: `sudo add-apt-repository ppa:opencpn/opencpn && sudo apt install opencpn`. +2. In Options: Connections, add a Network connection: TCP, address 127.0.0.1, port 54000. +3. The vehicle appears as an AIS target near 43.825 N, 70.330 W. Charts are optional (Options: Charts: Chart Downloader). + +See Goby's [visualization interfaces](https://github.com/GobySoft/goby3/blob/3.0/src/doc/markdown/doc507_goby_viz.md) for more. diff --git a/launch/goby/alpha/alpha.launch b/launch/goby/alpha/alpha.launch index 8a9939a..625900e 100755 --- a/launch/goby/alpha/alpha.launch +++ b/launch/goby/alpha/alpha.launch @@ -7,5 +7,7 @@ gobyd config/gobyd.pb.cfg 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 diff --git a/launch/goby/alpha/alpha.moos b/launch/goby/alpha/alpha.moos new file mode 100644 index 0000000..ceeb734 --- /dev/null +++ b/launch/goby/alpha/alpha.moos @@ -0,0 +1,87 @@ +// 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/alpha.bhv b/launch/goby/alpha/config/alpha.bhv index 26b21d5..7847455 100644 --- a/launch/goby/alpha/config/alpha.bhv +++ b/launch/goby/alpha/config/alpha.bhv @@ -12,22 +12,39 @@ Behavior = BHV_Waypoint condition = DEPLOY = true endflag = RETURN = true + endflag = SUMMARY=speed=$[SPEED],time=$[DUR_RUN_TIME] + + configflag = CRUISE_SPD = $[SPEED] + + activeflag = INFO=$[OWNSHIP] + activeflag = INFO=$[BHVNAME] + activeflag = INFO=$[BHVTYPE] + + cycleflag = CINFO=$[OSX],$[OSY] + + wptflag = PREV=$(PX),$(PY) + wptflag = NEXT=$(NX),$(NY) + wptflag = TEST=$(X),$(Y) + wptflag = OSPOS=$(OSX),$(OSY) + wptflag_on_start = true + updates = WPT_UPDATE perpetual = true + speed_alt = 1.2 + use_alt_speed = true lead = 8 lead_damper = 1 lead_to_start = true - speed = 12 // meters per second + speed = 3 // meters per second capture_line = true capture_radius = 5.0 slip_radius = 15.0 efficiency_measure = all - - polygon = 60,-40 : 60,-40 : 60,-160 : 150,-160 : 180,-100 : 150,-40 + polygon = 60,-40:60,-160:150,-160:180,-100:150,-40 order = normal - repeat = 1 + repeat = 0 visual_hints = nextpt_color=yellow visual_hints = nextpt_vertex_size=8 @@ -39,15 +56,15 @@ Behavior = BHV_Waypoint //---------------------------------------------- Behavior=BHV_Waypoint { - name = waypt_return - pwt = 100 - condition = RETURN = true - condition = DEPLOY = true - perpetual = true - updates = RETURN_UPDATE - endflag = RETURN = false - endflag = DEPLOY = false - endflag = MISSION = complete + name = waypt_return + pwt = 100 + condition = RETURN = true + condition = DEPLOY = true + perpetual = true + updates = RETURN_UPDATE + endflag = RETURN = false + endflag = DEPLOY = false + endflag = MISSION = complete speed = 2.0 capture_radius = 2.0 @@ -58,17 +75,17 @@ Behavior=BHV_Waypoint //---------------------------------------------- Behavior=BHV_ConstantSpeed { - name = const_speed - pwt = 200 - condition = SPD=true - condition = DEPLOY = true - perpetual = true - updates = SPEED_UPDATE - endflag = SPD = false + name = const_speed + pwt = 200 + condition = SPD=true + condition = DEPLOY = true + perpetual = true + updates = SPEED_UPDATE + endflag = SPD = false - speed = 0.5 - duration = 10 - duration_reset = CONST_SPD_RESET=true + speed = 0.5 + duration = 10 + duration_reset = CONST_SPD_RESET=true } diff --git a/launch/goby/alpha/config/frontseat.pb.cfg b/launch/goby/alpha/config/frontseat.pb.cfg index c7c86c2..6b8c91f 100644 --- a/launch/goby/alpha/config/frontseat.pb.cfg +++ b/launch/goby/alpha/config/frontseat.pb.cfg @@ -12,7 +12,7 @@ app { } interprocess { - platform: "frontseat_sim_example" + platform: "alpha" } frontseat_cfg { @@ -25,13 +25,13 @@ frontseat_cfg { tcp_address: "127.0.0.1" tcp_port: 54321 start { - lat: 43.825 # deg - lon: -70.330 # deg + lat: 43.825300 # deg + lon: -70.330400 # deg duration: 0 # 0 == no timeout, otherwise timeout in seconds control_freq: 10 # Hz vehicle { - accel: 1.0 # m/s^2 - hdg_rate: 50 # deg/s + accel: 0.25 # m/s^2 + hdg_rate: 4 # deg/s z_rate: 2 # m/s } } diff --git a/launch/goby/alpha/config/gobyd.pb.cfg b/launch/goby/alpha/config/gobyd.pb.cfg index c32d052..73b925c 100644 --- a/launch/goby/alpha/config/gobyd.pb.cfg +++ b/launch/goby/alpha/config/gobyd.pb.cfg @@ -8,5 +8,5 @@ app { } interprocess { - platform: "frontseat_sim_example" + platform: "alpha" } diff --git a/launch/goby/alpha/config/helm_ivp.pb.cfg b/launch/goby/alpha/config/helm_ivp.pb.cfg index 566a6aa..8d890a1 100644 --- a/launch/goby/alpha/config/helm_ivp.pb.cfg +++ b/launch/goby/alpha/config/helm_ivp.pb.cfg @@ -12,7 +12,7 @@ app { } interprocess { - platform: "frontseat_sim_example" + platform: "alpha" } helm { diff --git a/launch/goby/alpha/config/liaison.pb.cfg b/launch/goby/alpha/config/liaison.pb.cfg index a8771f1..f5022dd 100644 --- a/launch/goby/alpha/config/liaison.pb.cfg +++ b/launch/goby/alpha/config/liaison.pb.cfg @@ -8,7 +8,7 @@ app { } interprocess { - platform: "frontseat_sim_example" + platform: "alpha" } http_address: "localhost" diff --git a/launch/goby/alpha/config/moos_gateway.pb.cfg b/launch/goby/alpha/config/moos_gateway.pb.cfg new file mode 100644 index 0000000..435c408 --- /dev/null +++ b/launch/goby/alpha/config/moos_gateway.pb.cfg @@ -0,0 +1,22 @@ +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/config/opencpn.pb.cfg b/launch/goby/alpha/config/opencpn.pb.cfg new file mode 100644 index 0000000..c74f67e --- /dev/null +++ b/launch/goby/alpha/config/opencpn.pb.cfg @@ -0,0 +1,17 @@ +app { + simulation { + time { + use_sim_time: true + warp_factor: 10 + } + } +} + +interprocess { + platform: "alpha" +} + +# OpenCPN connects here (Options: Connections: Network, TCP, 127.0.0.1:54000) +ais_server { + bind_port: 54000 +} diff --git a/launch/goby/alpha/deploy.sh b/launch/goby/alpha/deploy.sh index 568fff4..75931b1 100755 --- a/launch/goby/alpha/deploy.sh +++ b/launch/goby/alpha/deploy.sh @@ -2,7 +2,7 @@ # Deploys the vehicle: DEPLOY=true for the behaviors, and lifts the manual override. set -euo pipefail -export GOBY_INTERPROCESS='platform: "frontseat_sim_example"' +export GOBY_INTERPROCESS='platform: "alpha"' export GOBY_TOOL_LOAD_SHARED_LIBRARY=${GOBY_TOOL_LOAD_SHARED_LIBRARY:-libivpico_interface.so} goby zeromq publish ivpico::helm::variable PROTOBUF/ivp.interface.Variable 'key: "DEPLOY" sval: "true"' diff --git a/launch/goby/alpha/set_datum_warp.sh b/launch/goby/alpha/set_datum_warp.sh new file mode 100755 index 0000000..36ca7c3 --- /dev/null +++ b/launch/goby/alpha/set_datum_warp.sh @@ -0,0 +1,74 @@ +#!/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. +# +# Usage: set_datum_warp.sh [--datum ] [--warp ] +# With no options, prints the current settings. +set -euo pipefail + +cd "$(dirname "$0")" + +usage() { sed -n '5,6p' "$0" | sed 's/^# //'; } +fail() { + echo "$1" >&2 + usage >&2 + exit 1 +} +is_number() { [[ $1 =~ ^-?[0-9]+(\.[0-9]+)?$ ]]; } + +lat="" lon="" warp="" +while [ $# -gt 0 ]; do + case "$1" in + --datum) + [ $# -ge 3 ] && is_number "$2" && is_number "$3" || fail "--datum needs a latitude and longitude" + lat=$2 lon=$3 + shift 3 + ;; + --warp) + [ $# -ge 2 ] && is_number "$2" || fail "--warp needs a number" + warp=$2 + shift 2 + ;; + -h | --help) + usage + exit 0 + ;; + *) fail "unknown option: $1" ;; + esac +done + +goby_cfgs=(config/*.pb.cfg) +image_dir=${IVP_IMAGE_DIRS:-../../../external/moos-ivp/ivp/data} + +# 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 +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' diff --git a/launch/goby/alpha/viewer.launch b/launch/goby/alpha/viewer.launch new file mode 100755 index 0000000..8a67761 --- /dev/null +++ b/launch/goby/alpha/viewer.launch @@ -0,0 +1,7 @@ +#!/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/../../../external/moos-ivp/ivp/data}] 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/bin/goby/CMakeLists.txt b/src/bin/goby/CMakeLists.txt index 3701d9d..be966c4 100644 --- a/src/bin/goby/CMakeLists.txt +++ b/src/bin/goby/CMakeLists.txt @@ -1 +1,7 @@ add_subdirectory(helm_ivp) + +if(GOBY_moos_FOUND AND MOOS_FOUND) + add_subdirectory(moos_gateway) +else() + message(STATUS "Goby's MOOS support not found: not building the goby_moos_gateway plugin") +endif() diff --git a/src/bin/goby/moos_gateway/CMakeLists.txt b/src/bin/goby/moos_gateway/CMakeLists.txt new file mode 100644 index 0000000..e6da492 --- /dev/null +++ b/src/bin/goby/moos_gateway/CMakeLists.txt @@ -0,0 +1,8 @@ +goby_add_library(TARGET ivpico_moos_gateway_plugin + SOURCES moos_gateway.cpp + PROTOS moos_gateway_config.proto + PROTOC_OUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/proto + PROTO_IMPORT_DIRS ${PROJECT_SOURCE_DIR}/src/lib ${openocean_messages_PROTO_DIR} + LINK_LIBRARIES goby_moos ivpico_helm) +target_include_directories(ivpico_moos_gateway_plugin PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) +install(TARGETS ivpico_moos_gateway_plugin LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/src/bin/goby/moos_gateway/moos_gateway.cpp b/src/bin/goby/moos_gateway/moos_gateway.cpp new file mode 100644 index 0000000..c28d507 --- /dev/null +++ b/src/bin/goby/moos_gateway/moos_gateway.cpp @@ -0,0 +1,97 @@ +// goby_moos_gateway plugin that gives a MOOS community what pHelmIvP and its +// navigation would post (NAV_*, the helm's postings, IVPHELM_*), so MOOS-IvP +// tools such as pNodeReporter and pMarineViewer work with goby_helm_ivp. + +#include + +#include +#include +#include +#include +#include + +#include "groups.h" +#include "helm/status_strings.h" +#include "interface/ivp.pb.h" +#include "moos_gateway_config.pb.h" + +namespace ivpico +{ +namespace goby +{ +class MOOSGatewayTranslation : public ::goby::moos::Translator +{ + public: + MOOSGatewayTranslation(const ::goby::apps::moos::protobuf::GobyMOOSGatewayConfig& cfg) + : ::goby::moos::Translator(cfg), cfg_(cfg.GetExtension(protobuf::ivpico)) + { + using ::goby::middleware::frontseat::protobuf::NodeStatus; + goby() + .interprocess() + .subscribe<::goby::middleware::frontseat::groups::node_status, NodeStatus, + ::goby::middleware::MarshallingScheme::PROTOBUF>( + [this](const NodeStatus& status) + { ::goby::moos::convert_and_publish_node_status(status, moos().comms()); }); + goby().interprocess().subscribe( + [this](const ivp::interface::Posting& posting) { post(posting); }); + goby().interprocess().subscribe([this](const ivp::interface::Status& status) + { post(status); }); + + for (const auto& var : cfg_.to_helm()) + moos().add_trigger(var, [this](const CMOOSMsg& msg) { to_helm(msg); }); + } + + private: + void post(const ivp::interface::Posting& posting) + { + if (posting.value_case() == ivp::interface::Posting::kSval) + moos().comms().Notify(posting.key(), posting.sval()); + else + moos().comms().Notify(posting.key(), posting.dval()); + } + + void post(const ivp::interface::Status& status) + { + auto& comms = moos().comms(); + comms.Notify("IVPHELM_STATE", ivp::interface::Status::State_Name(status.state())); + comms.Notify("IVPHELM_ALLSTOP", helm::allstop_string(status)); + const std::string modes = helm::modes_string(status); + comms.Notify("IVPHELM_SUMMARY", "modes=" + (modes.empty() ? "none" : modes)); + } + + void to_helm(const CMOOSMsg& msg) + { + // the helm's own postings of these variables come back from the MOOSDB + if (msg.GetSource() == moos().comms().GetMOOSName()) + return; + + ivp::interface::Variable variable; + variable.set_key(msg.GetKey()); + if (msg.IsString()) + variable.set_sval(msg.GetString()); + else + variable.set_dval(msg.GetDouble()); + goby().interprocess().publish(variable); + } + + protobuf::MOOSGatewayConfig cfg_; +}; +} // namespace goby +} // namespace ivpico + +extern "C" +{ + void goby3_moos_gateway_load( + goby::zeromq::MultiThreadApplication* + handler) + { + handler->launch_thread(); + } + + void goby3_moos_gateway_unload( + goby::zeromq::MultiThreadApplication* + handler) + { + handler->join_thread(); + } +} diff --git a/src/bin/goby/moos_gateway/moos_gateway_config.proto b/src/bin/goby/moos_gateway/moos_gateway_config.proto new file mode 100644 index 0000000..81ee3f2 --- /dev/null +++ b/src/bin/goby/moos_gateway/moos_gateway_config.proto @@ -0,0 +1,20 @@ +syntax = "proto2"; + +import "goby/moos/protobuf/moos_gateway_config.proto"; +import "goby/protobuf/option_extensions.proto"; + +package ivpico.goby.protobuf; + +message MOOSGatewayConfig +{ + repeated string to_helm = 1 [ + (.goby.field).description = + "MOOS variables given to goby_helm_ivp as Variables, such as those posted by " + "pMarineViewer's buttons" + ]; +} + +extend .goby.apps.moos.protobuf.GobyMOOSGatewayConfig +{ + optional MOOSGatewayConfig ivpico = 1100; +} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f901ee3..c16ed7a 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -8,9 +8,13 @@ protobuf_generate( target_include_directories(ivpico_interface PUBLIC $) target_link_libraries(ivpico_interface PUBLIC openocean_messages::openocean_messages) -add_library(ivpico_helm helm/helm.cpp helm/conversions.cpp) +add_library(ivpico_helm helm/helm.cpp helm/conversions.cpp helm/status_strings.cpp) target_include_directories(ivpico_helm PUBLIC $) target_link_libraries(ivpico_helm PUBLIC ivpico_interface PRIVATE IVP::helmivp IVP::geodaid IVP::behaviors IVP::contacts IVP::logic IVP::ivpbuild IVP::ivpsolve IVP::ivpcore IVP::mbutil) + +install(TARGETS ivpico_interface ivpico_helm + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/src/lib/helm/status_strings.cpp b/src/lib/helm/status_strings.cpp new file mode 100644 index 0000000..680396d --- /dev/null +++ b/src/lib/helm/status_strings.cpp @@ -0,0 +1,37 @@ +#include "status_strings.h" + +namespace ivpico +{ +namespace helm +{ +std::string allstop_string(const ivp::interface::Status& status) +{ + switch (status.allstop()) + { + case ivp::interface::CLEAR: return "clear"; + case ivp::interface::MANUAL_OVERRIDE: return "ManualOverride"; + case ivp::interface::BEHAVIOR_ERROR: return "BehaviorError"; + case ivp::interface::NOTHING_TO_DO: return "NothingToDo"; + case ivp::interface::NO_GOAL_BEHAVIOR: return "NoGoalBehavior"; + case ivp::interface::DISABLED_BY_STANDBY_HELM: return "DisabledByStandbyHelm"; + case ivp::interface::MISSING_DECISION_VARIABLES: + { + std::string s = "MissingDecVars:"; + for (int i = 0; i < status.missing_decision_variables_size(); ++i) + s += (i ? "," : "") + status.missing_decision_variables(i); + return s; + } + default: return ""; + } +} + +std::string modes_string(const ivp::interface::Status& status) +{ + std::string s; + for (const auto& mode : status.mode()) + s += (s.empty() ? "" : "#") + mode.variable() + "@" + mode.value(); + return s; +} + +} // namespace helm +} // namespace ivpico diff --git a/src/lib/helm/status_strings.h b/src/lib/helm/status_strings.h new file mode 100644 index 0000000..5827f3f --- /dev/null +++ b/src/lib/helm/status_strings.h @@ -0,0 +1,21 @@ +#ifndef IVPICO_HELM_STATUS_STRINGS_H +#define IVPICO_HELM_STATUS_STRINGS_H + +#include + +#include "interface/ivp.pb.h" + +namespace ivpico +{ +namespace helm +{ +// pHelmIvP's IVPHELM_ALLSTOP value +std::string allstop_string(const ivp::interface::Status& status); + +// The "modes" field of pHelmIvP's IVPHELM_SUMMARY, e.g. MODE@ACTIVE:SURVEYING +std::string modes_string(const ivp::interface::Status& status); + +} // namespace helm +} // namespace ivpico + +#endif diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 105aa71..d4582d1 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -16,8 +16,23 @@ foreach(fixture ${fixtures}) endif() endforeach() -if(GOBY_FOUND) +# 'goby test' is in Goby builds after 3.6.1, which set GOBY_EXECUTABLE +if(GOBY_FOUND AND GOBY_EXECUTABLE) + set(goby_test_defines + -D HELM=$ -D INTERFACE_LIB=$) + add_test(NAME goby_helm_ivp - COMMAND bash ${CMAKE_CURRENT_SOURCE_DIR}/goby/test_goby_helm_ivp.sh - $ $) + COMMAND ${GOBY_EXECUTABLE} test run ${CMAKE_CURRENT_SOURCE_DIR}/goby/helm_ivp/test.yml + ${goby_test_defines}) + + set(simulator_port 54400) + foreach(mission s1_alpha s2_bravo s3_charlie s4_delta) + add_test(NAME closed_loop_${mission} + COMMAND ${GOBY_EXECUTABLE} test run ${CMAKE_CURRENT_SOURCE_DIR}/missions/goby/${mission}/test.yml + ${goby_test_defines} -D MISSION=${mission} -D PORT=${simulator_port}) + set_tests_properties(closed_loop_${mission} PROPERTIES TIMEOUT 180) + math(EXPR simulator_port "${simulator_port} + 1") + endforeach() +elseif(GOBY_FOUND) + message(STATUS "Skipping the Goby tests: this Goby has no 'goby test' (needs a build after 3.6.1)") endif() diff --git a/src/test/goby/helm_ivp/east.bhv b/src/test/goby/helm_ivp/east.bhv new file mode 100644 index 0000000..fef847f --- /dev/null +++ b/src/test/goby/helm_ivp/east.bhv @@ -0,0 +1,6 @@ +Behavior = BHV_ConstantHeading +{ + name = east + heading = 90 + duration = no-time-limit +} diff --git a/src/test/goby/helm_ivp/helm_ivp.pb.cfg b/src/test/goby/helm_ivp/helm_ivp.pb.cfg new file mode 100644 index 0000000..fe83b02 --- /dev/null +++ b/src/test/goby/helm_ivp/helm_ivp.pb.cfg @@ -0,0 +1,8 @@ +app { geodesy { lat_origin: 43.8 lon_origin: -70.3 } } +helm { + behavior_file: "east.bhv" + domain { name: "course" low: 0 high: 359 points: 360 } + domain { name: "speed" low: 0 high: 4 points: 21 optional: true } + start_in_drive: true +} +frontseat { enable: true } diff --git a/src/test/goby/helm_ivp/test.yml b/src/test/goby/helm_ivp/test.yml new file mode 100644 index 0000000..8104b3f --- /dev/null +++ b/src/test/goby/helm_ivp/test.yml @@ -0,0 +1,44 @@ +# goby_helm_ivp's frontseat bridge, end to end: +# +# 1. a constant-heading behavior (course 90) with the helm starting in DRIVE; +# 2. one frontseat NodeStatus gives the helm its navigation, converted to x/y +# with the app's datum (time 0: the helm uses its own clock); +# 3. the helm should then publish a DesiredCourse with heading 90 and a +# HelmStateReport of HELM_DRIVE, and its own Status in DRIVE. +# +# Set with -D: HELM (goby_helm_ivp), INTERFACE_LIB (libivpico_interface.so) +load_shared_library: ["${INTERFACE_LIB}"] + +endpoint: + - { name: vehicle, zeromq: { platform: ivpico_helm_ivp } } + +fixture: + - name: gobyd + command: gobyd -v + env: ['GOBY_INTERPROCESS=platform: "ivpico_helm_ivp"'] + ready: { output_regex: "gobyd is ready" } + - name: helm + command: ${HELM} helm_ivp.pb.cfg + env: ['GOBY_INTERPROCESS=platform: "ivpico_helm_ivp"'] + +test: + name: goby_helm_ivp frontseat bridge + require_app: [{ name: [goby_helm_ivp] }] + run: + - name: navigation in, desired course out + publish: + - group: goby::middleware::frontseat::node_status + type: PROTOBUF/goby.middleware.frontseat.protobuf.NodeStatus + value: 'time: 0 name: "test" global_fix { lat: 43.8 lon: -70.3 } pose { heading: 0 } speed { over_ground: 0 }' + expect: + - group: goby::middleware::frontseat::desired_course + type: goby.middleware.frontseat.protobuf.DesiredCourse + condition: + - { field: heading, op: GE, value: 89.99 } + - { field: heading, op: LE, value: 90.01 } + - group: goby::middleware::frontseat::helm_state + type: goby.middleware.frontseat.protobuf.HelmStateReport + match: "state: HELM_DRIVE" + - group: ivpico::helm::status + type: ivp.interface.Status + match: "state: DRIVE" diff --git a/src/test/goby/test_goby_helm_ivp.sh b/src/test/goby/test_goby_helm_ivp.sh deleted file mode 100755 index 0a0a7c9..0000000 --- a/src/test/goby/test_goby_helm_ivp.sh +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env bash -# Runs goby_helm_ivp with gobyd and checks its frontseat bridge end to end: -# -# 1. a constant-heading behavior (course 90) with the helm starting in DRIVE; -# 2. one frontseat NodeStatus, published with `goby zeromq publish`, gives the -# helm its navigation, converted to x/y with the app's datum; -# 3. the helm should then publish a DesiredCourse with heading 90 and a -# HelmStateReport of HELM_DRIVE, and its own Status in DRIVE. -# -# Usage: test_goby_helm_ivp.sh -set -euo pipefail - -helm=$1 -export GOBY_TOOL_LOAD_SHARED_LIBRARY=$2 -work=$(mktemp -d) -export GOBY_INTERPROCESS="platform: \"ivpico_test_$$\"" -pids=() -cleanup() { - kill "${pids[@]}" 2> /dev/null || true - wait 2> /dev/null || true - rm -rf "${work}" -} -trap cleanup EXIT - -cat > "${work}/test.bhv" << 'BHV' -Behavior = BHV_ConstantHeading -{ - name = east - heading = 90 - duration = no-time-limit -} -BHV -cat > "${work}/helm_ivp.pb.cfg" << CFG -app { geodesy { lat_origin: 43.8 lon_origin: -70.3 } } -helm { - behavior_file: "${work}/test.bhv" - domain { name: "course" low: 0 high: 359 points: 360 } - domain { name: "speed" low: 0 high: 4 points: 21 optional: true } - start_in_drive: true -} -frontseat { enable: true } -CFG - -gobyd > "${work}/gobyd.log" 2>&1 & -pids+=($!) -sleep 1 -"${helm}" "${work}/helm_ivp.pb.cfg" > "${work}/helm.log" 2>&1 & -pids+=($!) -sleep 2 - -goby zeromq publish goby::middleware::frontseat::node_status \ - PROTOBUF/goby.middleware.frontseat.protobuf.NodeStatus \ - "time: $(date +%s) name: \"test\" global_fix { lat: 43.8 lon: -70.3 } pose { heading: 0 } speed { over_ground: 0 }" - -subscribe() { - timeout "$1" goby zeromq subscribe "$2" > "${work}/$3" 2>&1 || true -} -subscribe 4 goby::middleware::frontseat::desired_course desired.txt -subscribe 2 goby::middleware::frontseat::helm_state helm_state.txt -subscribe 2 ivpico::helm::status status.txt - -failed=0 -check() { - if ! grep -q "$2" "${work}/$1"; then - echo "failed: no \"$2\" in $1:" >&2 - tail -5 "${work}/$1" >&2 - failed=1 - fi -} -check desired.txt "heading: 90" -check helm_state.txt "HELM_DRIVE" -check status.txt "state: DRIVE" -if [ ${failed} -ne 0 ]; then - echo "goby_helm_ivp log:" >&2 - tail -20 "${work}/helm.log" >&2 -fi -exit ${failed} diff --git a/src/test/missions/README.md b/src/test/missions/README.md index 5e0b6e8..0be77bf 100644 --- a/src/test/missions/README.md +++ b/src/test/missions/README.md @@ -17,3 +17,22 @@ flowchart LR - `replay ` feeds the recorded inputs to the IvPico helm at pHelmIvP's iteration times and compares what each helm posted. `replay.cpp` describes what's compared and the tolerances. Re-record the fixtures whenever `external/moos-ivp` moves. + +## Closed-loop runs under Goby + +*This section was written by Claude.* + +`goby test` runs `goby//test.yml` (ctest: `closed_loop_`), which flies a mission's recorded `.bhv` with `goby_helm_ivp` against Goby's basic frontseat simulator, with time warp 10: + +```mermaid +flowchart LR + test["goby test"] -- "pokes (Variable)" --> helm["goby_helm_ivp"] + helm -- "Posting, Status" --> test + helm -- "DesiredCourse" --> fs["goby_frontseat_interface
+ basic simulator"] + fs -- "NodeStatus" --> helm +``` + +- `goby/closed_loop.yml` starts the fixtures (`gobyd`, the simulator, the frontseat interface and the helm) and waits for them; each `test.yml` includes it. +- `goby//test.yml` sends the pokes and expects postings by a deadline. Its times are real seconds, a tenth of mission time. +- `goby//helm.pb.cfg` is the helm configuration (the `.moos` file's pHelmIvP block). +- `goby test visualize -D MISSION= ...` draws the plan; a failed run's report names each fixture's log. diff --git a/src/test/missions/goby/closed_loop.yml b/src/test/missions/goby/closed_loop.yml new file mode 100644 index 0000000..b43ba6b --- /dev/null +++ b/src/test/missions/goby/closed_loop.yml @@ -0,0 +1,37 @@ +# Fixtures and startup for the closed-loop mission tests (/test.yml): +# goby_helm_ivp flies the mission's recorded .bhv against Goby's basic +# frontseat simulator with time warp 10, so test times are a tenth of mission +# time. Each test continues once the frontseat and helm are running. +# +# Set with -D: MISSION, PORT (simulator), HELM (goby_helm_ivp), +# INTERFACE_LIB (libivpico_interface.so) +load_shared_library: ["${INTERFACE_LIB}"] + +endpoint: + - { name: vehicle, zeromq: { platform: "ivpico_${MISSION}" } } + +fixture: + - name: gobyd + command: gobyd common.pb.cfg -v + env: ['GOBY_INTERPROCESS=platform: "ivpico_${MISSION}"'] + ready: { output_regex: "gobyd is ready" } + - name: simulator + command: goby_basic_frontseat_simulator ${PORT} + - name: frontseat + command: cat common.pb.cfg frontseat.pb.cfg | sed "s/@PORT@/${PORT}/" | goby_frontseat_interface_basic_simulator - + env: ['GOBY_INTERPROCESS=platform: "ivpico_${MISSION}"'] + - name: helm + command: cat common.pb.cfg ${MISSION}/helm.pb.cfg | ${HELM} - + env: ['GOBY_INTERPROCESS=platform: "ivpico_${MISSION}"'] + +test: + require_app: [{ name: [goby_frontseat_interface, goby_helm_ivp] }] + run: + - name: frontseat and helm running + timeout: 10 + expect: + - group: goby::middleware::frontseat::node_status + type: goby.middleware.frontseat.protobuf.NodeStatus + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: state, op: REGEX, value: "^(PARK|DRIVE)$" }] diff --git a/src/test/missions/goby/common.pb.cfg b/src/test/missions/goby/common.pb.cfg new file mode 100644 index 0000000..88a15e9 --- /dev/null +++ b/src/test/missions/goby/common.pb.cfg @@ -0,0 +1,4 @@ +app { + geodesy { lat_origin: 43.8253 lon_origin: -70.3304 } + simulation { time { use_sim_time: true warp_factor: 10 } } +} diff --git a/src/test/missions/goby/frontseat.pb.cfg b/src/test/missions/goby/frontseat.pb.cfg new file mode 100644 index 0000000..176c6e8 --- /dev/null +++ b/src/test/missions/goby/frontseat.pb.cfg @@ -0,0 +1,18 @@ +frontseat_cfg { + require_helm: true + helm_running_timeout: 10 + frontseat_connected_timeout: 10 + status_period: 5 + exit_on_error: false + [goby.middleware.frontseat.protobuf.basic_simulator_config] { + tcp_address: "127.0.0.1" + tcp_port: @PORT@ + start { + lat: 43.8253 + lon: -70.3304 + duration: 0 + control_freq: 10 + vehicle { accel: 1.0 hdg_rate: 50 z_rate: 2 } + } + } +} diff --git a/src/test/missions/goby/s1_alpha/helm.pb.cfg b/src/test/missions/goby/s1_alpha/helm.pb.cfg new file mode 100644 index 0000000..0d35793 --- /dev/null +++ b/src/test/missions/goby/s1_alpha/helm.pb.cfg @@ -0,0 +1,6 @@ +helm { + behavior_file: "../fixtures/s1_alpha/alpha.bhv" + domain { name: "speed" low: 0 high: 4 points: 41 } + domain { name: "course" low: 0 high: 359 points: 360 } +} +frontseat { enable: true } diff --git a/src/test/missions/goby/s1_alpha/test.yml b/src/test/missions/goby/s1_alpha/test.yml new file mode 100644 index 0000000..ba30b82 --- /dev/null +++ b/src/test/missions/goby/s1_alpha/test.yml @@ -0,0 +1,38 @@ +# s1_alpha: survey, then return. +# Times are real seconds: a tenth of mission time. +include: [../closed_loop.yml] + +test: + name: s1_alpha closed loop + run: + - name: mission + mode: PARALLEL + step: + - name: deploy + publish: + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "DEPLOY" sval: "true"' + delay: 0.5 + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "MOOS_MANUAL_OVERRIDE" sval: "false"' + delay: 0.5 + expect: + # recorded at 370 s of mission time + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "RETURN" sval: "true"' + timeout: 46 + - name: no errors + expect: + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: state, op: EQ, value: MALCONFIG }] + count: 0 + timeout: 48 + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: allstop, op: EQ, value: BEHAVIOR_ERROR }] + count: 0 + timeout: 48 diff --git a/src/test/missions/goby/s2_bravo/helm.pb.cfg b/src/test/missions/goby/s2_bravo/helm.pb.cfg new file mode 100644 index 0000000..db4a114 --- /dev/null +++ b/src/test/missions/goby/s2_bravo/helm.pb.cfg @@ -0,0 +1,6 @@ +helm { + behavior_file: "../fixtures/s2_bravo/bravo.bhv" + domain { name: "speed" low: 0 high: 4 points: 21 } + domain { name: "course" low: 0 high: 359 points: 360 } +} +frontseat { enable: true } diff --git a/src/test/missions/goby/s2_bravo/test.yml b/src/test/missions/goby/s2_bravo/test.yml new file mode 100644 index 0000000..d4df408 --- /dev/null +++ b/src/test/missions/goby/s2_bravo/test.yml @@ -0,0 +1,42 @@ +# s2_bravo: survey, then return. +# Times are real seconds: a tenth of mission time. +include: [../closed_loop.yml] + +test: + name: s2_bravo closed loop + run: + - name: mission + mode: PARALLEL + step: + - name: deploy + publish: + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "DEPLOY" sval: "true"' + delay: 0.5 + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "MOOS_MANUAL_OVERRIDE" sval: "false"' + delay: 0.5 + expect: + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "MODE" sval: "ACTIVE:SURVEYING"' + timeout: 1 + # recorded at 214 s of mission time + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "RETURN" sval: "true"' + timeout: 27 + - name: no errors + expect: + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: state, op: EQ, value: MALCONFIG }] + count: 0 + timeout: 30 + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: allstop, op: EQ, value: BEHAVIOR_ERROR }] + count: 0 + timeout: 30 diff --git a/src/test/missions/goby/s3_charlie/helm.pb.cfg b/src/test/missions/goby/s3_charlie/helm.pb.cfg new file mode 100644 index 0000000..5198ead --- /dev/null +++ b/src/test/missions/goby/s3_charlie/helm.pb.cfg @@ -0,0 +1,6 @@ +helm { + behavior_file: "../fixtures/s3_charlie/charlie.bhv" + domain { name: "speed" low: 0 high: 4 points: 21 } + domain { name: "course" low: 0 high: 359 points: 360 } +} +frontseat { enable: true } diff --git a/src/test/missions/goby/s3_charlie/test.yml b/src/test/missions/goby/s3_charlie/test.yml new file mode 100644 index 0000000..1a10a4a --- /dev/null +++ b/src/test/missions/goby/s3_charlie/test.yml @@ -0,0 +1,42 @@ +# s3_charlie: loiter, and still driving at the end. +# Times are real seconds: a tenth of mission time. +include: [../closed_loop.yml] + +test: + name: s3_charlie closed loop + run: + - name: mission + mode: PARALLEL + step: + - name: deploy + publish: + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "DEPLOY" sval: "true"' + delay: 0.5 + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "MOOS_MANUAL_OVERRIDE" sval: "false"' + delay: 0.5 + expect: + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "MODE" sval: "ACTIVE:LOITERING"' + timeout: 1 + - name: no errors + expect: + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: state, op: EQ, value: MALCONFIG }] + count: 0 + timeout: 20 + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: allstop, op: EQ, value: BEHAVIOR_ERROR }] + count: 0 + timeout: 20 + - name: still driving + expect: + - group: ivpico::helm::status + type: ivp.interface.Status + match: "state: DRIVE" diff --git a/src/test/missions/goby/s4_delta/helm.pb.cfg b/src/test/missions/goby/s4_delta/helm.pb.cfg new file mode 100644 index 0000000..c374db7 --- /dev/null +++ b/src/test/missions/goby/s4_delta/helm.pb.cfg @@ -0,0 +1,7 @@ +helm { + behavior_file: "../fixtures/s4_delta/delta.bhv" + domain { name: "depth" low: 0 high: 199 points: 200 } + domain { name: "speed" low: 0 high: 4 points: 41 } + domain { name: "course" low: 0 high: 359 points: 360 } +} +frontseat { enable: true } diff --git a/src/test/missions/goby/s4_delta/test.yml b/src/test/missions/goby/s4_delta/test.yml new file mode 100644 index 0000000..8b3478e --- /dev/null +++ b/src/test/missions/goby/s4_delta/test.yml @@ -0,0 +1,42 @@ +# s4_delta: loiter with periodic surfacing. +# Times are real seconds: a tenth of mission time. +include: [../closed_loop.yml] + +test: + name: s4_delta closed loop + run: + - name: mission + mode: PARALLEL + step: + - name: deploy + publish: + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "DEPLOY" sval: "true"' + delay: 0.5 + - group: ivpico::helm::variable + type: PROTOBUF/ivp.interface.Variable + value: 'key: "MOOS_MANUAL_OVERRIDE" sval: "false"' + delay: 0.5 + expect: + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "MODE" sval: "ACTIVE:LOITERING"' + timeout: 1 + # recorded at 142 s of mission time + - group: ivpico::helm::posting + type: ivp.interface.Posting + match: 'key: "TIME_AT_SURFACE" dval: 1' + timeout: 18 + - name: no errors + expect: + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: state, op: EQ, value: MALCONFIG }] + count: 0 + timeout: 20 + - group: ivpico::helm::status + type: ivp.interface.Status + condition: [{ field: allstop, op: EQ, value: BEHAVIOR_ERROR }] + count: 0 + timeout: 20 diff --git a/src/test/missions/replay.cpp b/src/test/missions/replay.cpp index c93f29b..a84dab0 100644 --- a/src/test/missions/replay.cpp +++ b/src/test/missions/replay.cpp @@ -50,6 +50,7 @@ #include #include "helm/helm.h" +#include "helm/status_strings.h" namespace fs = std::filesystem; using ivp::interface::HelmConfig; @@ -318,27 +319,6 @@ std::string number(double d) return os.str(); } -std::string allstop_string(const Status& status) -{ - switch (status.allstop()) - { - case ivp::interface::CLEAR: return "clear"; - case ivp::interface::MANUAL_OVERRIDE: return "ManualOverride"; - case ivp::interface::BEHAVIOR_ERROR: return "BehaviorError"; - case ivp::interface::NOTHING_TO_DO: return "NothingToDo"; - case ivp::interface::NO_GOAL_BEHAVIOR: return "NoGoalBehavior"; - case ivp::interface::DISABLED_BY_STANDBY_HELM: return "DisabledByStandbyHelm"; - case ivp::interface::MISSING_DECISION_VARIABLES: - { - std::string s = "MissingDecVars:"; - for (int i = 0; i < status.missing_decision_variables_size(); ++i) - s += (i ? "," : "") + status.missing_decision_variables(i); - return s; - } - default: return ""; - } -} - std::string behavior_event_var(ivp::interface::BehaviorEvent::Kind kind) { using ivp::interface::BehaviorEvent; @@ -558,7 +538,7 @@ int main(int argc, char* argv[]) const int tick = static_cast(k); 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, allstop_string(out.status)); + add_change(ivpico, "IVPHELM_ALLSTOP", tick, ivpico::helm::allstop_string(out.status)); std::string active; for (const auto& b : out.status.behavior())