Changed from RCPassThru to RCIN mode

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

thruster_hardware/README.md

@@ -2,15 +2,15 @@
 
 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 RCIN mode
 using MAVROS.
 
 > [!IMPORTANT]
 > Controllers that use the thruster hardware interface should be launched
 > *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 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,14 @@ 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`). [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)

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;
@@ -83,6 +84,8 @@ class ThrusterHardware : public hardware_interface::SystemInterface
   bool is_active_{false};
 
   int max_retries_;
+  std::chrono::duration<double> set_param_timeout_;
+
   rclcpp::Logger logger_{rclcpp::get_logger("ardusub_thruster_hardware")};
 };
 

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<double>(timeout_sec);
+
   // retrieve and validate the joint parameters
   for (const auto & joint : info_.joints) {
     const auto channel_it = get_param(joint.parameters, "channel");
@@ -58,12 +62,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()) {
@@ -73,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 = 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;
+    config.desired_value = desired_value;
 
     if (joint.parameters.find("neutral_pwm") != joint.parameters.cend()) {
       config.neutral_pwm = std::stoi(joint.parameters.at("neutral_pwm"));
@@ -144,28 +158,30 @@ 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);
   }
 
   auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
   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);
-    if (rclcpp::spin_until_future_complete(get_node(), future) == rclcpp::FutureReturnCode::SUCCESS) {
+    const std::future_status 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;
         }
       }
 
-      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();
@@ -175,11 +191,12 @@ 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
     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_);
 
@@ -195,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<rcl_interfaces::msg::Parameter> params;
@@ -209,29 +226,29 @@ 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_);
 
-    // 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
     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_);