BFMC Localization ROS 2 Workspace

This repository contains the ROS 2 Localization stack for UniPD DriveOps. It runs in its own Jazzy container and consumes ROS topics only.

Quick hardware-free simulator test

The simulator can mimic the hardware topics consumed by this stack. Start it from the simulator workspace:

cd ../Simulator_UniPD-DriveOps
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install
source install/setup.bash
ros2 launch sim_pkg map_with_car.launch camera_view:=false

This supplies encoder, GPS JSON, simulated IMU, and camera topics without any physical hardware. GPS/tag measurements use a randomized 0.9–1.1 second artificial delay by default, matching the approximately 1-second real delay.

Camera ownership is in Brain_UniPD-DriveOps; NVIDIA Isaac ROS Visual SLAM and the Isaac odometry adapter are in the separate Isaac_Visual_SLAM_UniPD-DriveOps repo. The complete three-container workflow is documented in ../Containers_UniPD_DriveOps/README.md.

Packages in this repository

Package	Responsibility
bfmc_car_description	base_link, imu_link, and GPS-tag TF frames
bfmc_state_odometry	Encoder/Ackermann odometry and car-IMU republishing
bfmc_odometry_fusion	Local EKF: Ackermann odometry + car IMU + external visual odometry
bfmc_gps_position	GPS/UWB tag pose conversion and quality-based covariance
bfmc_map_matching	BEV/RGB map matching from the Brain OAK RGB topic
bfmc_global_localization	Global EKF, sign correction, position and node publishing
automobile_imu	Optional BNO055 topic producer; not launched by the Localization master

The Isaac-specific bfmc_isaac_visual_odom republisher now belongs to Isaac_Visual_SLAM_UniPD-DriveOps. Localization consumes its /visual_odom_planar output as an external topic.

Main input topics

Topic	Type	Producer
/automobile/encoder/speed	std_msgs/Float32	Vehicle firmware/bridge
/automobile/encoder/distance	std_msgs/Float32	Vehicle firmware/bridge
/automobile/command/steer	std_msgs/Float32 degrees	Brain hardware adapter
/automobile/command	std_msgs/String JSON steerAngle in degrees	Brain simulator adapter
/automobile/imu/data	sensor_msgs/Imu	Car IMU producer
/automobile/localisation	std_msgs/String JSON (x, y, z, quality)	GPS/UWB producer or simulator mapping
/oak/rgb/image_raw	sensor_msgs/Image	Brain OAK camera
/oak/rgb/camera_info	sensor_msgs/CameraInfo	Brain OAK camera
/visual_odom_planar	nav_msgs/Odometry	Isaac_Visual_SLAM_UniPD-DriveOps adapter
/traffic/detection	std_msgs/String	Brain traffic-sign detection

Localization does not launch a camera, open an OAK device, open the IMU bus, or launch Isaac ROS. It subscribes to the ROS topics above.

Ackermann-aided local odometry

The localization-side encoder node uses the same kinematic model as Brain:

yaw_rate = encoder_speed * tan(commanded_steering) / wheelbase
wheelbase = 0.26 m
steering limit = +/-28 degrees

Brain's simulator adapter publishes steering as steerAngle in the JSON /automobile/command topic. Its hardware adapter publishes the same positive-left angle as Float32 on /automobile/command/steer. The node listens to both, so no simulator-only remapping is required. This intentionally assumes the steering servo reaches the commanded angle.

/localization/encoder_odom contains forward encoder velocity and the Ackermann-predicted yaw rate. Its integrated pose is diagnostic only and is not fused. The local EKF instead fuses:

• encoder forward velocity and Ackermann yaw rate;
• relative heading and measured yaw rate from /car/imu/data;
• position and heading from /visual_odom_planar, when available.

The first valid IMU quaternion establishes zero heading in the local odom frame. Absolute map position still comes from GPS, map matching, and sign observations in the global EKF. Before the first steering command, the model yaw-rate covariance is deliberately very large, so a default zero angle cannot incorrectly force straight-line motion.

Non-zero IMU covariance supplied by hardware is preserved. If a simulator bridge supplies an all-zero covariance array, the car-IMU republisher applies the documented BNO055 covariance profile so simulated heading is not treated as perfect.

Model covariance is propagated from encoder-speed, steering-angle, and wheelbase uncertainty, plus a base allowance for servo response and tyre slip. The calibration and uncertainty parameters are in src/bfmc_state_odometry/launch/state_odometry.launch.py.

Hardware-free simulator testing

The simulator repository contains a separate sim2real_mapping package. It converts simulator sensor topics into the same topics used by this stack, so the localization nodes can be tested without vehicle hardware:

/encoder_odom                         -> /automobile/encoder/speed
/encoder_odom                         -> /automobile/encoder/distance
/automobile/localisation_raw          -> /automobile/localisation
Gazebo chassis IMU                    -> /automobile/imu/data
simulated OAK-D                       -> /oak/rgb/image_raw and camera_info

Start the simulator and mapping layer in one terminal:

cd ../Simulator_UniPD-DriveOps
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install
source install/setup.bash
ros2 launch sim_pkg map_with_car.launch camera_view:=false

In a second terminal, build the localization image once if it does not exist:

cd ~/BFMC_2027/Localization_UniPD-DriveOps
./docker/build.sh

Then start the localization stack while the simulator remains running:

./docker/run.sh --normal-start

This uses the restored test-1 GPS quality covariance profile: 2 cm at quality 100, increasing toward 15 cm at quality 0. It does not add GPS noise.

