Localization
Localization is the next key step in navigation. For the robot to navigate through the environment, it is first important for it to know where it is in the map with respect to other things. This is called localization. ROS navigation stack implements Monte-Carlo Localization. It is an algorithm that generates several guesses about where the robot’s next move may be. It then corrects its guesses as the robot moves through the environment and collects more data. ROS navigation implements this algorithm in a node called amcl node provided by the amcl package. As a user of ROS navigation stack, all one has to do is configure this node for their robot/setup.
ROS nodes used: amcl
Just like we did for mapping, for the purpose of localization, we also need to install the required packages first. Let’s get that out of the way as explained below.
Prerequisites for Localization
- Install amcl package: This package provides the amcl node that implements Monte Carlo Localization method. This node subscribes to /tf topic, /laser topic, /map topic and /initialpose topic. The output of this node is robot’s estimated pose in the map alongwith its covariance. This is published on /amcl_pose topic. The amcl node also publishes another topic called /particlecloud. This topic publishes the set of pose estimates being maintained by the filter.
The amcl 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-amcl | ||
| $ 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 amcl |
| /opt/ros/melodic/share/amcl |
Once, we have installed this package, we can run amcl node and complete the localization step of navigation. The steps are explained below.
Localization Steps (How to perform localization 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, if the gazebo environment is already running, the robot is spawned and is able to move – 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 page.
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. 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.
Gazebo world with a mobile robot. This gazebo world is a default world that comes with gazebo. It is called cafe.world
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.
Another helpful tool to know the topics being published and relation between them is to view the rosgraph. This can be generated by running $ rosrun rqt_grapgh rqt_graph. Below is the rosgraph after launch Main.launch file.
Among the published topics in Figure 11, the ones of interest to the amcl_node are /tf and /hydra_one/laser/scan. You may have noticed that the other two topics that amcl_node subscribes to (/map and /initialpose) are not in the list. That is because those are not yet being published. We will see them being published soon. Let’s move to the next step.
STEP 2 – IN TERMINAL 2
Task: launch amcl_node
Next, we would want to launch the amcl_node. In order to do so, we first have the configure the parameters for our set up and create a launch file. An example launch file (that I used for my project) can be found on this github page. The file is called amcl_plus_map_server.launch. The explanation of the parameters is provided on ROS wiki page and they must be configured per the needs of your project.
Below is the screenshot of Terminal 2 from 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_navigation |
| $ cd launch |
| $ roslaunch amcl_plus_map_server.launch |
Now that we have the amcl node running, it is helpful to see teh 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). The topics in bold are new when compared to the list of topics that were being published before the amcl node was laucnhed.
| $ 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 |
| /odom |
| /particlecloud |
| /rosout |
| /rosout_agg |
| /tf |
| /tf_static |
Another helpful tool to know the topics being published and relation between them is to view the rosgraph. This can be generated by running $ rosrun rqt_grapgh rqt_graph. Below is a rosgrah after amcl node was launched.
Some of these topics need explanation here . . . Firstly, we discuss the /map and /map_metadata topics. Where did they come from?
Before we launch the amcl_node, we either have to launch the map_server node. This node was introduced in the Mapping section of this tutorial. This node is responsible for providing the map to any other node requesting it. In this case, we know that aamcl node need the map information, so it needs to subscribe to the /map and /map_metadata topics.
In my setup, the launch file that was used to launch the amcl node takes care of launching the map_server node as well. So, we only need to launch the amcl_launch. launch file. This will launch both map_server and amcl nodes at once.
So far, we have addressed most the new rostopics that we saw in figure 32. The one we have not discussed yet is the /initialpose. Who publishes this topic and what is it used for? Don’t worry! We will get to it shortly. Before, we move to the next step, it is helpful to fully understand the amcl node. Let’s do rosnode info amcl. See below.
| $ rosnode info amcl |
| ——————————————————————————– |
| Node [/amcl] |
| Publications: |
| * /amcl/parameter_descriptions [dynamic_reconfigure/ConfigDescription] |
| * /amcl/parameter_updates [dynamic_reconfigure/Config] |
| * /amcl_pose [geometry_msgs/PoseWithCovarianceStamped] |
| * /diagnostics [diagnostic_msgs/DiagnosticArray] |
| * /particlecloud [geometry_msgs/PoseArray] |
| * /rosout [rosgraph_msgs/Log] |
| * /tf [tf2_msgs/TFMessage] |
| Subscriptions: |
| * /clock [rosgraph_msgs/Clock] |
| * /hydra_one/laser/scan [sensor_msgs/LaserScan] |
| * /initialpose [unknown type] |
| * /map [nav_msgs/OccupancyGrid] |
| * /tf [tf2_msgs/TFMessage] |
| * /tf_static [tf2_msgs/TFMessage] |
| Services: |
| * /amcl/get_loggers |
| * /amcl/set_logger_level |
| * /amcl/set_parameters |
| * /global_localization |
| * /request_nomotion_update |
| * /set_map |
| contacting node http:// …………. |
| Pid: 14128 |
| Connections: |
| * topic: /rosout |
| * to: /rosout |
| * direction: outbound () |
| * transport: TCPROS |
| * topic: /tf |
| * to: /amcl |
| * direction: outbound |
| * transport: INTRAPROCESS |
| * topic: /clock |
| * to: /gazebo (http://prabhjot-Lenovo-Y520-15IKBM:42947/) |
| * direction: inbound (51532 – prabhjot-Lenovo-Y520-15IKBM:43297) [16] |
| * transport: TCPROS |
| * topic: /tf |
| * to: /amcl (http://prabhjot-Lenovo-Y520-15IKBM:33579/) |
| * direction: inbound |
| * transport: INTRAPROCESS |
| * topic: /tf |
| * to: /robot_state_publisher (http://prabhjot-Lenovo-Y520-15IKBM:37927/) |
| * direction: inbound (43978 – prabhjot-Lenovo-Y520-15IKBM:60119) [17] |
| * transport: TCPROS |
| * topic: /tf |
| * to: /gazebo (http://prabhjot-Lenovo-Y520-15IKBM:42947/) |
| * direction: inbound (51538 – prabhjot-Lenovo-Y520-15IKBM:43297) [19] |
| * transport: TCPROS |
| * topic: /tf_static |
| * to: /robot_state_publisher (http://prabhjot-Lenovo-Y520-15IKBM:37927/) |
| * direction: inbound (43982 – prabhjot-Lenovo-Y520-15IKBM:60119) [20] |
| * transport: TCPROS |
| * topic: /hydra_one/laser/scan |
| * to: /gazebo (http://prabhjot-Lenovo-Y520-15IKBM:42947/) |
| * direction: inbound (51542 – prabhjot-Lenovo-Y520-15IKBM:43297) [21] |
| * transport: TCPROS |
| * topic: /map |
| * to: /map_server (http://prabhjot-Lenovo-Y520-15IKBM:42823/) |
| * direction: inbound (36572 – prabhjot-Lenovo-Y520-15IKBM:40345) [12] |
| * transport: TCPROS |
That is a lot of good information. It tells everything amcl node is subscribing to, or publishing, etc. If you notice in the figure above, the * /initialpose [unknown type] says unknown type. This is a bit concerning at the moment. However, the next step will address this. So, let’s move on.
STEP 3 – IN TERMINAL 3
Task: Visualization in rviz ans set /initialpose
At this point, we have 3 things that have been launched. One is the robot/gazebo environment (launched in Terminal 1) and the other two are amcl node and map_server node (launched in Terminal 2). One final step before we start the localization step 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
- LaserScan
- Map
- PoseArray
The RobotModel, LaserScan and Map displays were introduced in Mapping tutorial on the previous page. PoseArray display allows us to visualize the localization process live.
Make sure the Fixed Frame is set to map. And subscribe to the topic /particlefileter for PoseArray display. (You will notice lots of arrows around the robot). These are the localization guesses (the estimated pose of the robot by the amcl node.
Below is the figure showing rviz and gazebo environment simultaneously. At this point, we need to take care of one important step. That is to take care of the /initalpose topic.
Use the 2D Pose Estimate tool in rviz to specify the initial pose of the robot. 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 which was previously empty.
As soon as we set the /initialpose, we have everything we need to proceed with the localization 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.
Alternately, we can check the amcl node once again by doing the rosnode info. Notice the /initialpose is no longer of type unknown. Compare to figure xyz.
| $ rosnode info amcl |
| ——————————————————————————– |
| Node [/amcl] |
| Publications: |
| * /amcl/parameter_descriptions [dynamic_reconfigure/ConfigDescription] |
| * /amcl/parameter_updates [dynamic_reconfigure/Config] |
| * /amcl_pose [geometry_msgs/PoseWithCovarianceStamped] |
| * /diagnostics [diagnostic_msgs/DiagnosticArray] |
| * /particlecloud [geometry_msgs/PoseArray] |
| * /rosout [rosgraph_msgs/Log] |
| * /tf [tf2_msgs/TFMessage] |
| Subscriptions: |
| * /clock [rosgraph_msgs/Clock] |
| * /hydra_one/laser/scan [sensor_msgs/LaserScan] |
| * /initialpose [geometry_msgs/PoseWithCovarianceStamped] |
| * /map [nav_msgs/OccupancyGrid] |
| * /tf [tf2_msgs/TFMessage] |
| * /tf_static [tf2_msgs/TFMessage] |
| Services: |
| * /amcl/get_loggers |
| * /amcl/set_logger_level |
| * /amcl/set_parameters |
| * /global_localization |
| * /request_nomotion_update |
| * /set_map |
Next, bring the focus of your cursor back on Terminal 1 and move the robot around using teleop node . Use the keys on the keyboard to move the robot. You will notice that, the more the robot moves, the scatter (red arrows) gets narrower. This is because as the robot moves, it collects more data using the laser and is able to gain more confidence about its guesses. Below is a short video that shows the localization process live.
It is quite impressive what amcl node does!
STEP 4 – IN TERMINAL 4
Finally, it is time to check the outputs of amcl node. We learned earlier that amcl node publishes on /amcl_pose and /particlecloud topics. Let’s check what is published.
Move the robot to any desired location and then echo /amcl_pose topic. See example below.
| $ rostopic echo -n1 /particlecloud |
| position: |
| x: 1.0823468779 |
| y: -6.27550194525 |
| z: 0.0 |
| orientation: |
| x: -0.0 |
| y: 0.0 |
| z: 0.749146431157 |
| w: -0.66240442683 |
We see that this topic gives the estimated pose of the robot at the location where the robot was when this topic was echoed. While we are here, let’s also echo the /particlecloud topic. See the results below.
| $ rostopic echo -n1 /amcl_pose |
| header: |
| seq: 71 |
| stamp: |
| secs: 2636 |
| nsecs: 693000000 |
| frame_id: “map” |
| pose: |
| pose: |
| position: |
| x: 1.0671769748 |
| y: -6.27484128651 |
| z: 0.0 |
| orientation: |
| x: 0.0 |
| y: 0.0 |
| z: -0.785836685195 |
| w: 0.618434074257 |
| covariance: [0.007361395386666247, -0.002008905890959589, 0.0, 0.0, 0.0, 0.0, -0.002008905890959589, 0.01623566427512202, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.023072322669952308] |
We are now done with localization part of navigation. The next section provides additional information or alternate ways of doing some of the things that were presented above, that may be helpful.
Additional Information
AN ALTERNATe to SETTING /INITIALPOSE
Previously, we used rviz to set the initial pose . See step 3 above. However, there is another approach available. In cases, where we cannot use rviz or we are unsure of the initial pose of the robot, the 2D Pose Estimate tool of rviz will not be very helpful. Luckily, there is way out.
As we can see in Figure 35, the amcl node provides a service called /global_localization. When called, this service scatters the initial pose estimate of the robot all across the map. This makes it even more challenging for amcl to localize. . .but we will see later that amcl node can handle this challenge and it is quite impressive. See the steps below.
Step 1
Keep terminal 1 running and close rest of the terminals. Then launch amcl node in terminal 2 (repeat step 2 – terminal 2 above).
Also, launch rviz in terminal 3 (as was done in Step 3 – Terminal 3 above). However, do not use 2D Pose Estimate tool of rviz. Instead, we will use the /global_localization service of amcl node.
Step 2
Write a service client. I took an example of this file from https://www.theconstructsim.com/. The file is called init_particles_caller.py. Create this file and place in your working package. The file can be accessed from this github page under scripts folder in hydra_one_navigation package.
| $source /opt/ros/melodic/setup.bash | |||||
| $ cd ~/catkin_ws/ | |||||
| $ catkin_make | |||||
| $ cd src/hydra_one_navigation/scripts | |||||
| $ gedit init_particles_caller.py |
Next, we will run this python script. In order do so, first make this file executable as shown below and then run it.
Navigate to the folder where the file is located and do chmod as shown below.
$ chmod a+x init_particles_caller.py
Next, run the file. (Make sure you are in the directory where this file is located)
$ ./init_particles_caller.py
Next, visualize what just happened in rviz. The particles / initial guess are spread all across the map.
Step 3
Bring the focus back in Terminal 1 and move the robot around with keyboard. The scatter will start to collapse as the robot moves. This is because as the robot moves, it is collecting more data and is able to provide a better estimate of the robot pose. So, the guesses tend to spread in a smaller region. See the video below of amcl in work. It is quite impressive!
Summary – Localization
Finally, we are done with Localization step of navigation. To summarize localization, 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.
- 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.
- Moved the robot around to gather more data and help amcl localize.
- Provided the estimated pose on topics /amcl_pose and /particlecloud.
That is it for localization! I have included few links in the Troubleshooting section below which helped me troubleshoot my set up when I ran into problems.
If would like to proceed with the next step called Path Planning, please see the next page. Otherwise, good job with localization. I hope you enjoyed!
Troubleshooting – Localization
The following links helped me troubleshoot some of the issues that I ran into. ROS Q/A forum is a great place to start when troubleshooting.
- (amcl error) https://answers.ros.org/question/303566/amcl-no-laser-scan-received-and-thus-no-pose-updates-have-been-published-for-xxxx-seconds/
- (explains frames) https://answers.ros.org/question/229978/how-amcl-works/
- (amcl parameters) https://github.com/ros-planning/navigation/blob/jade-devel/amcl/test/small_loop_crazy_driving_prg_corrected.xml
Great work, nice explanation!
Thanks Vishal!
Great Job!
May I know are you still responsible for GM’s Rd of DMS system? In fact we have a technology seminar for automotive DMS, wonder if we can invite you to attend as a speaker？
Hi Wiesen,
Thanks for the comment. I don’t work in that area anymore.
Thanks