Skip to content

Commit 7b3680d

Browse files
jlblancocjlblancoc
authored
Merge pull request #23 from MOLAorg/feat/fuse-ros2-demos
Add ros2 launch demo files to fuse 2 odometries
2 parents f0a3f75 + d421c57 commit 7b3680d

7 files changed

Lines changed: 649 additions & 210 deletions

File tree

‎docs/mola_state_estimators.rst‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,128 @@ Next we show different possible use cases.
4949
gnss_topic_name:=gps1
5050
5151
52+
.. dropdown:: Fusing two ``nav_msgs/Odometry`` sources (e.g. wheel + visual odometry)
53+
:icon: code-review
54+
55+
This demo subscribes to two ``nav_msgs/Odometry`` topics from ROS 2, fuses them
56+
in the sliding-window factor graph smoother alongside an optional IMU, and publishes
57+
the fused result back as ``nav_msgs/Odometry`` + ``/tf``.
58+
59+
Each odometry topic is assigned a distinct ``output_sensor_label``; the smoother
60+
treats them as independent odometry frames and estimates the optimal
61+
``T_map_to_odom_X`` transform for each one.
62+
63+
**Step 1 — Start fake sensor publishers (for testing without a real robot):**
64+
65+
.. code-block:: bash
66+
67+
# Fake wheel odometry: 2% systematic scale drift in X, 50 Hz
68+
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_wheel_odom_publisher.py
69+
70+
# Fake visual/lidar odometry: 3 mm/step lateral drift, 30 Hz
71+
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_visual_odom_publisher.py
72+
73+
# Fake IMU: gravity-aligned accelerometer + gyroscope, 100 Hz
74+
python3 $(ros2 pkg prefix mola_demos)/share/mola_demos/demos/fake_imu_publisher.py
75+
76+
Both fake odometry nodes share the same ground-truth motion (circle at vx=1 m/s,
77+
wz=0.2 rad/s) but have different noise and drift characteristics so that the smoother
78+
can demonstrate visible fusion benefit.
79+
80+
**Step 2 — Launch the smoother:**
81+
82+
.. code-block:: bash
83+
84+
ros2 launch mola_state_estimation_smoother ros2-fuse-two-odometries.launch.py \
85+
odom1_topic:=/wheel_odom \
86+
odom2_topic:=/visual_odom \
87+
imu_topic:=/imu
88+
89+
Or using ``mola-cli`` directly (set topic names via environment variables):
90+
91+
.. code-block:: bash
92+
93+
ODOM1_TOPIC=/wheel_odom \
94+
ODOM2_TOPIC=/visual_odom \
95+
IMU_TOPIC=/imu \
96+
mola-cli $(ros2 pkg prefix mola_state_estimation_smoother)/share/mola_state_estimation_smoother/mola-cli-launchs/state_estimator_ros2.yaml
97+
98+
Key launch arguments:
99+
100+
.. list-table::
101+
:header-rows: 1
102+
:widths: 30 15 55
103+
104+
* - Argument
105+
- Default
106+
- Description
107+
* - ``odom1_topic``
108+
- ``/wheel_odom``
109+
- First ``nav_msgs/Odometry`` topic (e.g. wheel encoders)
110+
* - ``odom2_topic``
111+
- ``/visual_odom``
112+
- Second ``nav_msgs/Odometry`` topic (e.g. visual/LiDAR odometry)
113+
* - ``imu_topic``
114+
- ``/imu``
115+
- IMU topic for gravity alignment
116+
* - ``gnss_topic``
117+
- ``/gps``
118+
- Optional GNSS topic for geo-referencing
119+
* - ``enforce_planar_motion``
120+
- ``False``
121+
- Constrain z=0, pitch=0, roll=0 for ground vehicles
122+
* - ``use_mola_gui``
123+
- ``True``
124+
- Show MolaViz visualization
125+
126+
**Step 3 — Verify fused output:**
127+
128+
.. code-block:: bash
129+
130+
# Fused pose as nav_msgs/Odometry:
131+
ros2 topic echo /state_estimation/pose
132+
133+
# Inspect all published topics:
134+
ros2 topic list | grep state_estimation
135+
136+
# View /tf tree:
137+
ros2 run tf2_tools view_frames
138+
139+
140+
.. dropdown:: LiDAR odometry + wheel odometry fused in the smoother
141+
:icon: code-review
142+
143+
This demo runs ``mola::LidarOdometry`` from a live ``PointCloud2`` topic alongside
144+
an external wheel odometry source from ROS 2. Both are fused by
145+
``StateEstimationSmoother``, and the fused result is published back to ROS 2.
146+
147+
Data flow::
148+
149+
ROS2 /lidar_points --> BridgeROS2 --> LidarOdometry ---+
150+
ROS2 /wheel_odom --> BridgeROS2 ----+ |
151+
ROS2 /imu --> BridgeROS2 ----+--> StateEstimationSmoother
152+
|
153+
advertiseUpdatedLocalization()
154+
|
155+
BridgeROS2 --> /state_estimation/pose
156+
--> /tf (map -> base_link)
157+
158+
.. code-block:: bash
159+
160+
ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
161+
lidar_topic_name:=/ouster/points \
162+
use_state_estimator:=True \
163+
forward_ros_tf_odom_to_mola:=False
164+
165+
To additionally subscribe to a wheel odometry topic, use the ``mola-cli`` YAML directly:
166+
167+
.. code-block:: bash
168+
169+
WHEEL_ODOM_TOPIC=/wheel_odom \
170+
MOLA_LIDAR_TOPIC=/ouster/points \
171+
mola-cli $(ros2 pkg prefix mola_state_estimation_smoother)/share/mola_state_estimation_smoother/mola-cli-launchs/demo_lidar_odom_plus_wheel_odom_fusion.yaml
172+
173+
52174
|
53175
54176
2.2. Launching the state estimator + LO/LIO

‎mola_state_estimation_smoother/mola-cli-launchs/demo_lidar_odom_plus_wheel_odom_fusion.yaml‎

Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
# -----------------------------------------------------------------------------
2+
# DEMO: LiDAR odometry + ROS2 wheel odometry fused in the Smoother
3+
#
4+
# This mola-cli launch file runs:
5+
# 1. mola::BridgeROS2 - subscribes to LiDAR PointCloud2 + wheel Odometry + IMU
6+
# 2. mola::LidarOdometry - computes LiDAR-based odometry from point clouds
7+
# 3. mola::StateEstimationSmoother - fuses LiDAR odom + wheel odom + IMU
8+
# 4. BridgeROS2 publishes fused result back to ROS2 as nav_msgs/Odometry + /tf
9+
#
10+
# Data flow:
11+
# ROS2 /lidar_points --> BridgeROS2 --> LidarOdometry ---+
12+
# ROS2 /wheel_odom --> BridgeROS2 ----+ |
13+
# ROS2 /imu --> BridgeROS2 ----+--> StateEstimationSmoother
14+
# |
15+
# advertiseUpdatedLocalization()
16+
# |
17+
# BridgeROS2 --> ROS2 /state_estimation/pose
18+
# --> ROS2 /tf (map->base_link)
19+
#
20+
# Usage:
21+
# mola-cli demo_lidar_odom_plus_wheel_odom_fusion.yaml
22+
#
23+
# Key environment variables:
24+
# MOLA_LIDAR_TOPIC - LiDAR PointCloud2 topic (default: /ouster/points)
25+
# WHEEL_ODOM_TOPIC - wheel odometry topic (default: /wheel_odom)
26+
# IMU_TOPIC - IMU topic (default: /imu)
27+
# -----------------------------------------------------------------------------
28+
29+
modules:
30+
# =====================
31+
# MolaViz (optional)
32+
# =====================
33+
- name: viz
34+
type: mola::MolaViz
35+
enabled: ${MOLA_WITH_GUI|true}
36+
verbosity_level: INFO
37+
params: ~
38+
39+
# =====================
40+
# LiDAR Odometry Front-End
41+
# =====================
42+
- name: lidar_odom
43+
type: mola::LidarOdometry
44+
verbosity_level: ${MOLA_VERBOSITY_MOLA_LO|INFO}
45+
raw_data_source: "ros2_bridge"
46+
params: "${MOLA_ODOMETRY_PIPELINE_YAML|../../mola_lidar_odometry/pipelines/lidar3d-default.yaml}"
47+
48+
# =====================
49+
# State Estimation Smoother
50+
# =====================
51+
- name: state_estimation
52+
type: mola::mola_state_estimation_smoother::StateEstimationSmoother
53+
verbosity_level: ${MOLA_VERBOSITY_STATE_ESTIMATOR|INFO}
54+
raw_data_source: "ros2_bridge"
55+
56+
execution_rate: ${MOLA_STATE_ESTIMATOR_PUBLISH_RATE|20} # Hz
57+
58+
params:
59+
vehicle_frame_name: "${MOLA_TF_BASE_LINK|base_link}"
60+
reference_frame_name: "${MOLA_TF_MAP|map}"
61+
62+
kinematic_model: "${MOLA_NAVSTATE_KINEMATIC_MODEL|KinematicModel::ConstantVelocity}"
63+
sliding_window_length: ${MOLA_NAVSTATE_SLIDING_WINDOW_SEC|2.5}
64+
min_time_difference_to_create_new_frame: 0.01
65+
max_time_to_use_velocity_model: 0.75
66+
67+
sigma_random_walk_acceleration_linear: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC|1.0}
68+
sigma_random_walk_acceleration_angular: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_ANGACC|10.0}
69+
sigma_integrator_position: 0.1
70+
sigma_integrator_orientation: 1.0
71+
72+
# Accept all odometry labels (wheel_odom will arrive via onNewObservation,
73+
# lidar_odom arrives directly via fuse_pose from LidarOdometry module):
74+
do_process_odometry_labels_re: ".*"
75+
do_process_imu_labels_re: ".*"
76+
do_process_gnss_labels_re: ".*"
77+
78+
estimate_geo_reference: false
79+
additional_isam2_update_steps: 3
80+
81+
# =====================
82+
# ROS2 <-> MOLA Bridge
83+
# =====================
84+
- name: ros2_bridge
85+
type: mola::BridgeROS2
86+
verbosity_level: ${MOLA_VERBOSITY_BRIDGE_ROS2|INFO}
87+
execution_rate: 20 # Hz
88+
89+
gui_preview_sensors:
90+
- raw_sensor_label: ${MOLA_LIDAR_NAME|lidar}
91+
decimation: 1
92+
enabled: ${MOLA_WITH_GUI|true}
93+
win_pos: 5 5 400 400
94+
95+
params:
96+
base_link_frame: "${MOLA_TF_BASE_LINK|base_link}"
97+
base_footprint_frame: ""
98+
odom_frame: "${MOLA_TF_ESTIMATED_ODOMETRY|odom}"
99+
reference_frame: "${MOLA_TF_MAP|map}"
100+
101+
# The smoother publishes fused results; direct mode:
102+
publish_localization_following_rep105: false
103+
104+
# Only forward the smoother's fused output (not the raw lidar_odom):
105+
publish_odometry_msgs_from_slam: true
106+
publish_odometry_msgs_from_slam_source: "state_estimation"
107+
publish_tf_from_slam: true
108+
publish_tf_from_slam_source: "state_estimation"
109+
110+
publish_in_sim_time: ${MOLA_ROS2_PUBLISH_IN_SIM_TIME|false}
111+
period_publish_new_map: 1.0
112+
113+
ros_args: '${ROS_ARGS|"[]"}'
114+
115+
subscribe:
116+
# LiDAR point cloud (processed by LidarOdometry front-end)
117+
- topic: ${MOLA_LIDAR_TOPIC|/ouster/points}
118+
msg_type: ${MOLA_LIDAR_TOPIC_TYPE|PointCloud2}
119+
output_sensor_label: ${MOLA_LIDAR_NAME|lidar}
120+
fixed_sensor_pose: "${LIDAR_POSE_X|0} ${LIDAR_POSE_Y|0} ${LIDAR_POSE_Z|0} ${LIDAR_POSE_YAW|0} ${LIDAR_POSE_PITCH|0} ${LIDAR_POSE_ROLL|0}"
121+
use_fixed_sensor_pose: ${MOLA_USE_FIXED_LIDAR_POSE|false}
122+
123+
# External wheel odometry (directly consumed by smoother via onNewObservation)
124+
- topic: ${WHEEL_ODOM_TOPIC|/wheel_odom}
125+
msg_type: Odometry
126+
output_sensor_label: "wheel_odom"
127+
128+
# IMU for gravity alignment
129+
- topic: ${IMU_TOPIC|/imu}
130+
msg_type: Imu
131+
output_sensor_label: "imu"
132+
fixed_sensor_pose: "${IMU_POSE_X|0} ${IMU_POSE_Y|0} ${IMU_POSE_Z|0} ${IMU_POSE_YAW|0} ${IMU_POSE_PITCH|0} ${IMU_POSE_ROLL|0}"
133+
use_fixed_sensor_pose: ${MOLA_USE_FIXED_IMU_POSE|false}
134+
135+
# Optional GNSS
136+
- topic: ${GNSS_TOPIC|/gps}
137+
msg_type: NavSatFix
138+
output_sensor_label: "gps"
139+
fixed_sensor_pose: "0 0 0 0 0 0"
140+
use_fixed_sensor_pose: ${MOLA_USE_FIXED_GNSS_POSE|false}

0 commit comments

Comments
 (0)