using two separate moveit instances, control arm from python commander (#5)

launch/2xbringup.launch

@@ -13,7 +13,7 @@
      <arg name="reverse_port" value="50001"/>
      <arg name="script_sender_port" value="50002"/>
    </include>
-   <!--include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/-->
+   <include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
  </group>
 
  <!-- ROBOT 2 -->
@@ -24,7 +24,7 @@
      <arg name="reverse_port" value="50003"/>
      <arg name="script_sender_port" value="50004"/>
    </include>
-   <!--include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/-->
+   <include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
  </group>
 
 </launch>

launch/sim2xbringup.launch

+<?xml version="1.0"?>
+<launch>
+
+ <!-- ROBOT 1 -->
+ <group ns="r1">
+     <include file="$(find ur5_e_moveit_config)/launch/demo.launch"/>
+ </group>
+
+ <!-- ROBOT 2 -->
+ <group ns="r2">
+     <include file="$(find ur5_e_moveit_config)/launch/demo.launch"/>
+ </group>
+
+</launch>

src/Controller.py

@@ -62,12 +62,15 @@ class Controller(object):
         hotend_srv='hotend',
         retractionDist_mm=12.0,
         retractionSpeed_mmpmin=4800.0,
-        disable_extruder=False):
+        disable_extruder=False,
+        robot=0):
         """
         Creates a controller to multiple robot arms. (TODO)
 
         close_deg: Euclidean distance of configurations within which two configurations are considered "close"
         timeScale: Scaling factor of time
+
+        robot can be 0 (if only one robot is turned on (not using 2xbringup), 1 for r1, 2 for r2
         """
         ## Initialise ROS things
         self.node = rospy.init_node('py_controller',
@@ -93,9 +96,22 @@ class Controller(object):
         # initialise MoveIt commander
         moveit_commander.roscpp_initialize(sys.argv)
 
-        self.robot = moveit_commander.RobotCommander()
-        self.scene = moveit_commander.PlanningSceneInterface()
-        self.group = moveit_commander.MoveGroupCommander('manipulator')
+        if robot == 0:
+            self.robot = moveit_commander.RobotCommander()
+            self.scene = moveit_commander.PlanningSceneInterface()
+            self.group = moveit_commander.MoveGroupCommander('manipulator')
+
+        else:
+            self.robot = moveit_commander.RobotCommander(
+                    "/r" + str(robot) + "/robot_description",
+                    ns="r" + str(robot)
+                    )
+            self.scene = moveit_commander.PlanningSceneInterface(ns="r"+ str(robot))
+            self.group = moveit_commander.MoveGroupCommander(
+                    'manipulator',
+                    robot_description="r" + str(robot) + "/robot_description",
+                    ns="r" + str(robot)
+                    )
 
         self.group.set_max_velocity_scaling_factor(timeScale)
         self.group.set_max_acceleration_scaling_factor(timeScale**2)

src/KeyboardController.py

@@ -37,7 +37,8 @@ class KeyboardController(Controller):
         homeConf_deg=np.array([-90,-90,45,-45,-90,-45]),
         waitForExtruder=True,
         timeScale=1.0,
-        disableExtruder=False):
+        disableExtruder=False,
+        robot=0):
         """
         """
         if DEBUG:
@@ -46,7 +47,8 @@ class KeyboardController(Controller):
         super(KeyboardController,self).__init__(
             timeScale=timeScale,
             blocking=waitForExtruder,
-            disable_extruder=disableExtruder)
+            disable_extruder=disableExtruder,
+            robot=robot)
 
         # 1x6
         homeConf_rad = np.deg2rad(homeConf_deg)
@@ -593,6 +595,7 @@ class KeyboardController(Controller):
 
 if __name__=="__main__":
     con = KeyboardController(disableExtruder=True)
+    #con = KeyboardController(disableExtruder=True, robot=2)
     #con = KeyboardController()
 
     print('Collecting configrations...')