Service to get software version of robot (#964)

URJala · web-flow · commit d1290a7f4e57 · 2024-10-08T11:36:00.000+02:00
* Implemented get_version service

* Implemented test of get_version service and integrated it with the test of tool contact

* Update ur_robot_driver/test/test_common.py

Co-authored-by: Felix Exner (fexner) &lt;felix_mauch@web.de&gt;

* Renamed service to "GetRobotSoftwareVersion" everywhere.

Also moved the version information from being stored in the command interface to be stored in the state interface

* Remove tool contact from test.

* Implemented test of get_version service and integrated it with the test of tool contact

* Renamed service to "GetRobotSoftwareVersion" everywhere.

Also moved the version information from being stored in the command interface to be stored in the state interface

* Remove tool contact from test.

* Create new URConfigurationController

And moved the get_robot_software_version service in to it.

* Make configuration controller thread safe

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

* Use ptr-safe RealTimeBoxBestEffort

the RealTimeBox used before is not really real-time safe and the way it was
implemented there was unnecessary data allocation in both, the activate method
and the service callback.

Using the RealTimeBoxBestEffort makes allocating additional memory unnecessary
and makes things really thread-safe.

* Added back files that were mistakenly deleted
diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt
@@ -54,10 +54,16 @@ generate_parameter_library(
   src/scaled_joint_trajectory_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  ur_configuration_controller_parameters
+  src/ur_configuration_controller_parameters.yaml
+)
+
 add_library(${PROJECT_NAME} SHARED
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
-  src/gpio_controller.cpp)
+  src/gpio_controller.cpp
+  src/ur_configuration_controller.cpp)
 
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
@@ -66,6 +72,7 @@ target_link_libraries(${PROJECT_NAME}
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
+  ur_configuration_controller_parameters
 )
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml
@@ -14,4 +14,9 @@
       This controller publishes the Tool IO.
     </description>
   </class>
+  <class name="ur_controllers/URConfigurationController" type="ur_controllers::URConfigurationController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller used to get and change the configuration of the robot
+    </description>
+  </class>
 </library>
diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp
@@ -0,0 +1,105 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen jala@universal-robots.com
+ * \date    2024-07-11
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
+#define UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
+
+// TODO(fmauch): Currently, the realtime_box_best_effort doesn't include this
+#include <functional>
+#include <realtime_tools/realtime_box_best_effort.h>  // NOLINT
+
+#include <memory>
+
+#include <controller_interface/controller_interface.hpp>
+
+#include "ur_msgs/srv/get_robot_software_version.hpp"
+#include "ur_configuration_controller_parameters.hpp"
+
+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 = 0,
+  ROBOT_VERSION_MINOR = 1,
+  ROBOT_VERSION_BUILD = 2,
+  ROBOT_VERSION_BUGFIX = 3,
+};
+
+class URConfigurationController : public controller_interface::ControllerInterface
+{
+public:
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_init() override;
+
+private:
+  realtime_tools::RealtimeBoxBestEffort<std::shared_ptr<VersionInformation>> robot_software_version_{
+    std::make_shared<VersionInformation>()
+  };
+
+  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);
+
+  std::shared_ptr<ur_configuration_controller::ParamListener> param_listener_;
+  ur_configuration_controller::Params params_;
+};
+}  // namespace ur_controllers
+
+#endif  // UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp
@@ -0,0 +1,128 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen jala@universal-robots.com
+ * \date    2024-07-11
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_controllers/ur_configuration_controller.hpp>
+#include <realtime_tools/realtime_box.h>
+namespace ur_controllers
+{
+
+controller_interface::CallbackReturn URConfigurationController::on_init()
+{
+  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;
+}
+
+controller_interface::InterfaceConfiguration URConfigurationController::command_interface_configuration() const
+{
+  // No command interfaces currently
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  return config;
+}
+
+controller_interface::InterfaceConfiguration URConfigurationController::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  const std::string tf_prefix = params_.tf_prefix;
+
+  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_build");
+  config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_bugfix");
+
+  return config;
+}
+
+controller_interface::return_type URConfigurationController::update(const rclcpp::Time& /* time */,
+                                                                    const rclcpp::Duration& /* period */)
+{
+  return controller_interface::return_type::OK;
+}
+
+controller_interface::CallbackReturn
+URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
+{
+  robot_software_version_.set([this](const std::shared_ptr<VersionInformation> ptr) {
+    ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value();
+    ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value();
+    ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value();
+    ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value();
+  });
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+URConfigurationController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */)
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+bool URConfigurationController::getRobotSoftwareVersion(
+    ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/,
+    ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp)
+{
+  std::shared_ptr<VersionInformation> temp;
+  return robot_software_version_.tryGet([resp](const std::shared_ptr<VersionInformation> ptr) {
+    resp->major = ptr->major;
+    resp->minor = ptr->minor;
+    resp->build = ptr->build;
+    resp->bugfix = ptr->bugfix;
+  });
+}
+}  // namespace ur_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(ur_controllers::URConfigurationController, controller_interface::ControllerInterface)
diff --git a/ur_controllers/src/ur_configuration_controller_parameters.yaml b/ur_controllers/src/ur_configuration_controller_parameters.yaml
@@ -0,0 +1,6 @@
+ur_configuration_controller:
+   tf_prefix: {
+    type: string,
+    default_value: "",
+    description: "URDF prefix of the corresponding arm"
+   }
diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml
@@ -24,6 +24,8 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
+    ur_configuration_controller:
+      type: ur_controllers/URConfigurationController
 
 speed_scaling_state_broadcaster:
   ros__parameters:
