move_base action server failing to send goals after using Rviz 2D Nav Goal tool
I wrote a simple python script in order to send goals to the navigation stack through the move_base action server. Everything works great if I launch move_base and then run my node, works like a charm as many times as I run it, changing the goal parameters and such.
However, if at any point, I decide to use the Rviz tool 2D Nav Goal to set a navigation goal, and then I try to run my python node, the robot doesn't move towards the goal. The 'move_base/goal' topic still gets the data from my node, but for some reason the goal doesn't seem to be getting to move_base. I don't get any warnings nor errors, apparently everything seems to be working fine, except that the robot doesn't move.
Any idea on why this might be happening? I'm attaching the code I'm using for my move_base/goal client, in case it helps. Also, running ROS Melodic on Ubuntu 18.04 LTS.
#!/usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
def movebase_client():
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
rospy.loginfo("Server ready!")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'robot_map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.35
goal.target_pose.pose.position.y = 3
goal.target_pose.pose.orientation.w = 1.0
# rospy.loginfo("Goal ready")
client.send_goal(goal)
rospy.loginfo("Goal sent")
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return client.get_result()
if __name__ == '__main__':
try:
rospy.init_node('simple_goal')
result = movebase_client()
if result:
rospy.loginfo("Goal execution done!")
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished")
Please take a look at this very interesting answer from @jayess on the topic and the proposed solution: https://answers.ros.org/question/2947...
@osilva's indirect suggestion to monitor the messages on
/cmd_velis a good one.There's a way to add a callback to your client that will report progress every second or so. Adding this might help you understand what is going on.
I seem to recall there's also something unusual about the value returned by move_base's action server when you use
client.get_result()-- maybe it returns None?Yes, see this tutorial that uses
get_state()notget_result()[this is a move_base quirk, not a general rule]:http://wiki.ros.org/navigation/Tutori...
Thank you both for the suggestions!! However, I'm sure it's not a cmd_vel issue, I actually already have a multiplexer and even then, if it was a cmd_vel issue I'd see the goal being sent in the rviz screen at least. The thing is, when I execute my node I do get a "done" result from the client, as in "I'm actually sending the goal". But the goal doesn't show up on Rviz, and therefore the robot doesn't move. So it's more of an issue between the client and move_base, somehow. Again, what's weird is that everything works if I don't send any goal through the Rviz tool... I don't know what this tools is messing up with that it doesn't work after that.
Alright seems like the status getting returned by the client is "RECALLED", which according to the description means "The goal received a cancel request before it started executing, but the action server has not yet confirmed that the goal is canceled". Well, anyone has any idea on why that's happening? Because I'm as confused as before.
I believe that the "2D Nav Goal" feature in rviz issues a goal directly to the
/move_base_simple/goaltopic i.e. the ActionServer is not involved. If move_base is is still trying to execute that first command, it would explain the behavior you describe. The ActionServer should have tried to cancel that first goal, but it seems like move_base ignored the cancel request.You might want to check how you have failure retries configured at the move_base level, and at the local planner level. Infinite retries are a bad idea.
I tested your code with Husky robot, it works, just made one modification to your code but I don't think has to do with your issue:
goal.target_pose.header.frame_id = 'odom'but I also startroslaunch husky_navigation move_base_mapless_demo.launch:I tried to recreate the issue in WSL, Ubuntu Melodic, Noetic and Windows but they always work.
I recorded video.