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()
