From c47ab315721359f7e0cee5a15e749c0e7acdf58f Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 24 Jul 2025 13:40:29 -0700 Subject: [PATCH 01/13] Update hardware interface and remove stale teleop package --- .devcontainer/Dockerfile | 10 +- ardusub_teleop/CMakeLists.txt | 27 ----- ardusub_teleop/LICENSE | 17 ---- ardusub_teleop/README.md | 39 -------- ardusub_teleop/config/joy_teleop.yaml | 98 ------------------- ardusub_teleop/launch/teleop.launch.yaml | 23 ----- ardusub_teleop/package.xml | 32 ------ ardusub_teleop/src/joy_interface.cpp | 92 ----------------- ardusub_teleop/src/joy_interface.hpp | 48 --------- .../thruster_hardware/thruster_hardware.hpp | 6 +- thruster_hardware/src/thruster_hardware.cpp | 54 +++++----- 11 files changed, 35 insertions(+), 411 deletions(-) delete mode 100644 ardusub_teleop/CMakeLists.txt delete mode 100644 ardusub_teleop/LICENSE delete mode 100644 ardusub_teleop/README.md delete mode 100644 ardusub_teleop/config/joy_teleop.yaml delete mode 100644 ardusub_teleop/launch/teleop.launch.yaml delete mode 100644 ardusub_teleop/package.xml delete mode 100644 ardusub_teleop/src/joy_interface.cpp delete mode 100644 ardusub_teleop/src/joy_interface.hpp diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 2a9fa23..c1204a4 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,5 +1,5 @@ ARG ROS_DISTRO=rolling -FROM ros:$ROS_DISTRO-ros-base as desktop +FROM ros:$ROS_DISTRO-ros-base AS desktop ENV DEBIAN_FRONTEND=noninteractive WORKDIR /root/ws_ros @@ -88,7 +88,7 @@ RUN sudo apt-get -q update \ # This prevents us from needing to pin the setuputils version (which doesn't always work) ENV PYTHONWARNINGS="ignore" -FROM desktop as desktop-nvidia +FROM desktop AS desktop-nvidia # Install NVIDIA software RUN sudo apt-get update \ @@ -105,6 +105,6 @@ RUN sudo apt-get update \ && sudo rm -rf /var/lib/apt/lists/* # Env vars for the nvidia-container-runtime. -ENV NVIDIA_VISIBLE_DEVICES all -ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute -ENV QT_X11_NO_MITSHM 1 +ENV NVIDIA_VISIBLE_DEVICES=all +ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute +ENV QT_X11_NO_MITSHM=1 diff --git a/ardusub_teleop/CMakeLists.txt b/ardusub_teleop/CMakeLists.txt deleted file mode 100644 index 88a3a3d..0000000 --- a/ardusub_teleop/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ardusub_teleop) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -include(GNUInstallDirs) - -set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp std_srvs mavros_msgs geometry_msgs) - -find_package(ament_cmake REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_executable(joy_interface) -target_sources(joy_interface PRIVATE src/joy_interface.cpp) - -ament_target_dependencies(joy_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_features(joy_interface PUBLIC cxx_std_23) -target_link_libraries(joy_interface PUBLIC) - -install(TARGETS joy_interface DESTINATION lib/ardusub_teleop) -install(DIRECTORY config launch DESTINATION share/ardusub_teleop) - -ament_package() diff --git a/ardusub_teleop/LICENSE b/ardusub_teleop/LICENSE deleted file mode 100644 index 30e8e2e..0000000 --- a/ardusub_teleop/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/ardusub_teleop/README.md b/ardusub_teleop/README.md deleted file mode 100644 index 4505c10..0000000 --- a/ardusub_teleop/README.md +++ /dev/null @@ -1,39 +0,0 @@ -# ardusub_teleop - -This package provides tools for controlling an ArduSub-enabled vehicle with a -gamepad. - -## joy_interface - -joy_interface is a simple node that converts `geometry_msgs/Twist` messages -into `mavros_msgs/OverrideRCIn` messages and publishes them on the topic -`/mavros/rc/override`. - -### Subscribers - -* /joy_interface/cmd [geometry_msgs::msg::Twist] - -### Services - -* /joy_interface/enable_pwm_control [std_srvs::srv::SetBool] - -### Publishers - -* /cmd_vel [geometry_msgs::msg::Twist] -* /mavros/rc/override [mavros_msgs::msg::OverrideRCIn] - -## Launch Files - -* `teleop.launch.yaml`: launches [joy_interface](#joy_interface), [joy_teleop](https://github.com/ros-teleop/teleop_tools), - and [joy_linux](https://github.com/ros-drivers/joystick_drivers/tree/ros2). - -## Configuration Files - -* `joy_teleop.yaml`: specifies gamepad configurations for switching between - common ArduSub flight modes, toggling velocity and PWM control, and - performing manual control with a joystick. - -> [!IMPORTANT] -> The current configuration uses the left trigger as a deadman switch for -> manual control. This button must be pressed and released to enable manual -> control. diff --git a/ardusub_teleop/config/joy_teleop.yaml b/ardusub_teleop/config/joy_teleop.yaml deleted file mode 100644 index b70246b..0000000 --- a/ardusub_teleop/config/joy_teleop.yaml +++ /dev/null @@ -1,98 +0,0 @@ -joy_node: - ros__parameters: - dev: /dev/input/js0 - -joy_interface: - ros__parameters: - min_pwm: 1300 - max_pwm: 1700 - -joy_teleop: - ros__parameters: - - vel_control: - type: service - interface_type: std_srvs/srv/SetBool - service_name: /joy_interface/enable_pwm_control - buttons: [5] - service_request: - data: False - - pwm_control: - type: service - interface_type: std_srvs/srv/SetBool - service_name: /joy_interface/enable_pwm_control - buttons: [4] - service_request: - data: True - - arm_ardusub: - type: service - interface_type: mavros_msgs/srv/CommandBool - service_name: /mavros/cmd/arming - service_request: - value: True - buttons: [7] - - disarm_ardusub: - type: service - interface_type: mavros_msgs/srv/CommandBool - service_name: /mavros/cmd/arming - service_request: - value: False - buttons: [6] - - manual_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: MANUAL - buttons: [0] - - depth_hold_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: ALT_HOLD - buttons: [1] - - guided_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: GUIDED - buttons: [2] - - auto_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: AUTO - buttons: [3] - - manual_control: - type: topic - interface_type: geometry_msgs/msg/Twist - topic_name: /joy_interface/cmd - deadman_axes: [2] - axis_mappings: - linear-x: - axis: 1 - scale: 0.5 - offset: 0 - linear-y: - axis: 0 - scale: 0.5 - offset: 0 - linear-z: - axis: 4 - scale: 0.5 - offset: 0 - angular-z: - axis: 3 - scale: 1.0 - offset: 0 diff --git a/ardusub_teleop/launch/teleop.launch.yaml b/ardusub_teleop/launch/teleop.launch.yaml deleted file mode 100644 index 5162523..0000000 --- a/ardusub_teleop/launch/teleop.launch.yaml +++ /dev/null @@ -1,23 +0,0 @@ -launch: - - arg: - name: joy_file - - - node: - pkg: ardusub_teleop - exec: joy_interface - param: - - from: $(var joy_file) - - - node: - pkg: joy_linux - exec: joy_linux_node - output: screen - param: - - from: $(var joy_file) - - - node: - pkg: joy_teleop - exec: joy_teleop - output: screen - param: - - from: $(var joy_file) diff --git a/ardusub_teleop/package.xml b/ardusub_teleop/package.xml deleted file mode 100644 index 52f1e57..0000000 --- a/ardusub_teleop/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - ardusub_teleop - 0.0.1 - Implements support for ROS 2 joystick control of an ArduSub vehicle - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/ardusub_driver.git - https://github.com/Robotic-Decision-Making-Lab/ardusub_driver/issues - - Evan Palmer - - ament_cmake - - rclcpp - std_srvs - mavros_msgs - geometry_msgs - joy_teleop - joy_linux - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ardusub_teleop/src/joy_interface.cpp b/ardusub_teleop/src/joy_interface.cpp deleted file mode 100644 index 8e9a83b..0000000 --- a/ardusub_teleop/src/joy_interface.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2024, Evan Palmer -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "joy_interface.hpp" - -namespace ardusub_teleop -{ - -namespace -{ - -auto scale_cmd(float value, int old_min, int old_max, int new_min, int new_max) -> int -{ - return static_cast((((value - old_min) * (new_max - new_min)) / (old_max - old_min)) + new_min); -} - -} // namespace - -JoyInterface::JoyInterface() -: Node("joy_interface") -{ - this->declare_parameter("min_pwm", 1100); - this->declare_parameter("max_pwm", 1900); - - pwm_range_ = std::make_tuple(this->get_parameter("min_pwm").as_int(), this->get_parameter("max_pwm").as_int()); - - enable_pwm_service_ = create_service( - "~/enable_pwm_control", - [this]( - const std::shared_ptr request, // NOLINT - std::shared_ptr response) { // NOLINT - pwm_enabled_ = request->data; - response->success = true; - return; - }); - - const auto qos_profile = rclcpp::SystemDefaultsQoS(); - velocity_pub_ = create_publisher("/cmd_vel", qos_profile); - rc_override_pub_ = create_publisher("/mavros/rc/override", qos_profile); - - cmd_sub_ = create_subscription( - "~/cmd", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - // If not using PWM control, publish the velocity message directly - if (!pwm_enabled_) { - velocity_pub_->publish(*msg); - } - - mavros_msgs::msg::OverrideRCIn rc_msg; - - for (uint16_t & channel : rc_msg.channels) { - channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; - } - - const int min_pwm = std::get<0>(pwm_range_); - const int max_pwm = std::get<1>(pwm_range_); - - // Scale the velocity commands to the PWM range - rc_msg.channels[4] = scale_cmd(msg->linear.x, -1.0, 1.0, min_pwm, max_pwm); - rc_msg.channels[5] = scale_cmd(-1 * msg->linear.y, -1.0, 1.0, min_pwm, max_pwm); - rc_msg.channels[2] = scale_cmd(msg->linear.z, -1.0, 1.0, min_pwm, max_pwm); - rc_msg.channels[3] = scale_cmd(-1 * msg->angular.z, -1.0, 1.0, min_pwm, max_pwm); - - rc_override_pub_->publish(rc_msg); - }); -} - -} // namespace ardusub_teleop - -auto main(int argc, char ** argv) -> int -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/ardusub_teleop/src/joy_interface.hpp b/ardusub_teleop/src/joy_interface.hpp deleted file mode 100644 index 986d250..0000000 --- a/ardusub_teleop/src/joy_interface.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2024, Evan Palmer -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include -#include - -#include "geometry_msgs/msg/twist.hpp" -#include "mavros_msgs/msg/override_rc_in.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_srvs/srv/set_bool.hpp" - -namespace ardusub_teleop -{ - -class JoyInterface : public rclcpp::Node -{ -public: - JoyInterface(); - -private: - bool pwm_enabled_{true}; - std::shared_ptr> enable_pwm_service_; - - std::tuple pwm_range_{1100, 1900}; - std::shared_ptr> rc_override_pub_; - - std::shared_ptr> cmd_sub_; - std::shared_ptr> velocity_pub_; -}; - -} // namespace ardusub_teleop diff --git a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp index ebdbd16..00527d9 100644 --- a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp +++ b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp @@ -46,7 +46,8 @@ class ThrusterHardware : public hardware_interface::SystemInterface public: ThrusterHardware() = default; - auto on_init(const hardware_interface::HardwareInfo & info) -> hardware_interface::CallbackReturn override; + auto on_init(const hardware_interface::HardwareComponentInterfaceParams & info) + -> hardware_interface::CallbackReturn override; auto on_configure(const rclcpp_lifecycle::State & previous_state) -> hardware_interface::CallbackReturn override; @@ -68,9 +69,6 @@ class ThrusterHardware : public hardware_interface::SystemInterface auto stop_thrusters() -> void; - // We need a node to interact with MAVROS - std::shared_ptr node_; - std::shared_ptr> override_rc_pub_; std::unique_ptr> rt_override_rc_pub_; diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 82eaf33..df1f851 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -20,13 +20,16 @@ #include "thruster_hardware/thruster_hardware.hpp" +#include + #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" namespace thruster_hardware { -auto ThrusterHardware::on_init(const hardware_interface::HardwareInfo & info) -> hardware_interface::CallbackReturn +auto ThrusterHardware::on_init(const hardware_interface::HardwareComponentInterfaceParams & info) + -> hardware_interface::CallbackReturn { RCLCPP_INFO(logger_, "Initializing the ThrusterHardware interface"); // NOLINT @@ -35,40 +38,44 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareInfo & info) -> return hardware_interface::CallbackReturn::ERROR; } - const auto retries_it = info_.hardware_parameters.find("max_set_param_attempts"); - if (retries_it == info_.hardware_parameters.cend()) { - RCLCPP_ERROR(logger_, "Missing the required parameter 'max_set_param_attempts'"); // NOLINT - return hardware_interface::CallbackReturn::ERROR; - } - max_retries_ = std::stoi(retries_it->second); + // simple helper method to get parameters from an interface + auto get_param = [&logger = logger_]( + const std::unordered_map & params, + const std::string & param_name) -> std::optional { + auto it = params.find(param_name); + return it == params.cend() ? std::nullopt : std::optional(std::stoi(it->second)); + }; + + // not required - default to 3 if not provided + max_retries_ = get_param(info_.hardware_parameters, "max_set_param_attempts").value_or(3); // retrieve and validate the joint parameters for (const auto & joint : info_.joints) { - const auto channel_it = joint.parameters.find("channel"); - if (channel_it == joint.parameters.cend()) { + const auto channel_it = get_param(joint.parameters, "channel"); + if (!channel_it.has_value()) { RCLCPP_ERROR(logger_, "Joint %s is missing the required parameter 'channel'", joint.name.c_str()); // NOLINT return hardware_interface::CallbackReturn::ERROR; } - const auto channel = std::stoi(channel_it->second); + const int channel = channel_it.value(); - const auto name_it = joint.parameters.find("param_name"); - if (name_it == joint.parameters.cend()) { + const auto name_it = get_param(joint.parameters, "param_name"); + if (!name_it.has_value()) { RCLCPP_ERROR(logger_, "Joint %s is missing the required parameter 'param_name'", joint.name.c_str()); // NOLINT return hardware_interface::CallbackReturn::ERROR; } - const auto param_name = name_it->second; + const int name = name_it.value(); - const auto default_value_it = joint.parameters.find("default_param_value"); - if (default_value_it == joint.parameters.cend()) { + const auto default_value_it = get_param(joint.parameters, "default_param_value"); + if (!default_value_it.has_value()) { // NOLINTNEXTLINE RCLCPP_ERROR(logger_, "Joint %s is missing the required parameter 'default_param_value'", joint.name.c_str()); return hardware_interface::CallbackReturn::ERROR; } - const auto default_value = std::stoi(default_value_it->second); + const int default_value = default_value_it.value(); // store the thruster configurations ThrusterConfig config; - config.param.name = param_name; + config.param.name = name; config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; config.param.value.integer_value = default_value; config.channel = channel; @@ -80,11 +87,6 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareInfo & info) -> thruster_configs_[joint.name] = config; } - // construct a node to use for interacting with MAVROS - rclcpp::NodeOptions options; - options.arguments({"--ros-args", "-r", "__node:=thruster_hardware" + info_.name}); - node_ = rclcpp::Node::make_shared("_", options); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -98,7 +100,7 @@ auto ThrusterHardware::on_configure(const rclcpp_lifecycle::State & /*previous_s // configure the publisher for the override RC messages override_rc_pub_ = - node_->create_publisher("mavros/rc/override", rclcpp::SystemDefaultsQoS()); + get_node()->create_publisher("mavros/rc/override", rclcpp::SystemDefaultsQoS()); rt_override_rc_pub_ = std::make_unique>(override_rc_pub_); @@ -110,7 +112,7 @@ auto ThrusterHardware::on_configure(const rclcpp_lifecycle::State & /*previous_s // configure a service client to set the ardusub thruster parameters using namespace std::chrono_literals; - set_params_client_ = node_->create_client("mavros/param/set_parameters"); + set_params_client_ = get_node()->create_client("mavros/param/set_parameters"); while (!set_params_client_->wait_for_service(1s)) { if (!rclcpp::ok()) { RCLCPP_ERROR(logger_, "Interrupted while waiting for the `mavros/set_parameters` service"); // NOLINT @@ -155,7 +157,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st // Wait until the result is available auto future = set_params_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(get_node(), future) == rclcpp::FutureReturnCode::SUCCESS) { const auto responses = future.get()->results; for (const auto & response : responses) { if (!response.successful) { @@ -213,7 +215,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ auto future = set_params_client_->async_send_request(request); // Wait until the result is available - if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(get_node(), future) == rclcpp::FutureReturnCode::SUCCESS) { const auto responses = future.get()->results; for (const auto & response : responses) { if (!response.successful) { From e943da21aaef43ab01420881e1975e6e62080284 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 23 Feb 2026 15:46:28 -0800 Subject: [PATCH 02/13] Update API according to recent upstream deprecations --- ardusub_manager/CMakeLists.txt | 36 ++++++++------- ardusub_manager/src/ardusub_manager.hpp | 2 +- thruster_hardware/CMakeLists.txt | 44 ++++++++++++------- .../thruster_hardware/thruster_hardware.hpp | 3 ++ thruster_hardware/src/thruster_hardware.cpp | 38 ++++++++-------- 5 files changed, 70 insertions(+), 53 deletions(-) diff --git a/ardusub_manager/CMakeLists.txt b/ardusub_manager/CMakeLists.txt index 8dd8f0c..bb7d8b3 100644 --- a/ardusub_manager/CMakeLists.txt +++ b/ardusub_manager/CMakeLists.txt @@ -7,21 +7,14 @@ endif() include(GNUInstallDirs) -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclcpp_lifecycle - tf2 - tf2_ros - mavros_msgs - geometry_msgs - geographic_msgs -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) generate_parameter_library(ardusub_manager_parameters @@ -30,9 +23,18 @@ generate_parameter_library(ardusub_manager_parameters add_executable(ardusub_manager) target_sources(ardusub_manager PRIVATE src/ardusub_manager.cpp) - -ament_target_dependencies(ardusub_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(ardusub_manager PUBLIC ardusub_manager_parameters) +target_link_libraries( + ardusub_manager + PUBLIC + ${mavros_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${geographic_msgs_TARGETS} + ardusub_manager_parameters + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + tf2_ros::tf2_ros +) target_compile_features(ardusub_manager PUBLIC cxx_std_23) install( diff --git a/ardusub_manager/src/ardusub_manager.hpp b/ardusub_manager/src/ardusub_manager.hpp index 8c0e8f3..55bfec2 100644 --- a/ardusub_manager/src/ardusub_manager.hpp +++ b/ardusub_manager/src/ardusub_manager.hpp @@ -26,7 +26,7 @@ #include "mavros_msgs/srv/message_interval.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.hpp" // auto-generated by generate_parameter_library #include diff --git a/thruster_hardware/CMakeLists.txt b/thruster_hardware/CMakeLists.txt index 9fa5c7c..58e56fd 100644 --- a/thruster_hardware/CMakeLists.txt +++ b/thruster_hardware/CMakeLists.txt @@ -7,21 +7,14 @@ endif() include(GNUInstallDirs) -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclcpp_lifecycle - pluginlib - hardware_interface - mavros_msgs - rcl_interfaces - rclcpp_lifecycle - realtime_tools -) - find_package(ament_cmake REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(pluginlib REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(realtime_tools REQUIRED) add_library(thruster_hardware SHARED) target_sources( @@ -34,7 +27,18 @@ target_sources( ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_hardware/thruster_hardware.hpp ) target_compile_features(thruster_hardware PUBLIC cxx_std_23) -ament_target_dependencies(thruster_hardware PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries( + thruster_hardware + PUBLIC + ${mavros_msgs_TARGETS} + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + pluginlib::pluginlib + hardware_interface::hardware_interface + realtime_tools::realtime_tools +) + pluginlib_export_plugin_description_file(hardware_interface thruster_hardware.xml) install( @@ -47,6 +51,14 @@ install( ) ament_export_targets(export_thruster_hardware HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies( + "rclcpp" + "rclcpp_lifecycle" + "pluginlib" + "hardware_interface" + "mavros_msgs" + "rcl_interfaces" + "realtime_tools" +) ament_package() diff --git a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp index 00527d9..11aa068 100644 --- a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp +++ b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp @@ -75,6 +75,9 @@ class ThrusterHardware : public hardware_interface::SystemInterface std::shared_ptr> set_params_client_; std::unordered_map thruster_configs_; + // maintain a message so that we don't have to allocate one every time we write to the hardware + mavros_msgs::msg::OverrideRCIn rc_override_msg_; + // the write loop run regardless of whether or not the hardware is active // so we need to keep track of this to ensure that we only send commands when the hardware is active bool is_active_{false}; diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index df1f851..30042dd 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -75,7 +75,7 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareComponentInterf // store the thruster configurations ThrusterConfig config; - config.param.name = name; + config.param.name = std::to_string(name); config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; config.param.value.integer_value = default_value; config.channel = channel; @@ -104,11 +104,9 @@ auto ThrusterHardware::on_configure(const rclcpp_lifecycle::State & /*previous_s rt_override_rc_pub_ = std::make_unique>(override_rc_pub_); - rt_override_rc_pub_->lock(); - for (auto & channel : rt_override_rc_pub_->msg_.channels) { + for (auto & channel : rc_override_msg_.channels) { channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; } - rt_override_rc_pub_->unlock(); // configure a service client to set the ardusub thruster parameters using namespace std::chrono_literals; @@ -127,11 +125,12 @@ auto ThrusterHardware::on_configure(const rclcpp_lifecycle::State & /*previous_s // NOLINTNEXTLINE(readability-convert-member-functions-to-static) auto ThrusterHardware::stop_thrusters() -> void { - if (rt_override_rc_pub_ && rt_override_rc_pub_->trylock()) { - for (const auto & [name, config] : thruster_configs_) { - rt_override_rc_pub_->msg_.channels[config.channel - 1] = config.neutral_pwm; - } - rt_override_rc_pub_->unlockAndPublish(); + for (const auto & [name, config] : thruster_configs_) { + rc_override_msg_.channels[config.channel - 1] = config.neutral_pwm; + } + + if (rt_override_rc_pub_ && rt_override_rc_pub_->can_publish()) { + rt_override_rc_pub_->try_publish(rc_override_msg_); } } @@ -252,17 +251,18 @@ auto ThrusterHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Durati return hardware_interface::return_type::OK; } - if (rt_override_rc_pub_ && rt_override_rc_pub_->trylock()) { - for (const auto & [name, desc] : joint_command_interfaces_) { - const auto command = get_command(name); - if (std::isnan(command)) { - continue; - } - - const auto config = thruster_configs_.at(desc.prefix_name); - rt_override_rc_pub_->msg_.channels[config.channel - 1] = static_cast(command); + for (const auto & [name, desc] : joint_command_interfaces_) { + const auto command = get_command(name); + if (std::isnan(command)) { + continue; } - rt_override_rc_pub_->unlockAndPublish(); + + const auto config = thruster_configs_.at(desc.prefix_name); + rc_override_msg_.channels[config.channel - 1] = static_cast(command); + } + + if (rt_override_rc_pub_ && rt_override_rc_pub_->can_publish()) { + rt_override_rc_pub_->try_publish(rc_override_msg_); } return hardware_interface::return_type::OK; From e4052f26fcf1a7863e0f69e13665303b4b4e941a Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 29 Mar 2026 21:50:18 -0700 Subject: [PATCH 03/13] fix remaining references to teleop packages --- .dockerignore | 1 - ardusub_bringup/launch/ardusub.launch.yaml | 15 --------------- 2 files changed, 16 deletions(-) diff --git a/.dockerignore b/.dockerignore index c7094ae..77b7e1e 100644 --- a/.dockerignore +++ b/.dockerignore @@ -8,4 +8,3 @@ !thruster_hardware !ardusub_bringup !ardusub_manager -!ardusub_teleop diff --git a/ardusub_bringup/launch/ardusub.launch.yaml b/ardusub_bringup/launch/ardusub.launch.yaml index d2da4c0..0917fc4 100644 --- a/ardusub_bringup/launch/ardusub.launch.yaml +++ b/ardusub_bringup/launch/ardusub.launch.yaml @@ -12,10 +12,6 @@ launch: name: use_manager default: "false" - - arg: - name: use_joy - default: "false" - - arg: name: model_name default: "" @@ -32,10 +28,6 @@ launch: name: manager_file default: "" - - arg: - name: joy_file - default: $(find-pkg-share ardusub_teleop)/config/joy_teleop.yaml - - arg: name: sitl_base_port default: "5760" @@ -69,10 +61,3 @@ launch: arg: - name: ardusub_manager_file value: $(var manager_file) - - - include: - file: $(find-pkg-share ardusub_teleop)/launch/teleop.launch.yaml - if: $(var use_joy) - arg: - - name: joy_file - value: $(var joy_file) From 0a1de080cca3669256fed1abbca31be7e66a5721 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 29 Mar 2026 22:02:54 -0700 Subject: [PATCH 04/13] fix stoi error --- thruster_hardware/src/thruster_hardware.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 30042dd..9f988ba 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -58,12 +58,13 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareComponentInterf } const int channel = channel_it.value(); - const auto name_it = get_param(joint.parameters, "param_name"); - if (!name_it.has_value()) { + // the `param_name` parameter is a string so we can't use our `get_param` helper here + auto name_it = joint.parameters.find("param_name"); + if (name_it == joint.parameters.cend()) { RCLCPP_ERROR(logger_, "Joint %s is missing the required parameter 'param_name'", joint.name.c_str()); // NOLINT return hardware_interface::CallbackReturn::ERROR; } - const int name = name_it.value(); + const std::string name = name_it->second; const auto default_value_it = get_param(joint.parameters, "default_param_value"); if (!default_value_it.has_value()) { From 821a2972d285bca91cc011278d77d0f163cffda0 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 29 Mar 2026 22:04:34 -0700 Subject: [PATCH 05/13] tired --- thruster_hardware/src/thruster_hardware.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 9f988ba..a011b8b 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -76,7 +76,7 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareComponentInterf // store the thruster configurations ThrusterConfig config; - config.param.name = std::to_string(name); + config.param.name = name; config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; config.param.value.integer_value = default_value; config.channel = channel; From b04b55356f52e591a893989d95ddf630db0d1c20 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 29 Mar 2026 23:27:53 -0700 Subject: [PATCH 06/13] fix bug in activation --- .../thruster_hardware/thruster_hardware.hpp | 2 ++ thruster_hardware/src/thruster_hardware.cpp | 23 ++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp index 11aa068..9c4a183 100644 --- a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp +++ b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp @@ -83,6 +83,8 @@ class ThrusterHardware : public hardware_interface::SystemInterface bool is_active_{false}; int max_retries_; + std::chrono::duration set_param_timeout_; + rclcpp::Logger logger_{rclcpp::get_logger("ardusub_thruster_hardware")}; }; diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index a011b8b..644c32f 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -49,6 +49,10 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareComponentInterf // not required - default to 3 if not provided max_retries_ = get_param(info_.hardware_parameters, "max_set_param_attempts").value_or(3); + // not required - default to 5 seconds if not provided + const auto timeout_sec = get_param(info_.hardware_parameters, "set_param_timeout_sec").value_or(5); + set_param_timeout_ = std::chrono::duration(timeout_sec); + // retrieve and validate the joint parameters for (const auto & joint : info_.joints) { const auto channel_it = get_param(joint.parameters, "channel"); @@ -157,12 +161,14 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st // Wait until the result is available auto future = set_params_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(get_node(), future) == rclcpp::FutureReturnCode::SUCCESS) { + const std::future_result = future.wait_for(set_param_timeout_); + + if (result == std::future_status::ready) { const auto responses = future.get()->results; for (const auto & response : responses) { if (!response.successful) { - RCLCPP_ERROR(logger_, "Failed to set thruster parameter %s.", response.reason.c_str()); // NOLINT - return hardware_interface::CallbackReturn::ERROR; + RCLCPP_ERROR(logger_, "Failed to set thruster parameter %s. Retrying...", response.reason.c_str()); // NOLINT + continue; } } @@ -176,6 +182,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st return hardware_interface::CallbackReturn::SUCCESS; } + RCLCPP_ERROR(logger_, "Timed out while waiting for parameter set response. Retrying..."); // NOLINT } RCLCPP_ERROR( // NOLINT @@ -213,21 +220,21 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ RCLCPP_WARN(logger_, "Attempting to leave RC passthrough mode..."); // NOLINT auto future = set_params_client_->async_send_request(request); + const std::future_result = future.wait_for(set_param_timeout_); - // Wait until the result is available - if (rclcpp::spin_until_future_complete(get_node(), future) == rclcpp::FutureReturnCode::SUCCESS) { + if (result == std::future_status::ready) { const auto responses = future.get()->results; for (const auto & response : responses) { if (!response.successful) { - RCLCPP_ERROR(logger_, "Failed to set thruster parameter %s.", response.reason.c_str()); // NOLINT - return hardware_interface::CallbackReturn::ERROR; + RCLCPP_ERROR(logger_, "Failed to set thruster parameter %s. Retrying...", response.reason.c_str()); // NOLINT + continue; } } RCLCPP_INFO(logger_, "Successfully restored the default thruster values!"); // NOLINT - return hardware_interface::CallbackReturn::SUCCESS; } + RCLCPP_ERROR(logger_, "Timed out while waiting for parameter set response. Retrying..."); // NOLINT } RCLCPP_ERROR( // NOLINT From 668b1b79ecea620c68361f1ca190117fc10da7f6 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 29 Mar 2026 23:29:38 -0700 Subject: [PATCH 07/13] pain --- thruster_hardware/src/thruster_hardware.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 644c32f..9de23e9 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -161,7 +161,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st // Wait until the result is available auto future = set_params_client_->async_send_request(request); - const std::future_result = future.wait_for(set_param_timeout_); + const std::future_result result = future.wait_for(set_param_timeout_); if (result == std::future_status::ready) { const auto responses = future.get()->results; @@ -220,7 +220,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ RCLCPP_WARN(logger_, "Attempting to leave RC passthrough mode..."); // NOLINT auto future = set_params_client_->async_send_request(request); - const std::future_result = future.wait_for(set_param_timeout_); + const std::future_result result = future.wait_for(set_param_timeout_); if (result == std::future_status::ready) { const auto responses = future.get()->results; From f92630c23053743c74e60532ff309f2429459f89 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 29 Mar 2026 23:30:46 -0700 Subject: [PATCH 08/13] i am dumb --- thruster_hardware/src/thruster_hardware.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 9de23e9..1a88c00 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -161,7 +161,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st // Wait until the result is available auto future = set_params_client_->async_send_request(request); - const std::future_result result = future.wait_for(set_param_timeout_); + const std::future_status result = future.wait_for(set_param_timeout_); if (result == std::future_status::ready) { const auto responses = future.get()->results; @@ -220,7 +220,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ RCLCPP_WARN(logger_, "Attempting to leave RC passthrough mode..."); // NOLINT auto future = set_params_client_->async_send_request(request); - const std::future_result result = future.wait_for(set_param_timeout_); + const std::future_status result = future.wait_for(set_param_timeout_); if (result == std::future_status::ready) { const auto responses = future.get()->results; From 01079bd3ebf0e64b7c98f1ff6fb7c5fd2134f4f7 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Thu, 31 Jul 2025 18:29:41 -0700 Subject: [PATCH 09/13] changed from RCPassThru to RCIN mode --- thruster_hardware/README.md | 8 +++++-- .../thruster_hardware/thruster_hardware.hpp | 1 + thruster_hardware/src/thruster_hardware.cpp | 23 +++++++++++++------ 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/thruster_hardware/README.md b/thruster_hardware/README.md index a623d6c..cd14c70 100644 --- a/thruster_hardware/README.md +++ b/thruster_hardware/README.md @@ -2,7 +2,7 @@ thruster_hardware implements a ros2_control system hardware interface for individual thruster-level control. This is accomplished by dynamically setting -the ArduSub `SERVON_FUNCTION` parameters for each thruster to PWM passthrough +the ArduSub `SERVON_FUNCTION` parameters for each thruster to PWM RCIN mode using MAVROS. > [!IMPORTANT] @@ -10,7 +10,7 @@ using MAVROS. > *after* MAVROS has fully loaded the "param", "command", and "rc_io" plugins. > [!CAUTION] -> Please exercise caution when using PWM passthrough. This mode disables all +> Please exercise caution when using PWM RCIN mode. This mode disables all > ArduSub arming checks. It is also recommended that you store a backup of the > ArduSub parameters should the hardware interface fail to restore the default > parameters. @@ -31,5 +31,9 @@ thruster_hardware/ThrusterHardware thruster hardware will attempt to restore these parameters on deactivation. [integer] * channel: The thruster channel number. [integer] +* desired_param_value: The desired value for the servo function parameter + (e.g., 51 corresponds to RCIN1). It is recommended to avoid using RCIN7 + through RCIN11, as these channels may conflict with other vehicle + functionalities. [integer] * neutral_pwm (optional): The neutral PWM value for the thruster. This defaults to 1500. [integer] diff --git a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp index 9c4a183..91be6f5 100644 --- a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp +++ b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp @@ -65,6 +65,7 @@ class ThrusterHardware : public hardware_interface::SystemInterface rcl_interfaces::msg::Parameter param; int channel; int neutral_pwm = 1500; + int desired_value; }; auto stop_thrusters() -> void; diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 1a88c00..14f48ff 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -78,12 +78,21 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareComponentInterf } const int default_value = default_value_it.value(); + const auto desired_value_it = joint.parameters.find("desired_param_value"); + if (desired_value_it == joint.parameters.cend()) { + // NOLINTNEXTLINE + RCLCPP_ERROR(logger_, "Joint %s is missing the required parameter 'desired_param_value'", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + const auto desired_value = std::stoi(desired_value_it->second); + // store the thruster configurations ThrusterConfig config; config.param.name = name; config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; config.param.value.integer_value = default_value; config.channel = channel; + config.desired_value = desired_value; if (joint.parameters.find("neutral_pwm") != joint.parameters.cend()) { config.neutral_pwm = std::stoi(joint.parameters.at("neutral_pwm")); @@ -149,7 +158,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st rcl_interfaces::msg::Parameter param; param.name = config.param.name; param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - param.value.integer_value = 1; // Set the thruster parameter values to RC passthrough here + param.value.integer_value = config.desired_value; // Set the thruster parameter values to the desired values here params.emplace_back(param); } @@ -157,7 +166,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st request->parameters = params; for (int i = 0; i < max_retries_; ++i) { - RCLCPP_WARN(logger_, "Attempting to set thruster parameters to RC passthrough..."); // NOLINT + RCLCPP_WARN(logger_, "Attempting to set thruster parameters to RCIN mode..."); // NOLINT // Wait until the result is available auto future = set_params_client_->async_send_request(request); @@ -172,7 +181,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st } } - RCLCPP_INFO(logger_, "Successfully set thruster parameters to RC passthrough!"); // NOLINT + RCLCPP_INFO(logger_, "Successfully set thruster parameters to RCIN mode!"); // NOLINT // Stop the thrusters before switching to an external controller stop_thrusters(); @@ -187,7 +196,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st RCLCPP_ERROR( // NOLINT logger_, - "Failed to set thruster parameters to passthrough mode after %d attempts. Make sure that the MAVROS parameter " + "Failed to set thruster parameters to RCIN mode after %d attempts. Make sure that the MAVROS parameter " "plugin is fully running and configured.", max_retries_); @@ -203,7 +212,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ // stop sending rc override messages from the write loop is_active_ = false; - // stop the thrusters before switching out of passthrough mode + // stop the thrusters before switching out of RCIN mode stop_thrusters(); std::vector params; @@ -217,7 +226,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ request->parameters = params; for (int i = 0; i < max_retries_; ++i) { - RCLCPP_WARN(logger_, "Attempting to leave RC passthrough mode..."); // NOLINT + RCLCPP_WARN(logger_, "Attempting to leave RCIN mode..."); // NOLINT auto future = set_params_client_->async_send_request(request); const std::future_status result = future.wait_for(set_param_timeout_); @@ -239,7 +248,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ RCLCPP_ERROR( // NOLINT logger_, - "Failed to fully leave passthrough mode after %d attempts. Make sure that the MAVROS parameter plugin is fully " + "Failed to fully leave RCIN mode after %d attempts. Make sure that the MAVROS parameter plugin is fully " "running and configured.", max_retries_); From 21c220dfca504fca9ab6cf62b56cc0e93265c8a1 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Thu, 31 Jul 2025 18:39:55 -0700 Subject: [PATCH 10/13] Updated README --- thruster_hardware/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/thruster_hardware/README.md b/thruster_hardware/README.md index cd14c70..81c7ca1 100644 --- a/thruster_hardware/README.md +++ b/thruster_hardware/README.md @@ -30,10 +30,10 @@ thruster_hardware/ThrusterHardware * default_param_value: The default value of the servo function parameter. The thruster hardware will attempt to restore these parameters on deactivation. [integer] -* channel: The thruster channel number. [integer] -* desired_param_value: The desired value for the servo function parameter - (e.g., 51 corresponds to RCIN1). It is recommended to avoid using RCIN7 - through RCIN11, as these channels may conflict with other vehicle +* channel: The thruster channel number. It is recommended to avoid using + channels 7 through 11, as these channels may conflict with other vehicle functionalities. [integer] +* desired_param_value: The desired value for the servo function parameter + (e.g., `51` corresponds to `RCIN1`). [integer] * neutral_pwm (optional): The neutral PWM value for the thruster. This defaults to 1500. [integer] From 9382a2427dda45d1f598631186bc5ee6a352cd57 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Thu, 31 Jul 2025 19:08:30 -0700 Subject: [PATCH 11/13] Updated Readme --- thruster_hardware/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/thruster_hardware/README.md b/thruster_hardware/README.md index 81c7ca1..188ec44 100644 --- a/thruster_hardware/README.md +++ b/thruster_hardware/README.md @@ -2,7 +2,7 @@ thruster_hardware implements a ros2_control system hardware interface for individual thruster-level control. This is accomplished by dynamically setting -the ArduSub `SERVON_FUNCTION` parameters for each thruster to PWM RCIN mode +the ArduSub `SERVON_FUNCTION` parameters for each thruster to RCIN mode using MAVROS. > [!IMPORTANT] @@ -10,7 +10,7 @@ using MAVROS. > *after* MAVROS has fully loaded the "param", "command", and "rc_io" plugins. > [!CAUTION] -> Please exercise caution when using PWM RCIN mode. This mode disables all +> Please exercise caution when using RCIN mode. This mode disables all > ArduSub arming checks. It is also recommended that you store a backup of the > ArduSub parameters should the hardware interface fail to restore the default > parameters. From 3a36e0f9fad9dbf638bc246fc112462721cbd77b Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Thu, 23 Apr 2026 19:12:26 -0700 Subject: [PATCH 12/13] minor changes --- thruster_hardware/README.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/thruster_hardware/README.md b/thruster_hardware/README.md index 188ec44..650ac2d 100644 --- a/thruster_hardware/README.md +++ b/thruster_hardware/README.md @@ -30,10 +30,15 @@ thruster_hardware/ThrusterHardware * default_param_value: The default value of the servo function parameter. The thruster hardware will attempt to restore these parameters on deactivation. [integer] -* channel: The thruster channel number. It is recommended to avoid using - channels 7 through 11, as these channels may conflict with other vehicle - functionalities. [integer] +* channel: The thruster channel number. [integer] * desired_param_value: The desired value for the servo function parameter (e.g., `51` corresponds to `RCIN1`). [integer] * neutral_pwm (optional): The neutral PWM value for the thruster. This defaults to 1500. [integer] + +> [!IMPORTANT] +> When specifying `channel` and `desired_param_value`, avoid the following ranges to prevent conflicts with other vehicle functionalities: +> +> * Channels 7–11 +> +> * RCIN7–RCIN11 (values 57–61) From 51616d96b84ca7677c9e8790aa45ea357eefa9d2 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Fri, 8 May 2026 12:38:25 -0700 Subject: [PATCH 13/13] clang-tidy fix --- ardusub_manager/src/ardusub_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ardusub_manager/src/ardusub_manager.cpp b/ardusub_manager/src/ardusub_manager.cpp index 9d0118e..08bd8c0 100644 --- a/ardusub_manager/src/ardusub_manager.cpp +++ b/ardusub_manager/src/ardusub_manager.cpp @@ -72,7 +72,7 @@ auto ArduSubManager::set_message_rate(int64_t msg_id, double rate) const -> void using namespace std::chrono_literals; - auto future_result = set_message_intervals_client_->async_send_request(std::move(request)).future.share(); + auto future_result = set_message_intervals_client_->async_send_request(request).future.share(); auto future_status = wait_for_result(future_result, 5s); if (future_status != std::future_status::ready) {