From 5646c022cfb5cee33ac90fdaf04ef0d7fdf33a32 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 8 Oct 2024 12:49:49 +0200 Subject: [PATCH] Service to get software version of robot (#964) (#1128) * 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) * 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 --- ur_controllers/CMakeLists.txt | 9 +- ur_controllers/controller_plugins.xml | 5 + .../ur_configuration_controller.hpp | 105 ++++++++++++++ .../src/ur_configuration_controller.cpp | 128 ++++++++++++++++++ ...r_configuration_controller_parameters.yaml | 6 + ur_robot_driver/config/ur_controllers.yaml | 6 + .../ur_robot_driver/hardware_interface.hpp | 4 + ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/hardware_interface.cpp | 20 +++ ur_robot_driver/test/robot_driver.py | 7 + ur_robot_driver/test/test_common.py | 15 +- 11 files changed, 304 insertions(+), 3 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/ur_configuration_controller.hpp create mode 100644 ur_controllers/src/ur_configuration_controller.cpp create mode 100644 ur_controllers/src/ur_configuration_controller_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..5ea7bbe86 100644 --- 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 index f0058ab55..fa4b63987 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + Controller used to get and change the configuration of the robot + + diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp new file mode 100644 index 000000000..3775ee72c --- /dev/null +++ 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 +#include // NOLINT + +#include + +#include + +#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> robot_software_version_{ + std::make_shared() + }; + + rclcpp::Service::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 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 new file mode 100644 index 000000000..a6ec2d24b --- /dev/null +++ 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 +#include +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(get_node()); + params_ = param_listener_->get_params(); + + get_robot_software_version_srv_ = get_node()->create_service( + "~/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 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 temp; + return robot_software_version_.tryGet([resp](const std::shared_ptr 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 new file mode 100644 index 000000000..4cbdf8aa1 --- /dev/null +++ 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 index a512dc1ca..544dd1fbd 100644 --- 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 index 67e27a222..6fc227766 100644 --- 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_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 9c1a036a7..053ed4996 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/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"] diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 6404e69fc..b682bfd9b 100644 --- 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 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(&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 index 96c859947..88fcffce3 100755 --- 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 index f1eac75af..8db7b6835 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -52,7 +52,7 @@ IsProgramRunning, Load, ) -from ur_msgs.srv import SetIO +from ur_msgs.srv import SetIO, GetRobotSoftwareVersion TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. @@ -240,7 +240,18 @@ class IoStatusInterface( _ServiceInterface, namespace="/io_and_status_controller", initial_services={"set_io": SetIO}, - services={"resend_robot_program": Trigger}, + services={ + "resend_robot_program": Trigger, + }, +): + pass + + +class ConfigurationInterface( + _ServiceInterface, + namespace="/ur_configuration_controller", + initial_services={"get_robot_software_version": GetRobotSoftwareVersion}, + services={}, ): pass