def _move_to_callback(self, move_to_goal): """ Triggered with a request to move_to action server """ rospy.loginfo('Received [move_to] action command.') if not self._set_new_goal(move_to_goal): return else: rate = rospy.Rate(self._controller_frequency) while not rospy.is_shutdown(): if not self._move_to_server.is_active(): # Exit if the goal was aborted return if self._move_to_server.is_preempt_requested(): # Process pending preemption requests rospy.loginfo('Action preemption requested.') if (self._move_to_server.is_new_goal_available() and self._set_new_goal( self._move_to_server.accept_new_goal())): # New goal already set pass else: # No new goals, preempt explicitly and exit the callback self._publish_zero_velocity() self._move_to_server.set_preempted() return if not self._move_towards_goal(): # Finish execution if the goal was reached self._move_to_server.set_succeeded(MoveToResult(), 'Goal reached.') self._publish_zero_velocity() return rate.sleep() self._move_to_server.set_aborted( MoveToResult(), 'Aborted. The node has been killed.')
def _set_new_goal(self, new_goal): """ Set new target pose as given in the goal message. Checks if the orientation provided in the target pose is valid. Publishes the goal pose for the visualization purposes. Returns true if the goal was accepted. """ if not self._is_quaternion_valid( new_goal.target_pose.pose.orientation): rospy.logwarn('Aborted. Target pose has invalid quaternion.') self._move_to_server.set_aborted( MoveToResult(), 'Aborted. Target pose has invalid quaternion.') return False else: x = new_goal.target_pose.pose.position.x y = new_goal.target_pose.pose.position.y yaw = tf.transformations.euler_from_quaternion([ new_goal.target_pose.pose.orientation.x, new_goal.target_pose.pose.orientation.y, new_goal.target_pose.pose.orientation.z, new_goal.target_pose.pose.orientation.w ])[2] pose = Pose2D(x, y, yaw) self._velocity_controller.set_target_pose(pose) self._current_goal_publisher.publish(new_goal.target_pose) rospy.loginfo('New target pose: {0}'.format(pose)) return True
def _obstacles_callback(self, obstacle_msg): rospy.logwarn( 'An obstacle was detected. Will stop the robot and cancel the current action.' ) if self._move_to_server.is_active(): self._move_to_server.set_aborted( MoveToResult(), 'Obstacle encountered, aborting...') self._publish_zero_velocity()