Make GPIO controller publishers realtime safe (backport #1807) (#1828)

Author: mergify[bot]

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -57,6 +57,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
 #include "std_msgs/msg/bool.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "ur_controllers/gpio_controller_parameters.hpp"
 
 namespace ur_controllers
@@ -123,9 +124,16 @@ class GPIOController : public controller_interface::ControllerInterface
 
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+
   CallbackReturn on_init() override;
 
 private:
+  // Rejects a service request when the controller is not active.
+  // Returns true if the controller is active and the request may proceed.
+  template <typename ResponseT>
+  bool ensureActive(const ResponseT& resp);
+
   bool setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs::srv::SetIO::Response::SharedPtr resp);
 
   bool setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req,
@@ -174,11 +182,12 @@ class GPIOController : public controller_interface::ControllerInterface
   rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
 
-  std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
-  std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;
-  std::shared_ptr<rclcpp::Publisher<ur_dashboard_msgs::msg::RobotMode>> robot_mode_pub_;
-  std::shared_ptr<rclcpp::Publisher<ur_dashboard_msgs::msg::SafetyMode>> safety_mode_pub_;
-  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> program_state_pub_;
+  // Publishing in realtime ros2_control loop, so these are wrapped in non-blocking tries to prevent controller overrun
+  std::shared_ptr<realtime_tools::RealtimePublisher<ur_msgs::msg::IOStates>> io_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::msg::RobotMode>> robot_mode_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::msg::SafetyMode>> safety_mode_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>> program_state_pub_;
 
   ur_msgs::msg::IOStates io_msg_;
   ur_msgs::msg::ToolDataMsg tool_data_msg_;

ur_controllers/src/gpio_controller.cpp

@@ -38,10 +38,45 @@
 #include "ur_controllers/gpio_controller.hpp"
 
 #include <cmath>
