A guide to implement ROS Navigation Stack on any robot

Path Planning

Path Planning is the ultimate goal of navigation. This allows the user to give a goal pose to a robot and have it navigate through a given environment from its current position to the goal position autonomously. This is where everything we have done so far (mapping and localization) comes together. ROS navigation stack already does the heavy work for us just like we saw with mapping and localization. The path planning portion of navigation is implemented in a package called move_base. Before, we start using this package, it is important to get a big picture and understand the crucial details.

ROS nodes used: move_base

Let’s start with the following diagram from ROS wiki. Understanding this diagram is the first step.

The big box in the middle represents move_base node which is the core of ROS navigation stack. It subscribes to several topics, including from the map_server node which was introduced in the Mapping section and the amcl node, which was introduced in the Localization section. And within the move_base node, there is a lot going on which needs to be understood before proceeding.

Within the move_base node, the diagram shows four major items. Let’s understand them one by one.

• global_planner
• local_planner
• global_costmap
• local_costmap

Planner is an algorithm that calculates a path for the robot to follow from its current position to the goal position provided by the user. Within ROS, the planner relies on maps called costmaps to generate a plan. The costmaps will be explained a little bit later. Let’s spend some time understanding various planners that are in play within move_base node.

The move_base node includes two kinds of planners – global planner and local planner. The global_planner outputs a path for the robot to follow from its current position to the goal position. There are few different algorithms/planners implemented in ROS that enable global planning. The common ones are – carrot_planner , navfn, and global_planner. These are explained here and here. The user gets to choose which global_planner to use. Further, there are parameters that the user is required to configure before using any of these global planners. The list of parameters is given here. Now, having a basic understanding of what a global_planner does in the grand scheme of navigation, it is also important to mention that the global_planner relies on global_costmap to create a plan. Before, we jump into costmaps, let’s also learn about the local_planner.

The local_planner also generates a plan or path for the robot to move in a given environment. The plan created by the globa_planner is passed on to the local_planner for execution. The local_planner relies on local_costmap and it generates a local plan, which takes into account the laser and odometry data. There are several local planners avaialble to use. The common ones are – base_local_planner, dwa_local_planner, eband_local_planner, teb_local_planner. So far, we have learned that both global_planner and local_planner depend on costmaps. Let’s study what they actually are.

The costmaps in ROS are 2D or 3D occupancy grid matrices/maps where each grid or a cell is assigned a value/cost between 0 and 255. The value or cost in each cell is used to plan a path by the globa_planner and local_planner. These values indicate whether a given grid is free, occupied or unknown. If the cell is free, it is assigned a cost of 0 and if the cell contains an obstacle, it is assigned a higher value such as 254. The cost of the cell containing an obstacle is inflated (spread across) the surrounding cells based on the inflation radius specified by the user. See the section about ‘Inflation’ on this page. To learn more about costmaps, see this page on ROS wiki and also this page. A short definition is also provided here. At this point, you may be wondering how are costmaps created?

Firstly, the costmaps have to be initialized. This can be done either by providing the static map from map_server node that was introduced in the Mapping section of this tutorial or by providing a fixed width and height of the region. ROS Navigation stack uses two costmaps – one is called global_costmap which is used by global_planner for creating long-term plans over the entire environment and the second one is called local_costmap, which is used by the local_planner to create short term plans, taking into account the obstacle information in the environment. We mostly use a static map to initialize the global_costmap. Note that in order to initialize the costmap using a static map provided by the user (via map_server node), this must be used in conjunction with the amcl node so that the obstacle information can be registered in the map frame. Finally, we initialize the local_costmap by giving it a fixed width and height. Once initialized, the local_costmap is updated based on the sensor data as the robot moves in the environment. So, if new objects/obstacles are added to the environment after the static map was created, they will show up in local_costmap but not in global_costmap. See ROS wiki for reference.

To summarize the concepts presented above, the move_base node contains two planners – the global planner relies on generating global plans based on the global_costmap and the local_planner relies on local_costmap for generating local plans. In short, the optimization of path planning at close proximity is done by the local_planner and the full path is optimized by the global_planner. These two planners and their costmaps work together to create a path for the robot that takes it from one point to another point by avoiding obstacles.

That is it for the conceptual part of move_base node. Next, we will get into using it to make the robot move in an environment autonomoulsy. However, we need to take care of the prerequisites first just like we did for mapping and localization.

Prerequisites for Path Planning

• Install move_base package: This package provides the move_basse node that implements various planners which are responsible for generating optimized paths for the robot to move to a given goal position from its current position. This node subscribes to /tf topic, /laser topic, /map topic and /odom topic. Additionally, it also needs a goal position which is published on /goal topic. The output of this node is the velocity command. This is published on /cmd_vel topic.

The move_base package can be installed as follows: Take a note of the ROS version. Below, I used melodic version. You will have to install the right package for the ROS version installed on your machine.

$ source /opt/ros/melodic/setup.bash

$ sudo apt-get install ros-melodic-move-base

~$ cd ~/catkin_ws/

$ catkin_make

$ source devel/setup.bash

How to verify that the package is successfully installed?

One way of verifying is to make sure the rospack can find the package. See expected results below.

$ rospack find move_base

/opt/ros/melodic/share/move_base

Once, we have installed this package, we can run move_base node and complete the path planning step of navigation. The steps are explained below.

Path Planning Steps (How to perform path planning using ROS packages?)

STEP 1 – IN TERMINAL 1

Task: launch gazebo world, spawn the robot, and launch the teleop node to move the robot. (Note, teleop node is not needed here because move_base node will be publishing on the /cmd_vel topic. However, the Main.launch file used so far will launch all three things mentioned above. Just keep in mind that telep node is redundant here.)

Note, if the gazebo environment is already running and the robot is spawned – then this step can obviously be skipped. Otherwise, follow the steps below to complete STEP 1. These steps are the same as detailed in Mapping section, STEP 1 of this tutorial in the previous pages.

The text in bold represents commands entered by the user and rest of it is the output from the system. I will run the launch file called Main.launch to spawn the robot in gazebo environment. My launch file also launches the teleop node that will be used to move the robot around. However, this node is not needed for our purposes.

You will obviously have to navigate to the place where your files are stored to launch the robot and environment. See the snapshot from Terminal 1 on my machine.

$ source /opt/ros/melodic/setup.bash

$ cd ~/catkin_ws/

$ catkin_make

####

[100%] Built target gazebo_tutorials

$ source devel/setup.bash

$ cd src

$ cd hydra_one_description

$ cd launch

$ roslaunch Main.launch

Below is a snapshot from gazebo for my setup. This is the environment that will be mapped.

At this point, it is also helpful to know what ROS topics are being published. In my case, the topics being published are shown in Figure 11. (This is optional – If you would like to see what topic are being published on your setup, run the command $rostopic list in a new terminal). I have bolded some of the topics that are of interest to us at this time.Figure 11.

STEP 2 – IN TERMINAL 2

Task: launch move_base node

Next, we want to launch the move_base node. In order to do so, we first have the configure the parameters for our set up and then create a launch file. For the move_base node, there are several parameters to be configured because there are quite a few things here such as planners, costmaps, etc that we need to think about. In the next section, we will configure parameters for the planners and their costmaps.

This page on ROS wiki is a great starting point to configure various parameters. The parameters will be defined in .yaml files for clarity and as a good practice. Firstly, we will configure the parameters for costmaps. Keep in mind that we have two costmaps – global and local. We will define parameters for both costmaps in separate files and we will also create a third file that will contain parameters that are common to both costmaps. See snippets of files below. For tuning other parameters outside of these basic ones, see the costmap package. Again, please refer to ROS wiki for explanation of what these parameters mean.

Before we proceed, note that in the configuration above, the global_costmap uses a static map whereas the local costmap does not use a static map. Therefore, we need to specify width and height of the map. Now that the costmap parameters are defined, we need to define the parameters for the planners that will be used by the move_base node. And those are defined in the .yaml file below.

The base_local_planner implements a controller and is responsible for sending the velocity commands to the robot after it gets the plan (path). The planner (local and global planners working together) create a path based on the map and obstacle information. This controller’s (implemented in base_local-planner) job is to connect that plan generated to the robot and move it move. The parameters for local_base_planner are defined here for further details. It subscribed to /odom topic and publishes on /global_plan, /local_plan and cost_cloud. See Figure 63 below.

Next, we will create a launch file to launch move_base node. Outside of the parameters that we defined above, move_base node has its own parameters that need to be configured. Those will be defined in a launch file. In this launch file, we have the ability to choose which global_planner and the local_planner we would like to use. The launch file below calls for navfn/NavfnROS as the global_planner and base_local_planner/TrajectoryPlannerROS as the local_planner. Based on which planner we select, the .yaml files shown above must be modified to define the parameters of those planners. See the launch file below.

Let’s understand this launch file. In this launch file, we specify the global_planner and local_planner that we would like to use. Then, we launch the move_base node. Finally,we load the parameters that were defined above for costmaps and the planners.

We are almost there with configuring things and getting it to run. The one last thing we need to do is put it all together for move_base to work. We create a launch file below that launches 3 things.

• map_server node – this is needed because we want the global_costmap to use the static map, which was created in Mapping section and is provided by the map_server node. See Mapping section of this tutorial.
• amcl_node – this is important for localization. See localization section of this tutorial.
• move_base node – this is the main node that actually sends the velocity commands to the robot after it gets the plan from the planners.

Below is the screenshot of Terminal 2 from my machine. It is basically launching the navigation_main.launch file explained above.

$ source /opt/ros/melodic/setup.bash

$ cd ~/catkin_ws/

$ catkin_make

####

[100%] Built target gazebo_tutorials

$ source devel/setup.bash

$ cd src

$ cd hydra_one_navigation

$ cd launch

$ roslaunch navigation_main.launch

Now that we have the move_base node running, it is helpful to see the topics being published. In my case, the following topics are being published. (This is optional, if you would like to see the topics being published then do rostopic list in a new terminal).

$ rostopic list

/amcl/parameter_descriptions

/amcl/parameter_updates

/amcl_pose

/base_pose_ground_truth

/clock

/cmd_vel

/diagnostics

/gazebo/link_states

/gazebo/model_states

/gazebo/parameter_descriptions

/gazebo/parameter_updates

/gazebo/set_link_state

/gazebo/set_model_state

/hydra_one/front_depth_sensor_camera/color/camera_info

/hydra_one/front_depth_sensor_camera_ir/depth/camera_info

/hydra_one/front_lower_camera_info

/hydra_one/front_lower_camera_sensor/image_raw

/hydra_one/laser/scan

/initialpose

/joint_states

/map

/map_metadata

/move_base/NavfnROS/plan

/move_base/TrajectoryPlannerROS/cost_cloud

/move_base/TrajectoryPlannerROS/global_plan

/move_base/TrajectoryPlannerROS/local_plan

/move_base/TrajectoryPlannerROS/parameter_descriptions

/move_base/TrajectoryPlannerROS/parameter_updates

/move_base/cancel

/move_base/current_goal

/move_base/feedback

/move_base/global_costmap/costmap

/move_base/global_costmap/costmap_updates

/move_base/global_costmap/footprint

/move_base/global_costmap/inflation_layer/parameter_descriptions

/move_base/global_costmap/inflation_layer/parameter_updates

/move_base/global_costmap/obstacle_layer/parameter_descriptions

/move_base/global_costmap/obstacle_layer/parameter_updates

/move_base/global_costmap/parameter_descriptions

/move_base/global_costmap/parameter_updates

/move_base/global_costmap/static_layer/parameter_descriptions

/move_base/global_costmap/static_layer/parameter_updates

/move_base/goal

/move_base/local_costmap/costmap

/move_base/local_costmap/costmap_updates

/move_base/local_costmap/footprint

/move_base/local_costmap/inflation_layer/parameter_descriptions

/move_base/local_costmap/inflation_layer/parameter_updates

/move_base/local_costmap/obstacle_layer/parameter_descriptions

/move_base/local_costmap/obstacle_layer/parameter_updates

/move_base/local_costmap/parameter_descriptions

/move_base/local_costmap/parameter_updates

/move_base/parameter_descriptions

/move_base/parameter_updates

/move_base/result

/move_base/status

/move_base_simple/goal

/odom

/particlecloud

/rosout

/rosout_agg

/tf

/tf_static

Alternatively, we can view the rosgraphs to see the interactions between various ndoes. Figure 64 shows all active nodes/topics. Note, the rectangular boxes are topics and ovals are nodes. Below, we see we have a move base node that publishes and subscribes to several topics.

Now that we have launched the move_base node, nothing has happened in the simulation yet. This is because we have not sent the goal pose to this node. The whole point of move_base is to move the robot from its current position to a goal position. Therefore, if the node has no goal position, it has nothing to do. Next, we will leverage Rviz for sending the goal pose.

STEP 3 – IN TERMINAL 3

Task: Visualization in rviz ans set /initialpose and /move_base/goal

At this point, we have four things that have been launched. One is the robot/gazebo environment (launched in Terminal 1) and the other three are amcl node, map_server node, and move_base node (launched in Terminal 2). One final step before we start the moving the robot around is to open Rviz. So, let’s launch rviz in this terminal.

$ source /opt/ros/melodic/setup.bash

$ cd ~/catkin_ws/

$ catkin_make

$ source devel/setup.bash

$ cd src

$ cd hydra_one_description

$ rosrun rviz rviz

[ INFO] [1595630704.143763397]: rviz version 1.13.9

[ INFO] [1595630704.143819783]: compiled against Qt version 5.9.5

After rviz is launched, add the following Displays.

• RobotModel
• Map (Add 2 of these and rename one as global costmap and the other as local costmap)
• Path (Add 2 of these and rename one as global path and the other as local path)
• PoseArray (refer to Localization section of this tutorial)

The RobotModel and PoseArray displays were introduced in Mapping and Localization tutorials respectively. Map display will be used to display the local and global costmaps. Similarly, Path display will be used to display the local and global plan/path generated by the planners. Be sure to subscribe to the appropriate topics and ensure that the Fixed Frame is set to map.

See rviz display below. The black footprint is the global costmap, the white square around the robot is local costmap and the red arrows are localization guesses. There is nothing to show for Path since there are no plans generated yet.

Let’s get to the grind!

Step 1 – Localization

Send the /initialpose using 2D Pose Estimate in RVIZ. You simply have to select 2D Pose Estimate in Rviz and then click on a spot in the map in rviz where you think the robot currently is. Try to see where the robot is in gazebo and click on a similar spot in rviz. This will set the initial pose of the robot and this is published on /initialpose topic. Then, move the robot around in gazebo until it localizes. At this point, the robot can be moved with the keyboard (teleop node) and make sure the focus of the cursor is in Terminal 1 when the keys are being pressed.

After localization is done, uncheck PoseArray display in rviz so that we can see other important things.

As soon as we set the /initialpose, we have everything we need to proceed with the navigation step. Let’s see the rosgraph below. It shows that amcl node is subscribing to /initialpose, /map, /laser/scan, and /tf. In return, it is publishing on /particlecloud and /amcl_pose topics.

Step 2 – Send the goal pose

Use the 2D Nav Goal tool in rviz to specify the goal pose of the robot. You simply have to select 2D Nav Goal in Rviz and then click on a spot in the map in rviz where you want the robot to navigate to. As soon the goal pose is published, it is sent to the global_planner. The global_planner creates a global plan (see the blue line in the videos below). This global plan is then sent to the local_planner. The local_planner uses the local_costmap to create a local_plan. Based on this local plan, it sends the velocity commands to the robot. And the robot starts following a trajectory governed by the local_planner.

See the video below. The left side shows the view in rviz. The blue line is the global plan and red (very faint) line that moves with the robot is a local plan. The local planner publishes the following information as well, which tells what part pf global plan is being executed and what local plan is being used. The right side of the video below shows the view in gazebo simulator.

• /move_base/TrajectoryPlannerROS/local_plan
• /move_base/TrajectoryPlannerROS/global_plan

Below is a video of another example of navigation in a different gazebo environment.

Summary – Path Planning

Finally, we are done with Path Planning (and the final step of navigation). To summarize path planning, we essentially did the following.

• Launched the amcl node which is implemented in the amcl package. It implements Monte Carlo localization method and allows us to localize a robot ina given map. This node subscribes to /tf , /map, and /LaserScan and /initialpose topics. This node publishes on /amcl-pose and /particlefilter topics.
• Launched the map_server node which provides the map to any node requesting it. This node was introduced in the Mapping section. The amcl node needs a static map, therefore map_server node needs to be launched for amcl node to work.
• Launched the move_base node which implements global_planner and local_planner
• Provided /initialpose using the rviz 2D Pose Estimate tool. This initializes the particle filter (Monte Carlo method). Note, instead of using the 2D pose Estimate tool of rviz, /global_localization service provided by the amcl node could also be used to initialize. This was explained in ‘Additional Information’ section above of the Localization section of this tutorial.
• Moved the robot around to gather more data and help amcl localize
• Provided /goalpose using the rviz 2D Nv Goal tool. As soon the goal pose is published, it is sent to the global_planner. The global_planner creates a global plan (see the blue line in the videos below). This global plan is then sent to the local_planner. The local_planner uses the local_costmap to create a local_plan. Based on this local plan, it sends the velocity commands to the robot. And the robot starts following a trajectory governed by the local_planner.

That is it for path planning! I have included few links in the Troubleshooting section below which helped me troubleshoot my set up when I ran into problems.

That is it. . . I hope you enjoyed!

Please leave any comments or questions for discussion below.

Troubleshooting – Path Planning

• (costmap definition) https://answers.ros.org/question/309308/what-is-a-costmap/
• (clearing ghost obstacles from costmaps) https://blog.zhaw.ch/icclab/configuring-the-ros-navigation-stack-on-a-new-robot/

References

• https://subscription.packtpub.com/book/hardware_and_creative/9781788478953/6/ch06lvl1sec63/building-a-map-using-slam
• https://www.theconstructsim.com/robotigniteacademy_learnros/ros-courses-library/ros-courses-ros-navigation-in-5-days/
• https://github.com/ros-planning/navigation/blob/jade-devel/amcl/test/small_loop_crazy_driving_prg_corrected.xml
• https://u.cs.biu.ac.il/~yehoshr1/89-685/Fall2015/ROS_Lesson7.pdf
• https://github.com/mkhuthir/RoboND-Robot-Localization-Project
• http://wiki.ros.org/navigation#Tutorials
• https://github.com/ros-planning/navigation/tree/melodic-devel
• https://answers.ros.org/question/309308/what-is-a-costmap/
• https://roboticsknowledgebase.com/wiki/state-estimation/ros-cost-maps/
• https://blog.zhaw.ch/icclab/configuring-the-ros-navigation-stack-on-a-new-robot/
• http://wiki.ros.org/navigation/Tutorials/RobotSetup
• http://gazebosim.org/tutorials?tut=actor
• https://raw.githubusercontent.com/osrf/gazebo/master/worlds/cafe.world