search

at-wat / mcl_3dl

A ROS node to perform a probabilistic 3-D/6-DOF localization system for mobile robots with 3-D LIDAR(s). It implements pointcloud based Monte Carlo localization that uses a reference pointcloud as a map.
BSD 3-Clause "New" or "Revised" License
476 stars 117 forks source link

Fix crushing when lidar poses are out of map #351

Closed nhatao closed 3 years ago

nhatao commented 3 years ago

This PR fixes crushing when lidar poses are out of map cloud.

nhatao commented 3 years ago

@at-wat PTAL.

at-wat commented 3 years ago

[#876] FAILED on noetic

Tested on Alpine ROS

[#876] FAILED on kinetic

Test failed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 6 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 12 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 6 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests, 0 errors, 1 failures, 0 skipped build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 125 tests, 0 errors, 1 failures, 0 skipped ```

[#876] FAILED on melodic

Test failed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 6 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 12 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 6 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests, 0 errors, 1 failures, 0 skipped build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 125 tests, 0 errors, 1 failures, 0 skipped ```
at-wat commented 3 years ago

[#877] PASSED on noetic

Tested on Alpine ROS

[#877] PASSED on kinetic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 6 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 12 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 6 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 125 tests, 0 errors, 0 failures, 0 skipped ```

[#877] PASSED on melodic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 6 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 12 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 6 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 125 tests, 0 errors, 0 failures, 0 skipped ```
codecov-io commented 3 years ago

Codecov Report

Merging #351 into master will decrease coverage by 0.12%. The diff coverage is 57.14%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master     #351      +/-   ##
==========================================
- Coverage   92.47%   92.35%   -0.13%     
==========================================
  Files          36       36              
  Lines        1954     1961       +7     
==========================================
+ Hits         1807     1811       +4     
- Misses        147      150       +3
Impacted Files Coverage Δ
include/mcl_3dl/raycasts/raycast_using_dda.h 93.02% <57.14%> (-3.18%) :arrow_down:

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update 83f64f1...a9beedf. Read the comment docs.