Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add validation for orientation of initial pose #317

Merged
merged 3 commits into from
May 27, 2020

Conversation

ykoga-kyutech
Copy link
Contributor

closes #316

@at-wat
Copy link
Owner

at-wat commented May 27, 2020

[#795] 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_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_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_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_incompatible_rostest.xml: 2 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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

[#795] 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_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_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_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_incompatible_rostest.xml: 2 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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

@codecov-commenter
Copy link

codecov-commenter commented May 27, 2020

Codecov Report

Merging #317 into master will decrease coverage by 0.10%.
The diff coverage is 71.42%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master     #317      +/-   ##
==========================================
- Coverage   94.34%   94.24%   -0.11%     
==========================================
  Files          32       32              
  Lines        1645     1651       +6     
==========================================
+ Hits         1552     1556       +4     
- Misses         93       95       +2     
Impacted Files Coverage Δ
src/mcl_3dl.cpp 91.04% <71.42%> (-0.22%) ⬇️

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 e7484d3...21fefad. Read the comment docs.

src/mcl_3dl.cpp Outdated
msg->pose.pose.orientation.w * msg->pose.pose.orientation.w;
if (std::abs(len2 - 1.0) > 0.1)
{
ROS_ERROR("Invalid pose is set. The orientation must be unit quaternion.");
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what about

Suggested change
ROS_ERROR("Invalid pose is set. The orientation must be unit quaternion.");
ROS_ERROR("Discarded invalid initialpose. The orientation must be unit quaternion.");

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

@ykoga-kyutech ykoga-kyutech requested a review from at-wat May 27, 2020 02:07
@ykoga-kyutech
Copy link
Contributor Author

@at-wat Addressed your comment. PTAL.

The CI failed due to rosdep related problem.

@at-wat
Copy link
Owner

at-wat commented May 27, 2020

/bin/sh: 1: rosdep: not found

Official ROS docker image seems broken.

@at-wat
Copy link
Owner

at-wat commented May 27, 2020

ros-infrastructure/rosdep#745
rosdep seems not installed by default and was installed as a dependency of some package.

@at-wat
Copy link
Owner

at-wat commented May 27, 2020

[#803] 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_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_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_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_incompatible_rostest.xml: 2 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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

[#803] 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_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_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_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_incompatible_rostest.xml: 2 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, 0 errors, 2 failures, 0 skipped
build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 2 failures, 0 skipped

[#803] 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_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_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_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_incompatible_rostest.xml: 2 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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

Copy link
Owner

@at-wat at-wat left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

@at-wat at-wat merged commit 66b561e into at-wat:master May 27, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Invalid initial pose hangs the node
3 participants