diff --git a/CMakeLists.txt b/CMakeLists.txt index 0872eaf..61f9a2d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,9 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/") option(BUILD_SHARED_LIBS "Build shared libraries" ON) +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) diff --git a/README.md b/README.md index 27f4f40..a14cc87 100644 --- a/README.md +++ b/README.md @@ -12,9 +12,12 @@ One way to think of this project is as a generator for the equivalent of the MOO - src: Source code - lib: Library source - interface: Interface messages (Protobuf) + - helm: The helm, pHelmIvP without MOOS (`ivpico::helm::Helm`) - bin: Binary source - - goby: Goby middleware app + - goby: Goby middleware app (`goby_helm_ivp`) - ros: ROS middleware node + - test: Tests, including replays of MOOS-IvP missions (`test/missions`) +- launch: Example missions - external: Submodules - openocean-messages: [openocean-messages](https://github.com/openocean-software/openocean-messages), for the `Navigation` and `ControlSetpoint` messages - moos-ivp: [MOOS-IvP](https://github.com/GobySoft/moos-ivp), for the IvP solver and behaviors @@ -38,3 +41,22 @@ ctest --test-dir build `build.sh` passes any arguments on to IvPico's CMake, and `BUILD_DIR` overrides the build directory. 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`. + +## Goby app + +*This section was written by Claude.* + +`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. + diff --git a/init.sh b/init.sh index 9fdc3d0..d2db034 100755 --- a/init.sh +++ b/init.sh @@ -45,6 +45,8 @@ fi 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 -apt_install libgoby3-dev goby3-apps +# 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/alpha.launch b/launch/goby/alpha/alpha.launch new file mode 100755 index 0000000..8a9939a --- /dev/null +++ b/launch/goby/alpha/alpha.launch @@ -0,0 +1,11 @@ +#!/usr/bin/env goby_launch + +# start the simulator +[kill=SIGTERM] goby_basic_frontseat_simulator 54321 + +# start the Goby processes +gobyd config/gobyd.pb.cfg +goby_frontseat_interface_basic_simulator config/frontseat.pb.cfg +goby_helm_ivp config/helm_ivp.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/config/alpha.bhv b/launch/goby/alpha/config/alpha.bhv new file mode 100644 index 0000000..26b21d5 --- /dev/null +++ b/launch/goby/alpha/config/alpha.bhv @@ -0,0 +1,74 @@ +//-------- FILE: alpha.bhv ------------- + +initialize DEPLOY = false +initialize RETURN = false + +//---------------------------------------------- +Behavior = BHV_Waypoint +{ + name = waypt_survey + pwt = 100 + condition = RETURN = false + condition = DEPLOY = true + endflag = RETURN = true + + updates = WPT_UPDATE + perpetual = true + + lead = 8 + lead_damper = 1 + lead_to_start = true + speed = 12 // 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 + + order = normal + repeat = 1 + + visual_hints = nextpt_color=yellow + visual_hints = nextpt_vertex_size=8 + visual_hints = nextpt_lcolor=gray70 + visual_hints = vertex_color=dodger_blue, edge_color=white + visual_hints = vertex_size=5, edge_size=1 +} + +//---------------------------------------------- +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 + + speed = 2.0 + capture_radius = 2.0 + slip_radius = 8.0 + points = 0,-20 + } + +//---------------------------------------------- +Behavior=BHV_ConstantSpeed +{ + 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 + +} + diff --git a/launch/goby/alpha/config/frontseat.pb.cfg b/launch/goby/alpha/config/frontseat.pb.cfg new file mode 100644 index 0000000..c7c86c2 --- /dev/null +++ b/launch/goby/alpha/config/frontseat.pb.cfg @@ -0,0 +1,39 @@ +app { + geodesy { + lat_origin: 43.825300 + lon_origin: -70.330400 + } + simulation { + time { + use_sim_time: true + warp_factor: 10 + } + } +} + +interprocess { + platform: "frontseat_sim_example" +} + +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: 54321 + start { + lat: 43.825 # deg + lon: -70.330 # 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 + z_rate: 2 # m/s + } + } + } +} diff --git a/launch/goby/alpha/config/gobyd.pb.cfg b/launch/goby/alpha/config/gobyd.pb.cfg new file mode 100644 index 0000000..c32d052 --- /dev/null +++ b/launch/goby/alpha/config/gobyd.pb.cfg @@ -0,0 +1,12 @@ +app { + simulation { + time { + use_sim_time: true + warp_factor: 10 + } + } +} + +interprocess { + platform: "frontseat_sim_example" +} diff --git a/launch/goby/alpha/config/helm_ivp.pb.cfg b/launch/goby/alpha/config/helm_ivp.pb.cfg new file mode 100644 index 0000000..566a6aa --- /dev/null +++ b/launch/goby/alpha/config/helm_ivp.pb.cfg @@ -0,0 +1,26 @@ +app { + geodesy { + lat_origin: 43.825300 + lon_origin: -70.330400 + } + simulation { + time { + use_sim_time: true + warp_factor: 10 + } + } +} + +interprocess { + platform: "frontseat_sim_example" +} + +helm { + behavior_file: "config/alpha.bhv" + domain { name: "course" low: 0 high: 359 points: 360 } + domain { name: "speed" low: 0 high: 4 points: 41 } +} + +frontseat { + enable: true +} diff --git a/launch/goby/alpha/config/liaison.pb.cfg b/launch/goby/alpha/config/liaison.pb.cfg new file mode 100644 index 0000000..a8771f1 --- /dev/null +++ b/launch/goby/alpha/config/liaison.pb.cfg @@ -0,0 +1,20 @@ +app { + simulation { + time { + use_sim_time: true + warp_factor: 10 + } + } +} + +interprocess { + platform: "frontseat_sim_example" +} + +http_address: "localhost" +http_port: 50001 +pb_commander_config { + sqlite3_database: "/tmp/liaison_commander_autosave.db" +} +pb_scope_config { +} diff --git a/launch/goby/alpha/deploy.sh b/launch/goby/alpha/deploy.sh new file mode 100755 index 0000000..568fff4 --- /dev/null +++ b/launch/goby/alpha/deploy.sh @@ -0,0 +1,9 @@ +#!/usr/bin/env bash +# 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_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"' +goby zeromq publish ivpico::helm::command PROTOBUF/ivp.interface.Command 'manual_override: false' diff --git a/src/bin/goby/CMakeLists.txt b/src/bin/goby/CMakeLists.txt index e69de29..3701d9d 100644 --- a/src/bin/goby/CMakeLists.txt +++ b/src/bin/goby/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(helm_ivp) diff --git a/src/bin/goby/groups.h b/src/bin/goby/groups.h new file mode 100644 index 0000000..4aae45a --- /dev/null +++ b/src/bin/goby/groups.h @@ -0,0 +1,29 @@ +#ifndef IVPICO_GOBY_GROUPS_H +#define IVPICO_GOBY_GROUPS_H + +#include + +namespace ivpico +{ +namespace goby +{ +namespace groups +{ +constexpr ::goby::middleware::Group navigation{"ivpico::helm::navigation"}; +constexpr ::goby::middleware::Group variable{"ivpico::helm::variable"}; +constexpr ::goby::middleware::Group command{"ivpico::helm::command"}; + +constexpr ::goby::middleware::Group solution{"ivpico::helm::solution"}; +constexpr ::goby::middleware::Group posting{"ivpico::helm::posting"}; +constexpr ::goby::middleware::Group status{"ivpico::helm::status"}; +constexpr ::goby::middleware::Group behavior_event{"ivpico::helm::behavior_event"}; +constexpr ::goby::middleware::Group visualization{"ivpico::helm::visualization"}; +constexpr ::goby::middleware::Group offboard_message{"ivpico::helm::offboard_message"}; +constexpr ::goby::middleware::Group life_event{"ivpico::helm::life_event"}; +constexpr ::goby::middleware::Group objective_function{"ivpico::helm::objective_function"}; +constexpr ::goby::middleware::Group configuration{"ivpico::helm::configuration"}; +} // namespace groups +} // namespace goby +} // namespace ivpico + +#endif diff --git a/src/bin/goby/helm_ivp/CMakeLists.txt b/src/bin/goby/helm_ivp/CMakeLists.txt new file mode 100644 index 0000000..8d4de55 --- /dev/null +++ b/src/bin/goby/helm_ivp/CMakeLists.txt @@ -0,0 +1,7 @@ +goby_add_app(TARGET goby_helm_ivp + SOURCES helm_ivp.cpp + PROTOS helm_ivp_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_zeromq ivpico_helm) +target_include_directories(goby_helm_ivp PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) diff --git a/src/bin/goby/helm_ivp/helm_ivp.cpp b/src/bin/goby/helm_ivp/helm_ivp.cpp new file mode 100644 index 0000000..0b7a35c --- /dev/null +++ b/src/bin/goby/helm_ivp/helm_ivp.cpp @@ -0,0 +1,259 @@ +#include + +#include + +#include +#include +#include +#include +#include + +#include "groups.h" +#include "helm/helm.h" +#include "helm_ivp_config.pb.h" + +using goby::glog; +namespace frontseat = goby::middleware::frontseat; + +namespace ivpico +{ +namespace goby +{ +class HelmIvP : public ::goby::zeromq::SingleThreadApplication +{ + public: + HelmIvP(); + + private: + void loop() override; + void health(::goby::middleware::protobuf::ThreadHealth& health) override; + + ivp::interface::HelmConfig helm_config(); + std::optional make_geodesy(); + void handle_node_status(const ::goby::middleware::frontseat::protobuf::NodeStatus& status); + void publish(const helm::Output& out); + void publish_frontseat(const helm::Output& out); + void log(const ivp::interface::BehaviorEvent& event); + + helm::Helm helm_; + ivp::interface::Status::State state_{ivp::interface::Status::STATE_UNKNOWN}; + std::string halt_message_; +}; +} // namespace goby +} // namespace ivpico + +namespace +{ +constexpr double degrees(double radians) { return radians * 180.0 / M_PI; } +constexpr double radians(double degrees) { return degrees * M_PI / 180.0; } +} // namespace + +int main(int argc, char* argv[]) { return goby::run(argc, argv); } + +ivpico::goby::HelmIvP::HelmIvP() + // cfg() isn't available until the base classes are constructed + : ::goby::zeromq::SingleThreadApplication(1 * boost::units::si::hertz), + helm_(helm_config(), make_geodesy()) +{ + set_loop_frequency_hertz(cfg().loop_freq()); + + for (const auto& warning : helm_.config_warnings()) + glog.is_warn() && glog << "Helm configuration: " << warning << std::endl; + + interprocess().subscribe([this](const ivp::interface::Navigation& nav) + { helm_.handle(nav); }); + interprocess().subscribe([this](const ivp::interface::Variable& variable) + { helm_.handle(variable); }); + interprocess().subscribe([this](const ivp::interface::Command& command) + { helm_.handle(command); }); + + if (cfg().frontseat().enable() && cfg().frontseat().use_node_status()) + interprocess().subscribe( + [this](const ::goby::middleware::frontseat::protobuf::NodeStatus& status) + { handle_node_status(status); }); +} + +ivp::interface::HelmConfig ivpico::goby::HelmIvP::helm_config() +{ + auto helm = cfg().helm(); + if (helm.ownship().empty()) + helm.set_ownship(cfg().interprocess().platform()); + return helm; +} + +std::optional ivpico::goby::HelmIvP::make_geodesy() +{ + // goby's datum (app.geodesy or a datum_update) must be set at startup + if (!has_geodesy()) + return std::nullopt; + + helm::Geodesy geodesy; + geodesy.to_local = [this](double lat, double lon, double& x, double& y) + { + auto xy = this->geodesy().convert( + {lat * boost::units::degree::degrees, lon * boost::units::degree::degrees}); + x = xy.x.value(); + y = xy.y.value(); + }; + geodesy.to_global = [this](double x, double y, double& lat, double& lon) + { + auto latlon = + this->geodesy().convert({x * boost::units::si::meters, y * boost::units::si::meters}); + lat = latlon.lat.value(); + lon = latlon.lon.value(); + }; + return geodesy; +} + +void ivpico::goby::HelmIvP::handle_node_status( + const ::goby::middleware::frontseat::protobuf::NodeStatus& status) +{ + ivp::interface::Navigation nav; + nav.set_self(true); + auto& n = *nav.mutable_nav(); + n.set_time(static_cast(std::llround(status.time() * 1e6))); + if (status.has_name()) + n.mutable_vehicle()->set_name(status.name()); + + auto& geodetic = *n.mutable_geodetic(); + geodetic.set_latitude(status.global_fix().lat()); + geodetic.set_longitude(status.global_fix().lon()); + geodetic.set_depth(status.global_fix().depth()); + if (status.global_fix().has_altitude() && !std::isnan(status.global_fix().altitude())) + n.set_altitude(status.global_fix().altitude()); + if (status.has_local_fix()) + { + n.mutable_enu()->set_e(status.local_fix().x()); + n.mutable_enu()->set_n(status.local_fix().y()); + } + + auto& attitude = *n.mutable_attitude(); + if (status.pose().has_heading()) + attitude.set_heading(radians(status.pose().heading())); + if (status.pose().has_pitch()) + attitude.set_pitch(radians(status.pose().pitch())); + if (status.pose().has_roll()) + attitude.set_roll(radians(status.pose().roll())); + + auto& over_ground = *n.add_speed(); + over_ground.set_value(status.speed().over_ground()); + over_ground.set_mode(openocean::SPEED_MODE_OVER_GROUND); + if (status.speed().has_over_water()) + { + auto& over_water = *n.add_speed(); + over_water.set_value(status.speed().over_water()); + over_water.set_mode(openocean::SPEED_MODE_OVER_WATER); + } + + helm_.handle(nav); +} + +void ivpico::goby::HelmIvP::loop() +{ + const auto& out = helm_.iterate(::goby::time::SystemClock::now<::goby::time::SITime>().value()); + state_ = out.status.state(); + halt_message_ = out.status.halt_message(); + publish(out); + if (cfg().frontseat().enable()) + publish_frontseat(out); +} + +void ivpico::goby::HelmIvP::publish(const helm::Output& out) +{ + if (out.solution) + interprocess().publish(*out.solution); + interprocess().publish(out.status); + for (const auto& p : out.postings) interprocess().publish(p); + for (const auto& e : out.behavior_events) + { + log(e); + interprocess().publish(e); + } + for (const auto& v : out.visualizations) interprocess().publish(v); + for (const auto& m : out.offboard_messages) interprocess().publish(m); + for (const auto& e : out.life_events) interprocess().publish(e); + for (const auto& f : out.objective_functions) + interprocess().publish(f); + if (out.configuration) + interprocess().publish(*out.configuration); +} + +void ivpico::goby::HelmIvP::publish_frontseat(const helm::Output& out) +{ + using ::goby::middleware::frontseat::protobuf::HelmStateReport; + + if (cfg().frontseat().publish_desired_course() && out.solution) + { + const auto& desired = out.solution->desired(); + ::goby::middleware::frontseat::protobuf::DesiredCourse course; + course.set_time(static_cast(desired.time()) * 1e-6); + if (desired.has_attitude() && desired.attitude().has_heading()) + course.set_heading(degrees(desired.attitude().heading())); + if (desired.has_speed() && desired.speed().has_value()) + course.set_speed(desired.speed().value()); + if (desired.has_depth()) + course.set_depth(desired.depth()); + if (desired.has_altitude()) + course.set_altitude(desired.altitude()); + if (desired.has_attitude() && desired.attitude().has_pitch()) + course.set_pitch(degrees(desired.attitude().pitch())); + if (desired.has_attitude() && desired.attitude().has_roll()) + course.set_roll(degrees(desired.attitude().roll())); + interprocess().publish(course); + } + + if (cfg().frontseat().publish_helm_state()) + { + HelmStateReport report; + switch (out.status.state()) + { + case ivp::interface::Status::DRIVE: + report.set_state(::goby::middleware::frontseat::protobuf::HELM_DRIVE); + break; + case ivp::interface::Status::PARK: + report.set_state(::goby::middleware::frontseat::protobuf::HELM_PARK); + break; + default: + report.set_state(::goby::middleware::frontseat::protobuf::HELM_NOT_RUNNING); + break; + } + interprocess().publish(report); + } +} + +void ivpico::goby::HelmIvP::log(const ivp::interface::BehaviorEvent& event) +{ + using ivp::interface::BehaviorEvent; + const std::string who = event.behavior().empty() ? "helm" : event.behavior(); + switch (event.kind()) + { + case BehaviorEvent::WARNING: + case BehaviorEvent::ERROR: + case BehaviorEvent::CONFIG_WARNING: + case BehaviorEvent::BAD_CONFIG: + glog.is_warn() && glog << who << ": " << event.text() << std::endl; + break; + case BehaviorEvent::EVENT: + case BehaviorEvent::WARNING_RETRACTED: + glog.is_verbose() && glog << who << ": " << event.text() << std::endl; + break; + default: glog.is_debug1() && glog << who << ": " << event.text() << std::endl; break; + } +} + +void ivpico::goby::HelmIvP::health(::goby::middleware::protobuf::ThreadHealth& health) +{ + SingleThreadApplication::health(health); + if (state_ == ivp::interface::Status::MALCONFIG) + { + health.set_state(::goby::middleware::protobuf::HEALTH__FAILED); + std::string message = "MALCONFIG"; + for (const auto& warning : helm_.config_warnings()) message += "; " + warning; + health.set_error_message(message); + } + else if (!halt_message_.empty()) + { + health.set_state(::goby::middleware::protobuf::HEALTH__DEGRADED); + health.set_error_message(halt_message_); + } +} diff --git a/src/bin/goby/helm_ivp/helm_ivp_config.proto b/src/bin/goby/helm_ivp/helm_ivp_config.proto new file mode 100644 index 0000000..0754f7d --- /dev/null +++ b/src/bin/goby/helm_ivp/helm_ivp_config.proto @@ -0,0 +1,48 @@ +syntax = "proto2"; + +import "goby/protobuf/option_extensions.proto"; +import "goby/middleware/protobuf/app_config.proto"; +import "goby/zeromq/protobuf/interprocess_config.proto"; +import "interface/config.proto"; + +package ivpico.goby.protobuf; + +message HelmIvPConfig +{ + optional .goby.middleware.protobuf.AppConfig app = 1; + optional .goby.zeromq.protobuf.InterProcessPortalConfig interprocess = 2 + [(.goby.field) = { cfg { env: "GOBY_INTERPROCESS" } }]; + + optional double loop_freq = 10 [ + default = 4, + (.goby.field).description = "Helm iterations per second (pHelmIvP's AppTick)" + ]; + + optional ivp.interface.HelmConfig helm = 11 [(.goby.field) = { + description: "pHelmIvP's configuration. helm.ownship defaults to interprocess.platform", + example: "behavior_file: \"alpha.bhv\" domain { name: \"course\" low: 0 high: 359 points: 360 } domain { name: \"speed\" low: 0 high: 4 points: 21 }" + }]; + + message FrontSeat + { + optional bool enable = 1 [ + default = false, + (.goby.field).description = "Bridge to goby_frontseat_interface" + ]; + optional bool use_node_status = 2 [ + default = true, + (.goby.field).description = "Use frontseat NodeStatus as the vehicle's navigation" + ]; + optional bool publish_desired_course = 3 [ + default = true, + (.goby.field).description = "Publish each Solution as a frontseat DesiredCourse" + ]; + optional bool publish_helm_state = 4 [ + default = true, + (.goby.field).description = "Publish HelmStateReport every loop" + ]; + } + optional FrontSeat frontseat = 12; + + extensions 1000 to max; +} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 7f6af65..f901ee3 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,6 +1,3 @@ -find_package(Protobuf REQUIRED) -find_package(openocean_messages 0.1 REQUIRED COMPONENTS cpp) - add_library(ivpico_interface) protobuf_generate( TARGET ivpico_interface diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 70d964c..105aa71 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -15,3 +15,9 @@ foreach(fixture ${fixtures}) add_test(NAME replay_${mission} COMMAND replay ${fixture}) endif() endforeach() + +if(GOBY_FOUND) + add_test(NAME goby_helm_ivp + COMMAND bash ${CMAKE_CURRENT_SOURCE_DIR}/goby/test_goby_helm_ivp.sh + $ $) +endif() diff --git a/src/test/goby/test_goby_helm_ivp.sh b/src/test/goby/test_goby_helm_ivp.sh new file mode 100755 index 0000000..0a0a7c9 --- /dev/null +++ b/src/test/goby/test_goby_helm_ivp.sh @@ -0,0 +1,77 @@ +#!/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}