created ros service for sending payload to arm

ur_driver/CMakeLists.txt

@@ -4,10 +4,32 @@ project(ur_driver)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED message_generation)
 
 catkin_python_setup()
 
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  URSetPayload.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   #control_msgs#   sensor_msgs
+ )
 
 ###################################
 ## catkin specific configuration ##

ur_driver/prog

@@ -8,6 +8,7 @@ def driverProg():
   MSG_WAYPOINT_FINISHED = 5
   MSG_STOPJ = 6
   MSG_SERVOJ = 7
+  MSG_SET_PAYLOAD = 8
   MSG_WRENCH = 9
   MSG_SET_DIGITAL_OUT = 10
   MSG_GET_IO = 11
@@ -15,6 +16,7 @@ def driverProg():
   MSG_SET_TOOL_VOLTAGE = 13
   MSG_SET_ANALOG_OUT = 14
   MULT_wrench = 10000.0
+  MULT_PAYLOAD = 1000.0
   MULT_jointstate = 10000.0
   MULT_time = 1000000.0
   MULT_blend = 1000.0
@@ -218,7 +220,14 @@ def driverProg():
         #send_waypoint_finished(waypoint_id)
         set_servo_setpoint(waypoint_id, q, t)
       elif mtype == MSG_SET_PAYLOAD:
-	send_out("Received new payload")
+	    params = socket_read_binary_integer(1)
+        if params[0] == 0:
+          send_out("Received no parameters for setPayload message")
+        end
+        
+        payload = params[1] / MULT_PAYLOAD
+        #send_out(payload)
+        send_out("Received new payload")
       elif mtype == MSG_STOPJ:
         send_out("Received stopj")
         stopj(1.0)

ur_driver/src/ur_driver/driver.py

@@ -21,6 +21,7 @@ from ur_driver.deserialize import RobotState, RobotMode
 
 from ur_msgs.srv import SetIO
 from ur_msgs.msg import *
+from ur_driver.srv import *
 
 # renaming classes
 DigitalIn = Digital
@@ -48,6 +49,7 @@ MSG_STOPJ = 6
 MSG_SERVOJ = 7
 MSG_SET_PAYLOAD = 8
 MSG_WRENCH = 9
+MULT_PAYLOAD = 1000.0
 MSG_SET_DIGITAL_OUT = 10
 MSG_GET_IO = 11
 MSG_SET_FLAG = 12
@@ -445,6 +447,12 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
         buf = struct.pack("!%ii" % len(params), *params)
         with self.socket_lock:
             self.request.send(buf)
+        
+	#Experimental set_payload implementation
+    def send_payload(self,payload):
+        buf = struct.pack('!ii', MSG_SET_PAYLOAD, payload * MULT_PAYLOAD)
+        with self.socket_lock:
+            self.request.send(buf)
 
     #Experimental set_digital_output implementation
     def set_digital_out(self, pinnum, value):
@@ -595,6 +603,26 @@ def within_tolerance(a_vec, b_vec, tol_vec):
             return False
     return True
 
+#HERE
+class URServiceProvider(object):
+    def __init__(self, robot):
+        self.robot = robot
+        rospy.Service('ur_driver/setPayload', URSetPayload, self.setPayload)
+
+    def set_robot(self, robot):
+        self.robot = robot
+
+    def setPayload(self, req):
+        if req.payload < 0 or req.payload > 20.00:
+            print 'ERROR: Payload out of bounce'
+            return False
+        
+        if self.robot:
+            self.robot.send_payload(req.payload)
+        else:
+            return False
+        return True
+
 class URTrajectoryFollower(object):
     RATE = 0.02
     def __init__(self, robot, goal_time_tolerance=None):
@@ -874,6 +902,7 @@ def main():
     connection.connect()
     connection.send_reset_program()
     
+    provider = None
     set_io_server()
 
     action_server = None
@@ -905,6 +934,12 @@ def main():
                         break
                 rospy.loginfo("Robot connected")
 
+                #provider for service calls
+                if provider:
+                    provider.set_robot(r)
+                else:
+                    provider = URServiceProvider(r)
+                
                 if action_server:
                     action_server.set_robot(r)
                 else: