diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index e86747fe..70cd1552 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -36,5 +36,5 @@ colcon build --packages-up-to buoy_tests --event-handlers console_direct+ source $COLCON_WS/install/setup.bash # Test all buoy packages -colcon test --packages-select-regex=buoy --event-handlers console_direct+ +colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ colcon test-result diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index c2805a21..003a21ab 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -15,8 +15,8 @@ else() endif() find_package(ament_cmake REQUIRED) +find_package(buoy_interfaces REQUIRED) find_package(rclcpp REQUIRED) -find_package(buoy_msgs REQUIRED) find_package(ignition-cmake2 REQUIRED) find_package(ignition-plugin1 REQUIRED COMPONENTS register) @@ -76,7 +76,7 @@ function(gz_add_plugin plugin_name) ${gz_add_plugin_PRIVATE_LINK_LIBS} ) if(gz_add_plugin_ROS) - ament_target_dependencies(${plugin_name} PUBLIC rclcpp buoy_msgs) + ament_target_dependencies(${plugin_name} PUBLIC rclcpp buoy_interfaces) endif() target_include_directories(${plugin_name} PUBLIC ${gz_add_plugin_INCLUDE_DIRS}) diff --git a/buoy_gazebo/package.xml b/buoy_gazebo/package.xml index 66aa1fe0..cb024959 100644 --- a/buoy_gazebo/package.xml +++ b/buoy_gazebo/package.xml @@ -11,8 +11,7 @@ ament_cmake buoy_description - buoy_examples - buoy_msgs + buoy_interfaces ignition-gazebo6 robot_state_publisher ros_ign_bridge diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 28043c88..6ff19f99 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -25,12 +25,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -54,10 +54,10 @@ struct PowerControllerROS2 rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_handler_{nullptr}; bool use_sim_time_{true}; - rclcpp::Publisher::SharedPtr pc_pub_{nullptr}; + rclcpp::Publisher::SharedPtr pc_pub_{nullptr}; std::unique_ptr pub_rate_{nullptr}; static const rcl_interfaces::msg::FloatingPointRange valid_pub_rate_range_; - buoy_msgs::msg::PCRecord pc_record_; + buoy_interfaces::msg::PCRecord pc_record_; double pub_rate_hz_{10.0}; }; const rcl_interfaces::msg::FloatingPointRange PowerControllerROS2::valid_pub_rate_range_ = @@ -68,9 +68,10 @@ const rcl_interfaces::msg::FloatingPointRange PowerControllerROS2::valid_pub_rat struct PowerControllerServices { // PCWindCurrCommand -- Winding Current (Torque) - rclcpp::Service::SharedPtr torque_command_service_{nullptr}; - std::function, - std::shared_ptr)> torque_command_handler_; + rclcpp::Service::SharedPtr torque_command_service_{ + nullptr}; + std::function, + std::shared_ptr)> torque_command_handler_; buoy_utils::StopwatchSimTime torque_command_watch_; rclcpp::Duration torque_command_duration_{0, 0U}; @@ -81,9 +82,9 @@ struct PowerControllerServices std::atomic new_torque_command_{false}; // PCScaleCommand -- winding current scale factor - rclcpp::Service::SharedPtr scale_command_service_{nullptr}; - std::function, - std::shared_ptr)> scale_command_handler_; + rclcpp::Service::SharedPtr scale_command_service_{nullptr}; + std::function, + std::shared_ptr)> scale_command_handler_; buoy_utils::StopwatchSimTime scale_command_watch_; rclcpp::Duration scale_command_duration_{0, 0U}; @@ -94,9 +95,10 @@ struct PowerControllerServices std::atomic new_scale_command_{false}; // PCRetractCommand -- winding current retract factor (additional scaling in retract direction) - rclcpp::Service::SharedPtr retract_command_service_{nullptr}; - std::function, - std::shared_ptr)> retract_command_handler_; + rclcpp::Service::SharedPtr retract_command_service_{ + nullptr}; + std::function, + std::shared_ptr)> retract_command_handler_; buoy_utils::StopwatchSimTime retract_command_watch_; rclcpp::Duration retract_command_duration_{0, 0U}; @@ -107,9 +109,10 @@ struct PowerControllerServices std::atomic new_retract_command_{false}; // PCBiasCurrCommand -- winding current bias offset - rclcpp::Service::SharedPtr bias_curr_command_service_{nullptr}; - std::function, - std::shared_ptr)> bias_curr_command_handler_; + rclcpp::Service::SharedPtr bias_curr_command_service_{ + nullptr}; + std::function, + std::shared_ptr)> bias_curr_command_handler_; buoy_utils::StopwatchSimTime bias_curr_command_watch_; rclcpp::Duration bias_curr_command_duration_{0, 0U}; @@ -255,7 +258,7 @@ struct PowerControllerPrivate std::atomic & services_command, std::atomic & new_command) { - int8_t result = buoy_msgs::msg::PBCommandResponse::OK; + int8_t result = buoy_interfaces::msg::PBCommandResponse::OK; std::unique_lock lock(services_->command_mutex_); if (valid_range.from_value > command_value || command_value > valid_range.to_value) @@ -266,7 +269,7 @@ struct PowerControllerPrivate valid_range.from_value), valid_range.to_value); - result = buoy_msgs::msg::PBCommandResponse::BAD_INPUT; + result = buoy_interfaces::msg::PBCommandResponse::BAD_INPUT; } services_command_value = command_value; @@ -283,8 +286,8 @@ struct PowerControllerPrivate // PCWindCurrCommand services_->torque_command_watch_.SetClock(ros_->node_->get_clock()); services_->torque_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO_STREAM( ros_->node_->get_logger(), @@ -308,15 +311,15 @@ struct PowerControllerPrivate } }; services_->torque_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "pc_wind_curr_command", services_->torque_command_handler_); // PCScaleCommand services_->scale_command_watch_.SetClock(ros_->node_->get_clock()); services_->scale_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO_STREAM( ros_->node_->get_logger(), @@ -340,15 +343,15 @@ struct PowerControllerPrivate } }; services_->scale_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "pc_scale_command", services_->scale_command_handler_); // PCRetractCommand services_->retract_command_watch_.SetClock(ros_->node_->get_clock()); services_->retract_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO_STREAM( ros_->node_->get_logger(), @@ -372,15 +375,15 @@ struct PowerControllerPrivate } }; services_->retract_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "pc_retract_command", services_->retract_command_handler_); // PCBiasCurrCommand services_->bias_curr_command_watch_.SetClock(ros_->node_->get_clock()); services_->bias_curr_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO_STREAM( ros_->node_->get_logger(), @@ -404,7 +407,7 @@ struct PowerControllerPrivate } }; services_->bias_curr_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "pc_bias_curr_command", services_->bias_curr_command_handler_); } @@ -624,7 +627,7 @@ void PowerController::Configure( // Publisher std::string topic = _sdf->Get("topic", "power_data").first; this->dataPtr->ros_->pc_pub_ = - this->dataPtr->ros_->node_->create_publisher(topic, 10); + this->dataPtr->ros_->node_->create_publisher(topic, 10); this->dataPtr->ros_->pub_rate_hz_ = _sdf->Get("publish_rate", this->dataPtr->ros_->pub_rate_hz_).first; @@ -647,7 +650,7 @@ void PowerController::Configure( // Only update and publish if not paused. if (this->dataPtr->paused_) {continue;} - buoy_msgs::msg::PCRecord pc_record; + buoy_interfaces::msg::PCRecord pc_record; // high prio data access std::unique_lock next(this->dataPtr->next_access_mutex_); std::unique_lock data(this->dataPtr->data_mutex_); diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp index 82b43ca3..34d5a88a 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp @@ -22,37 +22,37 @@ #include #include -#include +#include #include #include -#include "buoy_msgs/srv/bc_reset_command.hpp" -#include "buoy_msgs/srv/bender_command.hpp" -#include "buoy_msgs/srv/bus_command.hpp" -#include "buoy_msgs/srv/gain_command.hpp" -#include "buoy_msgs/srv/pc_batt_switch_command.hpp" -#include "buoy_msgs/srv/pc_charge_curr_lim_command.hpp" -#include "buoy_msgs/srv/pc_draw_curr_lim_command.hpp" -#include "buoy_msgs/srv/pc_std_dev_targ_command.hpp" -#include "buoy_msgs/srv/pcv_targ_max_command.hpp" -#include "buoy_msgs/srv/sc_reset_command.hpp" -#include "buoy_msgs/srv/tether_command.hpp" -#include "buoy_msgs/srv/tf_reset_command.hpp" -#include "buoy_msgs/srv/tf_set_actual_pos_command.hpp" -#include "buoy_msgs/srv/tf_set_charge_mode_command.hpp" -#include "buoy_msgs/srv/tf_set_curr_lim_command.hpp" -#include "buoy_msgs/srv/tf_set_mode_command.hpp" -#include "buoy_msgs/srv/tf_set_pos_command.hpp" -#include "buoy_msgs/srv/tf_set_state_machine_command.hpp" -#include "buoy_msgs/srv/tf_watch_dog_command.hpp" +#include "buoy_interfaces/srv/bc_reset_command.hpp" +#include "buoy_interfaces/srv/bender_command.hpp" +#include "buoy_interfaces/srv/bus_command.hpp" +#include "buoy_interfaces/srv/gain_command.hpp" +#include "buoy_interfaces/srv/pc_batt_switch_command.hpp" +#include "buoy_interfaces/srv/pc_charge_curr_lim_command.hpp" +#include "buoy_interfaces/srv/pc_draw_curr_lim_command.hpp" +#include "buoy_interfaces/srv/pc_std_dev_targ_command.hpp" +#include "buoy_interfaces/srv/pcv_targ_max_command.hpp" +#include "buoy_interfaces/srv/sc_reset_command.hpp" +#include "buoy_interfaces/srv/tether_command.hpp" +#include "buoy_interfaces/srv/tf_reset_command.hpp" +#include "buoy_interfaces/srv/tf_set_actual_pos_command.hpp" +#include "buoy_interfaces/srv/tf_set_charge_mode_command.hpp" +#include "buoy_interfaces/srv/tf_set_curr_lim_command.hpp" +#include "buoy_interfaces/srv/tf_set_mode_command.hpp" +#include "buoy_interfaces/srv/tf_set_pos_command.hpp" +#include "buoy_interfaces/srv/tf_set_state_machine_command.hpp" +#include "buoy_interfaces/srv/tf_watch_dog_command.hpp" #define CREATE_SERVICE(type, prefix, cmd_var, service, \ cmd_type, range_type) \ services_->prefix ## _command_handler_ = \ - [this](const std::shared_ptr request, \ - std::shared_ptr response) \ + [this](const std::shared_ptr request, \ + std::shared_ptr response) \ { \ RCLCPP_WARN_STREAM( \ ros_->node_->get_logger(), \ @@ -73,15 +73,15 @@ } \ }; \ services_->prefix ## _command_service_ = \ - ros_->node_->create_service( \ + ros_->node_->create_service( \ #service, \ services_->prefix ## _command_handler_) #define DECLARE_SERVICE(type, prefix, range_type) \ - rclcpp::Service::SharedPtr prefix ## _command_service_{nullptr}; \ - std::function, \ - std::shared_ptr)> prefix ## _command_handler_; \ + rclcpp::Service::SharedPtr prefix ## _command_service_{nullptr}; \ + std::function, \ + std::shared_ptr)> prefix ## _command_handler_; \ static const rcl_interfaces::msg::range_type valid_ ## prefix ## _range_ #define INIT_VALID_RANGE(prefix, range_type, low, high) \ @@ -120,26 +120,35 @@ struct NoOpServices DECLARE_SERVICE(TFSetStateMachineCommand, tfsetstatemachine, IntegerRange); // BCResetCommand - rclcpp::Service::SharedPtr bcreset_command_service_{nullptr}; - std::function, - std::shared_ptr)> bcreset_command_handler_; + rclcpp::Service< + buoy_interfaces::srv::BCResetCommand + >::SharedPtr bcreset_command_service_{nullptr}; + std::function, + std::shared_ptr)> bcreset_command_handler_; // SCResetCommand - rclcpp::Service::SharedPtr screset_command_service_{nullptr}; - std::function, - std::shared_ptr)> screset_command_handler_; + rclcpp::Service< + buoy_interfaces::srv::SCResetCommand + >::SharedPtr screset_command_service_{nullptr}; + std::function, + std::shared_ptr)> screset_command_handler_; // TFResetCommand - rclcpp::Service::SharedPtr tfreset_command_service_{nullptr}; - std::function, - std::shared_ptr)> tfreset_command_handler_; + rclcpp::Service< + buoy_interfaces::srv::TFResetCommand + >::SharedPtr tfreset_command_service_{nullptr}; + std::function, + std::shared_ptr)> tfreset_command_handler_; // TFWatchDogCommand rclcpp::Service< - buoy_msgs::srv::TFWatchDogCommand + buoy_interfaces::srv::TFWatchDogCommand >::SharedPtr tfwatchdog_command_service_{nullptr}; - std::function, - std::shared_ptr)> tfwatchdog_command_handler_; + std::function< + void( + std::shared_ptr, + std::shared_ptr + )> tfwatchdog_command_handler_; }; INIT_VALID_RANGE(bender, IntegerRange, 0, 2); INIT_VALID_RANGE(bus, IntegerRange, 0, 2); @@ -197,11 +206,11 @@ struct NoOpControllerPrivate T command_value, const U & valid_range) { - int8_t result = buoy_msgs::msg::PBCommandResponse::NOT_IMPLEMENTED; + int8_t result = buoy_interfaces::msg::PBCommandResponse::NOT_IMPLEMENTED; if (valid_range.from_value > command_value || command_value > valid_range.to_value) { - result = buoy_msgs::msg::PBCommandResponse::BAD_INPUT; + result = buoy_interfaces::msg::PBCommandResponse::BAD_INPUT; } return result; @@ -257,8 +266,8 @@ struct NoOpControllerPrivate // BCResetCommand services_->bcreset_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_WARN_STREAM( ros_->node_->get_logger(), @@ -267,14 +276,14 @@ struct NoOpControllerPrivate response->result.value = response->result.NOT_IMPLEMENTED; }; services_->bcreset_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "bc_reset_command", services_->bcreset_command_handler_); // SCResetCommand services_->screset_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_WARN_STREAM( ros_->node_->get_logger(), @@ -283,14 +292,14 @@ struct NoOpControllerPrivate response->result.value = response->result.NOT_IMPLEMENTED; }; services_->screset_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "sc_reset_command", services_->screset_command_handler_); // TFResetCommand services_->tfreset_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_WARN_STREAM( ros_->node_->get_logger(), @@ -299,14 +308,14 @@ struct NoOpControllerPrivate response->result.value = response->result.NOT_IMPLEMENTED; }; services_->tfreset_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "tf_reset_command", services_->tfreset_command_handler_); // TFWatchDogCommand services_->tfwatchdog_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_WARN_STREAM( ros_->node_->get_logger(), @@ -315,7 +324,7 @@ struct NoOpControllerPrivate response->result.value = response->result.NOT_IMPLEMENTED; }; services_->tfwatchdog_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "tf_watch_dog_command", services_->tfwatchdog_command_handler_); } diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index b01a5760..0442b605 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -51,21 +51,21 @@ struct SpringControllerROS2 rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_handler_{nullptr}; bool use_sim_time_{true}; - rclcpp::Publisher::SharedPtr sc_pub_{nullptr}; + rclcpp::Publisher::SharedPtr sc_pub_{nullptr}; std::unique_ptr pub_rate_{nullptr}; - buoy_msgs::msg::SCRecord sc_record_; + buoy_interfaces::msg::SCRecord sc_record_; double pub_rate_hz_{10.0}; }; struct SpringControllerServices { - rclcpp::Service::SharedPtr valve_command_service_{nullptr}; - std::function, - std::shared_ptr)> valve_command_handler_; + rclcpp::Service::SharedPtr valve_command_service_{nullptr}; + std::function, + std::shared_ptr)> valve_command_handler_; - rclcpp::Service::SharedPtr pump_command_service_{nullptr}; - std::function, - std::shared_ptr)> pump_command_handler_; + rclcpp::Service::SharedPtr pump_command_service_{nullptr}; + std::function, + std::shared_ptr)> pump_command_handler_; buoy_utils::StopwatchSimTime command_watch_; rclcpp::Duration command_duration_{0, 0U}; @@ -173,8 +173,8 @@ struct SpringControllerPrivate // ValveCommand services_->valve_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO_STREAM( ros_->node_->get_logger(), @@ -222,14 +222,14 @@ struct SpringControllerPrivate } }; services_->valve_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "valve_command", services_->valve_command_handler_); // PumpCommand services_->pump_command_handler_ = - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO_STREAM( ros_->node_->get_logger(), @@ -277,7 +277,7 @@ struct SpringControllerPrivate } }; services_->pump_command_service_ = - ros_->node_->create_service( + ros_->node_->create_service( "pump_command", services_->pump_command_handler_); } @@ -511,7 +511,7 @@ void SpringController::Configure( // Publisher std::string topic = _sdf->Get("topic", "spring_data").first; this->dataPtr->ros_->sc_pub_ = - this->dataPtr->ros_->node_->create_publisher(topic, 10); + this->dataPtr->ros_->node_->create_publisher(topic, 10); this->dataPtr->ros_->pub_rate_hz_ = _sdf->Get("publish_rate", this->dataPtr->ros_->pub_rate_hz_).first; @@ -534,7 +534,7 @@ void SpringController::Configure( // Only update and publish if not paused. if (this->dataPtr->paused_) {continue;} - buoy_msgs::msg::SCRecord sc_record; + buoy_interfaces::msg::SCRecord sc_record; // high prio data access std::unique_lock next(this->dataPtr->next_access_mutex_); std::unique_lock data(this->dataPtr->data_mutex_); diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index 68123c91..3f6652c7 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -24,7 +24,7 @@ #include -#include +#include #include #include @@ -39,12 +39,12 @@ struct buoy_gazebo::XBowAHRSPrivate rclcpp::Node::SharedPtr rosnode_{nullptr}; ignition::transport::Node node_; std::function imu_cb_; - rclcpp::Publisher::SharedPtr xb_pub_{nullptr}; + rclcpp::Publisher::SharedPtr xb_pub_{nullptr}; rclcpp::Publisher::SharedPtr imu_pub_{nullptr}; double pub_rate_hz_{10.0}; std::unique_ptr pub_rate_{nullptr}; std::chrono::steady_clock::duration current_time_; - buoy_msgs::msg::XBRecord xb_record_; + buoy_interfaces::msg::XBRecord xb_record_; std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; std::atomic imu_data_valid_{false}, velocity_data_valid_{false}; std::thread thread_executor_spin_, thread_publish_; @@ -171,7 +171,7 @@ void XBowAHRS::Configure( // Publisher std::string xb_topic = _sdf->Get("xb_topic", "ahrs_data").first; this->dataPtr->xb_pub_ = \ - this->dataPtr->rosnode_->create_publisher(xb_topic, 10); + this->dataPtr->rosnode_->create_publisher(xb_topic, 10); std::string imu_topic = _sdf->Get("imu_topic", "xb_imu").first; this->dataPtr->imu_pub_ = \ @@ -189,7 +189,7 @@ void XBowAHRS::Configure( // Only update and publish if not paused. if (this->dataPtr->paused_) {continue;} - buoy_msgs::msg::XBRecord xb_record; + buoy_interfaces::msg::XBRecord xb_record; // high prio data access std::unique_lock next(this->dataPtr->next_access_mutex_); std::unique_lock data(this->dataPtr->data_mutex_); diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index 2e0abd0d..d816a469 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -45,7 +45,7 @@ if(BUILD_TESTING) ignition-gazebo${GZ_SIM_VER}::ignition-gazebo${GZ_SIM_VER} ) if(buoy_add_test_ROS) - set(ROS_PKGS rclcpp buoy_msgs ${buoy_add_test_EXTRA_ROS_PKGS}) + set(ROS_PKGS rclcpp buoy_interfaces buoy_api_cpp ${buoy_add_test_EXTRA_ROS_PKGS}) foreach(PKG ${ROS_PKGS}) find_package(${PKG} REQUIRED) endforeach() @@ -67,7 +67,8 @@ if(BUILD_TESTING) # Helper function to generate pytest function(buoy_add_pytest PYTEST_NAME #[[optional: GTEST_NAME]]) - find_package(buoy_msgs REQUIRED) + find_package(buoy_api_cpp REQUIRED) + find_package(buoy_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) @@ -104,10 +105,7 @@ if(BUILD_TESTING) # Add gtests buoy_add_gtest(no_inputs) buoy_add_gtest(no_inputs_ros_feedback ROS LAUNCH_TEST) - buoy_add_gtest(pc_commands_ros_feedback - ROS LAUNCH_TEST - EXTRA_ROS_PKGS - buoy_examples) + buoy_add_gtest(pc_commands_ros_feedback ROS LAUNCH_TEST) buoy_add_gtest(sc_commands_ros_feedback ROS LAUNCH_TEST) # Add pytests diff --git a/buoy_tests/launch/no_inputs_py.launch.py b/buoy_tests/launch/no_inputs_py.launch.py index e43e3aef..1d93cea5 100644 --- a/buoy_tests/launch/no_inputs_py.launch.py +++ b/buoy_tests/launch/no_inputs_py.launch.py @@ -15,7 +15,7 @@ import time import unittest -from buoy_msgs.interface import Interface +from buoy_api import Interface import launch import launch.actions diff --git a/buoy_tests/launch/pc_bias_damping_ros_feedback_py.launch.py b/buoy_tests/launch/pc_bias_damping_ros_feedback_py.launch.py index c1887af5..a6c4868e 100644 --- a/buoy_tests/launch/pc_bias_damping_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_bias_damping_ros_feedback_py.launch.py @@ -17,7 +17,7 @@ from ament_index_python.packages import get_package_share_directory -from buoy_examples.bias_damping import NLBiasDampingPolicy +from buoy_api.examples.bias_damping import NLBiasDampingPolicy from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 50cf4c8b..416b8356 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -14,7 +14,7 @@ import time -from buoy_examples.torque_controller import PBTorqueControlPolicy +from buoy_api.examples.torque_controller import PBTorqueControlPolicy from testing_utils import BuoyPyTestAfterShutdown # noqa F401 -- runs if imported from testing_utils import BuoyPyTests diff --git a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py index 3142a6a7..0241f8cb 100644 --- a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py @@ -14,7 +14,7 @@ import time -from buoy_msgs.msg import SCRecord +from buoy_interfaces.msg import SCRecord from testing_utils import BuoyPyTestAfterShutdown # noqa F401 -- runs if imported from testing_utils import BuoyPyTests @@ -115,6 +115,11 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.node.sc_status_ & SCRecord.PUMP_TOGGLE, 'SC Pump Toggle should be ON') + # Check that valve command fails (controller returns BUSY) + self.node.send_valve_command(2) + self.assertEqual(self.node.valve_future_.result().result.value, + self.node.valve_future_.result().result.BUSY) + # Run to allow Pump command to finish self.test_helper.run(postCmdIterations) self.assertTrue(self.test_helper.run_status) diff --git a/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py b/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py index 08baad53..5154347b 100644 --- a/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_valve_ros_feedback_py.launch.py @@ -14,7 +14,7 @@ import time -from buoy_msgs.msg import SCRecord +from buoy_interfaces.msg import SCRecord from testing_utils import BuoyPyTestAfterShutdown # noqa F401 -- runs if imported from testing_utils import BuoyPyTests @@ -98,6 +98,11 @@ def test_sc_valve_ros(self, gazebo_test_fixture, proc_info): self.assertFalse(self.node.sc_status_ & SCRecord.LR_FAULT) self.assertFalse(self.node.sc_status_ & SCRecord.LR_FAULT) + # Check that pump command fails (controller returns BUSY) + self.node.send_pump_command(2) + self.assertEqual(self.node.pump_future_.result().result.value, + self.node.pump_future_.result().result.BUSY) + # Run to allow Valve command to finish self.test_helper.run(postCmdIterations) self.assertTrue(self.test_helper.run_status) diff --git a/buoy_tests/package.xml b/buoy_tests/package.xml index 6959fa5e..038a660e 100644 --- a/buoy_tests/package.xml +++ b/buoy_tests/package.xml @@ -13,11 +13,15 @@ rosidl_default_generators - buoy_gazebo - buoy_msgs + buoy_api_cpp + buoy_api_py buoy_examples - rclpy + buoy_gazebo + buoy_interfaces + rclcpp + rclpy + python3-ignition-gazebo6 ament_cmake_gtest ament_cmake_pytest diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index e46bd9a6..c5aeb84a 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -16,7 +16,7 @@ from threading import Thread import unittest -from buoy_msgs.interface import Interface +from buoy_api import Interface from buoy_tests.srv import RunServer diff --git a/buoy_tests/tests/no_inputs_ros_feedback.cpp b/buoy_tests/tests/no_inputs_ros_feedback.cpp index b52339fd..6828b986 100644 --- a/buoy_tests/tests/no_inputs_ros_feedback.cpp +++ b/buoy_tests/tests/no_inputs_ros_feedback.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -29,12 +29,12 @@ #include -class NoInputsROSNode final : public buoy_msgs::Interface +class NoInputsROSNode final : public buoy_api::Interface { public: float rpm_{10000.0F}, wcurrent_{10000.0F}; explicit NoInputsROSNode(const std::string & node_name) - : buoy_msgs::Interface(node_name) + : buoy_api::Interface(node_name) { set_parameter( rclcpp::Parameter( @@ -72,7 +72,7 @@ class NoInputsROSNode final : public buoy_msgs::Interface private: friend CRTP; // syntactic sugar (see https://stackoverflow.com/a/58435857/9686600) - void power_callback(const buoy_msgs::msg::PCRecord & data) + void power_callback(const buoy_interfaces::msg::PCRecord & data) { rpm_ = data.rpm; wcurrent_ = data.wcurrent; diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index f2a43790..628a8b6f 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -37,7 +37,7 @@ using namespace std::chrono; constexpr double INCHES_TO_METERS{0.0254}; -class PCROSNode final : public buoy_msgs::Interface +class PCROSNode final : public buoy_api::Interface { public: rclcpp::Clock::SharedPtr clock_{nullptr}; @@ -57,7 +57,7 @@ class PCROSNode final : public buoy_msgs::Interface PCRetractServiceResponseFuture pc_retract_response_future_; explicit PCROSNode(const std::string & node_name) - : buoy_msgs::Interface(node_name) + : buoy_api::Interface(node_name) { set_parameter( rclcpp::Parameter( @@ -97,7 +97,7 @@ class PCROSNode final : public buoy_msgs::Interface private: friend CRTP; // syntactic sugar (see https://stackoverflow.com/a/58435857/9686600) - void power_callback(const buoy_msgs::msg::PCRecord & data) + void power_callback(const buoy_interfaces::msg::PCRecord & data) { rpm_ = data.rpm; wind_curr_ = data.wcurrent; @@ -106,7 +106,7 @@ class PCROSNode final : public buoy_msgs::Interface retract_ = data.retract; } - void spring_callback(const buoy_msgs::msg::SCRecord & data) + void spring_callback(const buoy_interfaces::msg::SCRecord & data) { range_finder_ = data.range_finder; } diff --git a/buoy_tests/tests/sc_commands_ros_feedback.cpp b/buoy_tests/tests/sc_commands_ros_feedback.cpp index 05d81cef..74fa2796 100644 --- a/buoy_tests/tests/sc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/sc_commands_ros_feedback.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ using namespace std::chrono; constexpr double INCHES_TO_METERS{0.0254}; -class SCROSNode final : public buoy_msgs::Interface +class SCROSNode final : public buoy_api::Interface { public: rclcpp::Clock::SharedPtr clock_{nullptr}; @@ -48,7 +48,7 @@ class SCROSNode final : public buoy_msgs::Interface PumpServiceResponseFuture pump_response_future_; explicit SCROSNode(const std::string & node_name) - : buoy_msgs::Interface(node_name) + : buoy_api::Interface(node_name) { set_parameter( rclcpp::Parameter( @@ -88,7 +88,7 @@ class SCROSNode final : public buoy_msgs::Interface private: friend CRTP; // syntactic sugar (see https://stackoverflow.com/a/58435857/9686600) - void spring_callback(const buoy_msgs::msg::SCRecord & data) + void spring_callback(const buoy_interfaces::msg::SCRecord & data) { range_finder_ = data.range_finder; status_ = data.status; @@ -181,27 +181,29 @@ TEST_F(BuoySCTests, SCValveROS) // Check status field EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_REQUEST)) << "SC Valve Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_STATUS)) << "SC Valve should be CLOSED"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_REQUEST)) << "SC Pump Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_STATUS)) << "SC Pump should be OFF"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be OFF"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_OVER_TEMP)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_REQUEST)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_OVER_TEMP)); + EXPECT_FALSE( + static_cast(node->status_ & + buoy_interfaces::msg::SCRecord::TETHER_POWER_REQUEST)); EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::TETHER_POWER_STATUS)) << "SC Tether Power should be ON"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); // Now send Valve command to OPEN for 5 seconds node->valve_response_future_ = node->send_valve_command(5U); @@ -222,27 +224,37 @@ TEST_F(BuoySCTests, SCValveROS) // Check status field EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_REQUEST)) << "SC Valve Request should be TRUE"; EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_STATUS)) << "SC Valve should be OPEN"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_REQUEST)) << "SC Pump Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_STATUS)) << "SC Pump should be OFF"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be OFF"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_OVER_TEMP)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_REQUEST)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_OVER_TEMP)); + EXPECT_FALSE( + static_cast(node->status_ & + buoy_interfaces::msg::SCRecord::TETHER_POWER_REQUEST)); EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::TETHER_POWER_STATUS)) << "SC Tether Power should be ON"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); + + // Check that pump command fails (controller returns BUSY) + node->pump_response_future_ = node->send_pump_command(2U); + ASSERT_TRUE(node->pump_response_future_.valid()); + node->pump_response_future_.wait(); + EXPECT_EQ( + node->pump_response_future_.get()->result.value, + node->pump_response_future_.get()->result.BUSY); // Run to allow Valve command to finish fixture->Server()->Run(true /*blocking*/, postCmdIterations, false /*paused*/); @@ -255,27 +267,29 @@ TEST_F(BuoySCTests, SCValveROS) // Check Status goes back to normal EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_REQUEST)) << "SC Valve Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_STATUS)) << "SC Valve should be CLOSED"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_REQUEST)) << "SC Pump Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_STATUS)) << "SC Pump should be OFF"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be OFF"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_OVER_TEMP)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_REQUEST)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_OVER_TEMP)); + EXPECT_FALSE( + static_cast(node->status_ & + buoy_interfaces::msg::SCRecord::TETHER_POWER_REQUEST)); EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::TETHER_POWER_STATUS)) << "SC Tether Power should be ON"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); // Check piston motion float post_valve_range_finder = node->range_finder_; @@ -317,27 +331,29 @@ TEST_F(BuoySCTests, SCPumpROS) // Check status field EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_REQUEST)) << "SC Valve Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_STATUS)) << "SC Valve should be CLOSED"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_REQUEST)) << "SC Pump Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_STATUS)) << "SC Pump should be OFF"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be OFF"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_OVER_TEMP)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_REQUEST)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_OVER_TEMP)); + EXPECT_FALSE( + static_cast(node->status_ & + buoy_interfaces::msg::SCRecord::TETHER_POWER_REQUEST)); EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::TETHER_POWER_STATUS)) << "SC Tether Power should be ON"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); // Now send Pump command to run for 20 seconds node->pump_response_future_ = node->send_pump_command(20U); @@ -358,27 +374,29 @@ TEST_F(BuoySCTests, SCPumpROS) // Check status field EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_REQUEST)) << "SC Valve Request should be FALSE"; EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::RELIEF_VALVE_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::RELIEF_VALVE_STATUS)) << "SC Valve should be CLOSED"; EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_REQUEST)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_REQUEST)) << "SC Pump Request should be TRUE"; EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_STATUS)) << "SC Pump should be ON"; EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be ON"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_OVER_TEMP)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_REQUEST)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_OVER_TEMP)); + EXPECT_FALSE( + static_cast(node->status_ & + buoy_interfaces::msg::SCRecord::TETHER_POWER_REQUEST)); EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::TETHER_POWER_STATUS)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::TETHER_POWER_STATUS)) << "SC Tether Power should be ON"; - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); - EXPECT_FALSE(static_cast(node->status_ & buoy_msgs::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); + EXPECT_FALSE(static_cast(node->status_ & buoy_interfaces::msg::SCRecord::LR_FAULT)); // Check pump toggle for (size_t n = 1U; n < 5U; ++n) { @@ -392,15 +410,23 @@ TEST_F(BuoySCTests, SCPumpROS) if (n % 2U == 1) { EXPECT_FALSE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be OFF"; } else { EXPECT_TRUE( - static_cast(node->status_ & buoy_msgs::msg::SCRecord::PUMP_TOGGLE)) << + static_cast(node->status_ & buoy_interfaces::msg::SCRecord::PUMP_TOGGLE)) << "SC Pump Toggle should be ON"; } } + // Check that valve command fails (controller returns BUSY) + node->valve_response_future_ = node->send_valve_command(2U); + ASSERT_TRUE(node->valve_response_future_.valid()); + node->valve_response_future_.wait(); + EXPECT_EQ( + node->valve_response_future_.get()->result.value, + node->valve_response_future_.get()->result.BUSY); + // Run to allow Pump command to finish fixture->Server()->Run(true /*blocking*/, postCmdIterations, false /*paused*/); EXPECT_EQ(preCmdIterations + 500 + 4 * statusCheckIterations + postCmdIterations, iterations);