Creating a map in RViz

Using launch parameter "slam"

Earlier, in navigation_bot_04, we had to start a a costmap solution separately. To do it, we used a separate Terminal:

                    ros2 launch slam_toolbox online_async_launch.py
                

Then in RViz, we would select Add, and add a map. Now, as bot moved around, lidar based map was being built. Later, we could select to save it from RViz.

In navigation_bot_05, when started with

                    $ ros2 launch navigation_bot_05 main_simulation_launch.py world:=src/worlds/maze.sdf slam:=True
                

... the map is already there.

In other words, depending on the value (True or False) of a slam parameter that we pass to a launch file, we can either create a map using lidar, or load a pre-saved map from file.

Displaying map in RViz

As we drive around our world with lidar on, we are getting a lidar-based map.

We can save it from RViz and then load it.

First, let's run our simulation with slam:=True again.

Here is a difference: our RViz now has a new configuration file, fixed frame is set to map and Map topic is initially included.

However, there are some non-obvious details you should know. If you look in the rviz folder of the navigation_bot_05, you will see some new files: robot_old.rviz and robot_new.rviz. Here is the thing: as we record the map, which means we drive around with lidar on, we need RViz to display an "old" (hence the name) configuration.
In the same time, as we load the map and use navigation, we would do better with a different (new) RViz configuration.

In our previous (navigation_bot_04) project, working with map was rather tedious:

To create map, copy robot_old.rviz to robot.rviz, then:

                    T1:
                    ros2 launch navigation_bot_05 main_simulation_launch.py world:=src/worlds/maze.sdf map:='~/SnowCron/ros_projects/harsh/src/maps/map.yaml' slam:=True
                    T2:
                    ros2 run nav2_map_server map_server _params:=nav2_params.yaml
                    T3:
                    $ ros2 run nav2_map_server map_saver_cli -f ~/map
                

To load that map, copy the map you just saved to maps folder, copy robot_new.rviz to robot.rviz, then: then:

                    T1:
                    ros2 launch navigation_bot_05 main_simulation_launch.py world:=src/worlds/maze.sdf map:='~/SnowCron/ros_projects/harsh/src/maps/map.yaml' slam:=False
                    T2:
                    ros2 run nav2_map_server map_server _params:=nav2_params.yaml
                

In the navigation_bot_05 project, launch files take care of all things that have to be started, so all we need is:

To create map, copy robot_old.rviz to robot.rviz, then:

                    T1:
                    ros2 launch navigation_bot_05 main_simulation_launch.py world:=src/worlds/maze.sdf map:='~/SnowCron/ros_projects/harsh/src/maps/map.yaml' slam:=True
                

To load that map in RViz, copy the map you just saved to maps folder, copy robot_new.rviz to robot.rviz, then: then:

                    T1:
                    ros2 launch navigation_bot_05 main_simulation_launch.py world:=src/worlds/maze.sdf map:='~/SnowCron/ros_projects/harsh/src/maps/map.yaml' slam:=False
                

Few words about robot_new.rviz and robot_old.rviz: there is a persistent error that creates a lot of confusion in ROS2 community: the map server only serves map ONCE, when started. So if your RViz is fully up AFTER it happened, it will receive no map ("no map received" error). What does it mean "if your RViz is fully up"? Well, you have to specify map as a fixed frame, and add a map topic ("by topic - map"). In other words, you have to do it in advance, so you need to have it in RViz configuration file. You can NOT start RViz and then add a map topic "by hand" - that will be too late to "catch" the map.

To handle that, I have opened RViz, set map as a fixed frame, and added a map topic. Then I saved the RViz config file (from RViz menu) as robot_new.rviz. As for robot_old.rviz, it is a config file we had in our early projects, it works well with slam, and should be used to create map (as opposed to loading it at the stage two, where we use robot_new.rviz).

Additional note: if you do not want to deal with two RViz files, you can use the service that can re-publish map:

                    # URL of map resource
                    # Can be an absolute path to a file: file:///path/to/maps/floor1.yaml
                    # Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml
                    string map_url
                    ---
                    # Result code defintions
                    uint8 RESULT_SUCCESS=0
                    uint8 RESULT_MAP_DOES_NOT_EXIST=1
                    uint8 RESULT_INVALID_MAP_DATA=2
                    uint8 RESULT_INVALID_MAP_METADATA=3
                    uint8 RESULT_UNDEFINED_FAILURE=255
                    
                    # Returned map is only valid if result equals RESULT_SUCCESS
                    nav_msgs/OccupancyGrid map
                    uint8 result
                

It is part of navigation2/nav2_msgs/srv/LoadMap.srv, see ros-planning/navigation2. However, it is a bit of an overkill for our project.

Visualizing Costmaps in RViz

The global_costmap, local_costmap and the voxel representation of the detected obstacles can be visualized in RViz. To visualize the global_costmap in RViz, click the add button at the bottom-left part of the RViz window. Go to By topic tab then select the Map under the /global_costmap/costmap topic. The global_costmap should show in the RViz window. The global_costmap shows areas which should be avoided (black) by our robot when it navigates our simulated world in Gazebo.

To visualize the local_costmap in RViz, select the Map under the /local_costmap/costmap topic. Set the color scheme in RViz to costmap to make it appear similar to the image below.

To visualize the voxel representation of the detected object, open a new terminal and execute the following:

                    ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker
                

The line above sets the topic where the the markers will be published to /my_marker. To see the markers in RViz, select Marker under the /my_marker topic, as shown below. Then set the fixed frame in RViz to odom and you should now see the voxels in RViz, which represent the cube and the sphere that we have in the Gazebo world:

Side note: Setting an initial position

First of all, RViz supports "set initial pos" via a button you can add to its UI. However, this option is not very convenient, especially if you want to run something in an automated mode.

Two options that I know of if you do not want to use RVIZ to set the initial pose:
1) the amcl_node supports a set_initial_pose argument, as well as initial_pose.x, .y, .z, and .yaw. You can modify your launch files accordingly so this is set to your known or best guess initial position.
2) (probably the easier method) is to publish a message to the /intialpose topic. That should look something like this from the commandline:

                    ros2 topic pub -1 /initialpose geometry_msgs/PoseWithCovarianceStamped '{ header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, pose: { pose: {position: {x: 0.1, y: 0.0, z: 0.0}, orientation: {w: 0.1}}, } }'
                    ros2 run nav2_map_server map_server _params:=map_server_params.yaml