def move_to_next_node(self): print "moveto next node" if len(self._poses_chain): bug_goal = MoveToGoal() bug_goal.target_pose = self._poses_chain.pop(0) self._move_client.send_goal(bug_goal, done_cb=self.move_to_done_cb, feedback_cb=self.move_to_feedback_cb) else: return
def execute_cb(self, goal): """ This funciton is used to execute the move to goal """ self._path_result.visited=[] #Initialize the path result self._skip_unreachable=goal.skip_unreachable self._client.wait_for_server() self._poses = goal.path.poses self._poses_idx = 0 #Start with the first goal self._continue= True rospy.loginfo('Starting Number of Poses: %s'%len(self._poses)) move = MoveToGoal() move.target_pose.pose = self._poses[self._poses_idx].pose """ Send the goal to the _client and once done, go to callback function "move_to_done_cb" """ self._client.send_goal(move, done_cb=self.move_to_done_cb) while (self._continue==True): """ Check if current goal is preempted by other user action. If so, break out of loop """ if self._server.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._server.set_preempted() self._continue= False break
def execute_cb(self, goal): # Executed every time a goal is sent by the client self._move_to_client.wait_for_server() # printing the goals in the terminal rospy.loginfo(goal.path.poses) # Creating an instance of MoveToGoal to compose a goal message self.move = MoveToGoal() # Iterating through the goals and sending it to the move_to_client for i in range(len(goal.path.poses)): if i == 2: self.success = True else: self.success = False self._pose = goal.path.poses[i] self.move.target_pose = self._pose self._move_to_client.send_goal(self.move, done_cb=self.move_to_done_cb) self._move_to_client.wait_for_result() # self.success = True rospy.spin() # setting the server's preempted() function if it's requested while flag: if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() break
def handle_path(self, paramExecutePathGoal): ''' Action server callback to handle following path in succession ''' rospy.loginfo('handle_path') self.goal_index = 0 self.executePathGoal = paramExecutePathGoal self.executePathResult = ExecutePathResult() if self.executePathGoal is not None: self.visualization_publisher.publish(self.executePathGoal.path) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): if not self.path_server.is_active(): return if self.path_server.is_preempt_requested(): rospy.loginfo('Preempt requested...') # Stop bug2 self.goal_client.cancel_goal() # Stop path_server self.path_server.set_preempted() return if self.goal_index < len(self.executePathGoal.path.poses): moveto_goal = MoveToGoal() moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index] self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal, feedback_cb=self.handle_goal_preempt) # Wait for result while self.goal_client.get_result() is None: if self.path_server.is_preempt_requested(): self.goal_client.cancel_goal() else: rospy.loginfo('Done processing goals') self.goal_client.cancel_goal() self.path_server.set_succeeded(self.executePathResult, 'Done processing goals') return rate.sleep() self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def button_send_cb(self): x = self.x_spin.value() y = self.y_spin.value() yaw = self.yaw_spin.value() goal = MoveToGoal() q = quaternion_from_euler(0, 0, yaw * math.pi / 180) goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.x = q[0] goal.target_pose.pose.orientation.y = q[1] goal.target_pose.pose.orientation.z = q[2] goal.target_pose.pose.orientation.w = q[3] self.client.send_goal(goal, done_cb=self.action_done_cb, active_cb=self.action_active_cb, feedback_cb=self.action_feedback_cb) goal_text = u'%.2f; %.2f; %i°' % (x, y, yaw) self.update_state_text(goal_text)
def execute_cb(self, goal): rospy.loginfo(len(goal.path.poses)) if self.use_obstacle_avoidance: SERVER = '/bug2/move_to' else: SERVER = '/motion_controller/move_to' #after receiving the path, publish the path self.path_publisher.publish(goal.path) #creating the client to send each pose to the server self.client = SimpleActionClient(SERVER, MoveToAction) self.client.wait_for_server() #passing each pose to the server and handling the result from the server for pose in goal.path.poses: self.goal_to_move_to = MoveToGoal() self.feedback = ExecutePathFeedback() self.visited = ExecutePathResult() self.goal_to_move_to.target_pose = pose self.client.send_goal(self.goal_to_move_to, done_cb=self.move_to_done_cb) rospy.logwarn("goal passed to motion_controller") self.client.wait_for_result() rospy.loginfo('Before feedback pose') self._move_to_server.set_succeeded(self.visited) #client.get_result() pass
def move_to_done_cb(self, state, result): """ This call back function is called once one goal is completed. This checks if there are any other goal remaining to be executed and recurrsively calls itself. """ feedback = ExecutePathFeedback() feedback.pose=self._poses[self._poses_idx] """ #====================================================================== #* If state is 3, i.e., goal was successful, publish feedback # and move to next goal # #* Else publish feedback and check _skip_unreachable flag, # * If True go to next pose # * Else break #====================================================================== """ if(state == 3): feedback.reached = True self._path_result.visited.append(feedback.reached) self._server.publish_feedback(feedback) self._poses_idx = self._poses_idx + 1 """ * If more pose available, move to next goal * Else break """ if(self._poses_idx < len(self._poses)): move = MoveToGoal() move.target_pose.pose = self._poses[self._poses_idx].pose self._client.send_goal(move, done_cb=self.move_to_done_cb) else: self._continue=False self._server.set_succeeded(self._path_result) else: feedback.reached = False self._path_result.visited.append(False) self._server.publish_feedback(feedback) if(self._skip_unreachable==True): self._poses_idx = self._poses_idx + 1 """ * If more pose available, move to next goal * Else break """ if(self._poses_idx < len(self._poses)): move = MoveToGoal() move.target_pose.pose = self._poses[self._poses_idx].pose self._client.send_goal(move, done_cb=self.move_to_done_cb) else: self._continue=False self._server.set_succeeded(self._path_result) else: rospy.loginfo('Unreachable') self._continue=False self._server.set_succeeded(self._path_result)