fix(start_planner): remove inverse orientation points

[kosuke55]

Description

remove inverse orientation points after shifting path

edge case example

(④typo 3->2)

before

after

Related links

fix #4950

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[kosuke55]

memo

        elif map_name == 'virtual_fukuzatu':
            if route == 0: # invalid orientation
                self.initial_pose.pose.pose.position.x = 216.0785369873047
                self.initial_pose.pose.pose.position.y = 468.38623046875
                self.initial_pose.pose.pose.orientation.z = 0.24739376267590443
                self.initial_pose.pose.pose.orientation.w = 0.9689150252674681

                self.goal_pose.pose.position.x = 199.07623291015625
                self.goal_pose.pose.position.y = 429.3672180175781
                self.goal_pose.pose.orientation.z = -0.7117967622473772
                self.goal_pose.pose.orientation.w = 0.7023854847974514

[codecov]

Codecov Report

Patch coverage: 14.28% and project coverage change: +0.21% 🎉

Comparison is base (ec99b2f) 15.02% compared to head (283b0ed) 15.23%.
Report is 13 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5100      +/-   ##
==========================================
+ Coverage   15.02%   15.23%   +0.21%     
==========================================
  Files        1608     1608              
  Lines      111740   112318     +578     
  Branches    34645    35082     +437     
==========================================
+ Hits        16788    17113     +325     
- Misses      76244    76359     +115     
- Partials    18708    18846     +138     

Flag	Coverage Δ		*Carryforward flag
differential	18.60% <14.28%> (?)
total	15.02% <ø> (+<0.01%)	⬆️	Carriedforward from 9ca31b6

*This pull request uses carry forward flags. Click here to find out more.

Files Changed	Coverage Δ
...planner/src/utils/goal_planner/shift_pull_over.cpp	3.57% <ø> (+0.03%)	⬆️
...th_planner/src/utils/path_shifter/path_shifter.cpp	9.22% <0.00%> (-0.11%)	⬇️
...nner/src/utils/start_goal_planner_common/utils.cpp	30.76% <ø> (+3.74%)	⬆️
...planner/src/utils/start_planner/shift_pull_out.cpp	2.51% <ø> (+0.01%)	⬆️
...ils/include/motion_utils/trajectory/trajectory.hpp	66.90% <33.33%> (-0.13%)	⬇️

... and 11 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[kosuke55]

@kyoichi-sugahara
thanks for the discussion!
Updated to remove and insert in loop
use loop