execute plan for single arm in moveIt, support simulation in controller (implement #11)

src/Controller.py

@@ -59,7 +59,8 @@ class Controller(object):
         extruderTemp_C=190.0,
         hotend_srv='hotend',
         retractionDist_mm=12.0,
-        retractionSpeed_mmpmin=4800.0):
+        retractionSpeed_mmpmin=4800.0,
+        simulation=False):
         """
         Creates a controller to multiple robot arms. (TODO)
 
@@ -77,7 +78,8 @@ class Controller(object):
             queue_size=1)
 
         time.sleep(0.1)
-        rospy.wait_for_service(hotend_srv)
+        if not simulation:
+            rospy.wait_for_service(hotend_srv)
         try:
             self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
         except rospy.ServiceException as e:
@@ -100,9 +102,10 @@ class Controller(object):
         self.extruderReady = False
 
         # Start extruder
-        self.set_extruder_temp(extruderTemp_C)
-        if blocking:
-            self.wait_extruder_ready()
+        if not simulation:
+            self.set_extruder_temp(extruderTemp_C)
+            if blocking:
+                self.wait_extruder_ready()
 
 
     ## Arm methods

src/main.py

@@ -29,6 +29,10 @@ def execute_plan(plan, controller):
 
     Single arm only
     """
+    # plan to the start position of the plan
+    controller.group.go(plan.trajs[0][0].positions[0])
+
+    # execute trajectories in plan
     for t in plan.trajs[0]:
         jt = JTraj2ROS(t)
         try:
@@ -40,6 +44,6 @@ def execute_plan(plan, controller):
 
 
 if __name__ == '__main__':
-    plan = load_plan('plans/1.plan')
-    con = Controller()
+    plan = load_plan('plans/3.plan')
+    con = Controller(simulation=True)
     execute_plan(plan, con)

src/utils/planning_tools.py

@@ -51,11 +51,19 @@ def joints_at_time(traj, t):
     return (t-t1)/(t2-t1)*(j2-j1) + j1
 
 
-def JTraj2ROS(jtraj):
+default_joint_names = ['shoulder_pan_joint',
+ 'shoulder_lift_joint',
+ 'elbow_joint',
+ 'wrist_1_joint',
+ 'wrist_2_joint',
+ 'wrist_3_joint']
+
+def JTraj2ROS(jtraj,joint_names=default_joint_names):
     """
     convert JTrajectory object to ROS JointTrajectory
     """
     j = JointTrajectory()
+    j.joint_names = joint_names
 
     for i in range(len(jtraj.positions)):
         jp = JointTrajectoryPoint()