feat(path_generator): add path_generator package

planning/autoware_path_generator/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_path_generator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+generate_parameter_library(path_generator_parameters
+  param/path_generator_parameters.yaml
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+target_link_libraries(${PROJECT_NAME}
+  path_generator_parameters
+)
+
+target_compile_options(${PROJECT_NAME} PRIVATE
+  -Wno-error=deprecated-declarations
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::path_generator::PathGenerator"
+  EXECUTABLE path_generator_node
+)
+
+ament_auto_package()

planning/autoware_path_generator/README.md

@@ -0,0 +1,79 @@
+# Path Generator
+
+The `path_generator` node receives a route from `mission_planner` and converts the center line into a path.
+If the route has waypoints set, it generates a path passing through them.
+
+This package is a simple alternative of `behavior_path_generator`.
+
+## Path generation
+
+When input data is ready, it first searches for the lanelet closest to the vehicle.
+If found, it gets the lanelets within a distance of `backward_path_length` behind and `forward_path_length` in front.
+Their center lines are concatenated to generate a path.
+
+If waypoints exist in the route, it replaces the overlapped segment of the center line with them.
+The overlap interval is determined as shown in the following figure.
+
+![waypoint_group_overlap_interval_determination](./media/waypoint_group_overlap_interval_determination.drawio.svg)
+
+## Flowchart
+
+![Flowchart](https://www.plantuml.com/plantuml/svg/RL91RiCW4BppYZqwNt0EZTHh_u0sOHAZKa2mJkhV5qDmN2cMB2spEyCCSCh2IUOVfyIA0wNPgmefDGf_GniMFgenGtHqx3tI4x9N6cok2vt0PZcGJF0qBCW71PT1Wmy7HPGbH0Llx5MfMmfpf5L9HvQfT1joGr5cGKU9nXlaquMCB5_iuI373TUk8La_h4pMww8XsFKVlCHWlUYgdbjdTSn-eTwayYFTsL5DdEcCecMeYt_yIqIqgSU4kmrpxZRUk2YJ-1Nir1fUZw5MZyawq70DYS2NYIvOtP2p7bJjjIsRxN17SXmZ39rt0MgwTpORg_jq2xq4dkFRuFwc-ZUoCTikcIJGUYqzMK_nbj_PbRfdxdcrgWUWQjc2g5UbnZYVdStJawxg6pgLWV9K_m00)
+
+```plantuml
+@startuml
+title run
+start
+
+:take_data;
+:set_planner_data;
+if (is_data_ready) then (yes)
+else (no)
+  stop
+endif
+
+group plan_path
+  group generate_path
+    :getClosestLanelet;
+    :get_lanelets_within_route;
+    :get_waypoint_groups;
+    if (any waypoint interval starts behind lanelets?) then (yes)
+      :get_previous_lanelet_within_route;
+      :extend lanelets;
+    endif
+    while (for each center line point)
+      if (overlapped by waypoint group?) then (yes)
+        if (previously overlapped?) then
+        else (no)
+          :add waypoints to path;
+        endif
+      else (no)
+        :add point to path;
+      endif
+    endwhile
+  end group
+end group
+
+:publish path;
+
+stop
+@enduml
+```
+
+## Input topics
+
+| Name                 | Type                                        | Description                      |
+| :------------------- | :------------------------------------------ | :------------------------------- |
+| `~/input/odometry`   | `nav_msgs::msg::Odometry`                   | ego pose                         |
+| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin`     | vector map information           |
+| `~/input/route`      | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal |
+
+## Output topics
+
+| Name            | Type                                       | Description    | QoS Durability |
+| :-------------- | :----------------------------------------- | :------------- | :------------- |
+| `~/output/path` | `tier4_planning_msgs::msg::PathWithLaneId` | generated path | `volatile`     |
+
+## Parameters
+
+{{ json_to_markdown("planning/autoware_path_generator/schema/path_generator.schema.json") }}

planning/autoware_path_generator/config/path_generator.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    planning_hz: 10.0
+    backward_path_length: 5.0
+    forward_path_length: 300.0
+    waypoint_group_separation_threshold: 1.0
+    waypoint_group_interval_margin_ratio: 10.0

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

@@ -0,0 +1,43 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_
+#define AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_
+
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+
+#include <string>
+
+namespace autoware::path_generator
+{
+struct PlannerData
+{
+  lanelet::LaneletMapPtr lanelet_map_ptr{nullptr};
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr{nullptr};
+  lanelet::routing::RoutingGraphPtr routing_graph_ptr{nullptr};
+
+  std::string route_frame_id{};
+  geometry_msgs::msg::Pose goal_pose{};
+
+  lanelet::ConstLanelets route_lanelets{};
+  lanelet::ConstLanelets preferred_lanelets{};
+  lanelet::ConstLanelets start_lanelets{};
+  lanelet::ConstLanelets goal_lanelets{};
+};
+}  // namespace autoware::path_generator
+
+#endif  // AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_

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

@@ -0,0 +1,56 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__PATH_GENERATOR__UTILS_HPP_
+#define AUTOWARE__PATH_GENERATOR__UTILS_HPP_
+
+#include "autoware/path_generator/common_structs.hpp"
+
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <optional>
+#include <utility>
+#include <vector>
+
+namespace autoware::path_generator
+{
+using tier4_planning_msgs::msg::PathPointWithLaneId;
+using tier4_planning_msgs::msg::PathWithLaneId;
+
+namespace utils
+{
+std::optional<lanelet::ConstLanelets> get_lanelets_within_route(
+  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data,
+  const geometry_msgs::msg::Pose & current_pose, const double backward_distance,
+  const double forward_distance);
+
+std::optional<lanelet::ConstLanelets> get_lanelets_within_route_up_to(
+  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance);
+
+std::optional<lanelet::ConstLanelets> get_lanelets_within_route_after(
+  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance);
+
+std::optional<lanelet::ConstLanelet> get_previous_lanelet_within_route(
+  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data);
+
+std::optional<lanelet::ConstLanelet> get_next_lanelet_within_route(
+  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data);
+
+std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_waypoint_groups(
+  const lanelet::ConstLanelets & lanelets, const lanelet::LaneletMap & lanelet_map,
+  const double group_separation_threshold, const double interval_margin_ratio);
+}  // namespace utils
+}  // namespace autoware::path_generator
+
+#endif  // AUTOWARE__PATH_GENERATOR__UTILS_HPP_

planning/autoware_path_generator/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_path_generator</name>
+  <version>0.0.0</version>
+  <description>The autoware_path_generator package</description>
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
+  <maintainer email="[email protected]">Mitsuhiro Sakamoto</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Satoshi Ota</author>
+  <author email="[email protected]">Takayuki Murooka</author>
+  <author email="[email protected]">Mitsuhiro Sakamoto</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_trajectory</depend>
+  <depend>autoware_vehicle_info_utils</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_planning_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/autoware_path_generator/param/path_generator_parameters.yaml

@@ -0,0 +1,15 @@
+path_generator:
+  planning_hz:
+    type: double
+
+  backward_path_length:
+    type: double
+
+  forward_path_length:
+    type: double
+
+  waypoint_group_separation_threshold:
+    type: double
+
+  waypoint_group_interval_margin_ratio:
+    type: double

planning/autoware_path_generator/schema/path_generator.schema.json

@@ -0,0 +1,50 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Path Generator Node",
+  "type": "object",
+  "definitions": {
+    "autoware_path_generator": {
+      "type": "object",
+      "properties": {
+        "planning_hz": {
+          "type": "number",
+          "description": "Planning frequency [Hz]",
+          "default": "10.0",
+          "minimum": 0.0
+        },
+        "backward_path_length": {
+          "type": "number",
+          "description": "Length of generated path behind vehicle [m]",
+          "default": "5.0",
+          "minimum": 0.0
+        },
+        "forward_path_length": {
+          "type": "number",
+          "description": "Length of generated path in front of vehicle [m]",
+          "default": "300.0",
+          "minimum": 0.0
+        },
+        "waypoint_group_separation_threshold": {
+          "type": "number",
+          "description": "Maximum distance at which consecutive waypoints are considered to belong to the same group [m]",
+          "default": "1.0",
+          "minimum": 0.0
+        },
+        "waypoint_group_interval_margin_ratio": {
+          "type": "number",
+          "description": "Ratio for determining length of switching section from centerline to waypoints",
+          "default": "10.0",
+          "minimum": 0.0
+        }
+      },
+      "required": [
+        "planning_hz",
+        "backward_path_length",
+        "forward_path_length",
+        "waypoint_group_separation_threshold",
+        "waypoint_group_interval_margin_ratio"
+      ],
+      "additionalProperties": false
+    }
+  }
+}