#!/usr/bin/env python

import rospy
import actionlib
##from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
from math import sqrt
##from std_msgs.msg import Empty

class mbSmoother():
    def __init__(self):
        rospy.init_node('mbSmoother', anonymous=True)

        self.move_base = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
        rospy.loginfo("Waiting for move_base action server...")
        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting move base goals smoother")

        rospy.Subscriber("/odom", Odometry, self.odomCallback)
        rospy.Subscriber("/move_base/current_goal", PoseStamped, self.goalCallback)

        rospy.spin()

    def odomCallback(self, msg):
        self.robot_pose = msg.pose.pose
        ##print "odom:"
        ##print self.robot_pose

    def goalCallback(self, msg):
        self.goal_pose = msg.pose
        ##print "goal:"
        ##print self.goal_pose
        while True:
            self.distance = sqrt(pow(self.goal_pose.position.x - self.robot_pose.position.x, 2) + pow(self.goal_pose.position.y - self.robot_pose.position.y, 2))
            print self.distance
            if self.distance < 0.5:
                self.move_base.cancel_goal()
                ##rospy.sleep(4)
                ##wait = self.move_base.wait_for_result()
                rospy.loginfo("goal has been canceled")
                break

        ##while self.goal_pose.position.x

if __name__ == "__main__":
    mbSmoother()