diff --git a/planning/autoware_path_generator/CMakeLists.txt b/planning/autoware_path_generator/CMakeLists.txt new file mode 100644 index 00000000..bac4fb74 --- /dev/null +++ b/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() diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md new file mode 100644 index 00000000..d570108f --- /dev/null +++ b/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") }} diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml new file mode 100644 index 00000000..1a8d2c6c --- /dev/null +++ b/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 diff --git a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp new file mode 100644 index 00000000..a1b774e9 --- /dev/null +++ b/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 + +#include +#include + +#include + +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_ diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp new file mode 100644 index 00000000..b582eda3 --- /dev/null +++ b/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 + +#include +#include +#include + +namespace autoware::path_generator +{ +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +namespace utils +{ +std::optional 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 get_lanelets_within_route_up_to( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); + +std::optional get_lanelets_within_route_after( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); + +std::optional get_previous_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::optional get_next_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::vector>> 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_ diff --git a/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg b/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg new file mode 100644 index 00000000..64e5ffb8 --- /dev/null +++ b/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ larger than + waypoint_group_separation_threshold +
+
+
+
+ larger than waypoint_group_separation_threshold +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ margin +
+
+
+
+ margin +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ overlap interval +
+
+
+
+ overlap interval +
+
+
+
+ + + + + + + + + + + + + +
+
+
+ margin × +
+ waypoint_group_interval_margin_ratio +
+
+
+
+
+ margin ×... +
+
+
+
+ + + + + + + + + + + + + + + + + + +
+
+
+ smaller than + waypoint_group_separation_threshold +
+
+
+
+ smaller than waypoint_group_separation_threshold +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ waypoint group +
+
+
+
+ waypoint group +
+
+
+
+ + + + + + + + +
+
+
+ waypoint +
+
+
+
+ waypoint +
+
+
+
+ + + + + + + + + + + + + +
+
+
+ generated path +
+
+
+
+ generated path +
+
+
+
+ + + + + + + + + + + + + +
+
+
+ centerline point +
+
+
+
+ centerline point +
+
+
+
+ + + + + + + + + + + + + +
+
+
+ overlap interval +
+
+
+
+ overlap interval +
+
+
+
+
+
+
+
diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml new file mode 100644 index 00000000..32c872d3 --- /dev/null +++ b/planning/autoware_path_generator/package.xml @@ -0,0 +1,31 @@ + + + + autoware_path_generator + 0.0.0 + The autoware_path_generator package + Satoshi Ota + Takayuki Murooka + Mitsuhiro Sakamoto + Apache License 2.0 + + Satoshi Ota + Takayuki Murooka + Mitsuhiro Sakamoto + + ament_cmake_auto + autoware_cmake + + autoware_lanelet2_extension + autoware_motion_utils + autoware_trajectory + autoware_vehicle_info_utils + generate_parameter_library + rclcpp + rclcpp_components + tier4_planning_msgs + + + ament_cmake + + diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml new file mode 100644 index 00000000..8d9f77a7 --- /dev/null +++ b/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 diff --git a/planning/autoware_path_generator/schema/path_generator.schema.json b/planning/autoware_path_generator/schema/path_generator.schema.json new file mode 100644 index 00000000..8bb9c005 --- /dev/null +++ b/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 + } + } +} diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp new file mode 100644 index 00000000..08d36d82 --- /dev/null +++ b/planning/autoware_path_generator/src/node.cpp @@ -0,0 +1,368 @@ +// 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. + +#include "node.hpp" + +#include "autoware/path_generator/utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace +{ +template +double get_arc_length_along_centerline(const T & lanelet, const U & point) +{ + return lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), lanelet::utils::to2D(point)) + .length; +} +} // namespace + +namespace autoware::path_generator +{ +PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) +: Node("path_generator", node_options) +{ + param_listener_ = + std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface()); + + path_publisher_ = create_publisher("~/output/path", 1); + + const auto params = param_listener_->get_params(); + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params.planning_hz).period(), + std::bind(&PathGenerator::run, this)); +} + +void PathGenerator::run() +{ + const auto input_data = take_data(); + set_planner_data(input_data); + if (!is_data_ready(input_data)) { + return; + } + + const auto path = plan_path(input_data); + if (!path) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); + return; + } + + path_publisher_->publish(*path); +} + +PathGenerator::InputData PathGenerator::take_data() +{ + InputData input_data; + + // route + if (const auto msg = route_subscriber_.takeData()) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty, ignoring..."); + } else { + input_data.route_ptr = msg; + } + } + + // map + if (const auto msg = vector_map_subscriber_.takeData()) { + input_data.lanelet_map_bin_ptr = msg; + } + + // velocity + if (const auto msg = odometry_subscriber_.takeData()) { + input_data.odometry_ptr = msg; + } + + return input_data; +} + +void PathGenerator::set_planner_data(const InputData & input_data) +{ + if (input_data.lanelet_map_bin_ptr) { + planner_data_.lanelet_map_ptr = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *input_data.lanelet_map_bin_ptr, planner_data_.lanelet_map_ptr, + &planner_data_.traffic_rules_ptr, &planner_data_.routing_graph_ptr); + } + + if (input_data.route_ptr) { + set_route(input_data.route_ptr); + } +} + +void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) +{ + planner_data_.route_frame_id = route_ptr->header.frame_id; + planner_data_.goal_pose = route_ptr->goal_pose; + + planner_data_.route_lanelets.clear(); + planner_data_.preferred_lanelets.clear(); + planner_data_.start_lanelets.clear(); + planner_data_.goal_lanelets.clear(); + + size_t primitives_num = 0; + for (const auto & route_section : route_ptr->segments) { + primitives_num += route_section.primitives.size(); + } + planner_data_.route_lanelets.reserve(primitives_num); + + for (const auto & route_section : route_ptr->segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(id); + planner_data_.route_lanelets.push_back(lanelet); + if (id == route_section.preferred_primitive.id) { + planner_data_.preferred_lanelets.push_back(lanelet); + } + } + } + + const auto set_lanelets_from_segment = + [&]( + const autoware_planning_msgs::msg::LaneletSegment & segment, + lanelet::ConstLanelets & lanelets) { + lanelets.reserve(segment.primitives.size()); + for (const auto & primitive : segment.primitives) { + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(primitive.id); + lanelets.push_back(lanelet); + } + }; + set_lanelets_from_segment(route_ptr->segments.front(), planner_data_.start_lanelets); + set_lanelets_from_segment(route_ptr->segments.back(), planner_data_.goal_lanelets); +} + +bool PathGenerator::is_data_ready(const InputData & input_data) +{ + const auto notify_waiting = [this](const std::string & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for %s", name.c_str()); + }; + + if (!planner_data_.lanelet_map_ptr) { + notify_waiting("map"); + return false; + } + + if (planner_data_.route_lanelets.empty()) { + notify_waiting("route"); + return false; + } + + if (!input_data.odometry_ptr) { + notify_waiting("odometry"); + return false; + } + + return true; +} + +std::optional PathGenerator::plan_path(const InputData & input_data) +{ + const auto path = + generate_path(input_data.odometry_ptr->pose.pose, param_listener_->get_params()); + + if (!path) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); + return std::nullopt; + } else if (path->points.empty()) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is empty"); + return std::nullopt; + } + + return path; +} + +std::optional PathGenerator::generate_path( + const geometry_msgs::msg::Pose & current_pose, const Params & params) const +{ + lanelet::ConstLanelet current_lane; + if (!lanelet::utils::query::getClosestLanelet( + planner_data_.preferred_lanelets, current_pose, ¤t_lane)) { + return std::nullopt; + } + + const auto lanelets = utils::get_lanelets_within_route( + current_lane, planner_data_, current_pose, params.backward_path_length, + params.forward_path_length); + if (!lanelets) { + return std::nullopt; + } + + return generate_path(*lanelets, current_pose, params); +} + +std::optional PathGenerator::generate_path( + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & current_pose, + const Params & params) const +{ + if (lanelets.empty()) { + return std::nullopt; + } + + const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); + const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates + const auto s_start = std::max(0., s - params.backward_path_length); + const auto s_end = [&]() { + auto s_end = s + params.forward_path_length; + + if (!utils::get_next_lanelet_within_route(lanelets.back(), planner_data_)) { + s_end = std::min(s_end, lanelet::utils::getLaneletLength2d(lanelets)); + } + + if (std::any_of( + planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), + [&](const auto & goal_lanelet) { return lanelets.back().id() == goal_lanelet.id(); })) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(lanelets, planner_data_.goal_pose); + s_end = std::min(s_end, goal_arc_coordinates.length); + } + + return s_end; + }(); + + return generate_path(lanelets, s_start, s_end, params); +} + +std::optional PathGenerator::generate_path( + const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end, + const Params & params) const +{ + std::vector path_points_with_lane_id{}; + + const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) { + PathPointWithLaneId path_point_with_lane_id{}; + path_point_with_lane_id.lane_ids.push_back(lanelet.id()); + path_point_with_lane_id.point.pose.position = + lanelet::utils::conversion::toGeomMsgPt(path_point); + path_point_with_lane_id.point.longitudinal_velocity_mps = + planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); + path_points_with_lane_id.push_back(std::move(path_point_with_lane_id)); + }; + + const auto waypoint_groups = utils::get_waypoint_groups( + lanelets, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold, + params.waypoint_group_interval_margin_ratio); + + auto extended_lanelets = lanelets; + auto s_offset = 0.; + + for (const auto & [waypoints, interval] : waypoint_groups) { + if (interval.first > 0.) { + continue; + } + const auto prev_lanelet = + utils::get_previous_lanelet_within_route(lanelets.front(), planner_data_); + if (!prev_lanelet) { + break; + } + extended_lanelets.insert(extended_lanelets.begin(), *prev_lanelet); + s_offset = lanelet::geometry::length2d(*prev_lanelet); + break; + } + + const lanelet::LaneletSequence extended_lanelet_sequence(extended_lanelets); + std::optional overlapping_waypoint_group_index = std::nullopt; + + for (auto lanelet_it = extended_lanelet_sequence.begin(); + lanelet_it != extended_lanelet_sequence.end(); ++lanelet_it) { + const auto & centerline = lanelet_it->centerline(); + auto s = get_arc_length_along_centerline(extended_lanelet_sequence, centerline.front()); + + for (auto point_it = centerline.begin(); point_it != centerline.end(); ++point_it) { + if (point_it != centerline.begin()) { + s += lanelet::geometry::distance2d(*std::prev(point_it), *point_it); + } else if (lanelet_it != extended_lanelet_sequence.begin()) { + continue; + } + + if (overlapping_waypoint_group_index) { + const auto & [waypoints, interval] = waypoint_groups[*overlapping_waypoint_group_index]; + if (s >= interval.first + s_offset && s <= interval.second + s_offset) { + continue; + } + overlapping_waypoint_group_index = std::nullopt; + } + + for (size_t i = 0; i < waypoint_groups.size(); ++i) { + const auto & [waypoints, interval] = waypoint_groups[i]; + if (s < interval.first + s_offset || s > interval.second + s_offset) { + continue; + } + for (const auto & waypoint : waypoints) { + const auto s_waypoint = + get_arc_length_along_centerline(extended_lanelet_sequence, waypoint); + for (auto waypoint_lanelet_it = extended_lanelet_sequence.begin(); + waypoint_lanelet_it != extended_lanelet_sequence.end(); ++waypoint_lanelet_it) { + if ( + s_waypoint > get_arc_length_along_centerline( + extended_lanelet_sequence, waypoint_lanelet_it->centerline().back())) { + continue; + } + add_path_point(waypoint, *waypoint_lanelet_it); + break; + } + } + overlapping_waypoint_group_index = i; + break; + } + + if (overlapping_waypoint_group_index) { + continue; + } + add_path_point(*point_it, *lanelet_it); + } + } + + s_offset -= get_arc_length_along_centerline( + extended_lanelet_sequence, lanelet::utils::conversion::toLaneletPoint( + path_points_with_lane_id.front().point.pose.position)); + + auto trajectory = Trajectory::Builder().build(path_points_with_lane_id); + if (!trajectory) { + return std::nullopt; + } + + trajectory->crop(s_offset + s_start, s_end - s_start); + + PathWithLaneId path{}; + path.header.frame_id = planner_data_.route_frame_id; + path.header.stamp = now(); + path.points = trajectory->restore(); + + for (const auto & left_bound_point : extended_lanelet_sequence.leftBound()) { + path.left_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(left_bound_point)); + } + for (const auto & right_bound_point : extended_lanelet_sequence.rightBound()) { + path.right_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(right_bound_point)); + } + + return path; +} +} // namespace autoware::path_generator + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_generator::PathGenerator) diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp new file mode 100644 index 00000000..dd765294 --- /dev/null +++ b/planning/autoware_path_generator/src/node.hpp @@ -0,0 +1,97 @@ +// 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 NODE_HPP_ +#define NODE_HPP_ + +#include "autoware/path_generator/common_structs.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include +namespace autoware::path_generator +{ +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using nav_msgs::msg::Odometry; +using ::path_generator::Params; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using Trajectory = autoware::trajectory::Trajectory; + +class PathGenerator : public rclcpp::Node +{ +public: + explicit PathGenerator(const rclcpp::NodeOptions & node_options); + +private: + struct InputData + { + LaneletRoute::ConstSharedPtr route_ptr{nullptr}; + LaneletMapBin::ConstSharedPtr lanelet_map_bin_ptr{nullptr}; + Odometry::ConstSharedPtr odometry_ptr{nullptr}; + }; + + // subscriber + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletRoute, universe_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletMapBin, universe_utils::polling_policy::Newest> + vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber odometry_subscriber_{ + this, "~/input/odometry"}; + + // publisher + rclcpp::Publisher::SharedPtr path_publisher_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::shared_ptr<::path_generator::ParamListener> param_listener_; + + PlannerData planner_data_; + + void run(); + + InputData take_data(); + + void set_planner_data(const InputData & input_data); + + void set_route(const LaneletRoute::ConstSharedPtr & route_ptr); + + bool is_data_ready(const InputData & input_data); + + std::optional plan_path(const InputData & input_data); + + std::optional generate_path( + const geometry_msgs::msg::Pose & current_pose, const Params & params) const; + + std::optional generate_path( + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & current_pose, + const Params & params) const; + + std::optional generate_path( + const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end, + const Params & params) const; +}; +} // namespace autoware::path_generator + +#endif // NODE_HPP_ diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp new file mode 100644 index 00000000..1bc90c27 --- /dev/null +++ b/planning/autoware_path_generator/src/utils.cpp @@ -0,0 +1,210 @@ +// 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. + +#include "autoware/path_generator/utils.hpp" + +#include +#include + +#include + +#include +#include + +namespace autoware::path_generator +{ +namespace utils +{ +namespace +{ +template +bool exists(const std::vector & vec, const T & item) +{ + return std::find(vec.begin(), vec.end(), item) != vec.end(); +} +} // namespace + +std::optional 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) +{ + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + + const auto backward_lanelets = get_lanelets_within_route_up_to( + lanelet, planner_data, backward_distance - arc_coordinates.length); + if (!backward_lanelets) { + return std::nullopt; + } + + const auto forward_lanelets = get_lanelets_within_route_after( + lanelet, planner_data, forward_distance - (lanelet_length - arc_coordinates.length)); + if (!forward_lanelets) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelets(std::move(*backward_lanelets)); + lanelets.push_back(lanelet); + std::move(forward_lanelets->begin(), forward_lanelets->end(), std::back_inserter(lanelets)); + + return lanelets; +} + +std::optional get_lanelets_within_route_up_to( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) +{ + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelets{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < distance) { + const auto prev_lanelet = get_previous_lanelet_within_route(current_lanelet, planner_data); + if (!prev_lanelet) { + break; + } + + lanelets.push_back(*prev_lanelet); + current_lanelet = *prev_lanelet; + length += lanelet::utils::getLaneletLength2d(*prev_lanelet); + } + + std::reverse(lanelets.begin(), lanelets.end()); + return lanelets; +} + +std::optional get_lanelets_within_route_after( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) +{ + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelets{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < distance) { + const auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); + if (!next_lanelet) { + break; + } + + lanelets.push_back(*next_lanelet); + current_lanelet = *next_lanelet; + length += lanelet::utils::getLaneletLength2d(*next_lanelet); + } + + return lanelets; +} + +std::optional get_previous_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (exists(planner_data.start_lanelets, lanelet)) { + return std::nullopt; + } + + const auto prev_lanelets = planner_data.routing_graph_ptr->previous(lanelet); + if (prev_lanelets.size() > 1) { + RCLCPP_WARN( + rclcpp::get_logger("path_generator").get_child("utils"), + "The multiple previous lanelets in a route are found not as expected. Internal calculation " + "might have failed."); + } + + if (prev_lanelets.empty() || !exists(planner_data.route_lanelets, prev_lanelets.front())) { + return std::nullopt; + } + + return prev_lanelets.front(); +} + +std::optional get_next_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (planner_data.preferred_lanelets.empty()) { + return std::nullopt; + } + + if (exists(planner_data.goal_lanelets, lanelet)) { + return std::nullopt; + } + + const auto next_lanelets = planner_data.routing_graph_ptr->following(lanelet); + if (next_lanelets.size() > 1) { + RCLCPP_WARN( + rclcpp::get_logger("path_generator").get_child("utils"), + "The multiple next lanelets in a route are found not as expected. Internal calculation might " + "have failed."); + } + + if ( + next_lanelets.empty() || + next_lanelets.front().id() == planner_data.preferred_lanelets.front().id() || + !exists(planner_data.route_lanelets, next_lanelets.front())) { + return std::nullopt; + } + + return next_lanelets.front(); +} + +std::vector>> get_waypoint_groups( + const lanelet::ConstLanelets & lanelets, const lanelet::LaneletMap & lanelet_map, + const double group_separation_threshold, const double interval_margin_ratio) +{ + std::vector>> waypoint_groups{}; + const lanelet::LaneletSequence lanelet_sequence(lanelets); + + const auto get_interval_bound = + [&](const lanelet::ConstPoint3d & point, const double lateral_distance_factor) { + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + lanelet_sequence.centerline2d(), lanelet::utils::to2D(point)); + return arc_coordinates.length + lateral_distance_factor * std::abs(arc_coordinates.distance); + }; + + for (const auto & lanelet : lanelets) { + if (!lanelet.hasAttribute("waypoints")) { + continue; + } + + const auto waypoints_id = lanelet.attribute("waypoints").asId().value(); + const auto & waypoints = lanelet_map.lineStringLayer.get(waypoints_id); + + if ( + waypoint_groups.empty() || + lanelet::geometry::distance2d(waypoint_groups.back().first.back(), waypoints.front()) > + group_separation_threshold) { + waypoint_groups.emplace_back().second.first = + get_interval_bound(waypoints.front(), -interval_margin_ratio); + } + waypoint_groups.back().second.second = + get_interval_bound(waypoints.back(), interval_margin_ratio); + + waypoint_groups.back().first.insert( + waypoint_groups.back().first.end(), waypoints.begin(), waypoints.end()); + } + + return waypoint_groups; +} +} // namespace utils +} // namespace autoware::path_generator