feat(path_generator): add path_generator package #9216

mitukou1109 · 2024-11-01T06:39:32Z

Description

This PR adds path_generator package, a simplified alternative of behavior_path_planner.

How was this PR tested?

Psim on a lanelet2 map with waypoints

Screencast.from.2024.11.29.16.11.24.webm

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

github-actions · 2024-11-01T06:39:49Z

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.

takayuki5168 · 2024-11-01T07:10:08Z

planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp

+    return *this;
+  }
+
+private:


There seem several parameters which can be local variables.
IMO, increasing the member variables decreases the readability and maintainability since it's not clear that the variables are used and modified. So please try not to use the member variables as much as possible.

planning/autoware_path_generator/include/autoware/path_generator/node.hpp

planning/autoware_path_generator/src/node.cpp

planning/autoware_path_generator/CMakeLists.txt

planning/autoware_path_generator/param/path_generator_parameters.yaml

planning/autoware_path_generator/CMakeLists.txt

planning/autoware_path_generator/package.xml

planning/autoware_path_generator/src/path_handler.cpp

takayuki5168 · 2024-11-01T07:40:12Z

planning/autoware_path_generator/src/path_handler.cpp

+
+  // set yaw to each point
+  {
+    auto it = path_points.begin();


This can be in for(here;...

takayuki5168 · 2024-11-01T07:43:44Z

planning/autoware_path_generator/src/path_handler.cpp

+  const auto waypoint_groups = utils::getWaypointGroups(lanelet_sequence, *lanelet_map_ptr_);
+
+  double s = 0.;
+  for (const auto & lanelet : lanelet_sequence) {
+    std::vector<geometry_msgs::msg::Point> reference_points;
+    const auto & centerline = lanelet.centerline();
+
+    std::optional<size_t> overlapped_waypoint_group_index = std::nullopt;
+    for (auto it = centerline.begin(); it != centerline.end(); ++it) {
+      if (s <= s_end) {
+        const lanelet::Point3d point(*it);
+        if (s >= s_start) {
+          for (size_t i = 0; i < waypoint_groups.size(); ++i) {
+            const auto & [waypoints, interval] = waypoint_groups[i];
+            if (s >= interval.first && s <= interval.second) {
+              overlapped_waypoint_group_index = i;
+              break;
+            } else if (i == overlapped_waypoint_group_index) {
+              for (const auto & waypoint : waypoints) {
+                reference_points.push_back(lanelet::utils::conversion::toGeomMsgPt(waypoint));
+              }
+              overlapped_waypoint_group_index = std::nullopt;
+            }
+          }
+          if (!overlapped_waypoint_group_index) {
+            reference_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point));
+          }
+        }
+        if (it == std::prev(centerline.end())) {
+          break;
+        }
+
+        const lanelet::Point3d next_point(*std::next(it));
+        const auto distance = lanelet::geometry::distance2d(point, next_point);
+        std::optional<double> s_interpolation = std::nullopt;
+        if (s + distance > s_end) {
+          s_interpolation = s_end - s;
+        } else if (s < s_start && s + distance > s_start) {
+          s_interpolation = s_start - s;
+        }
+
+        if (s_interpolation) {
+          const auto interpolated_point = lanelet::geometry::interpolatedPointAtDistance(
+            lanelet::ConstLineString3d{lanelet::InvalId, {point, next_point}}, *s_interpolation);
+          reference_points.push_back(lanelet::utils::conversion::toGeomMsgPt(interpolated_point));
+        }
+        s += distance;
+      } else {
+        break;
+      }
+    }


FYI: No need to refactor here.

satoshi-ota · 2024-11-07T00:20:26Z

planning/autoware_path_generator/src/utils.cpp

+  return waypoint_groups;
+}
+
+void removeOverlappingPoints(PathWithLaneId & path)


@mitukou1109 Please write unit tests for utils functions. Code quality and reliability is very important for this package. It's okay to add tests in other PR.

planning/autoware_path_generator/src/node.cpp

takayuki5168 · 2024-11-07T08:24:27Z

planning/autoware_path_generator/src/node.cpp

+
+void PathGenerator::run()
+{
+  const auto input_data = takeData();


Each variable in input_data may be nullptr. We should check whether the input_data is valid or not (= whether the input data is ready or not) here.

The validation is now in the updatePlannerData.
I feel checking whether the input_data is valid or not should be before planPath

takayuki5168 · 2024-11-07T08:24:51Z

planning/autoware_path_generator/src/node.cpp

+    return std::nullopt;
+  }
+
+  auto path = utils::generateCenterLinePath(planner_data_);


takayuki5168 · 2024-11-07T08:27:51Z

planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp

+  lanelet::LaneletMapPtr lanelet_map_ptr{nullptr};
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr{nullptr};
+  lanelet::routing::RoutingGraphPtr routing_graph_ptr{nullptr};
+
+  geometry_msgs::msg::Pose current_pose;
+
+  autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_ptr{nullptr};
+
+  lanelet::ConstLanelets route_lanelets;
+  lanelet::ConstLanelets preferred_lanelets;
+  lanelet::ConstLanelets start_lanelets;
+  lanelet::ConstLanelets goal_lanelets;


I feel these variables do not have to be member variables.

takayuki5168 · 2024-11-07T08:29:46Z

planning/autoware_path_generator/src/node.cpp

+  planner_data_.start_lanelets.clear();
+  planner_data_.goal_lanelets.clear();
+
+  if (!planner_data_.route_ptr->segments.empty()) {


Please use early return.

takayuki5168 · 2024-11-07T08:32:18Z

planning/autoware_path_generator/src/utils.cpp

+std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> getWaypointGroups(
+  const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map)


FYI: No need to refactor and write a unit test for this function.

takayuki5168 · 2024-11-07T08:33:28Z

planning/autoware_path_generator/src/node.cpp

+  path_publisher_->publish(*path);
+}
+
+PathGenerator::InputData PathGenerator::takeData()


Please use the snake case for all the functions.

codecov · 2024-11-07T08:44:11Z

Codecov Report

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

Project coverage is 0.00%. Comparing base (ddfacd3) to head (f4dfe91).
Report is 10 commits behind head on main.

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

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #9216       +/-   ##
==========================================
- Coverage   29.76%   0.00%   -29.77%     
==========================================
  Files        1444      87     -1357     
  Lines      108686    3866   -104820     
  Branches    42664     671    -41993     
==========================================
- Hits        32352       0    -32352     
+ Misses      73150    3866    -69284     
+ Partials     3184       0     -3184

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

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

takayuki5168 · 2024-11-25T08:04:34Z

planning/autoware_path_generator/src/utils.cpp

