parameterizable reverse_port for multiarmsupport

ur_driver/prog

 def driverProg():
   HOSTNAME = "%(driver_hostname)s"
+  PORT = driver_reverseport
   MSG_OUT = 1
   MSG_QUIT = 2
   MSG_JOINT_STATES = 3
@@ -113,7 +114,7 @@ def driverProg():
     
   end
 
-  socket_open(HOSTNAME, 50001)
+  socket_open(HOSTNAME, PORT)
   send_out("hello")
 
   thread_state = run statePublisherThread()

ur_driver/src/ur_driver/driver.py

@@ -664,11 +664,18 @@ def main():
     joint_names = [prefix + name for name in JOINT_NAMES]
 
     # Parses command line arguments
-    parser = optparse.OptionParser(usage="usage: %prog robot_hostname")
+    parser = optparse.OptionParser(usage="usage: %prog robot_hostname [reverse_port]")
     (options, args) = parser.parse_args(rospy.myargv()[1:])
-    if len(args) != 1:
+    if len(args) < 1:
         parser.error("You must specify the robot hostname")
-    robot_hostname = args[0]
+    elif len(args == 1:
+        robot_hostname = args[0]
+        reverse_port = REVERSE_PORT
+    elif len(args) == 2:
+        robot_hostname = args[0]
+        reverse_port = args[1]
+    else:
+        parser.error("Wrong number of parameters")
 
     # Reads the calibrated joint offsets from the URDF
     global joint_offsets
@@ -680,13 +687,14 @@ def main():
     max_velocity = rospy.get_param("~max_velocity", 2.0)
 
     # Sets up the server for the robot to connect to
-    server = TCPServer(("", 50001), CommanderTCPHandler)
+    server = TCPServer(("", REVERSE_PORT), CommanderTCPHandler)
     thread_commander = threading.Thread(name="CommanderHandler", target=server.serve_forever)
     thread_commander.daemon = True
     thread_commander.start()
 
     with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin:
-        program = fin.read() % {"driver_hostname": get_my_ip(robot_hostname, PORT)}
+        program = fin.read() % {"driver_hostname": get_my_ip(robot_hostname, reverse_port)}
+        program = fin.read() % {"driver_reverseport": reverse_port}
     connection = URConnection(robot_hostname, PORT, program)
     connection.connect()
     connection.send_reset_program()