From 601c452f3d0c3841a550a472d0776869a18d0a4c Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 11 Apr 2024 10:29:47 +0000 Subject: [PATCH 1/4] Implemented get version service, and integrated it with the tool contact test --- .../ur_robot_driver/hardware_interface.h | 3 ++ ur_robot_driver/src/hardware_interface.cpp | 13 ++++++ ur_robot_driver/test/integration_test.py | 41 ++++++++++++++++++- 3 files changed, 56 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index 5d435aaf3..dcc54a29f 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -215,6 +216,7 @@ class HardwareInterface : public hardware_interface::RobotHW void commandCallback(const std_msgs::StringConstPtr& msg); bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res); bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res); + bool getVersion(ur_msgs::GetVersionRequest& req, ur_msgs::GetVersionResponse& res); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -239,6 +241,7 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer tare_sensor_srv_; ros::ServiceServer set_payload_srv_; ros::ServiceServer activate_spline_interpolation_srv_; + ros::ServiceServer get_version_srv; hardware_interface::JointStateInterface js_interface_; scaled_controllers::ScaledPositionJointInterface spj_interface_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 63ecdbe2b..3355c82fd 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -460,6 +460,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw activate_spline_interpolation_srv_ = robot_hw_nh.advertiseService( "activate_spline_interpolation", &HardwareInterface::activateSplineInterpolation, this); + // Calling this service will return the software version of the robot. + get_version_srv = robot_hw_nh.advertiseService("get_version", &HardwareInterface::getVersion, this); + ur_driver_->startRTDECommunication(); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface"); @@ -1175,6 +1178,16 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set return true; } +bool HardwareInterface::getVersion(ur_msgs::GetVersionRequest& req, ur_msgs::GetVersionResponse& res) +{ + urcl::VersionInformation version_info = this->ur_driver_->getVersion(); + res.major = version_info.major; + res.minor = version_info.minor; + res.bugfix = version_info.bugfix; + res.build = version_info.build; + return true; +} + void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) { std::string str = msg->data; diff --git a/ur_robot_driver/test/integration_test.py b/ur_robot_driver/test/integration_test.py index 1de325dbf..d9fae50b2 100755 --- a/ur_robot_driver/test/integration_test.py +++ b/ur_robot_driver/test/integration_test.py @@ -15,7 +15,7 @@ from std_srvs.srv import Trigger, TriggerRequest import tf from trajectory_msgs.msg import JointTrajectoryPoint -from ur_msgs.srv import SetIO, SetIORequest +from ur_msgs.srv import SetIO, SetIORequest, GetVersion from ur_msgs.msg import IOStates from cartesian_control_msgs.msg import ( @@ -120,6 +120,30 @@ def init_robot(self): "actually running in headless mode." " Msg: {}".format(err)) + self.get_version = rospy.ServiceProxy("ur_hardware_interface/get_version", GetVersion) + try: + self.get_version.wait_for_service(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach 'get version' service. Make sure that the driver is actually running." + " Msg: {}".format(err)) + + self.start_tool_contact = rospy.ServiceProxy('ur_hardware_interface/start_tool_contact', Trigger) + try: + self.start_tool_contact.wait_for_service(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach 'start tool contact' service. Make sure that the driver is actually running." + " Msg: {}".format(err)) + + self.end_tool_contact = rospy.ServiceProxy('ur_hardware_interface/end_tool_contact', Trigger) + try: + self.end_tool_contact.wait_for_service(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach 'end tool contact' service. Make sure that the driver is actually running." + " Msg: {}".format(err)) + self.script_publisher = rospy.Publisher("/ur_hardware_interface/script_command", std_msgs.msg.String, queue_size=1) self.tf_listener = tf.TransformListener() self.twist_pub = rospy.Publisher("/twist_controller/command", geometry_msgs.msg.Twist, queue_size=1) @@ -262,6 +286,21 @@ def test_set_io(self): messages += 1 self.assertEqual(pin_state, 1) + def test_tool_contact(self): + version_info = self.get_version.call() + if version_info.major >= 5: + start_response = self.start_tool_contact.call() + self.assertEqual(start_response.success,True) + + end_response = self.end_tool_contact.call() + self.assertEqual(end_response.success, True) + else: + start_response = self.start_tool_contact.call() + self.assertEqual(start_response.success,False) + + end_response = self.end_tool_contact.call() + self.assertEqual(end_response.success, False) + def test_cartesian_passthrough(self): #### Power cycle the robot in order to make sure it is running correctly#### self.assertTrue(self.set_robot_to_mode(RobotMode.POWER_OFF)) From 2ffd4f8f7b5c7b82d640c3acbcd2f056fd70ad98 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 1 Jul 2024 13:37:14 +0000 Subject: [PATCH 2/4] Renamed get_version service to "GetRobotSoftwareVersion" everywhere --- .../include/ur_robot_driver/hardware_interface.h | 6 +++--- ur_robot_driver/src/hardware_interface.cpp | 4 ++-- ur_robot_driver/test/integration_test.py | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index dcc54a29f..7fe9b4ad3 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include #include @@ -216,7 +216,7 @@ class HardwareInterface : public hardware_interface::RobotHW void commandCallback(const std_msgs::StringConstPtr& msg); bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res); bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res); - bool getVersion(ur_msgs::GetVersionRequest& req, ur_msgs::GetVersionResponse& res); + bool getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, ur_msgs::GetRobotSoftwareVersionResponse& res); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -241,7 +241,7 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer tare_sensor_srv_; ros::ServiceServer set_payload_srv_; ros::ServiceServer activate_spline_interpolation_srv_; - ros::ServiceServer get_version_srv; + ros::ServiceServer get_robot_software_version_srv; hardware_interface::JointStateInterface js_interface_; scaled_controllers::ScaledPositionJointInterface spj_interface_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3355c82fd..de998aba1 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -461,7 +461,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw "activate_spline_interpolation", &HardwareInterface::activateSplineInterpolation, this); // Calling this service will return the software version of the robot. - get_version_srv = robot_hw_nh.advertiseService("get_version", &HardwareInterface::getVersion, this); + get_robot_software_version_srv = robot_hw_nh.advertiseService("get_robot_software_version", &HardwareInterface::getRobotSoftwareVersion, this); ur_driver_->startRTDECommunication(); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface"); @@ -1178,7 +1178,7 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set return true; } -bool HardwareInterface::getVersion(ur_msgs::GetVersionRequest& req, ur_msgs::GetVersionResponse& res) +bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, ur_msgs::GetRobotSoftwareVersionResponse& res) { urcl::VersionInformation version_info = this->ur_driver_->getVersion(); res.major = version_info.major; diff --git a/ur_robot_driver/test/integration_test.py b/ur_robot_driver/test/integration_test.py index d9fae50b2..95695c198 100755 --- a/ur_robot_driver/test/integration_test.py +++ b/ur_robot_driver/test/integration_test.py @@ -15,7 +15,7 @@ from std_srvs.srv import Trigger, TriggerRequest import tf from trajectory_msgs.msg import JointTrajectoryPoint -from ur_msgs.srv import SetIO, SetIORequest, GetVersion +from ur_msgs.srv import SetIO, SetIORequest, GetRobotSoftwareVersion from ur_msgs.msg import IOStates from cartesian_control_msgs.msg import ( @@ -120,9 +120,9 @@ def init_robot(self): "actually running in headless mode." " Msg: {}".format(err)) - self.get_version = rospy.ServiceProxy("ur_hardware_interface/get_version", GetVersion) + self.get_robot_software_version = rospy.ServiceProxy("ur_hardware_interface/get_robot_software_version", GetRobotSoftwareVersion) try: - self.get_version.wait_for_service(timeout) + self.get_robot_software_version.wait_for_service(timeout) except rospy.exceptions.ROSException as err: self.fail( "Could not reach 'get version' service. Make sure that the driver is actually running." @@ -287,7 +287,7 @@ def test_set_io(self): self.assertEqual(pin_state, 1) def test_tool_contact(self): - version_info = self.get_version.call() + version_info = self.get_robot_software_version.call() if version_info.major >= 5: start_response = self.start_tool_contact.call() self.assertEqual(start_response.success,True) From 2a0bb43cdb080f0b0408912178a4b5357398b84d Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 1 Jul 2024 13:49:23 +0000 Subject: [PATCH 3/4] Formatting --- .../include/ur_robot_driver/hardware_interface.h | 3 ++- ur_robot_driver/src/hardware_interface.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index 7fe9b4ad3..172f436de 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -216,7 +216,8 @@ class HardwareInterface : public hardware_interface::RobotHW void commandCallback(const std_msgs::StringConstPtr& msg); bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res); bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res); - bool getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, ur_msgs::GetRobotSoftwareVersionResponse& res); + bool getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, + ur_msgs::GetRobotSoftwareVersionResponse& res); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index de998aba1..59c6d8937 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -461,7 +461,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw "activate_spline_interpolation", &HardwareInterface::activateSplineInterpolation, this); // Calling this service will return the software version of the robot. - get_robot_software_version_srv = robot_hw_nh.advertiseService("get_robot_software_version", &HardwareInterface::getRobotSoftwareVersion, this); + get_robot_software_version_srv = + robot_hw_nh.advertiseService("get_robot_software_version", &HardwareInterface::getRobotSoftwareVersion, this); ur_driver_->startRTDECommunication(); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface"); @@ -1178,7 +1179,8 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set return true; } -bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, ur_msgs::GetRobotSoftwareVersionResponse& res) +bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, + ur_msgs::GetRobotSoftwareVersionResponse& res) { urcl::VersionInformation version_info = this->ur_driver_->getVersion(); res.major = version_info.major; From 2188bd4f82913b0bfaac1520092bb5e918085602 Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 3 Jul 2024 14:01:29 +0000 Subject: [PATCH 4/4] Removed test of tool contact --- ur_robot_driver/test/integration_test.py | 31 ------------------------ 1 file changed, 31 deletions(-) diff --git a/ur_robot_driver/test/integration_test.py b/ur_robot_driver/test/integration_test.py index 95695c198..a24461687 100755 --- a/ur_robot_driver/test/integration_test.py +++ b/ur_robot_driver/test/integration_test.py @@ -128,22 +128,6 @@ def init_robot(self): "Could not reach 'get version' service. Make sure that the driver is actually running." " Msg: {}".format(err)) - self.start_tool_contact = rospy.ServiceProxy('ur_hardware_interface/start_tool_contact', Trigger) - try: - self.start_tool_contact.wait_for_service(timeout) - except rospy.exceptions.ROSException as err: - self.fail( - "Could not reach 'start tool contact' service. Make sure that the driver is actually running." - " Msg: {}".format(err)) - - self.end_tool_contact = rospy.ServiceProxy('ur_hardware_interface/end_tool_contact', Trigger) - try: - self.end_tool_contact.wait_for_service(timeout) - except rospy.exceptions.ROSException as err: - self.fail( - "Could not reach 'end tool contact' service. Make sure that the driver is actually running." - " Msg: {}".format(err)) - self.script_publisher = rospy.Publisher("/ur_hardware_interface/script_command", std_msgs.msg.String, queue_size=1) self.tf_listener = tf.TransformListener() self.twist_pub = rospy.Publisher("/twist_controller/command", geometry_msgs.msg.Twist, queue_size=1) @@ -286,21 +270,6 @@ def test_set_io(self): messages += 1 self.assertEqual(pin_state, 1) - def test_tool_contact(self): - version_info = self.get_robot_software_version.call() - if version_info.major >= 5: - start_response = self.start_tool_contact.call() - self.assertEqual(start_response.success,True) - - end_response = self.end_tool_contact.call() - self.assertEqual(end_response.success, True) - else: - start_response = self.start_tool_contact.call() - self.assertEqual(start_response.success,False) - - end_response = self.end_tool_contact.call() - self.assertEqual(end_response.success, False) - def test_cartesian_passthrough(self): #### Power cycle the robot in order to make sure it is running correctly#### self.assertTrue(self.set_robot_to_mode(RobotMode.POWER_OFF))