+    if (
+      planner_data.preferred_lanelets.front().id() != lanelet.id() &&
+      exists(planner_data.route_lanelets, lanelet)) {
+      return lanelet;


~~Returning the first lanelet in the next lanelets is not good as a specification of the utility function.~~
~~Let's change the function to get_next_lanelets_within_route, and the node side will use the first one.~~

The next lanelet in a route should be single.
So, please add the error log like

RCLCPP_WARN(rclcpp::get_logger("path_generator.utils"), "The multiple next lanelets in a route are found not as expected. Internal calculation might have failed.");

when there are multiple next lanelets.

Returning the first lanelet in this case is fine.

Signed-off-by: mitukou1109 <[email protected]>

github-actions · 2024-12-23T08:13:26Z

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-9216/
Modified URLs:

https://autowarefoundation.github.io/autoware.universe/pr-9216/planning/autoware_path_generator/

Signed-off-by: mitukou1109 <[email protected]>

takayuki5168 · 2025-01-07T07:57:11Z

New PR was created, so I close this PR.
autowarefoundation/autoware.core#138

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Nov 1, 2024

takayuki5168 reviewed Nov 1, 2024

View reviewed changes

planning/autoware_path_generator/include/autoware/path_generator/node.hpp Outdated Show resolved Hide resolved

planning/autoware_path_generator/src/node.cpp Outdated Show resolved Hide resolved

planning/autoware_path_generator/src/node.cpp Outdated Show resolved Hide resolved

takayuki5168 reviewed Nov 1, 2024

View reviewed changes

planning/autoware_path_generator/CMakeLists.txt Show resolved Hide resolved

takayuki5168 approved these changes Nov 1, 2024

View reviewed changes

planning/autoware_path_generator/param/path_generator_parameters.yaml Show resolved Hide resolved

planning/autoware_path_generator/CMakeLists.txt Outdated Show resolved Hide resolved

takayuki5168 self-requested a review November 1, 2024 07:34

takayuki5168 added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Nov 1, 2024

takayuki5168 reviewed Nov 1, 2024

View reviewed changes

planning/autoware_path_generator/package.xml Show resolved Hide resolved

takayuki5168 reviewed Nov 1, 2024

View reviewed changes

planning/autoware_path_generator/src/path_handler.cpp Outdated Show resolved Hide resolved

takayuki5168 reviewed Nov 1, 2024

View reviewed changes

satoshi-ota reviewed Nov 7, 2024

View reviewed changes

mitukou1109 force-pushed the feat/path_generator branch from 2b546d7 to 851c0bb Compare November 7, 2024 07:04

takayuki5168 reviewed Nov 7, 2024

View reviewed changes

planning/autoware_path_generator/src/node.cpp Show resolved Hide resolved

takayuki5168 reviewed Nov 7, 2024

View reviewed changes

mitukou1109 force-pushed the feat/path_generator branch 5 times, most recently from e0f2caa to d2c2cda Compare November 13, 2024 08:30

mitukou1109 force-pushed the feat/path_generator branch 2 times, most recently from 6717500 to 6fe9161 Compare November 22, 2024 09:53

takayuki5168 reviewed Nov 25, 2024

View reviewed changes

mitukou1109 added 19 commits December 23, 2024 12:34

fix data handling

dff7cd7

Signed-off-by: mitukou1109 <[email protected]>

add planning_hz parameter to description

cd360d7

Signed-off-by: mitukou1109 <[email protected]>

add configuration example

a0bf904

Signed-off-by: mitukou1109 <[email protected]>

set path bounds

182f913

Signed-off-by: mitukou1109 <[email protected]>

use only route lanelets

1f148fe

Signed-off-by: mitukou1109 <[email protected]>

refactor utility functions

649690b

Signed-off-by: mitukou1109 <[email protected]>

fix overlap removal function

d0a2b00

Signed-off-by: mitukou1109 <[email protected]>

refactor path generation function

b6a8b5b

Signed-off-by: mitukou1109 <[email protected]>

use lanelet::LaneletSequence for lanelet sequence representation

f67a254

Signed-off-by: mitukou1109 <[email protected]>

add parameters for handling waypoints

8d29f41

Signed-off-by: mitukou1109 <[email protected]>

fix path generation function to work with waypoints properly

271dafc

Signed-off-by: mitukou1109 <[email protected]>

log error when multiple next lanelets are found

3fc0a59

Signed-off-by: mitukou1109 <[email protected]>

add suffix to utility functions

2f44c46

Signed-off-by: mitukou1109 <[email protected]>

change utility function signatures

0006299

Signed-off-by: mitukou1109 <[email protected]>

use autoware_trajectory for path handling

bfaab95

Signed-off-by: mitukou1109 <[email protected]>

remove unnecessary functions

597cf3e

Signed-off-by: mitukou1109 <[email protected]>

remove parameters

ea6a985

Signed-off-by: mitukou1109 <[email protected]>

include necessary header

f712f34

Signed-off-by: mitukou1109 <[email protected]>

add README

409a791

Signed-off-by: mitukou1109 <[email protected]>

mitukou1109 force-pushed the feat/path_generator branch from 7b7dc3c to 409a791 Compare December 23, 2024 03:35

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Dec 23, 2024

pre-commit-ci bot and others added 4 commits December 23, 2024 03:38

style(pre-commit): autofix

0c9a70d

add json schema

3f08a34

Signed-off-by: mitukou1109 <[email protected]>

style(pre-commit): autofix

92fc51a

add flowchart to README

f4dfe91

Signed-off-by: mitukou1109 <[email protected]>

takayuki5168 added the run:deploy-docs Mark for deploy-docs action generation. (used-by-ci) label Dec 23, 2024

remove duplicate parameter list

c639be5

Signed-off-by: mitukou1109 <[email protected]>

takayuki5168 closed this Jan 7, 2025

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(path_generator): add path_generator package #9216

feat(path_generator): add path_generator package #9216

mitukou1109 commented Nov 1, 2024 •

edited

Loading

github-actions bot commented Nov 1, 2024 •

edited

Loading

takayuki5168 Nov 1, 2024

takayuki5168 Nov 1, 2024

takayuki5168 Nov 1, 2024

satoshi-ota Nov 7, 2024

takayuki5168 Nov 7, 2024

takayuki5168 Nov 7, 2024

takayuki5168 Nov 7, 2024

takayuki5168 Nov 7, 2024

takayuki5168 Nov 7, 2024

takayuki5168 Nov 7, 2024

takayuki5168 Nov 7, 2024

codecov bot commented Nov 7, 2024 •

edited

Loading

takayuki5168 Nov 25, 2024 •

edited

Loading

takayuki5168 Nov 25, 2024

github-actions bot commented Dec 23, 2024

takayuki5168 commented Jan 7, 2025

		std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> getWaypointGroups(
		const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map)

feat(path_generator): add path_generator package #9216

feat(path_generator): add path_generator package #9216

Conversation

mitukou1109 commented Nov 1, 2024 • edited Loading

Description

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Nov 1, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Nov 7, 2024 • edited Loading

Codecov Report

takayuki5168 Nov 25, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

github-actions bot commented Dec 23, 2024

takayuki5168 commented Jan 7, 2025

mitukou1109 commented Nov 1, 2024 •

edited

Loading

github-actions bot commented Nov 1, 2024 •

edited

Loading

codecov bot commented Nov 7, 2024 •

edited

Loading

takayuki5168 Nov 25, 2024 •

edited

Loading