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

planner_cspace: fix pose duplication on path interpolation #648

Merged
merged 5 commits into from Aug 5, 2022

Conversation

at-wat
Copy link
Owner

@at-wat at-wat commented Aug 5, 2022

Output path might contain duplicated poses due to interpolation logic and quantization error.

@at-wat at-wat self-assigned this Aug 5, 2022
@at-wat

This comment has been minimized.

@at-wat

This comment has been minimized.

@codecov-commenter
Copy link

codecov-commenter commented Aug 5, 2022

Codecov Report

Merging #648 (3f4a3d6) into master (b14f5cf) will not change coverage.
The diff coverage is 100.00%.

@@           Coverage Diff           @@
##           master     #648   +/-   ##
=======================================
  Coverage   90.51%   90.51%           
=======================================
  Files          61       61           
  Lines        4556     4556           
=======================================
  Hits         4124     4124           
  Misses        432      432           
Impacted Files Coverage Δ
planner_cspace/src/path_interpolator.cpp 100.00% <100.00%> (ø)

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

@at-wat at-wat requested a review from nhatao August 5, 2022 03:58
Copy link
Collaborator

@nhatao nhatao left a comment

Choose a reason for hiding this comment

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

LGTM. Minor comments added.

map_info.origin.position.y = -200.5;

CyclicVecFloat<3, 2> m_prev;
for (auto it = out.begin(); it != out.end(); ++it)
Copy link
Collaborator

Choose a reason for hiding this comment

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

const auto is better.

Copy link
Owner Author

Choose a reason for hiding this comment

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

++it requires non-const.
BTW, rewrote as range-based for.

*/

#include <list>
#include <iostream>
Copy link
Collaborator

Choose a reason for hiding this comment

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

<iostream> is not used.

@at-wat
Copy link
Owner Author

at-wat commented Aug 5, 2022

[226] PASSED on melodic

