🗺️ Building Pose Maps and Road Network Annotation

1. Course Content

Before road network planning and navigation, robots need two foundational components:

1. High-precision spatial map and pose graph: Use LiDAR and Cartographer SLAM to scan the environment, generating a 2D grid map (map.yaml / map.pgm) and a pose graph (map.pbstream) for fast relocalization;

2. Topological road network description file (map.geojson): On the completed map, use the visualization tool Route Tool to mark road points (Node) and connected road segments (Edge), set one-way or two-way traffic rules, and save.

2. Phase 1: Building Grid Map and Pose Map

2.1 Check ROS 2 Environment Configuration on Master and Slave Machines

Click the app on the desktop to configure robot parameters

Mainly configure two parts: ROS 2 communication middleware and Domain ID. Other configurations will be set up when needed later.

Click Save and Sync Configuration. Exit after configuration is complete.

2.2 Start Cartographer Mapping

This phase requires opening 3 terminals for collaborative mapping:

Terminal 1: Start Robot Chassis and Cartographer Mapping Node (Robot Side)

xxxxxxxxxx
ros2 launch yahboomcar_nav map_cartographer_launch.py

Terminal 2: Start RViz Mapping Visualization (RDK board and Raspberry Pi board recommend using the virtual machine side)

xxxxxxxxxx
ros2 launch yahboomcar_nav display_map_launch.py

Terminal 3: Start Keyboard Control Node

xxxxxxxxxx
ros2 run yahboomcar_ctrl yahboom_keyboard

2.3 Save Grid Map and Pose Map

After mapping is complete, execute the save commands in sequence in the terminal:

1. Save 2D Grid Map (map.yaml + map.pgm)

xxxxxxxxxx
# Save map to ~/map
ros2 launch yahboomcar_nav save_map_launch.py map_save_dir:=/home/jetson/map map_name:=map
# Raspberry Pi
ros2 launch yahboomcar_nav save_map_launch.py map_save_dir:=/root/map map_name:=map
# RDK board
ros2 launch yahboomcar_nav save_map_launch.py map_save_dir:=/home/sunrise/map map_name:=map

When the terminal prints Map saved successfully, the 2D map has been saved successfully:

2. Save Cartographer Pose Map (map.pbstream)

Through ROS 2 service call, write the current trajectory map and submap constraints to the .pbstream file:

xxxxxxxxxx
# RDK X5 system (sunrise user)
ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '/home/sunrise/map/map.pbstream'}"

# If master machine is jetson user
ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '/home/jetson/map/map.pbstream'}"

# pi5 user
ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '/root/map/map.pbstream'}"

Map files are saved in ~/map or the specified directory by default, containing three core assets: map.yaml, map.pgm, and map.pbstream:

3. Phase 2: Road Network Annotation (Route Annotation)

3.1 Start Global Localization and Annotation Tools

When annotating the road network, the system needs to know the robot's precise coordinates in the global map in real time. Therefore, first start the chassis driver and navigation localization node.

Terminal 1: Start Robot Chassis and LiDAR Driver (Robot Side)

xxxxxxxxxx
ros2 launch yahboomcar_nav laser_bringup_launch.py

Terminal 2: Start Docker Container Environment

If using the ros2_kilted container environment:

xxxxxxxxxx
# Quick start command
bringup_ros2kilted
# Enter container terminal
exec_ros2kilted

Enter in the terminal to ensure the container and host machine ROS_DOMAIN_ID are consistent:

xxxxxxxxxx
vi ~/.bashrc

Terminal 3: Start Road Network Annotation RViz Tool Panel (Road Network Side)

Run the annotation interface on the board/virtual machine or inside the container: (RDK and Raspberry Pi boards need to start RViz with integrated tools on the virtual machine)

xxxxxxxxxx
# Start annotation tool standalone panel
ros2 launch road_net_route route_tool.launch.py

This demonstrates the Orin board.

Terminal 4: Start Navigation Base to Load Map (Robot Side)

xxxxxxxxxx
# RDK X5 system
ros2 launch yahboomcar_nav navigation_teb_launch.py map:=/home/sunrise/map/map.yaml

# Or Jetson master machine
ros2 launch yahboomcar_nav navigation_teb_launch.py map:=/home/jetson/map/map.yaml

# Or pi5 master machine
ros2 launch yahboomcar_nav navigation_teb_launch.py map:=/root/map/map.yaml

In RViz, use the 2D Pose Estimate button in the toolbar to give the robot an initial pose on the map, so that the LiDAR scan contour precisely aligns with the black and white obstacle edges on the map:

3.2 Interactively Add Road Points (Nodes)

1. Click the Publish Point tool in the RViz top toolbar;

2. Move the mouse to the robot's current center position (or the center of the passage you want to set as a station on the map), and left-click the mouse;

3. The Route Tool panel in the lower left will automatically read and fill in the X/Y coordinates of that point. Click Create to create the road point:

4. Use keyboard control to move the robot to the next location that needs a road point marker, and repeat clicking Publish Point -> Create:

   xxxxxxxxxx
   ros2 run yahboomcar_ctrl yahboom_keyboard
   

   

5. Annotate all target key stations in sequence (such as road points 0, 1, 2, ...):

3.3 Interactively Add Directed Edges (Edges)

The road network is a typical directed topological graph structure. Only when "edges" are connected between points can the robot travel between them.

1. Switch to the Edge page at the top of the Route Tool tab;

   

2. Enter the starting point number in Start Node ID (such as 1), enter the ending point number in End Node ID (such as 2), and click Create:

3. Create reverse edges for two-way traffic lanes:

4. Complete the connections for all passable road segments in the site in sequence:

3.4 Save Road Network Description File (.geojson)

1. Click the Save button in the Route Tool panel;

2. In the pop-up file save dialog, name the file map.geojson (recommended to save in the same ~/map/ directory as the grid map):

Select overwrite.

3. The terminal prints the JSON record indicating successful save:

xxxxxxxxxx
# RDK X5
cd ~/map
scp map.geojson sunrise@<robotIP>:/home/sunrise/map/

4. Loading and Modifying Existing Road Networks

If the site layout changes later and you need to add, delete, or adjust road points and edges, there is no need to rebuild from scratch:

1. Start the Route Tool interface;

2. Click the Load button and select the previously saved map.geojson to open:

3. Click Edit to re-enter coordinates to modify road point locations, or update edge endpoints;

   

4. Click Remove to directly remove misannotated road points or obsolete road segments. After modifications are complete, click Save to overwrite and save.

5. Core Source Code Analysis: GeoJSON Data and RoadNetwork Parser

Source code reference: code_ws/src/road_net_route/road_net_route/roadnet.py

5.1 GeoJSON File Standard Format

The .geojson generated by the annotation tool follows the standard geographic information structure, containing the two basic elements of the road network: nodes (Point) and road edges (LineString):

xxxxxxxxxx
{
  "type": "FeatureCollection",
  "features": [
    {
      "type": "Feature",
      "geometry": { "type": "Point", "coordinates": [1.25, -0.85] },
      "properties": { "id": 1 }
    },
    {
      "type": "Feature",
      "geometry": { 
        "type": "LineString", 
        "coordinates": [[1.25, -0.85], [2.10, -0.85], [3.10, -0.85]] 
      },
      "properties": { "start_id": 1, "end_id": 2, "bidirectional": true }
    }
  ]
}

5.2 Python-side Road Network Graph Loading and Topology Construction Logic

When the system starts, the RoadNetwork class in roadnet.py fully loads this file and converts it into an in-memory data structure that graph algorithms can operate on:

xxxxxxxxxx
# roadnet.py —— Node and edge loading process
def _load(self, roadnet_file: str):
    with open(roadnet_file, 'r') as f:
        data = json.load(f)

    # Phase 1: Extract all Point features to build topology node dictionary
    for feature in data['features']:
        if feature['geometry']['type'] == 'Point':
            node_id = self._parse_node_id(feature['properties'].get("id"))
            if node_id is None:
                continue
            x, y = feature['geometry']['coordinates']
            self.node_coords.append([x, y])
            self.node_ids.append(node_id)
            self.node_coords_by_id[node_id] = (x, y)
            self.adjacency.setdefault(node_id, {})

    # Phase 2: Extract LineString features to build weighted adjacency list and edge geometry polylines
    for feature in data['features']:
        if feature['geometry']['type'] not in ('LineString', 'MultiLineString'):
            continue
        # Parse edge start and end points (supports multiple compatible aliases like start_id/end_id, source/target)
        start_id, end_id = self._resolve_edge_endpoints(feature)
        if start_id is None or end_id is None or start_id == end_id:
            continue

        # Calculate geometric Euclidean distance between two points as the topology cost weight
        weight = self._edge_weight(start_id, end_id)
        self.adjacency.setdefault(start_id, {})[end_id] = weight
        # If it is a bidirectional edge, add reverse adjacency relationship
        self.adjacency.setdefault(end_id, {})[start_id] = weight

        # Save the continuous polyline point set corresponding to this edge for subsequent path interpolation to generate smooth trajectories
        geometry_points = self._edge_geometry_points(feature['geometry'], start_id, end_id)
        if geometry_points:
            self.edge_geometry_by_nodes[(start_id, end_id)] = geometry_points
            self.edge_geometry_by_nodes[(end_id, start_id)] = list(reversed(geometry_points))