@@ -34,6 +36,10 @@ io_and_status_controller:
   ros__parameters:
     tf_prefix: "$(var tf_prefix)"
 
+ur_configuration_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+
 force_torque_sensor_broadcaster:
   ros__parameters:
     sensor_name: $(var tf_prefix)tcp_fts_sensor
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
@@ -191,6 +191,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   bool initialized_;
   double system_interface_initialized_;
   bool async_thread_shutdown_;
+  double get_robot_software_version_major_;
+  double get_robot_software_version_minor_;
+  double get_robot_software_version_bugfix_;
+  double get_robot_software_version_build_;
 
   // Passthrough trajectory controller interface values
   urcl::vector6d_t passthrough_trajectory_positions_;
diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py
@@ -130,6 +130,7 @@ def launch_setup(context):
                     "force_torque_sensor_broadcaster",
                     "joint_state_broadcaster",
                     "speed_scaling_state_broadcaster",
+                    "ur_configuration_controller",
                 ]
             },
         ],
@@ -165,6 +166,7 @@ def controller_spawner(controllers, active=True):
         "io_and_status_controller",
         "speed_scaling_state_broadcaster",
         "force_torque_sensor_broadcaster",
+        "ur_configuration_controller",
     ]
     controllers_inactive = [
         "scaled_joint_trajectory_controller",
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
@@ -44,6 +44,7 @@
 
 #include "ur_client_library/exceptions.h"
 #include "ur_client_library/ur/tool_communication.h"
+#include "ur_client_library/ur/version_information.h"
 
 #include "rclcpp/rclcpp.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
@@ -231,6 +232,18 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
   state_interfaces.emplace_back(
       hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_));
 
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+      tf_prefix + "get_robot_software_version", "get_version_major", &get_robot_software_version_major_));
+
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+      tf_prefix + "get_robot_software_version", "get_version_minor", &get_robot_software_version_minor_));
+
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+      tf_prefix + "get_robot_software_version", "get_version_bugfix", &get_robot_software_version_bugfix_));
+
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+      tf_prefix + "get_robot_software_version", "get_version_build", &get_robot_software_version_build_));
+
   return state_interfaces;
 }
 
@@ -463,6 +476,13 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
                         "README.md] for details.");
   }
 
+  // Export version information to state interfaces
+  urcl::VersionInformation version_info = ur_driver_->getVersion();
+  get_robot_software_version_major_ = version_info.major;
+  get_robot_software_version_minor_ = version_info.minor;
+  get_robot_software_version_build_ = version_info.build;
+  get_robot_software_version_bugfix_ = version_info.bugfix;
+
   async_thread_ = std::make_shared<std::thread>(&URPositionHardwareInterface::asyncThread, this);
 
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!");
diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py
@@ -49,6 +49,7 @@
     ControllerManagerInterface,
     DashboardInterface,
     IoStatusInterface,
+    ConfigurationInterface,
     generate_driver_test_description,
 )
 
@@ -92,6 +93,7 @@ def init_robot(self):
         self._dashboard_interface = DashboardInterface(self.node)
         self._controller_manager_interface = ControllerManagerInterface(self.node)
         self._io_status_controller_interface = IoStatusInterface(self.node)
+        self._configuration_controller_interface = ConfigurationInterface(self.node)
 
         self._scaled_follow_joint_trajectory = ActionInterface(
             self.node,
@@ -108,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(
diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py
