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