MoveBaseAction cancel_goal does not work from python script
I wrote some simple python script which is trying to cancel current goal.
The problem is that nothing happens, goal is not cancelling.
Can anyone please tell me what is wrong and why nothing happens?
The code:
#!/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()