Ask Your Question
0

MoveBaseAction cancel_goal does not work from python script

asked 2019-12-24 05:46:39 -0600

june2473 gravatar image

updated 2019-12-24 06:07:31 -0600

gvdhoorn gravatar image

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()
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
1

answered 2019-12-25 08:25:04 -0600

Orhan gravatar image

I don't know about the python client but as far as I remember, sending an empty message to this topic like this cancels all goals:

rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}

This works in terminal well.

You can send the message in python like:

from actionlib_msgs.msg import GoalID
cancel_pub = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1)
cancel_msg = GoalID()
cancel_pub.publish(cancel_msg)

Just tried these lines in python terminal to be sure and it works.

edit flag offensive delete link more
1

answered 2019-12-25 22:44:58 -0600

self.move_base.cancel_goal() only works if your self.move_base object has sent the goal itself on the first place. Which I don't think is the case.

What @Orhan suggested in his answer can actually be accomplished by: self.move_base.cancel_all_goals().

edit flag offensive delete link more

Comments

Cleaner and the correct one.

Orhan gravatar imageOrhan ( 2019-12-25 23:41:04 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-12-24 05:46:39 -0600

Seen: 57 times

Last updated: Dec 25 '19