All tests passed
build/test_results/costmap_cspace/gtest-test_costmap_3d.xml: 22 tests
build/test_results/costmap_cspace/gtest-test_pointcloud_accumulator.xml: 4 tests
build/test_results/costmap_cspace/roslint-costmap_cspace.xml: 1 tests
build/test_results/joystick_interrupt/roslint-joystick_interrupt.xml: 1 tests
build/test_results/joystick_interrupt/rostest-test_test_joystick_interrupt_rostest.xml: 1 tests
build/test_results/joystick_interrupt/rosunit-test_joystick_interrupt.xml: 12 tests
build/test_results/map_organizer/roslint-map_organizer.xml: 1 tests
build/test_results/map_organizer/rostest-test_test_map_organizer_rostest.xml: 1 tests
build/test_results/map_organizer/rostest-test_test_pointcloud_to_maps_rostest.xml: 1 tests
build/test_results/map_organizer/rosunit-test_map_organizer.xml: 8 tests
build/test_results/map_organizer/rosunit-test_pointcloud_to_maps.xml: 2 tests
build/test_results/neonavigation_common/roslint-neonavigation_common.xml: 1 tests
build/test_results/neonavigation_common/rostest-test_test_compat_rostest.xml: 1 tests
build/test_results/neonavigation_common/rosunit-test_compat.xml: 6 tests
build/test_results/obj_to_pointcloud/roslint-obj_to_pointcloud.xml: 1 tests
build/test_results/obj_to_pointcloud/rostest-test_test_obj_to_pointcloud_rostest.xml: 1 tests
build/test_results/obj_to_pointcloud/rosunit-test_obj_to_pointcloud.xml: 2 tests
build/test_results/planner_cspace/gtest-test_blockmem_gridmap.xml: 10 tests
build/test_results/planner_cspace/gtest-test_costmap_bbf.xml: 4 tests
build/test_results/planner_cspace/gtest-test_cyclic_vec.xml: 14 tests
build/test_results/planner_cspace/gtest-test_distance_map.xml: 14 tests
build/test_results/planner_cspace/gtest-test_grid_astar.xml: 12 tests
build/test_results/planner_cspace/gtest-test_grid_metric_converter.xml: 4 tests
build/test_results/planner_cspace/gtest-test_motion_cache.xml: 2 tests
build/test_results/planner_cspace/gtest-test_motion_primitive_builder.xml: 2 tests
build/test_results/planner_cspace/gtest-test_path_interpolator.xml: 4 tests
build/test_results/planner_cspace/gtest-test_planner_3d_cost.xml: 2 tests
build/test_results/planner_cspace/roslint-planner_cspace.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__antialias_start_true.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__antialias_start_true__fast_map_update_true.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__with_tolerance_true.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_abort_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_costmap_watchdog_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_debug_outputs_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_boundary_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_compat_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_planner_2dof_serial_joints_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_planner_3d_map_size_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_preempt_rostest.xml: 1 tests
build/test_results/planner_cspace/rosunit-test_abort.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_costmap_watchdog.xml: 4 tests
build/test_results/planner_cspace/rosunit-test_debug_outputs.xml: 6 tests
build/test_results/planner_cspace/rosunit-test_navigate.xml: 6 tests
build/test_results/planner_cspace/rosunit-test_navigate_boundary.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_planner_2dof_serial_joints.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_planner_3d_map_size.xml: 12 tests
build/test_results/planner_cspace/rosunit-test_preempt.xml: 2 tests
build/test_results/safety_limiter/roslint-safety_limiter.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter2_rostest.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter_compat_rostest.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter_rostest.xml: 1 tests
build/test_results/safety_limiter/rosunit-test_safety_limiter.xml: 22 tests
build/test_results/safety_limiter/rosunit-test_safety_limiter2.xml: 2 tests
build/test_results/track_odometry/gtest-test_tf_projection.xml: 2 tests
build/test_results/track_odometry/roslint-track_odometry.xml: 1 tests
build/test_results/track_odometry/rostest-test_test_tf_projection_rostest.xml: 1 tests
build/test_results/track_odometry/rostest-test_test_track_odometry_rostest.xml: 1 tests
build/test_results/track_odometry/rosunit-test_tf_projection_node.xml: 8 tests
build/test_results/track_odometry/rosunit-test_track_odometry.xml: 10 tests
build/test_results/trajectory_tracker/gtest-test_trajectory_tracker_filter.xml: 6 tests
build/test_results/trajectory_tracker/gtest-test_trajectory_tracker_path2d.xml: 8 tests
build/test_results/trajectory_tracker/roslint-trajectory_tracker.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_recorder_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_overshoot_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_with_odom_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-trajectory_tracker_rostest__odom_delay_0.xml: 1 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_recorder.xml: 2 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker.xml: 16 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker_overshoot.xml: 12 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker_with_odom.xml: 4 tests
build/test_results/trajectory_tracker_msgs/gtest-test_path_with_velocity_conversion.xml: 4 tests
build/test_results/trajectory_tracker_msgs/roslint-trajectory_tracker_msgs.xml: 1 tests
Summary: 293 tests, 0 errors, 0 failures, 0 skipped

@at-wat
Copy link
Owner Author

at-wat commented Aug 5, 2022

[226] PASSED on noetic

