Commit e56bc51e authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

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)
parent a6eeb104
...@@ -21,7 +21,7 @@ from trajectory_msgs.msg import ( ...@@ -21,7 +21,7 @@ from trajectory_msgs.msg import (
JointTrajectoryPoint, JointTrajectoryPoint,
JointTrajectory JointTrajectory
) )
from actionlib_msgs.msg import GoalStatusArray from control_msgs.msg import FollowJointTrajectoryActionResult
# MOVEit # MOVEit
import moveit_commander import moveit_commander
...@@ -81,8 +81,6 @@ class Controller(object): ...@@ -81,8 +81,6 @@ class Controller(object):
queue_size=1) queue_size=1)
self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory, self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
queue_size=1) 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) time.sleep(0.1)
if not disable_extruder: if not disable_extruder:
...@@ -100,7 +98,8 @@ class Controller(object): ...@@ -100,7 +98,8 @@ class Controller(object):
self.robot = moveit_commander.RobotCommander() self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface() self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('manipulator') 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: else:
self.robot = moveit_commander.RobotCommander( self.robot = moveit_commander.RobotCommander(
"/r" + str(robot) + "/robot_description", "/r" + str(robot) + "/robot_description",
...@@ -112,6 +111,8 @@ class Controller(object): ...@@ -112,6 +111,8 @@ class Controller(object):
robot_description="r" + str(robot) + "/robot_description", robot_description="r" + str(robot) + "/robot_description",
ns="r" + str(robot) 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_velocity_scaling_factor(timeScale)
self.group.set_max_acceleration_scaling_factor(timeScale**2) self.group.set_max_acceleration_scaling_factor(timeScale**2)
...@@ -121,7 +122,7 @@ class Controller(object): ...@@ -121,7 +122,7 @@ class Controller(object):
self.retractionDist_mm = retractionDist_mm self.retractionDist_mm = retractionDist_mm
self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0 self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0
self.extruderReady = False self.extruderReady = False
self.traj_complete = False self.traj_complete = True
# Start extruder # Start extruder
if not disable_extruder: if not disable_extruder:
...@@ -158,14 +159,9 @@ class Controller(object): ...@@ -158,14 +159,9 @@ class Controller(object):
plan = RobotTrajectory() plan = RobotTrajectory()
plan.joint_trajectory = JTraj2ROS(jtraj) plan.joint_trajectory = JTraj2ROS(jtraj)
# NOTE: This can only be done safely becuase assuming that the current # ensure previous trajectory is complete
# joint position is already very close to where we want it to be if not self.traj_complete:
cj = self.group.get_current_joint_values() print("Last Traj incomplete, please retry")
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)
return False return False
if contour is not None: if contour is not None:
...@@ -344,15 +340,10 @@ class Controller(object): ...@@ -344,15 +340,10 @@ class Controller(object):
def _extruderReady_cb(self,ready_boolMsg): def _extruderReady_cb(self,ready_boolMsg):
self.extruderReady = ready_boolMsg.data self.extruderReady = ready_boolMsg.data
def _update_motion_status(self, msg): def _update_motion_status(self, msg):
if len(msg.status_list)>0: #print(msg)
self.traj_complete = (msg.status_list[-1].status==3 or self.traj_complete = True
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")
def getR_ET(): def getR_ET():
......
...@@ -23,7 +23,9 @@ def execute_plan(plan, ...@@ -23,7 +23,9 @@ def execute_plan(plan,
r2_con, r2_con,
contours=None, contours=None,
contour_speed=0.2, contour_speed=0.2,
move_speed=1): move_speed=1,
retry_time=0.1,
max_traj_retries=20):
""" """
Executes a plan on MoveIt Executes a plan on MoveIt
...@@ -72,18 +74,25 @@ def execute_plan(plan, ...@@ -72,18 +74,25 @@ def execute_plan(plan,
print("Printing {}th trajectory of arm {}, which is contour {}".format( print("Printing {}th trajectory of arm {}, which is contour {}".format(
i_r[arm], arm, t.contour i_r[arm], arm, t.contour
)) ))
retries = 0
if t.contour is not None and contours is not None: if t.contour is not None and contours is not None:
traj2send = speed_multiplier(t, 1/contour_speed) traj2send = speed_multiplier(t, 1/contour_speed)
while not controllers[arm].exec_ctraj( while not controllers[arm].exec_ctraj(
traj2send, traj2send,
contour=contours[t.contour], contour=contours[t.contour],
wait=False): wait=False) and retries < max_traj_retries:
# while loop retries if too early to execute the first time retries += 1
pass time.sleep(retry_time)
else: else:
traj2send = speed_multiplier(t, 1/move_speed) traj2send = speed_multiplier(t, 1/move_speed)
while not controllers[arm].exec_ctraj(traj2send, wait=False): while not controllers[arm].exec_ctraj(traj2send, wait=False) \
pass 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 # Traj started now, measure delay
delays[arm] = (time.time() - s ) - next_time delays[arm] = (time.time() - s ) - next_time
...@@ -98,7 +107,7 @@ def execute_plan(plan, ...@@ -98,7 +107,7 @@ def execute_plan(plan,
if __name__ == '__main__': if __name__ == '__main__':
plan2arms = load_plan('two_arm_test') plan2arms = load_plan('two_arm_test2')
r1_con = Controller(disable_extruder=True, robot=1) r1_con = Controller(disable_extruder=True, robot=1)
r2_con = Controller(disable_extruder=True, robot=2) r2_con = Controller(disable_extruder=True, robot=2)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment