Mapping & Localization

1. Requirements

Before working with this repository, ensure that:

• ROS2 Jazzy is installed
• colcon is installed
• You are using a Linux environment, for example Ubuntu inside UTM

This README assumes ROS2 is installed under:

/opt/ros/jazzy

---

1.1 Source ROS2 in each new terminal

ROS2 is not active automatically when a terminal is opened. Before using colcon or ros2 commands, run:

source /opt/ros/jazzy/setup.bash

This must be done in every new terminal unless you make it automatic.

---

1.2 Optional automatic ROS2 sourcing

If you want ROS2 to always be available:

echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
source ~/.bashrc

You still need to source your workspace separately after building it.

---

2. Setting Up a Local Workspace

All ROS2 development must happen inside a workspace that contains:

• src for packages
• build created by colcon
• install created by colcon
• log created by colcon

Your repositories must go inside src.

---

2.1 Create the workspace structure

mkdir -p ~/mapping_ws/src
cd ~/mapping_ws/src

---

2.2 Clone this repository and ap1_msgs into src

From inside ~/mapping_ws/src:

cd ~/mapping_ws/src

# Mapping and localization repo
git clone git@github.com:WE-Autopilot/mapping-and-localization.git

# Shared messages repo used across AP1
git clone git@github.com:WE-Autopilot/ap1_msgs.git

Resulting structure:

mapping_ws
  src
    mapping_and_localization
      mapping_and_localization_cpp
      mapping_localization_python
    ap1_msgs

All shared custom messages for AP1 should live in ap1_msgs. If you need a new message type, add it to ap1_msgs rather than creating a new message package inside this repository.

---

2.3 Building the workspace

What building does

Running:

colcon build

makes colcon:

• Compile C plus plus code
• Install binaries into install
• Register Python entry points
• Generate environment setup scripts
• Enable ros2 run to find the nodes

You must build when:

• C plus plus code changes
• package.xml changes
• CMakeLists.txt changes
• setup.cfg entry points change

Build the workspace

cd ~/mapping_ws
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install

Symlink mode is recommended for development.

---

2.4 Source the workspace after building

To use your nodes:

source ~/mapping_ws/install/setup.bash

You must source the workspace:

• In new terminals
• After every colcon build
• Before running ros2 commands

Python code works as long as you have sourced the workspace once in that terminal. C plus plus requires sourcing after each rebuild.

---

2.5 Optional automatic workspace sourcing

If you use this workspace every day:

echo "source ~/mapping_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

Make sure the order in your ~/.bashrc is:

source /opt/ros/jazzy/setup.bash
source ~/mapping_ws/install/setup.bash

---

3. Repository Structure

mapping_and_localization
  mapping_and_localization_cpp
    CMakeLists.txt
    package.xml
    src
      <cpp_nodes>.cpp

  mapping_localization_python
    package.xml
    setup.cfg
    setup.py
    mapping_localization_python
      <python_nodes>.py

  .gitignore
  README.md

Only the two folders with package.xml are ROS2 packages inside this repository.

The ap1_msgs package is a separate repository at:

mapping_ws
  src
    ap1_msgs

It contains shared message definitions that can be used by any AP1 package. Add new messages there when needed.

---

4. Working With the Two Packages

---

4.1 C plus plus Package: mapping_and_localization_cpp

Adding a node

In CMakeLists.txt:

add_executable(localization_node src/localization_node.cpp)

ament_target_dependencies(localization_node
  rclcpp
)
install(TARGETS
  localization_node
  DESTINATION lib/${PROJECT_NAME}
)

Adding dependencies

In package.xml:

<depend>rclcpp</depend>
<depend>std_msgs</depend>

