def action_server_cb(self, goal):
rospy.loginfo(IDSTR + "Action server cb called")
rospy.loginfo(goal)
names = [FAMILY_NAME+"/"+NAME_1,FAMILY_NAME+"/"+NAME_2,
FAMILY_NAME+"/"+NAME_3,FAMILY_NAME+"/"+NAME_4]
# get current state
cur_pose = self.hebi_fb.position
cur_elbow = kin.get_elbow(cur_pose)
# Determine requested elbow position
req_elbow = cur_elbow
if (goal.type == TYPE_TARGET or goal.type == TYPE_CAMERA):
req_elbow = goal.waypoint_1[0] >= 0
# Create trajectory generaor object
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
# Change elbow position if necessary
# Will end on rest position of opposite elbow
if( False and (goal.type == TYPE_TARGET or goal.type == TYPE_CAMERA)
and not cur_elbow == req_elbow):
rospy.loginfo(IDSTR + "Switching elbow config")
# get rest pos after transition (ie opposite of current elbow)
rest_pos = []
if (cur_elbow):
rest_pos = REST_POS_ELBOW_DOWN
else:
rest_pos = REST_POS_ELBOW_UP
# time to travel from cur pose to VERT_POS
travel_time_1 = dist(kin.fk(cur_pose), VERT_POS) / SPEED_TRAVEL
# time to travel from vert_pos to rest pos
travel_time_2 = dist(VERT_POS, rest_pos) / SPEED_TRAVEL
t.addWaypoint(VERT_POS, 10, cur_elbow)
# t.addWaypoint(VERT_POS, 10, not cur_elbow)
# t.addWaypoint(VERT_POS, travel_time_1, cur_elbow)
# t.addWaypoint(VERT_POS, VERT_ELBOW_TRANSITION_TIME, not cur_elbow)
# send trajectory to switch in vert pose
hebi_goal = t.createTrajectory()
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
while (not self.hebi_is_done):
pass
rospy.loginfo(IDSTR + "done switching elbow config part 1")
# send trajectory to go to rest pose
cur_pose = self.hebi_fb.position
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
t.addWaypoint(VERT_POS, 10, not cur_elbow)
t.addWaypoint(rest_pos, 10, not cur_elbow)
hebi_goal = t.createTrajectory()
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
while (not self.hebi_is_done):
pass
rospy.loginfo(IDSTR + "done switching elbow config part 2")
# redo intial setup
cur_pose = self.hebi_fb.position
cur_elbow = kin.get_elbow(cur_pose)
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
if(goal.type == TYPE_TARGET):
# create waypoint to go to before goal.waypoint_1
approach_waypoint = list(goal.waypoint_1);
rough_approach_time = 0
if(goal.approach_from_above):
approach_waypoint[1] += VERT_APPROACH_DIST
rough_approach_time = dist(kin.fk(cur_pose), approach_waypoint) / SPEED_TRAVEL
# pre_approach_waypoint = approach_waypoint + \
# (VERT_HORZ_APPROACH_DIST * np.sign(-goal.waypoint_1[0]))
# pre_approach_time = dist(kin.fk(cur_pose), pre_approach_waypoint) / SPEED_TRAVEL
# t.addWaypoint(pre_approach_waypoint, pre_approach_time, req_elbow)
# rough_approach_time = dist(pre_approach_waypoint, approach_waypoint) / SPEED_APPROACH
else:
approach_waypoint[0] += (HORZ_APPROACH_DIST * np.sign(-goal.waypoint_1[0]))
rough_approach_time = dist(kin.fk(cur_pose), approach_waypoint) / SPEED_TRAVEL
# time to get from cur_pose to approach_waypoint
# time to get from approach_waypoint to goal.waypoint_1
fine_approach_time = dist(approach_waypoint, goal.waypoint_1) / SPEED_APPROACH
# create waypoint to go to after goal.waypoint_2
depart_waypoint = list(goal.waypoint_2);
if(goal.approach_from_above):
depart_waypoint[1] += VERT_APPROACH_DIST
else:
depart_waypoint[0] += (HORZ_APPROACH_DIST * np.sign(-goal.waypoint_2[0]))
# time to get from goal.waypoint_2 to depart_waypoint
fine_depart_time = dist(depart_waypoint, goal.waypoint_2) / SPEED_DEPART
# Add waypoints
t.addWaypoint(approach_waypoint, rough_approach_time, req_elbow)
t.addWaypoint(goal.waypoint_1,fine_approach_time, req_elbow)
t.addWaypoint(goal.waypoint_2,goal.duration, req_elbow)
t.addWaypoint(depart_waypoint, fine_depart_time, req_elbow)
if(goal.type == TYPE_REST):
if(cur_elbow):
travel_time = dist(kin.fk(cur_pose),REST_POS_ELBOW_UP) / SPEED_TRAVEL
t.addWaypoint(REST_POS_ELBOW_UP, travel_time, True)
else:
travel_time = dist(kin.fk(cur_pose),REST_POS_ELBOW_DOWN) / SPEED_TRAVEL
t.addWaypoint(REST_POS_ELBOW_DOWN, travel_time, True)
hebi_goal = None
try:
hebi_goal = t.createTrajectory()
except:
rospy.logwarn("Arm Planner Node FAILED to create trajectory")
self.action_server.setAborted()
return
# rospy.loginfo(goal)
# Send goal to action server
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
# self.as_feedback.percent_complete = 100;
# self.action_server.publish_feedback(self.as_feedback)
#
while (not self.hebi_is_done):
pass
self.action_server.set_succeeded(self.as_result)
