Added a service to zero the robot's ftsensor

ur_robot_driver/doc/ROS_INTERFACE.md

@@ -673,6 +673,10 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
 
     Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1. Only set this smaller than 1 if you are using the scaled controllers (as by default) or you know what you're doing. Using this with other controllers might lead to unexpected behaviors.
 
+ * "**zero_ftsensor**" ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
+
+    Calling this service will zero the robot's ftsensor. Note: On e-Series robots this will only work when the robot is in remote-control mode.
+
 #### Parameters
  * "**dashboard/receive_timeout**" (Required)
 

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -190,12 +190,14 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool setSpeedSlider(ur_msgs::SetSpeedSliderFractionRequest& req, ur_msgs::SetSpeedSliderFractionResponse& res);
   bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res);
   bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
+  bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
   void commandCallback(const std_msgs::StringConstPtr& msg);
 
   std::unique_ptr<UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> dashboard_client_;
 
   ros::ServiceServer deactivate_srv_;
+  ros::ServiceServer tare_sensor_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   ur_controllers::ScaledPositionJointInterface spj_interface_;

ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h

@@ -31,6 +31,7 @@
 #include "ur_robot_driver/comm/reverse_interface.h"
 #include "ur_robot_driver/comm/script_sender.h"
 #include "ur_robot_driver/ur/tool_communication.h"
+#include "ur_robot_driver/ur/version_information.h"
 #include "ur_robot_driver/primary/robot_message/version_message.h"
 #include "ur_robot_driver/rtde/rtde_writer.h"
 
@@ -173,6 +174,14 @@ class UrDriver
    */
   bool sendRobotProgram();
 
+  /*!
+   * \brief Returns version information about the currently connected robot
+   */
+  const VersionInformation& getVersion()
+  {
+    return robot_version_;
+  }
+
 private:
   std::string readScriptFile(const std::string& filename);
   std::string readKeepalive();
@@ -197,6 +206,8 @@ class UrDriver
   std::string robot_ip_;
   bool in_headless_mode_;
   std::string full_robot_program_;
+
+  VersionInformation robot_version_;
 };
 }  // namespace ur_driver
 #endif  // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED

ur_robot_driver/include/ur_robot_driver/ur/version_information.h

@@ -44,6 +44,12 @@ struct VersionInformation
     bugfix = 0;
     build = 0;
   }
+
+  friend std::ostream& operator<<(std::ostream& os, const VersionInformation& version_info)
+  {
+    os << version_info.major << "." << version_info.minor << "." << version_info.bugfix << "-" << version_info.build;
+    return os;
+  }
   uint32_t major;   ///< Major version number
   uint32_t minor;   ///< Minor version number
   uint32_t bugfix;  ///< Bugfix version number

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -315,6 +315,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // Calling this service will make the "External Control" program node on the UR-Program return.
   deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
 
+  // Calling this service will zero the robot's ftsensor. Note: On e-Series robots this will only
+  // work when the robot is in remote-control mode.
+  tare_sensor_srv_ = robot_hw_nh.advertiseService("zero_ftsensor", &HardwareInterface::zeroFTSensor, this);
+
   ur_driver_->startRTDECommunication();
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
 
@@ -694,6 +698,28 @@ bool HardwareInterface::resendRobotProgram(std_srvs::TriggerRequest& req, std_sr
   return true;
 }
 
+bool HardwareInterface::zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
+{
+  if (ur_driver_->getVersion().major < 5)
+  {
+    std::stringstream ss;
+    ss << "Zeroing the Force-Torque sensor is only available for e-Series robots (Major version >= 5). This robot's "
+          "version is "
+       << ur_driver_->getVersion();
+    ROS_ERROR_STREAM(ss.str());
+    res.message = ss.str();
+    res.success = false;
+  }
+  else
+  {
+    res.success = this->ur_driver_->sendScript(R"(sec tareSensor():
+  zero_ftsensor()
+end
+)");
+  }
+  return true;
+}
+
 void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
 {
   std::string str = msg->data;
@@ -745,4 +771,4 @@ void HardwareInterface::publishRobotAndSafetyMode()
 }
 }  // namespace ur_driver
 
-PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)
+PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)

ur_robot_driver/src/ur/ur_driver.cpp

@@ -92,16 +92,16 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
   prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
 
-  auto urcontrol_version = rtde_client_->getVersion();
+  robot_version_ = rtde_client_->getVersion();
 
   std::stringstream begin_replace;
   if (tool_comm_setup != nullptr)
   {
-    if (urcontrol_version.major < 5)
+    if (robot_version_.major < 5)
     {
       throw ToolCommNotAvailable("Tool communication setup requested, but this robot version does not support using "
                                  "the tool communication interface. Please check your configuration.",
-                                 5, urcontrol_version.major);
+                                 5, robot_version_.major);
     }
     begin_replace << "set_tool_voltage("
                   << static_cast<std::underlying_type<ToolVoltage>::type>(tool_comm_setup->getToolVoltage()) << ")\n";