From 5407dfe7aa623f75eec93fae8cd1ce15e8d6780e Mon Sep 17 00:00:00 2001 From: URJala <159417921+URJala@users.noreply.github.com> Date: Fri, 20 Sep 2024 17:56:01 +1000 Subject: [PATCH] Implemented get version service (#695) Add a service to get the polyscope version. --- .../include/ur_robot_driver/hardware_interface.h | 4 ++++ ur_robot_driver/src/hardware_interface.cpp | 15 +++++++++++++++ ur_robot_driver/test/integration_test.py | 10 +++++++++- 3 files changed, 28 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..172f436de 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,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); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -239,6 +242,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_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 63ecdbe2b..59c6d8937 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -460,6 +460,10 @@ 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_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"); @@ -1175,6 +1179,17 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set return true; } +bool HardwareInterface::getRobotSoftwareVersion(ur_msgs::GetRobotSoftwareVersionRequest& req, + ur_msgs::GetRobotSoftwareVersionResponse& 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 5be1330bf..34ef1516d 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, GetRobotSoftwareVersion from ur_msgs.msg import IOStates from cartesian_control_msgs.msg import ( @@ -120,6 +120,14 @@ def init_robot(self): "actually running in headless mode." " Msg: {}".format(err)) + self.get_robot_software_version = rospy.ServiceProxy("ur_hardware_interface/get_robot_software_version", GetRobotSoftwareVersion) + try: + 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." + " 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)