Support initialization from state estimator#45
Conversation
|
Note Reviews pausedIt looks like this branch is under active development. To avoid overwhelming you with review comments due to an influx of new commits, CodeRabbit has automatically paused this review. You can configure this behavior by changing the Use the following commands to manage reviews:
Use the checkboxes below for quick actions:
📝 Walkthrough🚥 Pre-merge checks | ✅ 3 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (3 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing touches
🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
jlblancoc
force-pushed
the
fix/init-from-gps-imu
branch
from
ce8986c to
208713a
Compare
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@mola-cli-launchs/lidar_odometry_ros2.yaml`:
- Around line 41-43: The params entry uses the environment variable
MOLA_STATE_ESTIMATOR_YAML without any guard, which can produce an empty params
block and obscure runtime failures; update the ROS2 launch path that loads
lidar_odometry_ros2.yaml to validate that MOLA_STATE_ESTIMATOR_YAML is set and
non-empty (or provide an explicit fallback file) and if not set raise a clear,
immediate error message, so the missing-value check prevents loading an empty
params: entry and surfaces a helpful failure at launch time.
In `@ros2-launchs/ros2-lidar-odometry.launch.py`:
- Around line 19-23: The unconditional call to
get_package_share_directory("mola_state_estimation_smoother") (and subsequent
construction of smoother_yaml_path) runs during launch generation and breaks
when the optional smoother package is missing; change this so the lookup is
deferred and only executed when use_state_estimator is true by moving the
get_package_share_directory and os.path.join logic into an OpaqueFunction or a
conditional action invoked at runtime (or inside the launch action that starts
the smoother), e.g., compute smoother_dir and smoother_yaml_path inside the
OpaqueFunction callback or under the same conditional that checks
use_state_estimator so the simple-estimator path can run without the smoother
package installed.
There was a problem hiding this comment.
Actionable comments posted: 1
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
module/src/LidarOdometry_GUI.cpp (1)
618-668: Run clang-format to fix CI failure.CI reports a formatting failure around Line 619 in this block. Please re-run clang-format on module/src/LidarOdometry_GUI.cpp to satisfy the check.
🤖 Fix all issues with AI agents
In `@module/src/LidarOdometry_InitialLocalization.cpp`:
- Around line 226-233: Run the project clang-format style on the source that
contains the covariance check (the block computing pos_sigma_max and
ori_sigma_max_deg using estimatedPose.cov(…, …)) to fix the CI formatting error;
reformat the function/method containing this block
(LidarOdometry_InitialLocalization.cpp's initial localization/covariance check)
so spacing/line breaks match the repo style and re-run CI to verify the
clang-format issue is resolved.
🧹 Nitpick comments (1)
module/src/LidarOdometry_InitialLocalization.cpp (1)
265-265: Track the TODO for auto-activation after convergence.If you want, I can help implement the auto-transition or open a tracking issue.
There was a problem hiding this comment.
Actionable comments posted: 3
🤖 Fix all issues with AI agents
In `@ros2-launchs/ros2-lidar-odometry.launch.py`:
- Around line 227-234: Remove the duplicate DeclareLaunchArgument named
state_estimator_config_yaml_arg and the redundant SetEnvironmentVariable
state_estimator_config_yaml_env_var that appears after the resolver: keep only
the first DeclareLaunchArgument for state_estimator_config_yaml and its
associated env var (if present before the resolver), delete the second
declaration and the later SetEnvironmentVariable that overwrites the
auto-resolved value so the OpaqueFunction resolver can populate the
LaunchConfiguration without being clobbered.
- Around line 205-211: The PythonExpression for use_state_estimator in
use_state_estimator_env_var can raise NameError for CLI values like false;
modify the expression passed to PythonExpression (used in SetEnvironmentVariable
with LaunchConfiguration('use_state_estimator')) to coerce the launch config to
a string, normalize to lowercase, and compare against 'true' (e.g., wrap the
LaunchConfiguration value in quotes and call .lower() before the equality check)
so both "False"/"false"/True variants are handled safely.
- Around line 6-44: In resolve_state_estimator_config, replace the CWD-dependent
relative default path for the simple estimator with a path resolved via
get_package_share_directory (use the appropriate package name that contains
state-estimator-params) and return that via SetEnvironmentVariable; change the
broad except Exception to except PackageNotFoundError as e (import
PackageNotFoundError from ament_index_python.packages) and re-raise the
RuntimeError with exception chaining (raise ... from e); also prefix unused
parameters (args, kwargs) with an underscore (e.g., _args, _kwargs) to mark them
as intentionally unused; keep references to
LaunchConfiguration('use_state_estimator'),
LaunchConfiguration('state_estimator_config_yaml'), SetEnvironmentVariable, and
resolve_state_estimator_config to locate the code.
🧹 Nitpick comments (1)
module/src/LidarOdometry_InitialLocalization.cpp (1)
264-264: TODO left in core flow.
If you want, I can draft the auto-transition logic or open a tracking issue.
jlblancoc
force-pushed
the
fix/init-from-gps-imu
branch
from
07ab801 to
b27d6bb
Compare
There was a problem hiding this comment.
Actionable comments posted: 0
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (2)
test/CMakeLists.txt (2)
43-48: Duplicate library in target_link_libraries.
mola::mola_state_estimation_simpleappears twice (lines 45 and 47). While CMake handles duplicates gracefully, this appears to be a copy-paste error.🔧 Proposed fix
target_link_libraries(test_lidar_odometry_kitti mrpt::obs mola::mola_state_estimation_simple mola::mola_lidar_odometry - mola::mola_state_estimation_simple )
67-72: Duplicate library in target_link_libraries.Same duplicate
mola::mola_state_estimation_simpleentry (lines 69 and 71).🔧 Proposed fix
target_link_libraries(test_lidar_odometry_mulran mrpt::obs mola::mola_state_estimation_simple mola::mola_lidar_odometry - mola::mola_state_estimation_simple )
🧹 Nitpick comments (2)
docs/mola_lo_ros_node.rst (1)
162-164: Missing descriptions for launch arguments.Lines 163 and 202 have "no description given" placeholders. Consider adding meaningful descriptions for
lidar_scan_validity_minimum_point_countandmola_tf_base_linkto help users understand their purpose.Also applies to: 201-203
ros2-launchs/ros2-lidar-odometry.launch.py (1)
13-44: Improve exception handling and unused parameter naming.The
resolve_state_estimator_configfunction has minor issues flagged by static analysis:
- Unused parameters:
argsandkwargscould be prefixed with_to indicate intentional non-use- Blind exception catch (line 34): Catching bare
Exceptionis overly broad- Missing exception chaining (lines 36-39): The RuntimeError should chain the original exception
♻️ Proposed fix
-def resolve_state_estimator_config(context, *args, **kwargs): +def resolve_state_estimator_config(context, *_args, **_kwargs): """ Runtime logic to resolve the YAML path. This prevents the launch file from crashing if the optional smoother package is missing. """ ... try: smoother_dir = get_package_share_directory( "mola_state_estimation_smoother") yaml_path = os.path.join( smoother_dir, "params", "state-estimation-smoother.yaml") - except Exception: + except Exception as err: # Package missing: We only throw an error if the user explicitly requested the smoother raise RuntimeError( "\n\n[ERROR] 'use_state_estimator' is True, but the package 'mola_state_estimation_smoother' " "was not found. Please install it or set 'use_state_estimator:=False'.\n" - ) + ) from err
jlblancoc
force-pushed
the
fix/init-from-gps-imu
branch
from
b27d6bb to
65464c4
Compare
jlblancoc
force-pushed
the
fix/init-from-gps-imu
branch
from
65464c4 to
4547cb1
Compare
jlblancoc
force-pushed
the
fix/init-from-gps-imu
branch
from
4547cb1 to
9fb9a56
Compare
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@module/src/LidarOdometry_InitialLocalization.cpp`:
- Around line 219-224: The code currently sets converged=false when
MOLA_VERSION_CHECK(2,5,0) is false, which silently disables FromStateEstimator
and leads to a confusing timeout; instead, add a version check during
initialization when the FromStateEstimator option is selected (e.g., in the
initialization logic that configures the estimator in
LidarOdometry_Initialize.cpp) and fail fast by throwing a clear exception or
returning an initialization error indicating that mola_kernel >= 2.5.0 is
required for FromStateEstimator; keep the runtime call to
state_.navstate_fuse->has_converged_localization(estimatedPose) unchanged for
supported versions but do not allow execution to reach the timed convergence
loop if the API is unavailable.
- Around line 196-216: The code reads state_.external_georef without holding
state_mtx_, causing a data race with onExternalMapUpdate; modify
handleInitialLocalizationStateEstimation to acquire state_mtx_ (or take a
guarded snapshot) before accessing state_.external_georef and
state_.local_map->georeferencing: lock state_mtx_, copy the optional(s) or
relevant fields into local variables, then unlock and use those locals for the
subsequent hasGeoRef check and possible apply-to-local-map logic (ensure you
still call state_.mark_local_map_georef_as_updated() under the same lock or
after safely updating state_).
| // Check if state estimator has converged | ||
| mrpt::poses::CPose3DPDFGaussian estimatedPose; | ||
|
|
||
| // First, ensure we have geo-referencing | ||
| // (either from loaded map or from state estimator) | ||
| bool hasGeoRef = | ||
| state_.local_map->georeferencing.has_value() || state_.external_georef.has_value(); | ||
|
|
||
| if (!hasGeoRef) { | ||
| MRPT_LOG_THROTTLE_INFO( | ||
| 5.0, "Waiting for geo-referencing (from loaded map or state estimator)..."); | ||
| return; // Not ready yet | ||
| } | ||
|
|
||
| // Apply external georef to local map if needed | ||
| if (!state_.local_map->georeferencing.has_value() && state_.external_georef.has_value()) { | ||
| state_.local_map->georeferencing.emplace(); | ||
| state_.local_map->georeferencing->geo_coord = state_.external_georef->geo_coord; | ||
| state_.local_map->georeferencing->T_enu_to_map = state_.external_georef->T_enu_to_map; | ||
| state_.mark_local_map_georef_as_updated(); | ||
| } |
There was a problem hiding this comment.
Data race on state_.external_georef — read without holding state_mtx_.
onExternalMapUpdate writes state_.external_georef under state_mtx_ (line 141–144), but handleInitialLocalizationStateEstimation reads it here without the lock. Since onExternalMapUpdate runs on the state-estimator callback thread while this runs on the lidar worker thread, this is a data race on std::optional.
Acquire the lock (or a snapshot) before reading:
Proposed fix
+ // Snapshot external georef under lock
+ std::optional<mola::Georeferencing> externalGeoref;
+ {
+ auto lck = mrpt::lockHelper(state_mtx_);
+ externalGeoref = state_.external_georef;
+ }
+
// First, ensure we have geo-referencing
// (either from loaded map or from state estimator)
bool hasGeoRef =
- state_.local_map->georeferencing.has_value() || state_.external_georef.has_value();
+ state_.local_map->georeferencing.has_value() || externalGeoref.has_value();
if (!hasGeoRef) {
MRPT_LOG_THROTTLE_INFO(
@@
// Apply external georef to local map if needed
- if (!state_.local_map->georeferencing.has_value() && state_.external_georef.has_value()) {
+ if (!state_.local_map->georeferencing.has_value() && externalGeoref.has_value()) {
state_.local_map->georeferencing.emplace();
- state_.local_map->georeferencing->geo_coord = state_.external_georef->geo_coord;
- state_.local_map->georeferencing->T_enu_to_map = state_.external_georef->T_enu_to_map;
+ state_.local_map->georeferencing->geo_coord = externalGeoref->geo_coord;
+ state_.local_map->georeferencing->T_enu_to_map = externalGeoref->T_enu_to_map;
state_.mark_local_map_georef_as_updated();
}🤖 Prompt for AI Agents
In `@module/src/LidarOdometry_InitialLocalization.cpp` around lines 196 - 216, The
code reads state_.external_georef without holding state_mtx_, causing a data
race with onExternalMapUpdate; modify handleInitialLocalizationStateEstimation
to acquire state_mtx_ (or take a guarded snapshot) before accessing
state_.external_georef and state_.local_map->georeferencing: lock state_mtx_,
copy the optional(s) or relevant fields into local variables, then unlock and
use those locals for the subsequent hasGeoRef check and possible
apply-to-local-map logic (ensure you still call
state_.mark_local_map_georef_as_updated() under the same lock or after safely
updating state_).
| #if MOLA_VERSION_CHECK(2, 5, 0) | ||
| bool converged = state_.navstate_fuse->has_converged_localization(estimatedPose); | ||
| #else | ||
| bool converged = false; | ||
| MRPT_LOG_THROTTLE_WARN(5.0, "Initialization from state estimation requires mola_kernel >= 2.5.0"); | ||
| #endif |
There was a problem hiding this comment.
Version-gated fallback silently disables FromStateEstimator on older mola_kernel.
When MOLA_VERSION_CHECK(2, 5, 0) is false, converged is hardcoded to false and the user gets only a throttled warning. Combined with the timeout, this will eventually throw an exception after from_state_estimator_timeout seconds with a confusing "convergence timeout" message rather than a clear "unsupported version" error. Consider failing fast at initialization time (in LidarOdometry_Initialize.cpp) when FromStateEstimator is selected but the API isn't available, rather than silently spinning until timeout.
🧰 Tools
🪛 Cppcheck (2.19.0)
[error] 219-219: failed to evaluate #if condition, undefined function-like macro invocation
(syntaxError)
🤖 Prompt for AI Agents
In `@module/src/LidarOdometry_InitialLocalization.cpp` around lines 219 - 224, The
code currently sets converged=false when MOLA_VERSION_CHECK(2,5,0) is false,
which silently disables FromStateEstimator and leads to a confusing timeout;
instead, add a version check during initialization when the FromStateEstimator
option is selected (e.g., in the initialization logic that configures the
estimator in LidarOdometry_Initialize.cpp) and fail fast by throwing a clear
exception or returning an initialization error indicating that mola_kernel >=
2.5.0 is required for FromStateEstimator; keep the runtime call to
state_.navstate_fuse->has_converged_localization(estimatedPose) unchanged for
supported versions but do not allow execution to reach the timed convergence
loop if the API is unavailable.
1 participant
Summary by CodeRabbit
New Features
Bug Fixes
Documentation