diff --git a/universal_robot/package.xml b/universal_robot/package.xml
index a61b77233a5df276baa725e4729af9358f7682e5..669fada9d42d9e0cb24ec0597c9b93e1ce8df590 100644
--- a/universal_robot/package.xml
+++ b/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/>
diff --git a/ur_bringup/launch/ur10_bringup.launch b/ur_bringup/launch/ur10_bringup.launch
index 445e0b3c89b57d8e47487a4cf6a98a9540a1d957..311bf5574ac1c7aed882b80515fdd579bb8d9ca8 100644
--- a/ur_bringup/launch/ur10_bringup.launch
+++ b/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>
diff --git a/ur_bringup/launch/ur10_bringup_joint_limited.launch b/ur_bringup/launch/ur10_bringup_joint_limited.launch
index 9964be4721aedf94f28b74397175a07a70748a32..b95ce8c10cb7f2f750019a793315039254047eae 100644
--- a/ur_bringup/launch/ur10_bringup_joint_limited.launch
+++ b/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>
diff --git a/ur_bringup/launch/ur5_bringup.launch b/ur_bringup/launch/ur5_bringup.launch
index 6e2bfd7c273768f8a79bccf51dd0d85bb70198aa..52f7ebd67b5cb0f357127b9cd42fc2dc370aafad 100644
--- a/ur_bringup/launch/ur5_bringup.launch
+++ b/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>
diff --git a/ur_bringup/launch/ur5_bringup_joint_limited.launch b/ur_bringup/launch/ur5_bringup_joint_limited.launch
index 4aa7a04f18170c11b7c4e367482cb8a238043df2..dfa7560e814601e4afd4f1e7ae134766e8460fe9 100644
--- a/ur_bringup/launch/ur5_bringup_joint_limited.launch
+++ b/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>
diff --git a/ur_bringup/launch/ur_common.launch b/ur_bringup/launch/ur_common.launch
index 73eff943c287cc956b03cf7941396005a67a09df..cd3b292caa6eb98df20ed788f474bc7fb31d47a1 100644
--- a/ur_bringup/launch/ur_common.launch
+++ b/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">
diff --git a/ur_driver/prog b/ur_driver/prog
index 6f118e1a5880171dd56f9cb3e14f73ec34aaaae0..18d6671705cf9d1a716434620e121a2b21b90a3f 100644
--- a/ur_driver/prog
+++ b/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
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index 85e6785377ad99ec168987d6aa4fea64f178e929..f213ddd4b029163dc2ab98c768766910883e5542 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/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:
diff --git a/ur_msgs/CMakeLists.txt b/ur_msgs/CMakeLists.txt
index f9bd5a2a98906411f9901c8660a0d159f095633d..1fc2dd9b027babcb8db1996ab0de0265aeb45ba5 100644
--- a/ur_msgs/CMakeLists.txt
+++ b/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
)
diff --git a/ur_msgs/srv/SetPayload.srv b/ur_msgs/srv/SetPayload.srv
new file mode 100644
index 0000000000000000000000000000000000000000..d68eb5c86d4c251e1a94ad8ae3ab329b19070cbe
--- /dev/null
+++ b/ur_msgs/srv/SetPayload.srv
@@ -0,0 +1,3 @@
+float32 payload
+-----------------------
+bool success