Added a service to setup the active payload

Author: fmauch

ur_robot_driver/doc/ROS_INTERFACE.md

@@ -669,6 +669,10 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
 
     Service to set any of the robot's IOs
 
+ * "**set_payload**" (ur_msgs/SetPayload)
+
+    Setup the mounted payload through a ROS service
+
  * "**set_speed_slider**" (ur_msgs/SetSpeedSliderFraction)
 
     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.

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -196,6 +196,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   std::unique_ptr<DashboardClientROS> dashboard_client_;
 
   ros::ServiceServer deactivate_srv_;
+  ros::ServiceServer set_payload_srv_;
 
   hardware_interface::JointStateInterface js_interface_;
   ur_controllers::ScaledPositionJointInterface spj_interface_;

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -29,6 +29,8 @@
 #include "ur_robot_driver/ur/tool_communication.h"
 #include <ur_robot_driver/exceptions.h>
 
+#include <ur_msgs/SetPayload.h>
+
 #include <Eigen/Geometry>
 
 namespace ur_driver
@@ -318,6 +320,18 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   ur_driver_->startRTDECommunication();
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface");
 
+  // Setup the mounted payload through a ROS service
+  set_payload_srv_ = robot_hw_nh.advertiseService<ur_msgs::SetPayload::Request, ur_msgs::SetPayload::Response>(
+      "set_payload", [&](ur_msgs::SetPayload::Request& req, ur_msgs::SetPayload::Response& resp) {
+        std::stringstream cmd;
+        cmd << "sec setup():" << std::endl
+            << " set_payload(" << req.payload << ", [" << req.center_of_gravity.x << ", " << req.center_of_gravity.y
+            << ", " << req.center_of_gravity.z << "])" << std::endl
+            << "end";
+        resp.success = this->ur_driver_->sendScript(cmd.str());
+        return true;
+      });
+
   return true;
 }