1. Project overview
Welcome to the " Where Am I ?" localization project! In this project, learn how to utilize the ROS AMCL package to accurately localize a mobile robot in a map of a Gazebo simulated environment.
In the course of completing this project, several aspects of robotics software engineering were involved, with an emphasis on ROS:
-
Create a ROS package to start a custom robot model in a custom Gazebo world
-
Use ROS AMCL package, teleoperation or navigation to locate the robot
-
Explore, add and tune specific parameters corresponding to each package to achieve the best localization results
The project tasks are as follows:
Environment: local installation or virtual machine
If using a native ROS installation or using a VM, some of the following packages will need to be installed. Install them as follows:
$ sudo apt-get install ros-kinetic-navigation
$ sudo apt-get install ros-kinetic-map-server
$ sudo apt-get install ros-kinetic-move-base
$ sudo apt-get install ros-kinetic-amcl2. Simulation settings
In the project project1: Go Chase it!, the simulated environment and the robot have been built. In this project, the simulated environment and robot are utilized again.
Set the catkin_ws folder and the src folder, and then grab the code from the previous project (copy the simulation environment and robot of the previous project) to the renamed file package (for example: myrobotb). Namely, create packages for launching Gazebo worlds and robot simulations. Then run catkin_make and source devel/setup.bash script. Fire up world.launch to verify it works!
roslaunch myrobotb world.launch3. Map setup
Now that the simulated environment is ready, the robot cannot be positioned yet, the robot has no idea of its surroundings! Let's generate a map for it so it knows what to expect in this environment.
Typically, during the development of a robotics project, engineers utilize mapping tools to measure and map the area where the robot will operate. Since we are developing in a simulated environment, the problem is simplified. We can directly use the ROS package: pgm_map_creator ( GitHub - udacity/pgm_map_creator: Create pgm map from Gazebo world file for ROS localization ) to generate a map from the Gazebo world.
PGM map file
The map used by the ROS AMCL package is a pgm file. A pgm file is a grayscale image file. See the Netpbm format wiki page ( en.wikipedia.org ) for more information on pgm files, or more generally pnm files .
By default, the AMCL package treats "dark" pixels as obstacles in the pgm map file, and "light" pixels as free space. The threshold can be set as a parameter, which we will discuss when building the launch file.
Navigate to the ROS package folder and create a maps folder to hold the map files.
$ cd /home/workspace/catkin_ws/src/myrobotb
$ mkdir mapsPGM map creator
installation dependencies
We need libignion-math2-dev and protobuf-compiler to compile the map creator:
sudo apt-get install libignition-math2-dev protobuf-compilerClone the repository
Clone the package pgm_map_creator to the src folder.
cd /home/workspace/catkin_ws/src/
git clone https://github.com/udacity/pgm_map_creator.gitbuild pack:
cd ..
catkin_makeAdd and edit World files
Copy the Gazebo world created in Project1 (for example: Robotroom1) to the world folder
cp Robotroom1 src/pgm_map_creator/world/Robotroom1Inserts the map creator plugin into the map file. Open the map file (Robotroom1) and add the following tags at the end of the file, before the </world> tag:
<plugin filename="libcollision_map_creator.so" name="collision_map_creator"/>Create a PGM map!
Open a terminal and run gzerver with the map file:
gzserver src/pgm_map_creator/world/Robotroom1Open another terminal and start the request_publisher node
roslaunch pgm_map_creator request_publisher.launchWait for the map generated by the plugin, it will be located in the pgm_map_creator's maps folder! Open it up and take a quick look at the map. If the map is clipped, you need to adjust the parameters in launch/request_publisher.launch, namely the x and y values, which define the dimensions of the map:
<arg name="xmin" default="-15" />
<arg name="xmax" default="15" />
<arg name="ymin" default="-15" />
<arg name="ymax" default="15" />
<arg name="scan_height" default="5" />
<arg name="resolution" default="0.01" />Editing a map
A map is an image that can be edited using image manipulation software such as gimp in Linux. If the map is not accurate due to the model, you can directly edit the pgm file!
Adding the map to the package
Now that you have a map file (example: map1227), move it to the created maps folder.
cd /home/workspace/catkin_ws/
cp src/pgm_map_creator/maps/map1227 src/Robotroom1/maps/map1227A yaml file is also required to provide metadata about the map, create a yaml file:
cd src/Robotroom1/src/maps
touch map1227.yamlOpen the yaml file and add the following lines of code:
image: map1227
resolution: 0.01
origin: [-15.0, -15.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0Note that the origin of the map should correspond to the size of the map. For example, the default map size is 30*30, so the origin would be [-15, -15, 0], which is half the size of the map.
The AMCL package
Adaptive Monte Carlo Localization (AMCL) dynamically adjusts the number of particles over a period of time as the robot navigates the map. This adaptive process offers significant computational advantages over MCL.
The ROS AMCL package (http://wiki.ros.org/amcl) implements this variant, integrating this package with the robot to localize it in the provided map.
ROS uses startup files to run ROS nodes. Start building the startup file for the AMCL package below!
4. AMCL launch file
First, create a launch file for the AMCL node in the package's launch folder:
$ cd /home/workspace/catkin_ws/src/myrobotb/launch/
$ vi amcl.launch # nano, geditFeel free to use an editor: vi, nano or gedit.
Add the root element <launch> tag to the amcl.launch file:
<launch>
<!-- TODO: Add nodes here -->
</launch>Now, add nodes to this amcl.launch file, a list of nodes is provided below as a progress track. Don't need to do it all now --- check back when all three nodes have been added!
Below is a list of nodes required for the AMCL package to run:
5.AMCL Launch 文件：Map Server Node_files
Map Server Node map server node
The first node is the map_server node ( map_server) - ROS Wiki ) . The map_server node provides map data as a ROS service to other nodes ( such as amcl nodes ) . Here, the map_server node will locate the map created in the map Setup step and send it out as map data.
First, add a path parameter to the map file, so you can easily change the loaded map and avoid typing long paths again:
<arg name="map_file" default="$(find <YOUR PACKAGE NAME>)/maps/<YOUR MAP NAME>.yaml"/>Then, add a map_server node, using the parameters you just created:
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />The amcl.launch file should look like this:
<launch>
<!-- Map Server -->
<arg name="map_file" ... />
<node name="map_server" ... />
</launch>Examples are as follows:
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
</launch>After setting up the map_server node, go to the amcl node.
AMCL Launch Documentation : AMCL Node
The amcl node requires odometry and laser scan data to perform AMCL positioning.
Add amcl node
First, add the amcl node in the amcl.launch file:
<launch>
<!-- Map Server -->
...
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
</node>
</launch>Now that the amcl node is created, more information is needed to locate the robot! For example, sensor readings from the lidar, etc.
Remap topicsscan
By default, the amcl package will find scan topics for LiDAR data . In the simulation, the Hokuyo LiDAR sensor is actually published on the <YOUR PACKAGE NAME>/laser/scan topic. We'll use the remap tag to remap the topic name scan to the actual topic name so the amcl package can use it!
Add this remap line to the amcl node in the amcl.launch file:
...
<!-- AMCL Node -->
<node name="amcl" ...>
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>
</node>For more information on remap, check out the ROS Wiki: http://wiki.ros.org/roslaunch/XML/remap
Adding AMCL parameters
The AMCL node also needs a set of parameters to connect the world (map frame) and robot (odom frame).
Add the following parameter tags to the amcl node in the amcl.launch file:
...
<!-- AMCL Node -->
<node name="amcl" ...>
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
</node>From the ROS Wiki (http://wiki.ros.org/amcl), the purpose of the parameters added above can be found:
odom_frame_id(string, default: "odom"): frame for odometryodom_model_type(string, default: "diff" ): Which model to use, "diff", "omni", "diff-corrected" or "omni-corrected"- (string, default: "base_link"): Which frame to use for the base of the robot
global_frame_id(string, default: "map"): The coordinate frame name issued by the positioning system
Remember: the AMCL package "links" the robot (odom frame) to the world (map frame), these parameters are necessary for the amcl package to achieve robot localization within the world.
Optional: To set the "initial position"
, you can use the RViz 2D Pose Estimate function to provide AMCL with a pose estimate as a position, or you can define it in the launch startup file.
Add the following parameters to the AMCL node, the values should correspond to the world.launch file:
<param name="initial_pose_x" value="<YOUR X VALUE>"/>
<param name="initial_pose_y" value="<YOUR Y VALUE>"/>The launch file
The amcl.launch launch file should now look like this:
<launch>
<!-- Map Server -->
<arg name="map_file" ... />
<node name="map_server" ... />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<!-- If you choose to define initial pose here -->
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
</node>
</launch>Examples are as follows:
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<remap from="scan" to="my_robotb/laser/scan"/>
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<!-- If you choose to define initial pose here -->
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
</node>
</launch>Now the robot has a map and amcl package for localization. But how does it navigate to other locations to gather more information about its surroundings? The answer is the ROS Navigation Stack.
6. AMCL Launch file : Move Base Node
Using the move_base package, you can define a navigation target position for the robot on the map, and the robot will navigate to the target position. Note that this step is optional if you choose to use the teleop node to control and position the robot.
The move_base package is a very powerful tool. It utilizes a costmap - each part of the map is divided into occupied areas, such as walls or obstacles, and unoccupied areas. As the robot moves around, the local costmap relative to the global costmap is constantly updated, allowing the package to define a continuous path of robot movement.
What makes this package even more compelling is that it has some built-in corrective behavior or actions. Depending on the situation, such as detecting a specific obstacle, or the robot getting stuck, it will send the robot around the obstacle, or rotate the robot until it finds a clear path ahead.
Adding the ROS move_base node
Let's add the move_base node to the amcl.launch file:
<launch>
<!-- Map Server -->
...
<!-- AMCL Node -->
...
<!-- Move Base -->
<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
</node>
</launch>Remapping the scan topic Again, the scan topic
needs to be remapped to the correct topic.
<remap from="scan" to="<YOUR PACKAGE NAME>/laser/scan"/>Add parameters for move_base node
Similar to the amcl node, the move_base node also has a set of adjustable parameters, which can be specified using the param tag. But when there are a large number of parameters, you can use the rosparam tag to include the configuration file to directly set multiple parameters!
planning
Add the following parameter labels to the move_base node:
<param name="base_global_planner" value="navfn/NavfnROS" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>Load configuration files (Config Files)
Then you need to create/get configuration files and load them into the launch file:
$ cd ..
$ mkdir config
$ cd configDownload the config file from Udacity S3 and add it to the config folder:
wget https://s3-us-west-1.amazonaws.com/udacity-robotics/Resource/where_am_i/config.zip
unzip config.zip
rm config.zipThese profiles have preset parameters defined for the navigating robot if you choose to send the robot navigation goals in the test.
Now that the configuration files are ready, we will include them in the launch file, in the move_base node:
<rosparam file="$(find udacity_bot)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find udacity_bot)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find udacity_bot)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find udacity_bot)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find udacity_bot)/config/base_local_planner_params.yaml" command="load" />After completing the above steps, an example amcl.launch file is as follows:
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
<param name="initial_pose_a" value="-1.57"/>
</node>
<!-- Move Base -->
<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
<remap from="scan" to="my_robotb/laser/scan"/>
<param name="base_global_planner" value="navfn/NavfnROS" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find my_robotb)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>
8. Optional: Teleop Package
If you want to control the robot like you do in the lab to help it orient itself, you need to add a teleop node to the package. Thanks to the ROS community, we can use the ROS -teleop package to send commands to the robot via keyboard or controller.
Clone the ros-teleop package to the src folder:
cd /home/workspace/catkin_ws/src
git clone https://github.com/ros-teleop/teleop_twist_keyboardBuild the package and set the source script:
cd ..
catkin_make
source devel/setup.bashThe teleop script can now be run as described in the README file:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py9. Positioning: Start Launching
Bot, map, localization and navigation nodes all ready, launch and test! First, start the simulation:
$ cd /home/workspace/catkin_ws/
$ roslaunch <YOUR PACKAGE NAME> <YOUR WORLD>.launchRight now:
$ cd /home/workspace/catkin_ws/
$ roslaunch myrobotb Robotroom1.launchIn another terminal, start the amcl launch file:
$ roslaunch <YOUR PACKAGE NAME> amcl.launchRight now:
$ roslaunch myrobotb amcl.launchRviz configuration
As done in the previous section, set up RViz by adding the necessary displays and selecting the desired theme to visualize the robot and the map.
add by display type
In Rviz,
- Select fixed frame odom
- Click the "Add" button and
- Add RobotModel: This will add the robot itself to RViz
- To add a Map, select the first theme/map
topic/map: the second and third themes in the list will show both the global costmap and the local costmap, both of which help to optimize parameters - Add
PoseArrayand Selecttopic/particlecloud: Display a set of arrows around the robot
Each arrow is essentially a particle that defines the pose of the robot created by the positioning package. Your goal is to add/tune parameters to help better position the robot, thus improving the pose array.
Note: You can save the RViz settings above in a configuration file and start RViz with the same configuration each time. This will make the process more efficient! Click file -> save config to save the current configuration.
add by topic
There is also an option to tab by theme when adding elements in RViz. Here, all available themes will be displayed, so you can find what you need faster!
Transform Timeout和Map Update Loop
If you receive warnings about Transform Timeout（transition timeouts ）and Map Update Loop（map update loops ）, you may need to configure the appropriate parameters. That is, the transform_tolerance value of the AMCL node becomes larger, and the update_frequency and publish_frequency values in the configuration file become smaller.
10. Positioning: parameters
The following is to determine and tune parameters for the amcl node in the amcl.launch file to achieve better results.
AMCL parameters
The amcl package ( amcl - ROS Wiki ) has a lot of parameters to choose from. Different parameter sets contribute to different aspects of the algorithm. Broadly speaking, they can be grouped into three categories - overall filtering, lasers, and odometry, parameters to consider or details to start with.
overall filter
min_particlesandmax_particles- As amcl dynamically adjusts its particles on each iteration, it expects a range of particle counts as input. Typically, this range is tuned according to system specifications. Larger ranges and higher maximum values can be computationally prohibitive for low-end systems.initial_pose- For the project, the position should be set to [0,0], feel free to fiddle with the average yaw value.update_min*- amcl relies on incoming laser scans. After receiving the scan, it checks the values of update_min_a and update_min_d and compares with the distance the robot moved. Based on this comparison, it decides whether to update the filter or discard the scanned data. Discarding data can lead to poor localization results, and too frequent filter updates can also cause computational problems for fast-moving robots.
laser
There are two different types of models to consider - likelihood_field and beam. Each model defines how the laser ranging sensor estimates obstacles relative to the robot.
Likelihood-field models are usually computationally more efficient and reliable for the kind of environment in which they are being used. Hence, one can focus on the parameters of a particular model like −
laser_*_rangelaser_max_beamslaser_z_hit和laser_z_rand
Tuning of these parameters must be experimental. While adjusting them, watch the laser scan information in RViz and try to make sure the laser scan matches or aligns with the actual map and how it updates as the robot moves. The better the estimation of the obstacle position, the better the localization result.
odometry
odom_model_type - Because differential drives are being used to move the robot, it is best to use the differential correction type. There are some additional parameters specific to this type - odom_alphas (1 to 4). These parameters define how much noise the robot's movements/motions are expected to generate as it navigates the map.
Note: The odometry information of this project is received directly from Gazebo, which is equivalent to the ground truth (without noise). Therefore, there is no need to tune these parameters, keep the default values. But feel free to experiment with some values and see if you notice any changes.
Important: The above set of parameters should help get started, but are not the only ones that can improve results. Read the documentation, determine which parameters can help improve localization results, and experiment with them. The remaining parameters and corresponding documentation can be found on the amcl page of the ROS wiki ( amcl - ROS Wiki ) . Identifying and tuning all of these parameters takes time and effort, and based on the information and resources provided, it helps to tackle the problem head-on!
After completing the above steps, an example amcl.launch file is as follows:
<launch>
<!-- Map Server -->
<arg name="map_file" default="$(find my_robotb)/maps/map1227.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL Node -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">
<param name="odom_frame_id" value="odom"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="base_frame_id" value="robot_footprint"/>
<param name="global_frame_id" value="map"/>
<param name="initial_pose_x" value="0"/>
<param name="initial_pose_y" value="0"/>
<param name="initial_pose_a" value="-1.57"/>
<!-- Tune parameters-->
<param name="transform_tolerance" value="0.2"/>
<param name="min_particles" value="100"/>
<param name="max_particles" value="3000"/>
<param name="update_min_a" value="pi/6.0"/>
<param name="update_min_d" value="0.1"/>
<param name="laser_min_range" value="0.1"/>
<param name="laser_max_range" value="10"/>
<param name="laser_max_beams" value="30"/>
<param name="laser_z_hit" value="0.95"/>
<param name="laser_z_rand" value="0.05"/>
<param name="odom_alpha1" value="0.02"/>
<param name="odom_alpha2" value="0.02"/>
<param name="odom_alpha3" value="0.02"/>
<param name="odom_alpha4" value="0.02"/>
<param name="odom_alpha5" value="0.02"/>
</node>
<!-- Move Base -->
<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen">
<remap from="scan" to="my_robotb/laser/scan"/>
<param name="base_global_planner" value="navfn/NavfnROS" />
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find my_robotb)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find my_robotb)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find my_robotb)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>
11. Positioning: Test
Now, test the performance of the AMCL package! There are two options to control the robot when positioning:
-
Send navigation goals via RViz )
navigation goal（ teleopSend movement commands via packets
Navigate the robot, observe its performance and tune parameters for AMCL!
Option 1: Send 2D navigation target
The first option is to send a 2D navigation target from RViz. move_base will attempt to navigate the robot based on positioning. On the basis of new observations and range measurements, the robot is further positioned.
Click the 2D Nav Goal button in the toolbar , then click and drag on the map to send the goal to the robot. It will start moving and go through the positioning process. If you want to give the amcl node a nudging motion, use 2D Pose Estimate to give the robot an initial position estimate on the map.
Option 2: Using a teleop Node
If you set up the robot in the Optional: Teleop Package ( Optional teleop Package section), you can also use a teleop node to control the robot and observe its own positioning in the environment.
Open another terminal and start the teleop script:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Now you can control your robot with keyboard commands.
