Merge pull request #102 from ipa-fxm/issue/heavy_payload_rebased

[Hydro] Issue/heavy payload rebased

universal_robot/package.xml

@@ -24,6 +24,7 @@
   <run_depend>ur_driver</run_depend>
   <run_depend>ur_gazebo</run_depend>
   <run_depend>ur_kinematics</run_depend>
+  <run_depend>ur_msgs</run_depend>
 
   <export>
     <metapackage/>

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/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
@@ -217,6 +219,16 @@ def driverProg():
         #servoj(q, 3, 0.1, t)
         #send_waypoint_finished(waypoint_id)
         set_servo_setpoint(waypoint_id, q, t)
+      elif mtype == MSG_SET_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")
+        set_payload(payload)
       elif mtype == MSG_STOPJ:
         send_out("Received stopj")
         stopj(1.0)
@@ -228,7 +240,7 @@ def driverProg():
           send_out("Received no parameters for set_out message")
         end
         if params_mult[2] > 0:
-	  set_digital_out(params_mult[1], True)
+      set_digital_out(params_mult[1], True)
         elif params_mult[2] == 0:
           set_digital_out(params_mult[1], False)
         end
@@ -240,7 +252,7 @@ def driverProg():
           send_out("Received no parameters for set_out message")
         end
         if params_mult[2] != 0:
-	  set_flag(params_mult[1], True)
+      set_flag(params_mult[1], True)
         elif params_mult[2] == 0:
           set_flag(params_mult[1], False)
         end

ur_driver/src/ur_driver/driver.py

@@ -19,7 +19,7 @@ from geometry_msgs.msg import WrenchStamped
 
 from ur_driver.deserialize import RobotState, RobotMode
 
-from ur_msgs.srv import SetIO
+from ur_msgs.srv import SetPayload, SetIO
 from ur_msgs.msg import *
 
 # renaming classes
@@ -46,17 +46,25 @@ MSG_MOVEJ = 4
 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
 MSG_SET_FLAG = 12
 MSG_SET_TOOL_VOLTAGE = 13
 MSG_SET_ANALOG_OUT = 14
+MULT_payload = 1000.0
 MULT_wrench = 10000.0
 MULT_jointstate = 10000.0
 MULT_time = 1000000.0
 MULT_blend = 1000.0
 
+#Bounds for SetPayload service
+MIN_PAYLOAD = 0.0
+MAX_PAYLOAD = 1.0
+#Using a very conservative value as it should be set throught the parameter server
+
+
 FUN_SET_DIGITAL_OUT = 1
 FUN_SET_FLAG = 2
 FUN_SET_ANALOG_OUT = 3
@@ -432,7 +440,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
     def send_quit(self):
         with self.socket_lock:
             self.request.send(struct.pack("!i", MSG_QUIT))
-            
+
     def send_servoj(self, waypoint_id, q_actual, t):
         assert(len(q_actual) == 6)
         q_robot = [0.0] * 6
@@ -444,6 +452,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):
@@ -594,6 +608,25 @@ def within_tolerance(a_vec, b_vec, tol_vec):
             return False
     return True
 
+class URServiceProvider(object):
+    def __init__(self, robot):
+        self.robot = robot
+        rospy.Service('ur_driver/setPayload', SetPayload, self.setPayload)
+
+    def set_robot(self, robot):
+        self.robot = robot
+
+    def setPayload(self, req):
+        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:
+            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):
@@ -860,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)
@@ -874,7 +916,8 @@ def main():
     connection.send_reset_program()
     
     set_io_server()
-
+    
+    service_provider = None
     action_server = None
     try:
         while not rospy.is_shutdown():
@@ -904,6 +947,12 @@ def main():
                         break
                 rospy.loginfo("Robot connected")
 
+                #provider for service calls
+                if service_provider:
+                    service_provider.set_robot(r)
+                else:
+                    service_provider = URServiceProvider(r)
+                
                 if action_server:
                     action_server.set_robot(r)
                 else:

ur_msgs/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
 ## Generate services in the 'srv' folder
  add_service_files(
    FILES
+   SetPayload.srv
    SetIO.srv
  )
 

ur_msgs/srv/SetPayload.srv

+float32 payload
+-----------------------
+bool success