add extrusion capability for both robots simultaneously (#5)

launch/2xbringup.launch

@@ -4,27 +4,52 @@
   <arg name="r2_kinematics_config" default="$(find twins_utils)/calibrations/r2_cal.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
    <arg name="r1_robot_ip" default="192.168.1.101"/>
    <arg name="r2_robot_ip" default="192.168.1.103"/>
+   <arg name="extrude" default="True"/>
 
  <!-- ROBOT 1 -->
  <group ns="r1">
+
+   <!-- ROBOT DRIVER -->
    <include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch">
      <arg name="robot_ip" value="$(arg r1_robot_ip)"/>
      <arg name="kinematics_config" value="$(arg r1_kinematics_config)"/>
      <arg name="reverse_port" value="50003"/>
      <arg name="script_sender_port" value="50004"/>
    </include>
+
+   <!-- MOVEIT WRAPPER -->
    <include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
+
+   <!-- EXTRUDER NODE-->
+   <node if="$(arg extrude)"
+       pkg="extrudex"
+       name="r1_extruder"
+       type="extruder_node.py"
+       args="/dev/ttyACM0"
+       output="screen" />
  </group>
 
  <!-- ROBOT 2 -->
  <group ns="r2">
+
+   <!-- ROBOT DRIVER -->
    <include file="$(find ur_robot_driver)/launch/ur5e_bringup.launch">
      <arg name="robot_ip" value="$(arg r2_robot_ip)"/>
      <arg name="kinematics_config" value="$(arg r2_kinematics_config)"/>
      <arg name="reverse_port" value="50001"/>
      <arg name="script_sender_port" value="50002"/>
    </include>
+
+   <!-- MOVEIT WRAPPER -->
    <include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
+
+   <!-- EXTRUDER NODE-->
+   <node if="$(arg extrude)"
+       pkg="extrudex"
+       name="r2_extruder"
+       type="extruder_node.py"
+       args="/dev/ttyACM1"
+       output="screen" />
  </group>
 
 </launch>

src/Controller.py

@@ -72,35 +72,40 @@ class Controller(object):
 
         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',
             anonymous = False)
-        self.extrusion_pub = rospy.Publisher('extrusion_cmd',ExtruderControl,
-            queue_size=2)
-        self.extruderReady_sub = rospy.Subscriber('ready',Bool,self._extruderReady_cb,
-            queue_size=1)
         self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
             queue_size=1)
 
-        time.sleep(0.1)
-        if not disable_extruder:
-            print("Waiting for Hotend service")
-            rospy.wait_for_service(hotend_srv)
-            try:
-                self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
-            except rospy.ServiceException as e:
-                print('Service call failed: {}'.format(e))
-
         # initialise MoveIt commander
         moveit_commander.roscpp_initialize(sys.argv)
 
         if robot == 0:
+            # MoveIt related
             self.robot = moveit_commander.RobotCommander()
             self.scene = moveit_commander.PlanningSceneInterface()
             self.group = moveit_commander.MoveGroupCommander('manipulator')
-            self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/result', FollowJointTrajectoryActionResult,
+
+            # Traj completion feedback
+            self.motion_feedback_sub = rospy.Subscriber(
+                    'scaled_pos_traj_controller/follow_joint_trajectory/result',
+                    FollowJointTrajectoryActionResult,
                     self._update_motion_status)
+
+            # extruder related
+            self.extrusion_pub = rospy.Publisher(
+                    'extrusion_cmd',
+                    ExtruderControl,
+                    queue_size=2)
+            self.extruderReady_sub = rospy.Subscriber(
+                    'ready',
+                    Bool,
+                    self._extruderReady_cb,
+                    queue_size=1)
         else:
+            # MoveIt related
             self.robot = moveit_commander.RobotCommander(
                     "/r" + str(robot) + "/robot_description",
                     ns="r" + str(robot)
@@ -111,9 +116,36 @@ class Controller(object):
                     robot_description="r" + str(robot) + "/robot_description",
                     ns="r" + str(robot)
                     )
-            self.motion_feedback_sub = rospy.Subscriber("r" + str(robot) + '/scaled_pos_traj_controller/follow_joint_trajectory/result', FollowJointTrajectoryActionResult,
+
+            # Traj completion feedback
+            self.motion_feedback_sub = rospy.Subscriber(
+                    "r" + str(robot) + '/scaled_pos_traj_controller/follow_joint_trajectory/result',
+                    FollowJointTrajectoryActionResult,
                     self._update_motion_status)
 
+            # Extruder related
+            self.extrusion_pub = rospy.Publisher(
+                    "r" + str(robot) + '/extrusion_cmd',
+                    ExtruderControl,
+                    queue_size=2)
+            self.extruderReady_sub = rospy.Subscriber(
+                    "r" + str(robot) + '/ready',
+                    Bool,
+                    self._extruderReady_cb,
+                    queue_size=1)
+            hotend_srv = "/r" + str(robot) + '/hotend'
+
+        time.sleep(0.1)
+        if not disable_extruder:
+            print("Waiting for Hotend service")
+            print(hotend_srv)
+            rospy.wait_for_service(hotend_srv)
+            try:
+                self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
+            except rospy.ServiceException as e:
+                print('Service call failed: {}'.format(e))
+
+
         self.group.set_max_velocity_scaling_factor(timeScale)
         self.group.set_max_acceleration_scaling_factor(timeScale**2)
 

src/KeyboardController.py

@@ -595,13 +595,13 @@ class KeyboardController(Controller):
 
 if __name__=="__main__":
     #con = KeyboardController(disableExtruder=True)
-    con = KeyboardController()
-    #con1 = KeyboardController(disableExtruder=True, robot=1)
-    #con2 = KeyboardController(disableExtruder=True, robot=2)
+    #con = KeyboardController()
+    #con1 = KeyboardController(robot=1)
+    con2 = KeyboardController(robot=2)
 
     print('Collecting configrations...')
-    configuations = con.prompt_configurations()
+    #configuations = con.prompt_configurations()
     #configuations = con1.prompt_configurations()
-    #configuations = con2.prompt_configurations()
+    configuations = con2.prompt_configurations()
     print('Your configurations:')
     print(configuations)