make payload bounds parameterizable through parameter server and launch files

ur_bringup/launch/ur10_bringup.launch

@@ -12,6 +12,8 @@
   <arg name="robot_ip"/>
   <arg name="reverse_port" default="50001"/>
   <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="20.0"/>
 
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
@@ -22,6 +24,8 @@
   <include file="$(find ur_bringup)/launch/ur_common.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="reverse_port" value="$(arg reverse_port)"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
   </include>
 
 </launch>

ur_bringup/launch/ur10_bringup_joint_limited.launch

@@ -11,10 +11,14 @@
   <!-- robot_ip: IP-address of the robot's socket-messaging server -->
   <arg name="robot_ip"/>
   <arg name="reverse_port" default="50001"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="20.0"/>
 
   <include file="$(find ur_bringup)/launch/ur10_bringup.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="reverse_port" value="$(arg reverse_port)"/>
     <arg name="limited"  value="true"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
   </include>
 </launch>

ur_bringup/launch/ur5_bringup.launch

@@ -12,6 +12,8 @@
   <arg name="robot_ip"/>
   <arg name="reverse_port" default="50001"/>
   <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="10.0"/>
 
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur5_upload.launch">
@@ -22,6 +24,8 @@
   <include file="$(find ur_bringup)/launch/ur_common.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="reverse_port" value="$(arg reverse_port)"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
   </include>
 
 </launch>

ur_bringup/launch/ur5_bringup_joint_limited.launch

@@ -11,10 +11,14 @@
   <!-- robot_ip: IP-address of the robot's socket-messaging server -->
   <arg name="robot_ip"/>
   <arg name="reverse_port" default="50001"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="10.0"/>
 
   <include file="$(find ur_bringup)/launch/ur5_bringup.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="reverse_port" value="$(arg reverse_port)"/>
     <arg name="limited"  value="true"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
   </include>
 </launch>

ur_bringup/launch/ur_common.launch

@@ -13,6 +13,8 @@
   <!-- robot_ip: IP-address of the robot's socket-messaging server -->
   <arg name="robot_ip"/>
   <arg name="reverse_port" default="50001"/>
+  <arg name="min_payload"/>
+  <arg name="max_payload"/>
   
   <!-- copy the specified IP address to be consistant with ROS-Industrial spec.
   NOTE: The ip address is actually passed to the driver on the command line -->
@@ -22,7 +24,10 @@
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
 
   <!-- driver -->
-  <node name="ur_driver" pkg="ur_driver" type="driver.py" args="$(arg robot_ip) $(arg reverse_port)" output="screen" />
+  <node name="ur_driver" pkg="ur_driver" type="driver.py" args="$(arg robot_ip) $(arg reverse_port)" output="screen">
+    <param name="min_payload" type="double" value="$(arg min_payload)"/>
+    <param name="max_payload" type="double" value="$(arg max_payload)"/>
+  </node>
    
   <!-- TF Buffer Server -->
   <node pkg="tf2_ros" type="buffer_server" name="tf2_buffer_server">

ur_driver/src/ur_driver/driver.py

@@ -61,8 +61,8 @@ MULT_blend = 1000.0
 
 #Bounds for SetPayload service
 MIN_PAYLOAD = 0.0
-MAX_PAYLOAD = 5.0    #UR5
-#MAX_PAYLOAD = 10.0    #UR10
+MAX_PAYLOAD = 1.0
+#Using a very conservative value as it should be set throught the parameter server
 
 
 FUN_SET_DIGITAL_OUT = 1
@@ -617,8 +617,8 @@ class URServiceProvider(object):
         self.robot = robot
 
     def setPayload(self, req):
-        if req.payload < MIN_PAYLOAD or req.payload > MAX_PAYLOAD:
-            print 'ERROR: Payload out of bounds'
+        if req.payload < min_payload or req.payload > max_payload:
+            print 'ERROR: Payload ' + str(req.payload) + ' out of bounds (' + str(min_payload) + ', ' + str(max_payload) + ')'
             return False
         
         if self.robot:
@@ -893,6 +893,15 @@ def main():
     # Reads the maximum velocity
     global max_velocity
     max_velocity = rospy.get_param("~max_velocity", 2.0)
+    
+    # Reads the minimum payload
+    global min_payload
+    min_payload = rospy.get_param("~min_payload", MIN_PAYLOAD)
+    # Reads the maximum payload
+    global max_payload
+    max_payload = rospy.get_param("~max_payload", MAX_PAYLOAD)
+    rospy.loginfo("Bounds for Payload: [%s, %s]" % (min_payload, max_payload))
+    
 
     # Sets up the server for the robot to connect to
     server = TCPServer(("", reverse_port), CommanderTCPHandler)