After localization is running, Brain can be started from a third terminal:

cd ~/BFMC_2027/Brain_UniPD-DriveOps
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install
source ~/BFMC_2027/Simulator_UniPD-DriveOps/install/setup.bash
source install/setup.bash
ros2 launch brain_bringup simulation.launch.py

For the isolated checkpoint/path-following test used by the rosbag validation, run the dedicated launch file instead:

ros2 launch brain_bringup checkpoint_test.launch.py

The Brain package does not contain lane_following.launch.py.

Test 2 rerun and image-free rosbag

The clean rerun of Test 1 is recorded as Test 2 at:

~/BFMC_2027/Simulator_UniPD-DriveOps/bags/checkpoint_localization_test2_20260826_0957

The recorder includes GPS, wheel/IMU odometry, fused poses, commands, checkpoint/node state, diagnostics, and TF only. It deliberately excludes RGB, stereo, depth, camera-info, and lane-mask image topics. Start the recorder before the checkpoint controller, then run:

ros2 launch brain_bringup checkpoint_test.launch.py

Test 2 started at node 472 and reached checkpoint transitions 472, 393, and 306. Before the simulator left the canonical map bounds, global localization error was MAE 0.152 m, RMSE 0.168 m, and P95 0.268 m; exact nearest-node agreement was 78.71% and same/direct-neighbor agreement was 98.87%. The BEV node published no map-match poses in this run because synchronized stereo depth contained zero valid lane points, so the result is GPS plus wheel/IMU fusion only.

See the complete Test 2 report and line chart.

If Docker reports Localization image is missing, run ./docker/build.sh first. The image is reused on later runs.

The mapping layer applies a randomized 0.9–1.1 second GPS/tag delay. Extra GPS noise stays disabled because the simulator GPS plugin already supplies its sensor error:

ros2 launch sim_pkg map_with_car.launch camera_view:=false \
  gps_delay_s:=1.0 gps_delay_jitter_s:=0.1 gps_xy_noise_stddev_m:=0.0

In a second terminal, start this localization container/workspace using its normal run command. Both processes must use the same ROS domain/network. No real encoder, GPS, IMU, camera, or localization hardware is required.

The simulator's raw source topics remain available for evaluating the localization result. Note that /automobile/localisation_raw is the simulated GPS measurement and includes simulator sensor noise; it is not a pure ground-truth pose topic. /visual_odom_planar still requires Isaac Visual SLAM, unless external visual odometry is disabled for a wheel/IMU/GPS-only test.

Verify the simulator-to-hardware topics from a ROS-sourced terminal:

source /opt/ros/jazzy/setup.bash
ros2 topic list | grep -E \
'automobile/(encoder|localisation|imu)|encoder_odom|odometry'

ros2 topic echo /automobile/localisation
ros2 topic echo /automobile/encoder/speed
ros2 topic echo /automobile/imu/data
ros2 topic echo /automobile/command
ros2 topic echo /localization/encoder_odom

Expected localization outputs include:

/odometry/local
/odometry/global
/automobile/gps/base_pose
/automobile/current_coordinate

Monitor the stack with:

./monitor_localization.sh --no-bag --topic-publish

Compare the delayed mapped GPS and localization outputs with:

ros2 topic echo /automobile/localisation
ros2 topic echo /automobile/gps/base_pose
ros2 topic echo /odometry/local
ros2 topic echo /odometry/global

The raw simulator GPS source is available at /automobile/localisation_raw. The mapped /automobile/localisation message is delayed by approximately 1 second (randomized between 0.9 and 1.1 seconds) and has the JSON form {"x": ..., "y": ..., "z": ..., "quality": ...}.

Outputs

/localization/encoder_odom
/car/imu/data
/odometry/local
/automobile/gps/base_pose
/automobile/map_match/base_pose
/automobile/map_match/lane_mask
/odometry/global
/automobile/current_coordinate
/automobile/current_node
/automobile/sign/base_pose
/automobile/total_distance
/automobile/current_speed

The output consumed by Brain is /odometry/global.

EKF sensor selection

Sensor use is edited in code, as requested, not passed as runtime arguments:

src/bfmc_global_localization/launch/bfmc_localization.launch.py

ENABLE_WHEEL_ODOM = True
ENABLE_CAR_IMU = True
ENABLE_EXTERNAL_VISUAL_ODOM = True
ENABLE_GPS = True

The external visual odometry switch controls whether the local EKF subscribes to /visual_odom_planar. The producer itself remains outside this repository.

Build and run

Build the image once:

./docker/build.sh

Run the already-built image:

./docker/run.sh --normal-start
./docker/run.sh --random-start

The start mode selects the path planner policy. It does not change sensor selection. run_localization.sh remains as a compatibility wrapper around docker/run.sh.

--normal-start seeds the map-frame EKF at checkpoint node 472, (12.9154898505, 2.0165327404, yaw=0). Brain, Localization, and the simulator use exact copies of the same UniPD GraphML and clean map PNG from Brain_UniPD-DriveOps. Their shared raster/map extent is 22.612256424738774 m × 15.0 m, so no simulator-only scale, rotation, offset, or axis flip is needed.

TF ownership

map
 └── odom       ← global EKF
      └── base_link  ← local EKF
           ├── imu_link
           └── gps_tag_link

Isaac Visual SLAM publishes odometry messages but its TF broadcasters are disabled in the wrapper so it cannot compete with the Localization EKFs. Camera frame TF must be provided by the camera/robot-description side of the system.