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)