Added service to set force mode through ROS

Author: urmarp

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -54,6 +54,7 @@
 #include <ur_msgs/SetIO.h>
 #include <ur_msgs/SetSpeedSliderFraction.h>
 #include <ur_msgs/SetPayload.h>
+#include <ur_msgs/SetForceMode.h>
 
 #include <cartesian_interface/cartesian_command_interface.h>
 #include <cartesian_interface/cartesian_state_handle.h>
@@ -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 setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res);
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> 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 set_force_mode_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   scaled_controllers::ScaledPositionJointInterface spj_interface_;

ur_robot_driver/src/hardware_interface.cpp

@@ -455,6 +455,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // Setup the mounted payload through a ROS service
   set_payload_srv_ = robot_hw_nh.advertiseService("set_payload", &HardwareInterface::setPayload, this);
 
+  // Calling this service will set the robot in force mode
+  set_force_mode_srv_ = robot_hw_nh.advertiseService("set_force_mode", &HardwareInterface::setForceMode, this);
+
   // Call this to activate or deactivate using spline interpolation locally on the UR controller, when forwarding
   // trajectories to the UR robot.
   activate_spline_interpolation_srv_ = robot_hw_nh.advertiseService(
@@ -1175,6 +1178,31 @@ bool HardwareInterface::setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::Set
   return true;
 }
 
+bool HardwareInterface::setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res)
+{
+  if (req.task_frame.size() != 6 || req.selection_vector.size() != 6 || req.wrench.size() != 6 ||
+      req.limits.size() != 6)
+  {
+    URCL_LOG_WARN("Size of received SetForceMode message is wrong");
+    res.success = false;
+    return false;
+  }
+  urcl::vector6d_t task_frame;
+  urcl::vector6uint32_t selection_vector;
+  urcl::vector6d_t wrench;
+  urcl::vector6d_t limits;
+  for (size_t i = 0; i < 6; i++)
+  {
+    task_frame[i] = req.task_frame[i];
+    selection_vector[i] = req.selection_vector[i];
+    wrench[i] = req.wrench[i];
+    limits[i] = req.limits[i];
+  }
+  unsigned int type = req.type;
+  res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits);
+  return res.success;
+}
+
 void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
 {
   std::string str = msg->data;