+#include <lifecycle_msgs/msg/state.hpp>
 #include <string>
 
 namespace ur_controllers
 {
+namespace
+{
+// Publishes only when `field` changes, and commits the new value into `msg` only on a successful
+// try_publish.
+// If the publish is dropped (e.g. lock contention), `msg` is left unchanged and the change is retried on the next
+// cycle.
+template <typename MsgT, typename FieldT>
+bool try_publish_on_change(realtime_tools::RealtimePublisher<MsgT>& pub, MsgT& msg, FieldT MsgT::* field,
+                           const FieldT& new_value)
+{
+  if (msg.*field == new_value) {
+    return true;
+  }
+  MsgT candidate = msg;
+  candidate.*field = new_value;
+  if (!pub.try_publish(candidate)) {
+    return false;
+  }
+  msg.*field = new_value;
+  return true;
+}
+}  // namespace
+
+template <typename ResponseT>
+bool GPIOController::ensureActive(const ResponseT& resp)
+{
+  if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new requests. Controller is not running.");
+    resp->success = false;
+    return false;
+  }
+  return true;
+}
+
 controller_interface::CallbackReturn GPIOController::on_init()
 {
   try {
@@ -196,6 +231,55 @@ ur_controllers::GPIOController::on_configure(const rclcpp_lifecycle::State& /*pr
   // get parameters from the listener in case they were updated
   params_ = param_listener_->get_params();
 
+  try {
+    auto qos_latched = rclcpp::SystemDefaultsQoS();
+    qos_latched.transient_local();
+    qos_latched.reliable();
+    // register publishers outside the realtime update loop
+    io_pub_ = std::make_shared<realtime_tools::RealtimePublisher<ur_msgs::msg::IOStates>>(
+        get_node()->create_publisher<ur_msgs::msg::IOStates>("~/io_states", rclcpp::SystemDefaultsQoS()));
+
+    tool_data_pub_ = std::make_shared<realtime_tools::RealtimePublisher<ur_msgs::msg::ToolDataMsg>>(
+        get_node()->create_publisher<ur_msgs::msg::ToolDataMsg>("~/tool_data", rclcpp::SystemDefaultsQoS()));
+
+    robot_mode_pub_ = std::make_shared<realtime_tools::RealtimePublisher<ur_dashboard_msgs::msg::RobotMode>>(
+        get_node()->create_publisher<ur_dashboard_msgs::msg::RobotMode>("~/robot_mode", qos_latched));
+
+    safety_mode_pub_ = std::make_shared<realtime_tools::RealtimePublisher<ur_dashboard_msgs::msg::SafetyMode>>(
+        get_node()->create_publisher<ur_dashboard_msgs::msg::SafetyMode>("~/safety_mode", qos_latched));
+
+    program_state_pub_ = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(
+        get_node()->create_publisher<std_msgs::msg::Bool>("~/robot_program_running", qos_latched));
+
+    // register services outside the realtime update loop; calls are rejected while the controller is not active
+    set_io_srv_ = get_node()->create_service<ur_msgs::srv::SetIO>(
+        "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2));
+    set_analog_output_srv_ = get_node()->create_service<ur_msgs::srv::SetAnalogOutput>(
+        "~/set_analog_output",
+        std::bind(&GPIOController::setAnalogOutput, this, std::placeholders::_1, std::placeholders::_2));
+
+    set_speed_slider_srv_ = get_node()->create_service<ur_msgs::srv::SetSpeedSliderFraction>(
+        "~/set_speed_slider",
+        std::bind(&GPIOController::setSpeedSlider, this, std::placeholders::_1, std::placeholders::_2));
+
+    resend_robot_program_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
+        "~/resend_robot_program",
+        std::bind(&GPIOController::resendRobotProgram, this, std::placeholders::_1, std::placeholders::_2));
+
+    hand_back_control_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
+        "~/hand_back_control",
+        std::bind(&GPIOController::handBackControl, this, std::placeholders::_1, std::placeholders::_2));
+
+    set_payload_srv_ = get_node()->create_service<ur_msgs::srv::SetPayload>(
+        "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2));
+
+    tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
+        "~/zero_ftsensor",
+        std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
+  } catch (...) {
+    return LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -226,7 +310,7 @@ void GPIOController::publishIO()
         static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_optional().value_or(0.0));
   }
 
-  io_pub_->publish(io_msg_);
+  io_pub_->try_publish(io_msg_);
 }
 
 void GPIOController::publishToolData()
@@ -247,38 +331,27 @@ void GPIOController::publishToolData()
       static_cast<float>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_optional().value_or(0.0));
   tool_data_msg_.tool_temperature =
       static_cast<float>(state_interfaces_[StateInterfaces::TOOL_TEMPERATURE].get_optional().value_or(0.0));
-  tool_data_pub_->publish(tool_data_msg_);
+  tool_data_pub_->try_publish(tool_data_msg_);
 }
 
 void GPIOController::publishRobotMode()
 {
   auto robot_mode = static_cast<int8_t>(state_interfaces_[StateInterfaces::ROBOT_MODE].get_optional().value_or(0.0));
-
-  if (robot_mode_msg_.mode != robot_mode) {
-    robot_mode_msg_.mode = robot_mode;
-    robot_mode_pub_->publish(robot_mode_msg_);
-  }
+  try_publish_on_change(*robot_mode_pub_, robot_mode_msg_, &ur_dashboard_msgs::msg::RobotMode::mode, robot_mode);
 }
 
 void GPIOController::publishSafetyMode()
 {
   auto safety_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::SAFETY_MODE].get_optional().value_or(0.0));
-
-  if (safety_mode_msg_.mode != safety_mode) {
-    safety_mode_msg_.mode = safety_mode;
-    safety_mode_pub_->publish(safety_mode_msg_);
-  }
+  try_publish_on_change(*safety_mode_pub_, safety_mode_msg_, &ur_dashboard_msgs::msg::SafetyMode::mode, safety_mode);
 }
 
 void GPIOController::publishProgramRunning()
 {
   auto program_running_value =
       static_cast<uint8_t>(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_optional().value_or(0.0));
   bool program_running = program_running_value == 1.0 ? true : false;
-  if (program_running_msg_.data != program_running) {
-    program_running_msg_.data = program_running;
-    program_state_pub_->publish(program_running_msg_);
-  }
+  try_publish_on_change(*program_state_pub_, program_running_msg_, &std_msgs::msg::Bool::data, program_running);
 }
 
 controller_interface::CallbackReturn
@@ -289,63 +362,34 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
   }
 
-  try {
-    auto qos_latched = rclcpp::SystemDefaultsQoS();
-    qos_latched.transient_local();
-    qos_latched.reliable();
-    // register publisher
-    io_pub_ = get_node()->create_publisher<ur_msgs::msg::IOStates>("~/io_states", rclcpp::SystemDefaultsQoS());
-
-    tool_data_pub_ =
-        get_node()->create_publisher<ur_msgs::msg::ToolDataMsg>("~/tool_data", rclcpp::SystemDefaultsQoS());
-
-    robot_mode_pub_ = get_node()->create_publisher<ur_dashboard_msgs::msg::RobotMode>("~/robot_mode", qos_latched);
-
-    safety_mode_pub_ = get_node()->create_publisher<ur_dashboard_msgs::msg::SafetyMode>("~/safety_mode", qos_latched);
-
-    program_state_pub_ = get_node()->create_publisher<std_msgs::msg::Bool>("~/robot_program_running", qos_latched);
-    set_io_srv_ = get_node()->create_service<ur_msgs::srv::SetIO>(
-        "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2));
-    set_analog_output_srv_ = get_node()->create_service<ur_msgs::srv::SetAnalogOutput>(
-        "~/set_analog_output",
-        std::bind(&GPIOController::setAnalogOutput, this, std::placeholders::_1, std::placeholders::_2));
-
-    set_speed_slider_srv_ = get_node()->create_service<ur_msgs::srv::SetSpeedSliderFraction>(
-        "~/set_speed_slider",
-        std::bind(&GPIOController::setSpeedSlider, this, std::placeholders::_1, std::placeholders::_2));
-
-    resend_robot_program_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
-        "~/resend_robot_program",
-        std::bind(&GPIOController::resendRobotProgram, this, std::placeholders::_1, std::placeholders::_2));
-
-    hand_back_control_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
-        "~/hand_back_control",
-        std::bind(&GPIOController::handBackControl, this, std::placeholders::_1, std::placeholders::_2));
-
-    set_payload_srv_ = get_node()->create_service<ur_msgs::srv::SetPayload>(
-        "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2));
-
-    tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
-        "~/zero_ftsensor",
-        std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
-  } catch (...) {
-    return LifecycleNodeInterface::CallbackReturn::ERROR;
-  }
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn
 ur_controllers::GPIOController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  // No teardown. Publishers and services are created in on_configure and released in on_cleanup.
+  // on_activate/on_deactivate run inside the realtime update loop, so we keep DDS management separate to avoid
+  // stalling the controller_manager update cycle.
+  return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+ur_controllers::GPIOController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
 {
   try {
-    // reset publisher
     io_pub_.reset();
     tool_data_pub_.reset();
     robot_mode_pub_.reset();
     safety_mode_pub_.reset();
     program_state_pub_.reset();
     set_io_srv_.reset();
+    set_analog_output_srv_.reset();
     set_speed_slider_srv_.reset();
+    resend_robot_program_srv_.reset();
+    hand_back_control_srv_.reset();
+    set_payload_srv_.reset();
+    tare_sensor_srv_.reset();
   } catch (...) {
     return LifecycleNodeInterface::CallbackReturn::ERROR;
   }
@@ -354,6 +398,10 @@ ur_controllers::GPIOController::on_deactivate(const rclcpp_lifecycle::State& /*p
 
 bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs::srv::SetIO::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   if (req->fun == req->FUN_SET_DIGITAL_OUT && req->pin >= 0 && req->pin <= 17) {
     // io async success
     std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
@@ -413,6 +461,10 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
 bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req,
                                      ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   std::string domain_string = "UNKNOWN";
   switch (req->data.domain) {
     case ur_msgs::msg::Analog::CURRENT:
@@ -456,6 +508,10 @@ bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::Sha
 bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req,
                                     ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   if (req->speed_slider_fraction >= 0.01 && req->speed_slider_fraction <= 1.0) {
     RCLCPP_INFO(get_node()->get_logger(), "Setting speed slider to %.2f%%.", req->speed_slider_fraction * 100.0);
     // reset success flag
@@ -486,6 +542,10 @@ bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Reques
 bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
                                         std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   // reset success flag
   std::ignore = command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   // call the service in the hardware
@@ -515,6 +575,10 @@ bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedP
 bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
                                      std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   // reset success flag
   std::ignore = command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   // call the service in the hardware
@@ -543,6 +607,10 @@ bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr
 bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req,
                                 ur_msgs::srv::SetPayload::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   // reset success flag
   std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
 
@@ -586,6 +654,10 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
 bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
                                   std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
+  if (!ensureActive(resp)) {
+    return false;
+  }
+
   // reset success flag
   std::ignore = command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   // call the service in the hardware