All tests passed
build/test_results/costmap_cspace/gtest-test_costmap_3d.xml: 22 tests
build/test_results/costmap_cspace/gtest-test_pointcloud_accumulator.xml: 4 tests
build/test_results/costmap_cspace/roslint-costmap_cspace.xml: 1 tests
build/test_results/joystick_interrupt/roslint-joystick_interrupt.xml: 1 tests
build/test_results/joystick_interrupt/rostest-test_test_joystick_interrupt_rostest.xml: 1 tests
build/test_results/joystick_interrupt/rosunit-test_joystick_interrupt.xml: 12 tests
build/test_results/map_organizer/roslint-map_organizer.xml: 1 tests
build/test_results/map_organizer/rostest-test_test_map_organizer_rostest.xml: 1 tests
build/test_results/map_organizer/rostest-test_test_pointcloud_to_maps_rostest.xml: 1 tests
build/test_results/map_organizer/rosunit-test_map_organizer.xml: 8 tests
build/test_results/map_organizer/rosunit-test_pointcloud_to_maps.xml: 2 tests
build/test_results/neonavigation_common/roslint-neonavigation_common.xml: 1 tests
build/test_results/neonavigation_common/rostest-test_test_compat_rostest.xml: 1 tests
build/test_results/neonavigation_common/rosunit-test_compat.xml: 6 tests
build/test_results/obj_to_pointcloud/roslint-obj_to_pointcloud.xml: 1 tests
build/test_results/obj_to_pointcloud/rostest-test_test_obj_to_pointcloud_rostest.xml: 1 tests
build/test_results/obj_to_pointcloud/rosunit-test_obj_to_pointcloud.xml: 2 tests
build/test_results/planner_cspace/gtest-test_blockmem_gridmap.xml: 10 tests
build/test_results/planner_cspace/gtest-test_costmap_bbf.xml: 4 tests
build/test_results/planner_cspace/gtest-test_cyclic_vec.xml: 14 tests
build/test_results/planner_cspace/gtest-test_distance_map.xml: 14 tests
build/test_results/planner_cspace/gtest-test_grid_astar.xml: 12 tests
build/test_results/planner_cspace/gtest-test_grid_metric_converter.xml: 4 tests
build/test_results/planner_cspace/gtest-test_motion_cache.xml: 2 tests
build/test_results/planner_cspace/gtest-test_motion_primitive_builder.xml: 2 tests
build/test_results/planner_cspace/gtest-test_path_interpolator.xml: 4 tests
build/test_results/planner_cspace/gtest-test_planner_3d_cost.xml: 2 tests
build/test_results/planner_cspace/roslint-planner_cspace.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__antialias_start_true.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__antialias_start_true__fast_map_update_true.xml: 1 tests
build/test_results/planner_cspace/rostest-navigation_rostest__with_tolerance_true.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_abort_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_costmap_watchdog_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_debug_outputs_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_boundary_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_compat_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_navigation_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_planner_2dof_serial_joints_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_planner_3d_map_size_rostest.xml: 1 tests
build/test_results/planner_cspace/rostest-test_test_preempt_rostest.xml: 1 tests
build/test_results/planner_cspace/rosunit-test_abort.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_costmap_watchdog.xml: 4 tests
build/test_results/planner_cspace/rosunit-test_debug_outputs.xml: 6 tests
build/test_results/planner_cspace/rosunit-test_navigate.xml: 6 tests
build/test_results/planner_cspace/rosunit-test_navigate_boundary.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_planner_2dof_serial_joints.xml: 2 tests
build/test_results/planner_cspace/rosunit-test_planner_3d_map_size.xml: 12 tests
build/test_results/planner_cspace/rosunit-test_preempt.xml: 2 tests
build/test_results/safety_limiter/roslint-safety_limiter.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter2_rostest.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter_compat_rostest.xml: 1 tests
build/test_results/safety_limiter/rostest-test_test_safety_limiter_rostest.xml: 1 tests
build/test_results/safety_limiter/rosunit-test_safety_limiter.xml: 22 tests
build/test_results/safety_limiter/rosunit-test_safety_limiter2.xml: 2 tests
build/test_results/track_odometry/gtest-test_tf_projection.xml: 2 tests
build/test_results/track_odometry/roslint-track_odometry.xml: 1 tests
build/test_results/track_odometry/rostest-test_test_tf_projection_rostest.xml: 1 tests
build/test_results/track_odometry/rostest-test_test_track_odometry_rostest.xml: 1 tests
build/test_results/track_odometry/rosunit-test_tf_projection_node.xml: 8 tests
build/test_results/track_odometry/rosunit-test_track_odometry.xml: 10 tests
build/test_results/trajectory_tracker/gtest-test_trajectory_tracker_filter.xml: 6 tests
build/test_results/trajectory_tracker/gtest-test_trajectory_tracker_path2d.xml: 8 tests
build/test_results/trajectory_tracker/roslint-trajectory_tracker.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_recorder_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_overshoot_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-test_test_trajectory_tracker_with_odom_rostest.xml: 1 tests
build/test_results/trajectory_tracker/rostest-trajectory_tracker_rostest__odom_delay_0.xml: 1 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_recorder.xml: 2 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker.xml: 16 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker_overshoot.xml: 12 tests
build/test_results/trajectory_tracker/rosunit-test_trajectory_tracker_with_odom.xml: 4 tests
build/test_results/trajectory_tracker_msgs/gtest-test_path_with_velocity_conversion.xml: 4 tests
build/test_results/trajectory_tracker_msgs/roslint-trajectory_tracker_msgs.xml: 1 tests
Summary: 293 tests, 0 errors, 0 failures, 0 skipped

@at-wat at-wat merged commit f185e5e into master Aug 5, 2022
@at-wat at-wat deleted the planner_cspace/fix-pose-dup-in-path-interpolation branch August 5, 2022 05:45
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.

None yet

3 participants