Make configuration controller thread safe

Also minor cleanup and add testing of the robot software version service

Author: URJala

ur_controllers/include/ur_controllers/ur_configuration_controller.hpp

@@ -41,6 +41,8 @@
 #ifndef UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
 #define UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
 
+#include <realtime_tools/realtime_box.h>
+
 #include <memory>
 
 #include <controller_interface/controller_interface.hpp>
@@ -51,12 +53,19 @@
 namespace ur_controllers
 {
 
+// Struct to hold version information
+struct VersionInformation
+{
+  uint32_t major = 0, minor = 0, build = 0, bugfix = 0;
+};
+
+// Enum for indexing into state interfaces.
 enum StateInterfaces
 {
-  ROBOT_VERSION_MAJOR,
-  ROBOT_VERSION_MINOR,
-  ROBOT_VERSION_BUILD,
-  ROBOT_VERSION_BUGFIX
+  ROBOT_VERSION_MAJOR = 0,
+  ROBOT_VERSION_MINOR = 1,
+  ROBOT_VERSION_BUILD = 2,
+  ROBOT_VERSION_BUGFIX = 3,
 };
 
 class URConfigurationController : public controller_interface::ControllerInterface
@@ -77,7 +86,10 @@ class URConfigurationController : public controller_interface::ControllerInterfa
   CallbackReturn on_init() override;
 
 private:
+  realtime_tools::RealtimeBox<std::shared_ptr<VersionInformation>> robot_software_version_;
+
   rclcpp::Service<ur_msgs::srv::GetRobotSoftwareVersion>::SharedPtr get_robot_software_version_srv_;
+
   bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req,
                                ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp);
 

ur_controllers/src/gpio_controller.cpp

@@ -158,12 +158,6 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
   // program running
   config.names.emplace_back(tf_prefix + "gpio/program_running");
 
-  // Get version service
-  config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major");
-  config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_minor");
-  config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_bugfix");
-  config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_build");
-
   return config;
 }
 

ur_controllers/src/ur_configuration_controller.cpp

@@ -39,19 +39,25 @@
 //----------------------------------------------------------------------
 
 #include <ur_controllers/ur_configuration_controller.hpp>
+#include <realtime_tools/realtime_box.h>
 namespace ur_controllers
 {
 
 controller_interface::CallbackReturn URConfigurationController::on_init()
 {
-  param_listener_ = std::make_shared<ur_configuration_controller::ParamListener>(get_node());
-  params_ = param_listener_->get_params();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn
 URConfigurationController::on_configure(const rclcpp_lifecycle::State& /* previous_state */)
 {
+  param_listener_ = std::make_shared<ur_configuration_controller::ParamListener>(get_node());
+  params_ = param_listener_->get_params();
+
+  get_robot_software_version_srv_ = get_node()->create_service<ur_msgs::srv::GetRobotSoftwareVersion>(
+      "~/get_robot_software_version", std::bind(&URConfigurationController::getRobotSoftwareVersion, this,
+                                                std::placeholders::_1, std::placeholders::_2));
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -61,8 +67,6 @@ controller_interface::InterfaceConfiguration URConfigurationController::command_
   controller_interface::InterfaceConfiguration config;
   config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 
-  const std::string tf_prefix = params_.tf_prefix;
-
   return config;
 }
 
@@ -90,28 +94,32 @@ controller_interface::return_type URConfigurationController::update(const rclcpp
 controller_interface::CallbackReturn
 URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
 {
-  get_robot_software_version_srv_ = get_node()->create_service<ur_msgs::srv::GetRobotSoftwareVersion>(
-      "~/get_robot_software_version", std::bind(&URConfigurationController::getRobotSoftwareVersion, this,
-                                                std::placeholders::_1, std::placeholders::_2));
+  VersionInformation temp;
+  temp.major = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value());
+  temp.minor = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value());
+  temp.bugfix = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value());
+  temp.build = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value());
 
+  robot_software_version_.set(std::make_shared<VersionInformation>(temp));
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn
 URConfigurationController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */)
 {
-  get_robot_software_version_srv_.reset();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 bool URConfigurationController::getRobotSoftwareVersion(
     ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/,
     ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp)
 {
-  resp->major = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value());
-  resp->minor = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value());
-  resp->bugfix = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value());
-  resp->build = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value());
+  std::shared_ptr<VersionInformation> temp;
+  robot_software_version_.get(temp);
+  resp->major = temp->major;
+  resp->minor = temp->minor;
+  resp->bugfix = temp->bugfix;
+  resp->build = temp->build;
 
   return true;
 }

ur_controllers/src/ur_configuration_controller_parameters.yaml

@@ -2,5 +2,5 @@ ur_configuration_controller:
    tf_prefix: {
     type: string,
     default_value: "",
-    description: "Urdf prefix of the corresponding arm"
+    description: "URDF prefix of the corresponding arm"
    }

ur_robot_driver/test/robot_driver.py

@@ -110,6 +110,11 @@ def setUp(self):
     # Test functions
     #
 
+    def test_get_robot_software_version(self):
+        self.assertNotEqual(
+            self._configuration_controller_interface.get_robot_software_version().major, 0
+        )
+
     def test_start_scaled_jtc_controller(self):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(