If your C plus plus node uses messages from ap1_msgs (e.g., #include "ap1_msgs/msg/..."), add:

<depend>ap1_msgs</depend>

In CMakeLists.txt:

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(ap1_msgs REQUIRED)  # Only if you use ap1_msgs

ament_target_dependencies(localization_node
  rclcpp
  std_msgs
  ap1_msgs        # Only if you use ap1_msgs
)

---

4.2 Python Package: mapping_localization_python

Adding a node

In setup.cfg:

[options.entry_points]
console_scripts =
  pose_node = mapping_localization_python.pose_node:main

Dependencies

In package.xml:

<depend>rclpy</depend>
<depend>std_msgs</depend>

If your Python node uses messages from ap1_msgs, also add:

<depend>ap1_msgs</depend>

Python files live inside:

mapping_localization_python/mapping_localization_python/

---

5. Running Nodes

You must source the workspace first:

source ~/mapping_ws/install/setup.bash

Run a C plus plus node:

ros2 run mapping_and_localization_cpp localization_node

Run a Python node:

ros2 run mapping_localization_python pose_node

---

6. Development Workflow and Rebuild Rules

---

6.1 When to Rebuild

What changed	Need colcon build --symlink-install	Reason
Python file (.py)	No	Symlink points to source file
Launch, YAML, configuration	No	Symlinks update instantly
C plus plus file (.cpp, .hpp)	Yes	Must recompile
package.xml	Yes	Metadata changed
CMakeLists.txt	Yes	Build system instructions changed
New Python node in setup.cfg	Yes	Must regenerate entry point
Editing setup.cfg text only	Usually no	Not used at runtime
Adding a new package	Yes	Build system must detect it

---

6.2 Recommended Developer Workflow

Python workflow

1. Edit the Python file
2. Source the workspace once in any terminal
3. Run the node

No rebuild required unless adding new entry points.

C plus plus workflow

1. Edit the C plus plus file

2. Run:

   colcon build --symlink-install

3. Source again:

   source install/setup.bash

4. Run the node

After modifying package.xml, CMakeLists.txt, or setup.cfg

• Rebuild with symlink
• Source workspace
• Run the node

---

7. Notes for Developers

• Only folders with package.xml are ROS2 packages
• build, install, log and .vscode should not be committed
• Always run colcon build from the workspace root, never inside a package
• Never run CMake manually
• Shared messages should live in ap1_msgs. If you need a new message type for mapping or localization, add it there and then depend on ap1_msgs in your package.

---

8. Bare-Minimum Perception to P&C Pipeline

This repository now includes a minimum mapping/localization pipeline in the mapping_localization_python package.

It is designed to:

• Consume perception entities and lane boundaries
• Publish immediately on each incoming perception callback
• Fill missing lane boundaries by holding the last valid left/right boundary for a short timeout
• Generate deterministic AP1 sample perception data when no Perception sample exists yet
• Optionally launch Kitware lidar_slam_node and bridge /slam_odom to AP1 localization topics
• Publish an accumulated distance estimate from SLAM odometry
• Merge Kitware keypoint maps into /slam_map and expose /save_map
• Expose a point-registry service for planning and mapping

8.1 Nodes

perception_pipeline_node

Inputs:

• /ap1/perception/entities (ap1_msgs/msg/EntityStateArray)
• /ap1/perception/lanes (ap1_msgs/msg/LaneBoundaries)

Outputs:

• /ap1/mapping/entities (ap1_msgs/msg/EntityStateArray)
• /ap1/mapping/lanes (ap1_msgs/msg/LaneBoundaries)

Key params:

• lane_timeout_sec (default 1.0)
• min_lane_points (default 2)

slam_bridge_node

Input:

• /slam_odom (nav_msgs/msg/Odometry)

Outputs:

• /ap1/localization/odom (nav_msgs/msg/Odometry)
• /ap1/localization/pose (geometry_msgs/msg/PoseStamped)
• /ap1/localization/slam_pose (geometry_msgs/msg/PoseWithCovarianceStamped, frame map)
• /ap1/localization/distance (ap1_msgs/msg/FloatStamped, meters travelled)

Key params:

• publish_pose (default true)
• publish_slam_pose (default true)
• publish_distance (default true)
• slam_pose_frame (default map)
• slam_pose_publish_rate_hz (default 20.0, clamped to minimum 10.0)
• slam_pose_frequency_log_interval_sec (default 5.0)
• distance_jump_threshold_m (default 10.0)

slam_map_bridge_node

Inputs:

• /maps/edges (sensor_msgs/msg/PointCloud2)
• /maps/intensity_edges (sensor_msgs/msg/PointCloud2)
• /maps/planes (sensor_msgs/msg/PointCloud2)
• /maps/blobs (sensor_msgs/msg/PointCloud2)

Outputs:

• /slam_map (sensor_msgs/msg/PointCloud2, frame map)
• /slam_map_grid (nav_msgs/msg/OccupancyGrid, optional)

Services:

• /save_map (std_srvs/srv/Empty)

Behavior:

• Merges the latest Kitware keypoint map topics into a single point cloud
• Publishes the merged map at a configurable low rate (default 1.0 Hz)
• Saves a merged ASCII PCD and, when Kitware is available, triggers lidar_slam/save_pc so the same prefix can be reused via maps.initial_maps

Key params:

• map_publish_rate_hz (default 1.0)
• map_frame (default map)
• publish_occupancy_grid (default false)
• save_map_prefix (default ~/slam_maps/slam_map)

synthetic_perception_publisher_node

Outputs:

• /ap1/perception/entities (ap1_msgs/msg/EntityStateArray)
• /ap1/perception/lanes (ap1_msgs/msg/LaneBoundaries)

Behavior:

• Publishes both left and right lane boundaries on every cycle
• Uses 16 waypoints per lane by default
• Adds larger deterministic noise to the tail of each lane because Perception only specified that predictions become unstable near the end

Key params:

• publish_rate_hz (default 10.0)
• waypoint_count (default 16)
• waypoint_spacing_m (default 0.5, soft target of 2 waypoints per meter)
• lane_width_m (default 3.5)
• tail_instability_points (default 4)
• tail_noise_max_m (default 0.9)

stored_point_registry_node

Services:

• /ap1/mapping/point_registry/create (ap1_msgs/srv/CreateStoredPoint)
• /ap1/mapping/point_registry/delete (ap1_msgs/srv/DeleteStoredPoint)
• /ap1/mapping/point_registry/lookup (ap1_msgs/srv/LookupStoredPoint)

Behavior:

• Create stores a geometry_msgs/Point and returns a generated ID
• Delete removes a stored point by ID
• Lookup returns a stored point by ID when present

8.2 Launch

Launch everything:

ros2 launch mapping_localization_python mapping_pipeline.launch.py

Enable Kitware SLAM in the same launch:

ros2 launch mapping_localization_python mapping_pipeline.launch.py use_kitware_slam:=true

Enable synthetic AP1 perception input for pipeline smoke tests:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  use_synthetic_perception:=true

Run synthetic perception and Kitware SLAM together, including /slam_map:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  use_synthetic_perception:=true \
  use_kitware_slam:=true

By default, this launch passes:

mapping_localization_python/config/kitware_slam_params.yaml

to lidar_slam_node (configured for odometry_frame: map and pose output at 20 Hz, keypoint map outputs enabled, and empty initial map prefix). Replace it with your tuned config when available:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  use_kitware_slam:=true \
  kitware_slam_params:=/absolute/path/to/your_slam_params.yaml

Tune SLAM pose output frame and rate:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  use_kitware_slam:=true \
  slam_pose_frame:=map \
  slam_pose_publish_rate_hz:=20.0

Override the default distance topic if needed:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  output_distance_topic:=/ap1/localization/distance

Tune /slam_map output or enable the optional occupancy grid:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  use_kitware_slam:=true \
  slam_map_publish_rate_hz:=0.5 \
  publish_slam_map_grid:=true

Preload a saved Kitware map prefix for localization mode:

ros2 launch mapping_localization_python mapping_pipeline.launch.py \
  use_kitware_slam:=true \
  kitware_initial_maps_path:=/absolute/path/to/slam_map

Save the current map:

ros2 service call /save_map std_srvs/srv/Empty