From 3d707140460f7378a937b7dfc03b0cc8ec74b242 Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 11 Apr 2024 08:28:21 +0000 Subject: [PATCH 1/7] Implemented get_version service --- .../ur_controllers/gpio_controller.hpp | 10 ++++++ ur_controllers/src/gpio_controller.cpp | 33 +++++++++++++++++++ .../ur_robot_driver/hardware_interface.hpp | 6 ++++ ur_robot_driver/src/hardware_interface.cpp | 29 ++++++++++++++++ ur_robot_driver/urdf/ur.ros2_control.xacro | 9 +++++ 5 files changed, 87 insertions(+) diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index c20bd680e..0573ad521 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -57,6 +57,7 @@ #include "rclcpp/duration.hpp" #include "std_msgs/msg/bool.hpp" #include "gpio_controller_parameters.hpp" +#include "ur_msgs/srv/get_version.hpp" namespace ur_controllers { @@ -79,6 +80,12 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, + GET_VERSION_CMD = 39, + GET_VERSION_ASYNC_SUCCESS = 40, + GET_VERSION_MAJOR = 41, + GET_VERSION_MINOR = 42, + GET_VERSION_BUGFIX = 43, + GET_VERSION_BUILD = 44 }; enum StateInterfaces @@ -136,6 +143,8 @@ class GPIOController : public controller_interface::ControllerInterface bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); + bool getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr req, ur_msgs::srv::GetVersion::Response::SharedPtr resp); + void publishIO(); void publishToolData(); @@ -163,6 +172,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; + rclcpp::Service::SharedPtr get_version_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 670cf9c8b..e2c4a067f 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,6 +97,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); + // Get version service + config.names.emplace_back(tf_prefix + "get_version/get_version_cmd"); + config.names.emplace_back(tf_prefix + "get_version/get_version_async_success"); + config.names.emplace_back(tf_prefix + "get_version/get_version_major"); + config.names.emplace_back(tf_prefix + "get_version/get_version_minor"); + config.names.emplace_back(tf_prefix + "get_version/get_version_bugfix"); + config.names.emplace_back(tf_prefix + "get_version/get_version_build"); + return config; } @@ -316,6 +324,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre tare_sensor_srv_ = get_node()->create_service( "~/zero_ftsensor", std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2)); + + get_version_srv_ = get_node()->create_service( + "~/get_version", std::bind(&GPIOController::getVersion, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -524,6 +535,28 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r return true; } +bool GPIOController::getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr /*req*/, + ur_msgs::srv::GetVersion::Response::SharedPtr resp) +{ + // reset success flag + command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + + // call the service in the hardware + command_interfaces_[CommandInterfaces::GET_VERSION_CMD].set_value(1.0); + + if (!waitForAsyncCommand( + [&]() { return command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify software version of robot."); + } + + resp->major = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_MAJOR].get_value()); + resp->minor = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_MINOR].get_value()); + resp->bugfix = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_BUGFIX].get_value()); + resp->build = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_BUILD].get_value()); + + return true; +} + void GPIOController::initMsgs() { io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size()); 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 fe1ede6f7..dafbe7606 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -190,6 +190,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; + double get_version_async_success_; + double get_version_cmd_; + double get_version_major_; + double get_version_minor_; + double get_version_bugfix_; + double get_version_build_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 8ff61080e..c4fd7ff3e 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -298,6 +298,24 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_cmd", &get_version_cmd_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "get_version", "get_version_async_success", &get_version_async_success_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_major", &get_version_major_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_minor", &get_version_minor_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_bugfix", &get_version_bugfix_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_build", &get_version_build_)); + return command_interfaces; } @@ -604,6 +622,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; + get_version_cmd_ = NO_NEW_CMD_; initialized_ = true; } @@ -728,6 +747,16 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); zero_ftsensor_cmd_ = NO_NEW_CMD_; } + + if (!std::isnan(get_version_cmd_) && ur_driver_ != nullptr) { + urcl::VersionInformation version_info = ur_driver_->getVersion(); + get_version_cmd_ = NO_NEW_CMD_; + get_version_major_ = version_info.major; + get_version_minor_ = version_info.minor; + get_version_bugfix_ = version_info.bugfix; + get_version_build_ = version_info.build; + get_version_async_success_ = 1.0; + } } void URPositionHardwareInterface::updateNonDoubleValues() diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 308071cc1..00c6bd670 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -223,6 +223,15 @@ + + + + + + + + + From cf29521d14a5de2754f9355442790027c874b37a Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 11 Apr 2024 08:32:43 +0000 Subject: [PATCH 2/7] Implemented test of get_version service and integrated it with the test of tool contact --- ur_robot_driver/test/robot_driver.py | 14 ++++++++++++++ ur_robot_driver/test/test_common.py | 9 +++++++-- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 96c859947..f797c8db7 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -249,6 +249,20 @@ def test_trajectory_scaled(self, tf_prefix): ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + def test_tool_contact(self, tf_prefix): + if self._io_status_controller_interface.get_version().major < 5: + start_result = self._io_status_controller_interface.start_tool_contact() + self.assertEqual(start_result.success, False) + + end_result = self._io_status_controller_interface.end_tool_contact() + self.assertEqual(end_result.success, False) + else: + start_result = self._io_status_controller_interface.start_tool_contact() + self.assertEqual(start_result.success, True) + + end_result = self._io_status_controller_interface.end_tool_contact() + self.assertEqual(end_result.success, True) + def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): """Test that the robot correctly aborts the trajectory when the constraints are violated.""" # Construct test trajectory diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 5b6033cea..3421df8a0 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, GetVersion 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,12 @@ class IoStatusInterface( _ServiceInterface, namespace="/io_and_status_controller", initial_services={"set_io": SetIO}, - services={"resend_robot_program": Trigger}, + services={ + "resend_robot_program": Trigger, + "start_tool_contact": Trigger, + "end_tool_contact": Trigger, + "get_version": GetVersion, + }, ): pass From dffccc60afa6873cdb545335ce8a9371ec6b2089 Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 11 Apr 2024 08:28:21 +0000 Subject: [PATCH 3/7] Implemented get_version service --- .../ur_controllers/gpio_controller.hpp | 10 ++++++ ur_controllers/src/gpio_controller.cpp | 33 +++++++++++++++++++ .../ur_robot_driver/hardware_interface.hpp | 6 ++++ ur_robot_driver/src/hardware_interface.cpp | 29 ++++++++++++++++ ur_robot_driver/urdf/ur.ros2_control.xacro | 9 +++++ 5 files changed, 87 insertions(+) diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index c20bd680e..0573ad521 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -57,6 +57,7 @@ #include "rclcpp/duration.hpp" #include "std_msgs/msg/bool.hpp" #include "gpio_controller_parameters.hpp" +#include "ur_msgs/srv/get_version.hpp" namespace ur_controllers { @@ -79,6 +80,12 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, + GET_VERSION_CMD = 39, + GET_VERSION_ASYNC_SUCCESS = 40, + GET_VERSION_MAJOR = 41, + GET_VERSION_MINOR = 42, + GET_VERSION_BUGFIX = 43, + GET_VERSION_BUILD = 44 }; enum StateInterfaces @@ -136,6 +143,8 @@ class GPIOController : public controller_interface::ControllerInterface bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); + bool getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr req, ur_msgs::srv::GetVersion::Response::SharedPtr resp); + void publishIO(); void publishToolData(); @@ -163,6 +172,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; + rclcpp::Service::SharedPtr get_version_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index d5e6d16ee..99d10436d 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,6 +97,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); + // Get version service + config.names.emplace_back(tf_prefix + "get_version/get_version_cmd"); + config.names.emplace_back(tf_prefix + "get_version/get_version_async_success"); + config.names.emplace_back(tf_prefix + "get_version/get_version_major"); + config.names.emplace_back(tf_prefix + "get_version/get_version_minor"); + config.names.emplace_back(tf_prefix + "get_version/get_version_bugfix"); + config.names.emplace_back(tf_prefix + "get_version/get_version_build"); + return config; } @@ -312,6 +320,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre tare_sensor_srv_ = get_node()->create_service( "~/zero_ftsensor", std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2)); + + get_version_srv_ = get_node()->create_service( + "~/get_version", std::bind(&GPIOController::getVersion, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -520,6 +531,28 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r return true; } +bool GPIOController::getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr /*req*/, + ur_msgs::srv::GetVersion::Response::SharedPtr resp) +{ + // reset success flag + command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + + // call the service in the hardware + command_interfaces_[CommandInterfaces::GET_VERSION_CMD].set_value(1.0); + + if (!waitForAsyncCommand( + [&]() { return command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify software version of robot."); + } + + resp->major = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_MAJOR].get_value()); + resp->minor = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_MINOR].get_value()); + resp->bugfix = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_BUGFIX].get_value()); + resp->build = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_BUILD].get_value()); + + return true; +} + void GPIOController::initMsgs() { io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size()); 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..5128f1ee8 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,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; + double get_version_async_success_; + double get_version_cmd_; + double get_version_major_; + double get_version_minor_; + double get_version_bugfix_; + double get_version_build_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5826cf595..bee480394 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -298,6 +298,24 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_cmd", &get_version_cmd_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "get_version", "get_version_async_success", &get_version_async_success_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_major", &get_version_major_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_minor", &get_version_minor_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_bugfix", &get_version_bugfix_)); + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_build", &get_version_build_)); + return command_interfaces; } @@ -603,6 +621,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; + get_version_cmd_ = NO_NEW_CMD_; initialized_ = true; } @@ -727,6 +746,16 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor(); zero_ftsensor_cmd_ = NO_NEW_CMD_; } + + if (!std::isnan(get_version_cmd_) && ur_driver_ != nullptr) { + urcl::VersionInformation version_info = ur_driver_->getVersion(); + get_version_cmd_ = NO_NEW_CMD_; + get_version_major_ = version_info.major; + get_version_minor_ = version_info.minor; + get_version_bugfix_ = version_info.bugfix; + get_version_build_ = version_info.build; + get_version_async_success_ = 1.0; + } } void URPositionHardwareInterface::updateNonDoubleValues() diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 77e0c22cd..539cefa12 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -223,6 +223,15 @@ + + + + + + + + + From 0585800f6d57ed6639dbea6547ef8f4c5e082c12 Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 11 Apr 2024 08:32:43 +0000 Subject: [PATCH 4/7] Implemented test of get_version service and integrated it with the test of tool contact --- ur_robot_driver/test/robot_driver.py | 14 ++++++++++++++ ur_robot_driver/test/test_common.py | 9 +++++++-- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 96c859947..f797c8db7 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -249,6 +249,20 @@ def test_trajectory_scaled(self, tf_prefix): ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + def test_tool_contact(self, tf_prefix): + if self._io_status_controller_interface.get_version().major < 5: + start_result = self._io_status_controller_interface.start_tool_contact() + self.assertEqual(start_result.success, False) + + end_result = self._io_status_controller_interface.end_tool_contact() + self.assertEqual(end_result.success, False) + else: + start_result = self._io_status_controller_interface.start_tool_contact() + self.assertEqual(start_result.success, True) + + end_result = self._io_status_controller_interface.end_tool_contact() + self.assertEqual(end_result.success, True) + def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): """Test that the robot correctly aborts the trajectory when the constraints are violated.""" # Construct test trajectory diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 435a20c37..91a6b7951 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, GetVersion 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,12 @@ class IoStatusInterface( _ServiceInterface, namespace="/io_and_status_controller", initial_services={"set_io": SetIO}, - services={"resend_robot_program": Trigger}, + services={ + "resend_robot_program": Trigger, + "start_tool_contact": Trigger, + "end_tool_contact": Trigger, + "get_version": GetVersion, + }, ): pass From c26e233e138df308bb714c9c63551d7b91ae2066 Mon Sep 17 00:00:00 2001 From: URJala <159417921+URJala@users.noreply.github.com> Date: Mon, 1 Jul 2024 12:02:39 +0200 Subject: [PATCH 5/7] Update ur_robot_driver/test/test_common.py Co-authored-by: Felix Exner (fexner) --- ur_robot_driver/test/test_common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 91a6b7951..5378fe352 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -244,7 +244,7 @@ class IoStatusInterface( "resend_robot_program": Trigger, "start_tool_contact": Trigger, "end_tool_contact": Trigger, - "get_version": GetVersion, + "get_robot_software_version": GetVersion, }, ): pass From 668d807e6e9c031f0311d72dba0c252e068ffb68 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 1 Jul 2024 11:34:09 +0000 Subject: [PATCH 6/7] Renamed service to "GetRobotSoftwareVersion" everywhere. Also moved the version information from being stored in the command interface to be stored in the state interface --- .../ur_controllers/gpio_controller.hpp | 19 ++++---- ur_controllers/src/gpio_controller.cpp | 33 +++++++------ .../ur_robot_driver/hardware_interface.hpp | 12 ++--- ur_robot_driver/src/hardware_interface.cpp | 47 ++++++++++--------- ur_robot_driver/test/robot_driver.py | 2 +- ur_robot_driver/test/test_common.py | 4 +- ur_robot_driver/urdf/ur.ros2_control.xacro | 10 ++-- 7 files changed, 66 insertions(+), 61 deletions(-) diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index 0573ad521..2797de618 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -57,7 +57,7 @@ #include "rclcpp/duration.hpp" #include "std_msgs/msg/bool.hpp" #include "gpio_controller_parameters.hpp" -#include "ur_msgs/srv/get_version.hpp" +#include "ur_msgs/srv/get_robot_software_version.hpp" namespace ur_controllers { @@ -80,12 +80,8 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, - GET_VERSION_CMD = 39, - GET_VERSION_ASYNC_SUCCESS = 40, - GET_VERSION_MAJOR = 41, - GET_VERSION_MINOR = 42, - GET_VERSION_BUGFIX = 43, - GET_VERSION_BUILD = 44 + GET_VERSION_CMD = 35, + GET_VERSION_ASYNC_SUCCESS = 36 }; enum StateInterfaces @@ -107,6 +103,10 @@ enum StateInterfaces SAFETY_STATUS_BITS = 58, INITIALIZED_FLAG = 69, PROGRAM_RUNNING = 70, + GET_VERSION_MAJOR = 71, + GET_VERSION_MINOR = 72, + GET_VERSION_BUGFIX = 73, + GET_VERSION_BUILD = 74 }; class GPIOController : public controller_interface::ControllerInterface @@ -143,7 +143,8 @@ class GPIOController : public controller_interface::ControllerInterface bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp); - bool getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr req, ur_msgs::srv::GetVersion::Response::SharedPtr resp); + bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req, + ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp); void publishIO(); @@ -172,7 +173,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr set_io_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; - rclcpp::Service::SharedPtr get_version_srv_; + rclcpp::Service::SharedPtr get_robot_software_version_srv_; std::shared_ptr> io_pub_; std::shared_ptr> tool_data_pub_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 99d10436d..3540ca4c2 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,13 +97,9 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); - // Get version service - config.names.emplace_back(tf_prefix + "get_version/get_version_cmd"); - config.names.emplace_back(tf_prefix + "get_version/get_version_async_success"); - config.names.emplace_back(tf_prefix + "get_version/get_version_major"); - config.names.emplace_back(tf_prefix + "get_version/get_version_minor"); - config.names.emplace_back(tf_prefix + "get_version/get_version_bugfix"); - config.names.emplace_back(tf_prefix + "get_version/get_version_build"); + // Robot software version service + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_cmd"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_async_success"); return config; } @@ -166,6 +162,12 @@ 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; } @@ -321,8 +323,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre "~/zero_ftsensor", std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2)); - get_version_srv_ = get_node()->create_service( - "~/get_version", std::bind(&GPIOController::getVersion, this, std::placeholders::_1, std::placeholders::_2)); + get_robot_software_version_srv_ = get_node()->create_service( + "~/get_robot_software_version", + std::bind(&GPIOController::getRobotSoftwareVersion, this, std::placeholders::_1, std::placeholders::_2)); } catch (...) { return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -531,8 +534,8 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r return true; } -bool GPIOController::getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr /*req*/, - ur_msgs::srv::GetVersion::Response::SharedPtr resp) +bool GPIOController::getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/, + ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp) { // reset success flag command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].set_value(ASYNC_WAITING); @@ -545,10 +548,10 @@ bool GPIOController::getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr /*r RCLCPP_WARN(get_node()->get_logger(), "Could not verify software version of robot."); } - resp->major = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_MAJOR].get_value()); - resp->minor = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_MINOR].get_value()); - resp->bugfix = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_BUGFIX].get_value()); - resp->build = static_cast(command_interfaces_[CommandInterfaces::GET_VERSION_BUILD].get_value()); + resp->major = static_cast(state_interfaces_[StateInterfaces::GET_VERSION_MAJOR].get_value()); + resp->minor = static_cast(state_interfaces_[StateInterfaces::GET_VERSION_MINOR].get_value()); + resp->bugfix = static_cast(state_interfaces_[StateInterfaces::GET_VERSION_BUGFIX].get_value()); + resp->build = static_cast(state_interfaces_[StateInterfaces::GET_VERSION_BUILD].get_value()); return true; } 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 5128f1ee8..926d817d7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -191,12 +191,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; - double get_version_async_success_; - double get_version_cmd_; - double get_version_major_; - double get_version_minor_; - double get_version_bugfix_; - double get_version_build_; + double get_robot_software_version_async_success_; + double get_robot_software_version_cmd_; + 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/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index bee480394..cedec1c96 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -231,6 +231,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; } @@ -298,23 +310,12 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_cmd", &get_version_cmd_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "get_version", "get_version_async_success", &get_version_async_success_)); + tf_prefix + "get_robot_software_version", "get_version_cmd", &get_robot_software_version_cmd_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_major", &get_version_major_)); - - command_interfaces.emplace_back( - hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_minor", &get_version_minor_)); - - command_interfaces.emplace_back( - hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_bugfix", &get_version_bugfix_)); - - command_interfaces.emplace_back( - hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_build", &get_version_build_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "get_robot_software_version", + "get_version_async_success", + &get_robot_software_version_async_success_)); return command_interfaces; } @@ -621,7 +622,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: resend_robot_program_cmd_ = NO_NEW_CMD_; zero_ftsensor_cmd_ = NO_NEW_CMD_; hand_back_control_cmd_ = NO_NEW_CMD_; - get_version_cmd_ = NO_NEW_CMD_; + get_robot_software_version_cmd_ = NO_NEW_CMD_; initialized_ = true; } @@ -747,14 +748,14 @@ void URPositionHardwareInterface::checkAsyncIO() zero_ftsensor_cmd_ = NO_NEW_CMD_; } - if (!std::isnan(get_version_cmd_) && ur_driver_ != nullptr) { + if (!std::isnan(get_robot_software_version_cmd_) && ur_driver_ != nullptr) { urcl::VersionInformation version_info = ur_driver_->getVersion(); - get_version_cmd_ = NO_NEW_CMD_; - get_version_major_ = version_info.major; - get_version_minor_ = version_info.minor; - get_version_bugfix_ = version_info.bugfix; - get_version_build_ = version_info.build; - get_version_async_success_ = 1.0; + get_robot_software_version_cmd_ = NO_NEW_CMD_; + get_robot_software_version_major_ = version_info.major; + get_robot_software_version_minor_ = version_info.minor; + get_robot_software_version_bugfix_ = version_info.bugfix; + get_robot_software_version_build_ = version_info.build; + get_robot_software_version_async_success_ = 1.0; } } diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index f797c8db7..f73bfc40a 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -250,7 +250,7 @@ def test_trajectory_scaled(self, tf_prefix): self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) def test_tool_contact(self, tf_prefix): - if self._io_status_controller_interface.get_version().major < 5: + if self._io_status_controller_interface.get_robot_software_version().major < 5: start_result = self._io_status_controller_interface.start_tool_contact() self.assertEqual(start_result.success, False) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 5378fe352..8bbc79411 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, GetVersion +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. @@ -244,7 +244,7 @@ class IoStatusInterface( "resend_robot_program": Trigger, "start_tool_contact": Trigger, "end_tool_contact": Trigger, - "get_robot_software_version": GetVersion, + "get_robot_software_version": GetRobotSoftwareVersion, }, ): pass diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 539cefa12..0555fb650 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -223,13 +223,13 @@ - + - - - - + + + + From d2c14a4502cb50651271fd66ec390db3fdafbd8b Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 4 Jul 2024 10:50:35 +0000 Subject: [PATCH 7/7] Remove tool contact from test. --- ur_robot_driver/test/robot_driver.py | 14 -------------- ur_robot_driver/test/test_common.py | 2 -- 2 files changed, 16 deletions(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index f73bfc40a..96c859947 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -249,20 +249,6 @@ def test_trajectory_scaled(self, tf_prefix): ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) - def test_tool_contact(self, tf_prefix): - if self._io_status_controller_interface.get_robot_software_version().major < 5: - start_result = self._io_status_controller_interface.start_tool_contact() - self.assertEqual(start_result.success, False) - - end_result = self._io_status_controller_interface.end_tool_contact() - self.assertEqual(end_result.success, False) - else: - start_result = self._io_status_controller_interface.start_tool_contact() - self.assertEqual(start_result.success, True) - - end_result = self._io_status_controller_interface.end_tool_contact() - self.assertEqual(end_result.success, True) - def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): """Test that the robot correctly aborts the trajectory when the constraints are violated.""" # Construct test trajectory diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 03e92fbba..0a10ffe0b 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -242,8 +242,6 @@ class IoStatusInterface( initial_services={"set_io": SetIO}, services={ "resend_robot_program": Trigger, - "start_tool_contact": Trigger, - "end_tool_contact": Trigger, "get_robot_software_version": GetRobotSoftwareVersion, }, ):