Commit e56bc51e authored by Jayant Khatkar's avatar Jayant Khatkar

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 (
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():
......
......@@ -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)
......
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