Service to get software version of robot (backport #964)

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}

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>

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 [email protected]
+ * \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_

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 [email protected]
+ * \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)

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"
+   }

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

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_;
 
   // payload stuff
   urcl::vector3d_t payload_center_of_gravity_;

ur_robot_driver/launch/ur_control.launch.py

@@ -295,6 +295,7 @@ def launch_setup(context, *args, **kwargs):
                     "force_torque_sensor_broadcaster",
                     "joint_state_broadcaster",
                     "speed_scaling_state_broadcaster",
+                    "ur_configuration_controller",
                 ]
             },
         ],
@@ -337,6 +338,7 @@ def controller_spawner(controllers, active=True):
         "io_and_status_controller",
         "speed_scaling_state_broadcaster",
         "force_torque_sensor_broadcaster",
+        "ur_configuration_controller",
     ]
     controllers_inactive = ["forward_position_controller"]
 

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;
 }
 
@@ -454,6 +467,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!");

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(