From a859953e12c4b55e5feca295b0f58295d2052f1d Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 11 Apr 2024 10:13:23 +0000 Subject: [PATCH] Implement tool contact and test --- .../ur_robot_driver/hardware_interface.h | 4 +++ ur_robot_driver/src/hardware_interface.cpp | 19 ++++++++++++++ ur_robot_driver/test/integration_test.py | 25 ++++++++++++++++++- 3 files changed, 47 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..50c84cc29 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -215,6 +215,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 startToolContact(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); + bool endToolContact(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -239,6 +241,8 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer tare_sensor_srv_; ros::ServiceServer set_payload_srv_; ros::ServiceServer activate_spline_interpolation_srv_; + ros::ServiceServer start_tool_contact_srv_; + ros::ServiceServer end_tool_contact_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..0458c4a5d 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -460,6 +460,13 @@ 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 enable the tool contact functionality on the robot. + start_tool_contact_srv_ = + robot_hw_nh.advertiseService("start_tool_contact", &HardwareInterface::startToolContact, this); + + // Calling this service will disable the tool contact functionality on the robot. + end_tool_contact_srv_ = robot_hw_nh.advertiseService("end_tool_contact", &HardwareInterface::endToolContact, this); + ur_driver_->startRTDECommunication(); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface"); @@ -1175,6 +1182,18 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set return true; } +bool HardwareInterface::startToolContact(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) +{ + res.success = this->ur_driver_->startToolContact(); + return true; +} + +bool HardwareInterface::endToolContact(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) +{ + res.success = this->ur_driver_->endToolContact(); + 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..e6f878e4b 100755 --- a/ur_robot_driver/test/integration_test.py +++ b/ur_robot_driver/test/integration_test.py @@ -12,7 +12,7 @@ FollowJointTrajectoryResult, JointTolerance) from ur_dashboard_msgs.msg import SetModeAction, SetModeGoal, RobotMode -from std_srvs.srv import Trigger, TriggerRequest +from std_srvs.srv import Trigger import tf from trajectory_msgs.msg import JointTrajectoryPoint from ur_msgs.srv import SetIO, SetIORequest @@ -120,6 +120,22 @@ def init_robot(self): "actually running in headless mode." " 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 +278,13 @@ def test_set_io(self): messages += 1 self.assertEqual(pin_state, 1) + def test_tool_contact(self): + 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) + 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))