feat(autoware_path_generator): function to smooth the route

[sasakisasaki]

Description

This is kind of feature porting from autoware.universe as follows

• Import PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection from the autoware.universe side code
• Also import all related functions from the autoware.universe side

Quality of Code

• NOT ENOUGH YET. I'm improving the code quality so this PR is ready for review.
• The unit tests for the added feature has not been added yet.

The refactoring plan is mentioned in this issue.
Perhaps we can work on the refactoring with a few PRs. Including this idea for refactoring, please feel free to share your idea for the improvement of code quality. Thank you!

How was this PR tested?

• I did vcs import at around 18:00 JST on 26th Feb. 2025
• The executed commands are as follows:

$ git clone https://github.com/autowarefoundation/autoware.git
$ cd autoware

# Before the build
$ ./setup-dev-env.sh -y

(Enter password for user)

(ADDED: Perhaps you need to do `sudo shutdown -r now` for applying NVIDIA driver update)

$ vcs import src < autoware.repos

$ vim autoware-nightly.repos

(Edited `autoware.core` part so it used my branch on forked)

$ vcs import src < autoware-nightly.repos
$ source ~/autoware/install/setup.bash
$ sudo apt update && sudo apt upgrade
$ rosdep update
$ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

# Deploy the attached three files for the following place, respectively
# - src/launcher/autoware_launch/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/path_generator.param.yaml
# - src/launcher/autoware_launch/autoware_launch/launch/components/tier4_planning_component.launch.xml
# - src/universe/autoware.universe/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

behavior_planning_launch_xml.txt
tier4_planning_component_launch_xml.txt
path_generator_param_yaml.txt

# Now start colcon build
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

# Checked by psim
$ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

Screencast_psim.webm

Status of Additional Tests

(ADDED) It seems the tests on the evaluator shows a failed scenario link to failed one. I'm now investigating the issue.

Notes for reviewers

Please do not enable auto merge as this PR needed to be merged with this launch side PR at the same time.
Please feel free to provide all the needed tests for merging this PR. I'm really happy for performing the tests 👍

Effects on system behavior

Needed to be investigated during the testing process.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 125 lines in your changes missing coverage. Please review.

Project coverage is 0.00%. Comparing base (4cb18f5) to head (04596a1).
Report is 58 commits behind head on main.

Files with missing lines	Patch %	Lines
planning/autoware_path_generator/src/utils.cpp	0.00%	120 Missing ⚠️
planning/autoware_path_generator/src/node.cpp	0.00%	5 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #227       +/-   ##
==========================================
- Coverage   78.75%   0.00%   -78.76%     
==========================================
  Files          11      87       +76     
  Lines         193    8714     +8521     
  Branches       73     889      +816     
==========================================
- Hits          152       0      -152     
- Misses         11    8714     +8703     
+ Partials       30       0       -30     

Flag	Coverage Δ
differential	0.00% <0.00%> (?)
total	?

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

[kosuke55]

this is just calc_distance2d ?

[sasakisasaki]

Thank you for this informative comment. I also guessed this is just a calculation for 2-dimensional distance between two points. If this is not my misunderstanding, the calcSignedArcLength() function calculates the total path length by summing up the 2D distances between consecutive points, as shown in the following code:

Ref. to code:

• https://github.com/autowarefoundation/autoware.core/blob/main/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp#L714-L733

[sasakisasaki]

Perhaps we need to ensure which method should we use.

[sasakisasaki]

Fixing error in some CI checks.

[sasakisasaki]

Oh, it seems the test is failing. Let me investigate and fix it. Sorry.

[youtalk]

Please add unit tests starting with only the simple cases.

[sasakisasaki]

@youtalk Thank you so much for your proposal! Yes, that must be a practical approach for the improvement of the code quality! I'll add some small, but would be effective tests.

@kosuke55 In this PR's domain, I could detect the following hidden issue (expected package does not exist) thanks to the already existing tests. That's why I'm welcome adding the tests in this PR. Do you have any proposal what kind of tests would be effective? 👀

Breakpoint 1 (PlanningModuleInterfaceTest_NodeTestWithExceptionTrajectory) pending.
(gdb) run
Starting program: /***/build/autoware_path_generator/test_autoware_path_generator --gtest_filter=PlanningModuleInterfaceTest.NodeTestWithExceptionTrajectory
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
Running main() from /opt/ros/humble/src/gtest_vendor/src/gtest_main.cc
Note: Google Test filter = PlanningModuleInterfaceTest.NodeTestWithExceptionTrajectory
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from PlanningModuleInterfaceTest
[ RUN      ] PlanningModuleInterfaceTest.NodeTestWithExceptionTrajectory
[New Thread 0x7ffff51ff640 (LWP 88717)]
[New Thread 0x7ffff49fe640 (LWP 88718)]
[New Thread 0x7ffff41fd640 (LWP 88719)]
[New Thread 0x7ffff39fc640 (LWP 88720)]
[New Thread 0x7ffff31fb640 (LWP 88721)]
[New Thread 0x7ffff28f9640 (LWP 88722)]
[New Thread 0x7ffff1ff7640 (LWP 88723)]
[New Thread 0x7ffff16f5640 (LWP 88724)]
[New Thread 0x7ffff0ef4640 (LWP 88725)]
[Thread 0x7ffff0ef4640 (LWP 88725) exited]
[Thread 0x7ffff1ff7640 (LWP 88723) exited]
[Thread 0x7ffff28f9640 (LWP 88722) exited]
[Thread 0x7ffff16f5640 (LWP 88724) exited]
[Thread 0x7ffff31fb640 (LWP 88721) exited]
[Thread 0x7ffff49fe640 (LWP 88718) exited]
[Thread 0x7ffff41fd640 (LWP 88719) exited]
[Thread 0x7ffff39fc640 (LWP 88720) exited]
unknown file: Failure
C++ exception with description "package 'autoware_test_utils' not found, searching: [/opt/ros/humble]" thrown in the test body.
[  FAILED  ] PlanningModuleInterfaceTest.NodeTestWithExceptionTrajectory (594 ms)
[----------] 1 test from PlanningModuleInterfaceTest (594 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (595 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] PlanningModuleInterfaceTest.NodeTestWithExceptionTrajectory

 1 FAILED TEST
[Thread 0x7ffff6211300 (LWP 88696) exited]
[Thread 0x7ffff51ff640 (LWP 88717) exited]
[New process 88696]
[Inferior 1 (process 88696) exited with code 01]
(gdb)