class SimpleMoveServer( object ):
"""
SimpleMoveServer node
"""
def __init__(self):
self._goal_orientation_tolerance = \
rospy.get_param("~goal_orientation_tolerance", 0.05)
self.odometry_frame = rospy.get_param("~odometry_frame")
self._mover = SimpleMover("~simple_motion_params/",
stop_function = self.mover_stop_cb)
self._as = actionlib.SimpleActionServer("simple_move", SimpleMoveAction,
execute_cb = self.execute_cb, auto_start=False)
self._as.start()
def execute_cb(self, goal):
"""
Called by SimpleActionServer when a new goal is ready
"""
velocity = None if (goal.velocity == 0) else goal.velocity
acceleration = None if (goal.acceleration == 0) else goal.acceleration
try:
if goal.type == SimpleMoveGoal.SPIN:
error = self._mover.execute_spin(goal.angle,
max_velocity = velocity,
acceleration = acceleration)
rospy.loginfo("EXECUTED SPIN: %.1f, error %.3f" %( np.degrees(goal.angle),
np.degrees(error)))
elif goal.type == SimpleMoveGoal.STRAFE:
error = self._mover.execute_strafe(goal.angle,
goal.distance,
max_velocity = velocity,
acceleration = acceleration)
rospy.loginfo("EXECUTED STRAFE angle: %.1f, distance: %.1f, error %.3f" %(
np.degrees(goal.angle),
goal.distance,
error))
else:
rospy.logwarn('SIMPLE MOVE SERVER received invalid type')
self._as.set_aborted(SimpleMoveResult(False, None))
except TimeoutException:
rospy.logerr("Timeout during move execution, aborting")
self._as.set_succeeded(SimpleMoveResult(False, None))
except Exception, e:
rospy.logerr("Unexpected exception during simple move, aborting: ",
e)
self._as.set_succeeded(SimpleMoveResult(False, None))
else:
def __init__(self):
self._goal_orientation_tolerance = \
rospy.get_param("~goal_orientation_tolerance", 0.05)
self.odometry_frame = rospy.get_param("~odometry_frame")
self._mover = SimpleMover("~simple_motion_params/",
stop_function = self.mover_stop_cb)
self._as = actionlib.SimpleActionServer("simple_move", SimpleMoveAction,
execute_cb = self.execute_cb, auto_start=False)
self._as.start()