Description: The Navigation Stack serves to drive a mobile base from one location to another while safely avoiding obstacles. Often, the robot is tasked to move to a goal location using a pre-existing tool such as rviz in conjunction with a map. For example, to tell the robot to go to a particular office, a user could click on the location of the office in a map and the robot would attempt to go there. However, it is also important to be able to send the robot goals to move to a particular location using code, much like rviz does under the hood. For example, code to plug the robot in might first detect the outlet, then tell the robot to drive to a location a foot away from the wall, and then attempt to insert the plug into the outlet using the arm. The goal of this tutorial is to provide an example of sending the navigation stack a simple goal from user code.
This tutorial assumes basic knowledge of how to bring up and configure the navigation stack. This may be different depending on the platform that you're running on. Below is a list of tutorials for known platforms at the time of this writing. Please feel free to update it as more tutorials for different robots become available.
Now that we have our package, we need to write the code that will send goals to the base. Fire up a text editor and paste the following into a file called src/simple_navigation_goals.cpp. Don't worry if there are things you don't understand, we'll walk through the details of this file line-by-line shortly.
This line includes the action specification for move_base which is a ROS action that exposes a high level interface to the navigation stack. Essentially, the move_base action accepts goals from clients and attempts to move the robot to the specified position/orientation in the world. For a detailed discussion of ROS actions see the actionlib documentation.
This line creates a convenience typedef for a SimpleActionClient that will allow us to communicate with actions that adhere to the MoveBaseAction action interface.
This line constructs an action client that we'll use to communicate with the action named "move_base" that adheres to the MoveBaseAction interface. It also tells the action client to start a thread to call ros::spin() so that ROS callbacks will be processed by passing "true" as the second argument of the MoveBaseClient constructor.
These lines wait for the action server to report that it has come up and is ready to begin processing goals.
Here we create a goal to send to move_base using the move_base_msgs::MoveBaseGoal message type which is included automatically with the MoveBaseAction.h header. We'll just tell the base to move 1 meter forward in the "base_link" coordinate frame. The call to ac.sendGoal will actually push the goal out over the wire to the move_base node for processing.
The only thing left to do now is to wait for the goal to finish using the ac.waitForGoalToFinish call which will block until the move_base action is done processing the goal we sent it. After it finishes, we can check if the goal succeded or failed and output a message to the user accordingly.
Now that we have a package and a source file, we'll want to build and then try things out. The first step will be to add our src/simple_navigation_goals.cpp file to our CMakeLists.txt file to get it to build. Open up CMakeLists.txt in your editor of choice and add the following line to the bottom of the file.
Once this is done, we can build our executable by typing make.
If something shows up, then you're good to go. Otherwise, you need to find the name of the action that the navigation stack is using, a common alternative on the PR2 platform is "move_base_local," and update all references in the src/simple_navigation_goals.cpp file accordingly.
After this, its as simple as running the executable we created.
And, if all goes well, the robot should begin to move a meter forward.