Changed setForce mode to startForceMode and endForceMode. Also made c…

…hanges to account for changes in msg in PR (ros-industrial/ur_msgs#20)

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -214,7 +214,8 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
   void commandCallback(const std_msgs::StringConstPtr& msg);
   bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res);
-  bool setForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res);
+  bool startForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res);
+  bool endForceMode(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> dashboard_client_;
@@ -238,7 +239,8 @@ class HardwareInterface : public hardware_interface::RobotHW
   ros::ServiceServer deactivate_srv_;
   ros::ServiceServer tare_sensor_srv_;
   ros::ServiceServer set_payload_srv_;
-  ros::ServiceServer set_force_mode_srv_;
+  ros::ServiceServer start_force_mode_srv_;
+  ros::ServiceServer end_force_mode_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   scaled_controllers::ScaledPositionJointInterface spj_interface_;

ur_robot_driver/src/hardware_interface.cpp

@@ -449,7 +449,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   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);
+  start_force_mode_srv_ = robot_hw_nh.advertiseService("start_force_mode", &HardwareInterface::startForceMode, this);
+
+  // Calling this service will stop the robot from being in force mode
+  end_force_mode_srv_ = robot_hw_nh.advertiseService("end_force_mode", &HardwareInterface::endForceMode, this);
 
   ur_driver_->startRTDECommunication();
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
@@ -1166,30 +1169,54 @@ 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;
-  }
+bool HardwareInterface::startForceMode(ur_msgs::SetForceModeRequest& req, ur_msgs::SetForceModeResponse& res)
+{
   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];
-  }
+
+  task_frame[0] = req.task_frame.pose.position.x;
+  task_frame[1] = req.task_frame.pose.position.x;
+  task_frame[2] = req.task_frame.pose.position.x;
+  KDL::Rotation rot = KDL::Rotation::Quaternion(req.task_frame.pose.orientation.x, req.task_frame.pose.orientation.y,
+                                                req.task_frame.pose.orientation.z, req.task_frame.pose.orientation.w);
+  task_frame[3] = rot.GetRot().x();
+  task_frame[4] = rot.GetRot().y();
+  task_frame[5] = rot.GetRot().z();
+
+  selection_vector[0] = req.selection_vector_x;
+  selection_vector[1] = req.selection_vector_y;
+  selection_vector[2] = req.selection_vector_z;
+  selection_vector[3] = req.selection_vector_rx;
+  selection_vector[4] = req.selection_vector_ry;
+  selection_vector[5] = req.selection_vector_rz;
+
+  wrench[0] = req.wrench.wrench.force.x;
+  wrench[1] = req.wrench.wrench.force.y;
+  wrench[2] = req.wrench.wrench.force.z;
+  wrench[3] = req.wrench.wrench.torque.x;
+  wrench[4] = req.wrench.wrench.torque.y;
+  wrench[5] = req.wrench.wrench.torque.z;
+
+  limits[0] = req.limits.twist.linear.x;
+  limits[1] = req.limits.twist.linear.y;
+  limits[2] = req.limits.twist.linear.z;
+  limits[3] = req.limits.twist.angular.x;
+  limits[4] = req.limits.twist.angular.y;
+  limits[5] = req.limits.twist.angular.z;
+
   unsigned int type = req.type;
   res.success = this->ur_driver_->startForceMode(task_frame, selection_vector, wrench, type, limits);
   return res.success;
 }
 
+bool HardwareInterface::endForceMode(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
+{
+  res.success = this->ur_driver_->endForceMode();
+  return res.success;
+}
+
 void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
 {
   std::string str = msg->data;