Added a service to setup the active payload (#50)

* Added a service to setup the active payload

Author: Felix Exner

ur_robot_driver/doc/ROS_INTERFACE.md

@@ -751,6 +751,10 @@ Service to set any of the robot's IOs
 ##### 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.
+ 
+#### set_payload (ur_msgs/SetPayload)
+
+Setup the mounted payload through a ROS service
 
 ##### zero_ftsensor ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html))
 

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -214,6 +214,7 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   ros::ServiceServer deactivate_srv_;
   ros::ServiceServer tare_sensor_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>
 
 using industrial_robot_status_interface::RobotMode;
@@ -383,6 +385,19 @@ 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.imbue(std::locale::classic());  // Make sure, decimal divider is actually '.'
+        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;
 }