safer retry system, fix joint value bug by avoiding using that for trajectory,...

safer retry system, fix joint value bug by avoiding using that for trajectory, use follow../result rostopic (#5)

src/Controller.py

@@ -21,7 +21,7 @@ from trajectory_msgs.msg import (
     JointTrajectoryPoint,
     JointTrajectory
 )
-from actionlib_msgs.msg import GoalStatusArray
+from control_msgs.msg import FollowJointTrajectoryActionResult
 
 # MOVEit
 import moveit_commander
@@ -81,8 +81,6 @@ class Controller(object):
             queue_size=1)
         self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
             queue_size=1)
-        self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/status',GoalStatusArray,
-            self._update_motion_status)
 
         time.sleep(0.1)
         if not disable_extruder:
@@ -100,7 +98,8 @@ class Controller(object):
             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,
+                self._update_motion_status)
         else:
             self.robot = moveit_commander.RobotCommander(
                     "/r" + str(robot) + "/robot_description",
@@ -112,6 +111,8 @@ 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,
+                self._update_motion_status)
 
         self.group.set_max_velocity_scaling_factor(timeScale)
         self.group.set_max_acceleration_scaling_factor(timeScale**2)
@@ -121,7 +122,7 @@ class Controller(object):
         self.retractionDist_mm = retractionDist_mm
         self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0
         self.extruderReady = False
-        self.traj_complete = False
+        self.traj_complete = True
 
         # Start extruder
         if not disable_extruder:
@@ -158,14 +159,9 @@ class Controller(object):
         plan = RobotTrajectory()
         plan.joint_trajectory = JTraj2ROS(jtraj)
 
-        # NOTE: This can only be done safely becuase assuming that the current
-        # joint position is already very close to where we want it to be
-        cj = self.group.get_current_joint_values()
-        sj = plan.joint_trajectory.points[0].positions
-        if any([0.01 < abs(cj[i]-sj[i]) for i in range(6)]):
-            print("Potentially would've failed here, Retrying")
-            time.sleep(0.1)
-            #self.group.go(plan.joint_trajectory.points[0].positions, wait=True)
+        # ensure previous trajectory is complete
+        if not self.traj_complete:
+            print("Last Traj incomplete, please retry")
             return False
 
         if contour is not None:
@@ -344,15 +340,10 @@ class Controller(object):
     def _extruderReady_cb(self,ready_boolMsg):
         self.extruderReady = ready_boolMsg.data
 
+
     def _update_motion_status(self, msg):
-        if len(msg.status_list)>0:
-            self.traj_complete = (msg.status_list[-1].status==3 or
-                msg.status_list[-1].status==2)
-            if msg.status_list[-1].status >= 4 and msg.status_list[-1].status!=6:
-                print("Unexpected Trajectory Status")
-                print("STATUS: {}".format(msg.status_list[-1].status))
-                print("For info on status meaning, go to:\n" + \
-                    "http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html")
+        #print(msg)
+        self.traj_complete = True
 
 
 def getR_ET():

src/main2.py

@@ -23,7 +23,9 @@ def execute_plan(plan,
         r2_con,
         contours=None,
         contour_speed=0.2,
-        move_speed=1):
+        move_speed=1,
+        retry_time=0.1,
+        max_traj_retries=20):
     """
     Executes a plan on MoveIt
 
@@ -72,18 +74,25 @@ def execute_plan(plan,
         print("Printing {}th trajectory of arm {}, which is contour {}".format(
             i_r[arm], arm, t.contour
             ))
+        retries = 0
         if t.contour is not None and contours is not None:
             traj2send = speed_multiplier(t, 1/contour_speed)
             while not controllers[arm].exec_ctraj(
                     traj2send,
                     contour=contours[t.contour],
-                    wait=False):
-                # while loop retries if too early to execute the first time
-                pass
+                    wait=False) and retries < max_traj_retries:
+                retries += 1
+                time.sleep(retry_time)
         else:
             traj2send = speed_multiplier(t, 1/move_speed)
-            while not controllers[arm].exec_ctraj(traj2send, wait=False):
-                pass
+            while not controllers[arm].exec_ctraj(traj2send, wait=False) \
+                    and retries < max_traj_retries:
+                retries += 1
+                time.sleep(retry_time)
+
+        if retries == max_traj_retries:
+            print("---\nFailed after {} retries, quitting".format(max_traj_retries))
+            sys.exit(0)
 
         # Traj started now, measure delay
         delays[arm] = (time.time() - s ) - next_time
@@ -98,7 +107,7 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    plan2arms = load_plan('two_arm_test')
+    plan2arms = load_plan('two_arm_test2')
 
     r1_con = Controller(disable_extruder=True, robot=1)
     r2_con = Controller(disable_extruder=True, robot=2)