Skip to content

Commit

Permalink
Update to new state estimation packages
Browse files Browse the repository at this point in the history
jlblancoc committed Dec 26, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 917cc61 commit f997599
Showing 6 changed files with 24 additions and 14 deletions.
3 changes: 0 additions & 3 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -26,7 +26,6 @@ find_package(mrpt-tclap REQUIRED)
find_mola_package(mola_kernel)
find_mola_package(mp2p_icp)
find_mola_package(mp2p_icp_filters)
find_mola_package(mola_navstate_fuse)
find_mola_package(mola_pose_list)

# -----------------------
@@ -42,15 +41,13 @@ mola_add_library(
mola::mola_kernel
mola::mp2p_icp
mola::mp2p_icp_filters
mola::mola_navstate_fuse
mola::mola_pose_list
# PRIVATE_LINK_LIBRARIES
# mrpt::maps
CMAKE_DEPENDENCIES
mola_kernel
mp2p_icp
mp2p_icp_filters
mola_navstate_fuse
mrpt-maps
)

2 changes: 1 addition & 1 deletion docs/mola_lo_pipelines.rst
Original file line number Diff line number Diff line change
@@ -296,7 +296,7 @@ These settings only have effects if launched via :ref:`MOLA-LO GUI applications

Motion model
^^^^^^^^^^^^^^^^^^^^^^
A constant velocity motion model is used by default, provided by the ``mola_navstate_fuse`` module.
A constant velocity motion model is used by default, provided by the ``mola_state_estimation_simple`` module.

- ``MOLA_MAX_TIME_TO_USE_VELOCITY_MODEL`` (Default: 0.75 s): Maximum time between LiDAR frames to use the velocity model. Larger delays will cause using the latest vehicle pose as initial guess.
- ``MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC`` (Default: 1.0 m/s²): Linear acceleration standard deviation.
4 changes: 3 additions & 1 deletion package.xml
Original file line number Diff line number Diff line change
@@ -18,7 +18,6 @@
<depend>mola_common</depend>
<depend>mola_kernel</depend>
<depend>mp2p_icp</depend>
<depend>mola_navstate_fuse</depend>
<depend>mola_pose_list</depend>

<depend>mrpt_libtclap</depend>
@@ -47,6 +46,9 @@
<test_depend>ament_cmake_xmllint</test_depend>
<test_depend>ament_lint_cmake</test_depend>
<test_depend>ament_cmake_gtest</test_depend>

<!-- for C++ unit tests -->
<test_depend>mola_state_estimation_simple</test_depend>

<!-- Needed by the mola-lo-* scripts: -->
<exec_depend>mola_launcher</exec_depend>
17 changes: 14 additions & 3 deletions test/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -36,8 +36,15 @@ ament_add_gtest(test_lidar_odometry_kitti test_lidar_odometry_rawlog.cpp
ENV LO_TEST_GT_TUM=${KITTI00_GT_TUM}
ENV MOLA_PROFILER=false
)
ament_target_dependencies(test_lidar_odometry_kitti mrpt-obs)
target_link_libraries(test_lidar_odometry_kitti mola::mola_lidar_odometry)
ament_target_dependencies(test_lidar_odometry_kitti
mrpt-obs
mola_state_estimation_simple
)
target_link_libraries(test_lidar_odometry_kitti
mola::mola_lidar_odometry
mola::mola_state_estimation_simple
)



# Test: from rosbag2 file, RSLIDAR data with XYZIRT channels
@@ -62,10 +69,14 @@ if(EXISTS "${RSLIDAR_DATASET_FILE}")
MOLA_USE_FIXED_LIDAR_POSE=true # so we don't need /tf topic in test dataset
MOLA_IGNORE_NO_POINT_STAMPS=${IGNORE_POINT_STAMPS} # make it fail if no timestamps are found
)
ament_target_dependencies(test_lidar_odometry_rslidar mrpt-obs)
ament_target_dependencies(test_lidar_odometry_rslidar
mrpt-obs
mola_state_estimation_simple
)
target_link_libraries(test_lidar_odometry_rslidar
mola::mola_lidar_odometry
mola::mola_input_rosbag2
mola::mola_state_estimation_simple
)
else()
message(STATUS "Skipping test for missing dataset: ${RSLIDAR_DATASET_FILE}")
6 changes: 3 additions & 3 deletions test/test_lidar_odometry_rawlog.cpp
Original file line number Diff line number Diff line change
@@ -26,7 +26,7 @@
#include <mola_kernel/interfaces/OfflineDatasetSource.h>
#include <mola_kernel/pretty_print_exception.h>
#include <mola_lidar_odometry/LidarOdometry.h>
#include <mola_navstate_fuse/NavStateFuse.h>
#include <mola_state_estimation_simple/StateEstimationSimple.h>
#include <mola_yaml/yaml_helpers.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/core/get_env.h>
@@ -44,10 +44,10 @@ int main_odometry(
const std::string & rawlogFile, const std::string & gtTrajectory)
{
auto liodom = mola::LidarOdometry::Create();
auto stateEstimator = mola::NavStateFuse::Create();
auto stateEstimator = mola::state_estimation_simple::StateEstimationSimple::Create();

// Put both modules together so LO can find the state estimator:
mola::MinimalModuleContainer moduleContainer = {liodom, stateEstimator};
mola::MinimalModuleContainer moduleContainer = {{liodom, stateEstimator}};

// Initialize LiDAR Odometry:
const auto cfg = mola::load_yaml_file(yamlConfigFile);
6 changes: 3 additions & 3 deletions test/test_lidar_odometry_rosbag2.cpp
Original file line number Diff line number Diff line change
@@ -27,7 +27,7 @@
#include <mola_kernel/interfaces/OfflineDatasetSource.h>
#include <mola_kernel/pretty_print_exception.h>
#include <mola_lidar_odometry/LidarOdometry.h>
#include <mola_navstate_fuse/NavStateFuse.h>
#include <mola_state_estimation_simple/StateEstimationSimple.h>
#include <mola_yaml/yaml_helpers.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/core/get_env.h>
@@ -75,10 +75,10 @@ int main_odometry(
const std::string & rosbag2File, const std::string & lidarTopic, const std::string & gtTrajectory)
{
auto liodom = mola::LidarOdometry::Create();
auto stateEstimator = mola::NavStateFuse::Create();
auto stateEstimator = mola::state_estimation_simple::StateEstimationSimple::Create();

// Put both modules together so LO can find the state estimator:
mola::MinimalModuleContainer moduleContainer = {liodom, stateEstimator};
mola::MinimalModuleContainer moduleContainer = {{liodom, stateEstimator}};

// Initialize LiDAR Odometry:
const auto cfg = mola::load_yaml_file(yamlConfigFile);

0 comments on commit f997599

Please sign in to comment.