From 92bdc1d0fe8fe6beb58669aeb1a83ee84d1c80a6 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 1 Nov 2024 15:37:04 +0900 Subject: [PATCH 01/34] add path_generator package Signed-off-by: mitukou1109 --- .../autoware_path_generator/CMakeLists.txt | 30 ++ .../include/autoware/path_generator/node.hpp | 64 +++ .../autoware/path_generator/path_handler.hpp | 97 ++++ .../include/autoware/path_generator/utils.hpp | 35 ++ planning/autoware_path_generator/package.xml | 26 ++ .../param/path_generator_parameters.yaml | 21 + planning/autoware_path_generator/src/node.cpp | 121 +++++ .../src/path_handler.cpp | 420 ++++++++++++++++++ .../autoware_path_generator/src/utils.cpp | 88 ++++ 9 files changed, 902 insertions(+) create mode 100644 planning/autoware_path_generator/CMakeLists.txt create mode 100644 planning/autoware_path_generator/include/autoware/path_generator/node.hpp create mode 100644 planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp create mode 100644 planning/autoware_path_generator/include/autoware/path_generator/utils.hpp create mode 100644 planning/autoware_path_generator/package.xml create mode 100644 planning/autoware_path_generator/param/path_generator_parameters.yaml create mode 100644 planning/autoware_path_generator/src/node.cpp create mode 100644 planning/autoware_path_generator/src/path_handler.cpp create mode 100644 planning/autoware_path_generator/src/utils.cpp diff --git a/planning/autoware_path_generator/CMakeLists.txt b/planning/autoware_path_generator/CMakeLists.txt new file mode 100644 index 0000000000000..12aafeff4ebc1 --- /dev/null +++ b/planning/autoware_path_generator/CMakeLists.txt @@ -0,0 +1,30 @@ +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 + src/node.cpp + src/path_handler.cpp + src/utils.cpp +) + +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/include/autoware/path_generator/node.hpp b/planning/autoware_path_generator/include/autoware/path_generator/node.hpp new file mode 100644 index 0000000000000..1aa1f90e5dab0 --- /dev/null +++ b/planning/autoware_path_generator/include/autoware/path_generator/node.hpp @@ -0,0 +1,64 @@ +// 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__NODE_HPP_ +#define AUTOWARE__PATH_GENERATOR__NODE_HPP_ + +#include "autoware/path_generator/path_handler.hpp" + +#include +#include + +#include + +namespace autoware::path_generator +{ +class PathGenerator : public rclcpp::Node +{ +public: + explicit PathGenerator(const rclcpp::NodeOptions & node_options); + +private: + // subscriber + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_planning_msgs::msg::LaneletRoute, universe_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_map_msgs::msg::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_; + + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; + autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr lanelet_map_bin_ptr_{nullptr}; + nav_msgs::msg::Odometry::ConstSharedPtr self_odometry_ptr_{nullptr}; + + std::unique_ptr path_handler_; + + void takeData(); + + bool isDataReady(); + + void run(); +}; +} // namespace autoware::path_generator + +#endif // AUTOWARE__PATH_GENERATOR__NODE_HPP_ diff --git a/planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp b/planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp new file mode 100644 index 0000000000000..8704b84a9f17f --- /dev/null +++ b/planning/autoware_path_generator/include/autoware/path_generator/path_handler.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 AUTOWARE__PATH_GENERATOR__PATH_HANDLER_HPP_ +#define AUTOWARE__PATH_GENERATOR__PATH_HANDLER_HPP_ + +#include +#include + +#include +#include +#include + +#include + +namespace autoware::path_generator +{ +using geometry_msgs::msg::Pose; +using ::path_generator::Params; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +class PathHandler +{ +public: + PathHandler() = default; + + explicit PathHandler(const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) + : vehicle_info_(vehicle_info) + { + } + + PathWithLaneId generateCenterLinePath(const Pose & current_pose, const Params & param); + + PathWithLaneId generateCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const Pose & current_pose, + const Params & param); + + PathHandler & setRoute( + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & lanelet_map_bin_ptr, + const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr); + + PathHandler & setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) + { + vehicle_info_ = vehicle_info; + return *this; + } + +private: + lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_{nullptr}; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_{nullptr}; + + 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_; + + std::optional vehicle_info_{std::nullopt}; + + PathWithLaneId getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end); + + lanelet::ConstLanelets getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const Pose & current_pose, + const double backward_path_length, const double forward_path_length); + + lanelet::ConstLanelets getLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, const double min_length); + + lanelet::ConstLanelets getLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, const double min_length); + + bool getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & next_lanelets); + + bool getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet & next_lanelet); + + bool getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & prev_lanelets); +}; +} // namespace autoware::path_generator + +#endif // AUTOWARE__PATH_GENERATOR__PATH_HANDLER_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 0000000000000..6aecb6a528253 --- /dev/null +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -0,0 +1,35 @@ +// 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 + +#include + +namespace autoware::path_generator +{ +using tier4_planning_msgs::msg::PathWithLaneId; + +namespace utils +{ +std::vector>> getWaypointGroups( + const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map); + +void removeOverlappingPoints(PathWithLaneId & path); +} // namespace utils +} // namespace autoware::path_generator + +#endif // AUTOWARE__PATH_GENERATOR__UTILS_HPP_ diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml new file mode 100644 index 0000000000000..3ae0426997c53 --- /dev/null +++ b/planning/autoware_path_generator/package.xml @@ -0,0 +1,26 @@ + + + + autoware_path_generator + 0.1.0 + The autoware_path_generator package + Mitsuhiro Sakamoto + Apache License 2.0 + + Mitsuhiro Sakamoto + + ament_cmake_auto + autoware_cmake + + autoware_lanelet2_extension + autoware_motion_utils + 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 0000000000000..87489f040087c --- /dev/null +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -0,0 +1,21 @@ +path_generator: + forward_path_length: + type: double + + backward_path_length: + type: double + + input_path_interval: + type: double + + enable_akima_spline_first: + type: bool + + enable_cog_on_centerline: + type: bool + + ego_nearest_dist_threshold: + type: double + + ego_nearest_yaw_threshold: + type: double diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp new file mode 100644 index 0000000000000..539deef25a020 --- /dev/null +++ b/planning/autoware_path_generator/src/node.cpp @@ -0,0 +1,121 @@ +// 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/node.hpp" + +#include +#include + +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_handler_ = std::make_unique( + autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()); + + // publisher + path_publisher_ = create_publisher("~/output/path", 1); + + { + const auto planning_hz = declare_parameter("planning_hz"); + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(planning_hz).period(), std::bind(&PathGenerator::run, this)); + } +} + +void PathGenerator::takeData() +{ + // route + if (const auto msg = route_subscriber_.takeData()) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty, ignoring..."); + } else { + route_ptr_ = msg; + if (lanelet_map_bin_ptr_) { + path_handler_->setRoute(lanelet_map_bin_ptr_, route_ptr_); + } + } + } + + // map + if (const auto msg = vector_map_subscriber_.takeData()) { + lanelet_map_bin_ptr_ = msg; + if (route_ptr_) { + path_handler_->setRoute(lanelet_map_bin_ptr_, route_ptr_); + } + } + + // velocity + if (const auto msg = odometry_subscriber_.takeData()) { + self_odometry_ptr_ = msg; + } +} + +// wait until mandatory data is ready +bool PathGenerator::isDataReady() +{ + const auto is_missing = [this](const std::string & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for %s...", name.c_str()); + return false; + }; + + if (!route_ptr_) { + return is_missing("route"); + } + if (!lanelet_map_bin_ptr_) { + return is_missing("map"); + } + + return true; +} + +void PathGenerator::run() +{ + takeData(); + if (!isDataReady()) { + return; + } + + const auto & current_pose = self_odometry_ptr_->pose.pose; + const auto param = param_listener_->get_params(); + + auto path = path_handler_->generateCenterLinePath(current_pose, param); + + if (!path.points.empty()) { + const auto current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, param.ego_nearest_dist_threshold, + param.ego_nearest_yaw_threshold); + path.points = autoware::motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, param.forward_path_length, + param.backward_path_length + param.input_path_interval); + + if (!path.points.empty()) { + path_publisher_->publish(path); + } else { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "path output is empty!"); + } + } else { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "path output is empty!"); + } +} +} // 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/path_handler.cpp b/planning/autoware_path_generator/src/path_handler.cpp new file mode 100644 index 0000000000000..5df16769478ac --- /dev/null +++ b/planning/autoware_path_generator/src/path_handler.cpp @@ -0,0 +1,420 @@ +// 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/path_handler.hpp" + +#include "autoware/path_generator/utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace autoware::path_generator +{ +namespace +{ +template +bool exists(const std::vector & vec, const T & item) +{ + return std::find(vec.begin(), vec.end(), item) != vec.end(); +} +} // namespace + +PathWithLaneId PathHandler::generateCenterLinePath(const Pose & current_pose, const Params & param) +{ + if (!route_ptr_) { + return PathWithLaneId{}; + } + + lanelet::ConstLanelet current_lane; + if (!lanelet::utils::query::getClosestLanelet(preferred_lanelets_, current_pose, ¤t_lane)) { + return PathWithLaneId{}; + } + + const auto lanelet_sequence = getLaneletSequence( + current_lane, current_pose, param.backward_path_length, param.forward_path_length); + + auto centerline_path = generateCenterLinePath(lanelet_sequence, current_pose, param); + centerline_path.header = route_ptr_->header; + + return centerline_path; +} + +PathWithLaneId PathHandler::generateCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const Pose & current_pose, const Params & param) +{ + if (lanelet_sequence.empty() || !vehicle_info_) { + return PathWithLaneId{}; + } + + const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, current_pose); + const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates + const auto s_start = std::max(0., s - param.backward_path_length); + const auto s_end = [&]() { + auto s_end = s + param.forward_path_length; + + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet_sequence.back(), next_lanelet)) { + const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + s_end = std::clamp(s_end, 0.0, lane_length); + } + + if (exists(goal_lanelets_, lanelet_sequence.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(lanelet_sequence, route_ptr_->goal_pose); + s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); + } + + return s_end; + }(); + + const auto raw_path_with_lane_id = getCenterLinePath(lanelet_sequence, s_start, s_end); + const auto resampled_path_with_lane_id = autoware::motion_utils::resamplePath( + raw_path_with_lane_id, param.input_path_interval, param.enable_akima_spline_first); + + // convert centerline, which we consider as CoG center, to rear wheel center + if (param.enable_cog_on_centerline) { + const auto rear_to_cog = vehicle_info_->vehicle_length_m / 2 - vehicle_info_->rear_overhang_m; + return autoware::motion_utils::convertToRearWheelCenter( + resampled_path_with_lane_id, rear_to_cog); + } + + return resampled_path_with_lane_id; +} + +PathHandler & PathHandler::setRoute( + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & lanelet_map_bin_ptr, + const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr) +{ + lanelet::utils::conversion::fromBinMsg( + *lanelet_map_bin_ptr, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + + if (!lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map_ptr_)) { + return *this; + } + route_ptr_ = route_ptr; + + route_lanelets_.clear(); + preferred_lanelets_.clear(); + start_lanelets_.clear(); + goal_lanelets_.clear(); + + if (!route_ptr_->segments.empty()) { + size_t primitive_size = 0; + for (const auto & route_section : route_ptr_->segments) { + primitive_size += route_section.primitives.size(); + } + route_lanelets_.reserve(primitive_size); + + for (const auto & route_section : route_ptr_->segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + const auto & lanelet = lanelet_map_ptr_->laneletLayer.get(id); + route_lanelets_.push_back(lanelet); + if (id == route_section.preferred_primitive.id) { + 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 = lanelet_map_ptr_->laneletLayer.get(primitive.id); + lanelets.push_back(lanelet); + } + }; + set_lanelets_from_segment(route_ptr_->segments.front(), start_lanelets_); + set_lanelets_from_segment(route_ptr_->segments.back(), goal_lanelets_); + } + + return *this; +} + +PathWithLaneId PathHandler::getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end) +{ + if (!lanelet_map_ptr_ || !traffic_rules_ptr_) { + return PathWithLaneId{}; + } + + PathWithLaneId centerline_path{}; + auto & path_points = centerline_path.points; + + const auto waypoint_groups = utils::getWaypointGroups(lanelet_sequence, *lanelet_map_ptr_); + + double s = 0.; + for (const auto & lanelet : lanelet_sequence) { + std::vector reference_points; + const auto & centerline = lanelet.centerline(); + + std::optional 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 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; + } + } + + const auto speed_limit = traffic_rules_ptr_->speedLimit(lanelet).speedLimit.value(); + for (const auto & reference_point : reference_points) { + PathPointWithLaneId path_point{}; + path_point.point.pose.position = reference_point; + path_point.lane_ids.push_back(lanelet.id()); + path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); + path_points.push_back(path_point); + } + } + + utils::removeOverlappingPoints(centerline_path); + + // append a point if having only one point so that yaw calculation would work + if (path_points.size() == 1) { + const auto & lane_id = path_points.front().lane_ids.front(); + const auto & lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id); + const auto & point = path_points.front().point.pose.position; + const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); + + PathPointWithLaneId path_point{}; + path_point.lane_ids.push_back(lane_id); + constexpr double ds = 0.1; + path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); + path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); + path_point.point.pose.position.z = point.z; + path_points.push_back(path_point); + } + + // set yaw to each point + { + auto it = path_points.begin(); + for (; it != std::prev(path_points.end()); ++it) { + const auto angle = autoware::universe_utils::calcAzimuthAngle( + it->point.pose.position, std::next(it)->point.pose.position); + it->point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(angle); + } + path_points.back().point.pose.orientation = it->point.pose.orientation; + } + + return centerline_path; +} + +lanelet::ConstLanelets PathHandler::getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const Pose & current_pose, + const double backward_path_length, const double forward_path_length) +{ + auto lanelet_sequence = [&]() { + const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + if (arc_coordinate.length < backward_path_length) { + return getLaneletSequenceUpTo(lanelet, backward_path_length); + } + return lanelet::ConstLanelets{}; + }(); + const auto lanelet_sequence_forward = getLaneletSequenceAfter(lanelet, forward_path_length); + + // loop check + if ( + !lanelet_sequence_forward.empty() && !lanelet_sequence.empty() && + lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) { + return lanelet_sequence_forward; + } + + lanelet_sequence.push_back(lanelet); + std::move( + lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(), + std::back_inserter(lanelet_sequence)); + + return lanelet_sequence; +} + +lanelet::ConstLanelets PathHandler::getLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, const double min_length) +{ + if (!routing_graph_ptr_) { + return lanelet::ConstLanelets{}; + } + + lanelet::ConstLanelets lanelet_sequence{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < min_length) { + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(current_lanelet, next_lanelet)) { + const auto next_lanes = routing_graph_ptr_->following(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); + } + + // loop check + if (lanelet.id() == next_lanelet.id()) { + break; + } + + lanelet_sequence.push_back(next_lanelet); + current_lanelet = next_lanelet; + length += lanelet::utils::getLaneletLength2d(next_lanelet); + } + + return lanelet_sequence; +} + +lanelet::ConstLanelets PathHandler::getLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, const double min_length) +{ + if (!routing_graph_ptr_) { + return lanelet::ConstLanelets{}; + } + + lanelet::ConstLanelets lanelet_sequence{}; + lanelet::ConstLanelets previous_lanelets; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < min_length) { + bool is_route_lanelets = getPreviousLaneletsWithinRoute(current_lanelet, previous_lanelets); + if (!is_route_lanelets) { + previous_lanelets = routing_graph_ptr_->previous(current_lanelet); + if (previous_lanelets.empty()) { + break; + } + } + + if ( + std::count_if(previous_lanelets.begin(), previous_lanelets.end(), [&](const auto & prev_llt) { + return lanelet.id() == prev_llt.id(); + }) >= (is_route_lanelets ? static_cast(previous_lanelets.size()) : 1)) { + break; + } + + for (const auto & prev_lanelet : previous_lanelets) { + if ( + lanelet.id() == prev_lanelet.id() || + std::any_of( + lanelet_sequence.begin(), lanelet_sequence.end(), + [&](const auto & llt) { return llt.id() == prev_lanelet.id(); }) || + exists(goal_lanelets_, prev_lanelet)) { + continue; + } + lanelet_sequence.push_back(prev_lanelet); + length += lanelet::utils::getLaneletLength2d(prev_lanelet); + current_lanelet = prev_lanelet; + break; + } + } + + std::reverse(lanelet_sequence.begin(), lanelet_sequence.end()); + return lanelet_sequence; +} + +bool PathHandler::getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & next_lanelets) +{ + if (!routing_graph_ptr_ || preferred_lanelets_.empty()) { + return false; + } + + if (exists(goal_lanelets_, lanelet)) { + return false; + } + + next_lanelets.clear(); + for (const auto & lanelet : routing_graph_ptr_->following(lanelet)) { + if (preferred_lanelets_.front().id() != lanelet.id() && exists(route_lanelets_, lanelet)) { + next_lanelets.push_back(lanelet); + } + } + + return !next_lanelets.empty(); +} + +bool PathHandler::getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet & next_lanelet) +{ + lanelet::ConstLanelets next_lanelets{}; + if (getNextLaneletsWithinRoute(lanelet, next_lanelets)) { + next_lanelet = next_lanelets.front(); + return true; + } + return false; +} + +bool PathHandler::getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & prev_lanelets) +{ + if (!routing_graph_ptr_) { + return false; + } + + if (exists(start_lanelets_, lanelet)) { + return false; + } + + prev_lanelets.clear(); + for (const auto & lanelet : routing_graph_ptr_->previous(lanelet)) { + if (exists(route_lanelets_, lanelet)) { + prev_lanelets.push_back(lanelet); + } + } + + return !prev_lanelets.empty(); +} +} // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp new file mode 100644 index 0000000000000..926b246f18d4a --- /dev/null +++ b/planning/autoware_path_generator/src/utils.cpp @@ -0,0 +1,88 @@ +// 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 + +namespace autoware::path_generator +{ +namespace utils +{ +std::vector>> getWaypointGroups( + const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map) +{ + std::vector>> waypoint_groups; + + double arc_length_offset = 0.; + for (const auto & lanelet : lanelet_sequence) { + if (!lanelet.hasAttribute("waypoints")) { + arc_length_offset += lanelet::utils::getLaneletLength2d(lanelet); + continue; + } + + const auto waypoints_id = lanelet.attribute("waypoints").asId().value(); + const auto & waypoints = lanelet_map.lineStringLayer.get(waypoints_id); + const auto get_interval_bound = + [&](const lanelet::ConstPoint3d & point, const double lateral_distance_factor) { + const auto arc_coordinates = + lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), lanelet::utils::to2D(point)); + return arc_length_offset + arc_coordinates.length + + lateral_distance_factor * std::abs(arc_coordinates.distance); + }; + + constexpr auto new_group_threshold = 1.0; + constexpr auto lateral_distance_factor = 10.0; + if ( + waypoint_groups.empty() || + lanelet::geometry::distance2d(waypoint_groups.back().first.back(), waypoints.front()) > + new_group_threshold) { + waypoint_groups.emplace_back().second.first = + get_interval_bound(waypoints.front(), -lateral_distance_factor); + } + waypoint_groups.back().second.second = + get_interval_bound(waypoints.back(), lateral_distance_factor); + + for (const auto & waypoint : waypoints) { + waypoint_groups.back().first.push_back(waypoint); + } + + arc_length_offset += lanelet::utils::getLaneletLength2d(lanelet); + } + + return waypoint_groups; +} + +void removeOverlappingPoints(PathWithLaneId & path) +{ + auto & filtered_path_end = path.points.front(); + for (auto it = std::next(path.points.begin()); it != path.points.end();) { + constexpr auto min_interval = 0.001; + if ( + autoware::universe_utils::calcDistance3d(filtered_path_end.point, it->point) < min_interval) { + filtered_path_end.lane_ids.push_back(it->lane_ids.front()); + filtered_path_end.point.longitudinal_velocity_mps = std::min( + it->point.longitudinal_velocity_mps, filtered_path_end.point.longitudinal_velocity_mps); + it = path.points.erase(it); + } else { + filtered_path_end = *it; + ++it; + } + } +} +} // namespace utils +} // namespace autoware::path_generator From ade918d808f8b9d4fbc6df85b5b7e2e12cd4c8e9 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 7 Nov 2024 14:42:37 +0900 Subject: [PATCH 02/34] specify src directory instead of individual files Signed-off-by: mitukou1109 --- planning/autoware_path_generator/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/autoware_path_generator/CMakeLists.txt b/planning/autoware_path_generator/CMakeLists.txt index 12aafeff4ebc1..bac4fb74982ce 100644 --- a/planning/autoware_path_generator/CMakeLists.txt +++ b/planning/autoware_path_generator/CMakeLists.txt @@ -9,9 +9,7 @@ generate_parameter_library(path_generator_parameters ) ament_auto_add_library(${PROJECT_NAME} SHARED - src/node.cpp - src/path_handler.cpp - src/utils.cpp + DIRECTORY src ) target_link_libraries(${PROJECT_NAME} From d1413dcb3a54479a5554c1f5812fd67743fd9cbc Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 7 Nov 2024 14:42:49 +0900 Subject: [PATCH 03/34] add maintainers Signed-off-by: mitukou1109 --- planning/autoware_path_generator/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index 3ae0426997c53..b25c04e36f9ec 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -4,9 +4,13 @@ autoware_path_generator 0.1.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 From 369b37efef2dc70459c0ed58a222fff3ddf7969c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 7 Nov 2024 15:47:19 +0900 Subject: [PATCH 04/34] reduce member variables as much as possible Signed-off-by: mitukou1109 --- .../autoware/path_generator/path_handler.hpp | 97 ---- .../autoware/path_generator/planner_data.hpp | 49 ++ .../include/autoware/path_generator/utils.hpp | 32 +- planning/autoware_path_generator/src/node.cpp | 158 +++++-- .../autoware/path_generator => src}/node.hpp | 43 +- .../src/path_handler.cpp | 420 ------------------ .../autoware_path_generator/src/utils.cpp | 376 ++++++++++++++++ 7 files changed, 594 insertions(+), 581 deletions(-) delete mode 100644 planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp create mode 100644 planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp rename planning/autoware_path_generator/{include/autoware/path_generator => src}/node.hpp (54%) delete mode 100644 planning/autoware_path_generator/src/path_handler.cpp diff --git a/planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp b/planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp deleted file mode 100644 index 8704b84a9f17f..0000000000000 --- a/planning/autoware_path_generator/include/autoware/path_generator/path_handler.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// 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__PATH_HANDLER_HPP_ -#define AUTOWARE__PATH_GENERATOR__PATH_HANDLER_HPP_ - -#include -#include - -#include -#include -#include - -#include - -namespace autoware::path_generator -{ -using geometry_msgs::msg::Pose; -using ::path_generator::Params; -using tier4_planning_msgs::msg::PathPointWithLaneId; -using tier4_planning_msgs::msg::PathWithLaneId; - -class PathHandler -{ -public: - PathHandler() = default; - - explicit PathHandler(const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) - : vehicle_info_(vehicle_info) - { - } - - PathWithLaneId generateCenterLinePath(const Pose & current_pose, const Params & param); - - PathWithLaneId generateCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const Pose & current_pose, - const Params & param); - - PathHandler & setRoute( - const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & lanelet_map_bin_ptr, - const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr); - - PathHandler & setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) - { - vehicle_info_ = vehicle_info; - return *this; - } - -private: - lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_{nullptr}; - lanelet::routing::RoutingGraphPtr routing_graph_ptr_{nullptr}; - - 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_; - - std::optional vehicle_info_{std::nullopt}; - - PathWithLaneId getCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end); - - lanelet::ConstLanelets getLaneletSequence( - const lanelet::ConstLanelet & lanelet, const Pose & current_pose, - const double backward_path_length, const double forward_path_length); - - lanelet::ConstLanelets getLaneletSequenceAfter( - const lanelet::ConstLanelet & lanelet, const double min_length); - - lanelet::ConstLanelets getLaneletSequenceUpTo( - const lanelet::ConstLanelet & lanelet, const double min_length); - - bool getNextLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & next_lanelets); - - bool getNextLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet & next_lanelet); - - bool getPreviousLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & prev_lanelets); -}; -} // namespace autoware::path_generator - -#endif // AUTOWARE__PATH_GENERATOR__PATH_HANDLER_HPP_ diff --git a/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp b/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp new file mode 100644 index 0000000000000..9a93afda92c22 --- /dev/null +++ b/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp @@ -0,0 +1,49 @@ +// 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__PLANNER_DATA_HPP_ +#define AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ + +#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}; + + 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; + + double forward_path_length; + double backward_path_length; + double input_path_interval; + double enable_akima_spline_first; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; +}; +} // namespace autoware::path_generator + +#endif // AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ \ No newline at end of file diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 6aecb6a528253..825207ccd8ec1 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -15,16 +15,46 @@ #ifndef AUTOWARE__PATH_GENERATOR__UTILS_HPP_ #define AUTOWARE__PATH_GENERATOR__UTILS_HPP_ +#include "autoware/path_generator/planner_data.hpp" + #include -#include +#include namespace autoware::path_generator { +using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; namespace utils { +std::optional generateCenterLinePath(const PlannerData & planner_data); + +std::optional getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const PlannerData & planner_data); + +std::optional getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, + const PlannerData & planner_data); + +std::optional getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::optional getLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::optional getLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::optional getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::optional getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +std::optional getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + std::vector>> getWaypointGroups( const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map); diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 539deef25a020..7811d164b471a 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/path_generator/node.hpp" +#include "node.hpp" + +#include "autoware/path_generator/utils.hpp" -#include #include +#include namespace autoware::path_generator { @@ -25,9 +27,6 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) param_listener_ = std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface()); - path_handler_ = std::make_unique( - autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()); - // publisher path_publisher_ = create_publisher("~/output/path", 1); @@ -38,81 +37,142 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) } } -void PathGenerator::takeData() +void PathGenerator::run() +{ + const auto input_data = takeData(); + + const auto path = planPath(input_data); + if (!path) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); + } + + path_publisher_->publish(*path); +} + +PathGenerator::InputData PathGenerator::takeData() { + 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 { - route_ptr_ = msg; - if (lanelet_map_bin_ptr_) { - path_handler_->setRoute(lanelet_map_bin_ptr_, route_ptr_); - } + input_data.route_ptr = msg; } } // map if (const auto msg = vector_map_subscriber_.takeData()) { - lanelet_map_bin_ptr_ = msg; - if (route_ptr_) { - path_handler_->setRoute(lanelet_map_bin_ptr_, route_ptr_); - } + input_data.lanelet_map_bin_ptr = msg; } // velocity if (const auto msg = odometry_subscriber_.takeData()) { - self_odometry_ptr_ = msg; + input_data.odometry_ptr = msg; } + + return input_data; } -// wait until mandatory data is ready -bool PathGenerator::isDataReady() +std::optional PathGenerator::planPath(const InputData & input_data) { - const auto is_missing = [this](const std::string & name) { - RCLCPP_INFO_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting for %s...", name.c_str()); - return false; - }; - - if (!route_ptr_) { - return is_missing("route"); + if (!updatePlannerData(input_data, param_listener_->get_params())) { + return std::nullopt; } - if (!lanelet_map_bin_ptr_) { - return is_missing("map"); + + auto path = utils::generateCenterLinePath(planner_data_); + 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 true; + return path; } -void PathGenerator::run() +bool PathGenerator::updatePlannerData( + const InputData & input_data, const ::path_generator::Params & param) { - takeData(); - if (!isDataReady()) { - return; + if (!planner_data_.lanelet_map_ptr && !input_data.lanelet_map_bin_ptr) { + return false; } - const auto & current_pose = self_odometry_ptr_->pose.pose; - const auto param = param_listener_->get_params(); + if (!planner_data_.route_ptr && !input_data.route_ptr) { + return false; + } - auto path = path_handler_->generateCenterLinePath(current_pose, param); + if (input_data.lanelet_map_bin_ptr) { + 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 (!path.points.empty()) { - const auto current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, param.ego_nearest_dist_threshold, - param.ego_nearest_yaw_threshold); - path.points = autoware::motion_utils::cropPoints( - path.points, current_pose.position, current_seg_idx, param.forward_path_length, - param.backward_path_length + param.input_path_interval); + if (input_data.route_ptr) { + if (!lanelet::utils::route::isRouteValid( + *input_data.route_ptr, planner_data_.lanelet_map_ptr)) { + return false; + } + setRoute(input_data.route_ptr); + } - if (!path.points.empty()) { - path_publisher_->publish(path); - } else { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "path output is empty!"); + if (input_data.odometry_ptr) { + planner_data_.current_pose = input_data.odometry_ptr->pose.pose; + } + + planner_data_.forward_path_length = param.forward_path_length; + planner_data_.backward_path_length = param.backward_path_length; + planner_data_.input_path_interval = param.input_path_interval; + planner_data_.enable_akima_spline_first = param.enable_akima_spline_first; + planner_data_.ego_nearest_dist_threshold = param.ego_nearest_dist_threshold; + planner_data_.ego_nearest_yaw_threshold = param.ego_nearest_yaw_threshold; + + return true; +} + +void PathGenerator::setRoute(const LaneletRoute::ConstSharedPtr & route_ptr) +{ + planner_data_.route_ptr = route_ptr; + + planner_data_.route_lanelets.clear(); + planner_data_.preferred_lanelets.clear(); + planner_data_.start_lanelets.clear(); + planner_data_.goal_lanelets.clear(); + + if (!planner_data_.route_ptr->segments.empty()) { + size_t primitives_num = 0; + for (const auto & route_section : planner_data_.route_ptr->segments) { + primitives_num += route_section.primitives.size(); + } + planner_data_.route_lanelets.reserve(primitives_num); + + for (const auto & route_section : planner_data_.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); + } + } } - } else { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "path output is empty!"); + + 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( + planner_data_.route_ptr->segments.front(), planner_data_.start_lanelets); + set_lanelets_from_segment( + planner_data_.route_ptr->segments.back(), planner_data_.goal_lanelets); } } } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/include/autoware/path_generator/node.hpp b/planning/autoware_path_generator/src/node.hpp similarity index 54% rename from planning/autoware_path_generator/include/autoware/path_generator/node.hpp rename to planning/autoware_path_generator/src/node.hpp index 1aa1f90e5dab0..0487c8ca13868 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -15,49 +15,64 @@ #ifndef AUTOWARE__PATH_GENERATOR__NODE_HPP_ #define AUTOWARE__PATH_GENERATOR__NODE_HPP_ -#include "autoware/path_generator/path_handler.hpp" +#include "autoware/path_generator/planner_data.hpp" #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 tier4_planning_msgs::msg::PathWithLaneId; + 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< - autoware_planning_msgs::msg::LaneletRoute, universe_utils::polling_policy::Newest> + LaneletRoute, universe_utils::polling_policy::Newest> route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber< - autoware_map_msgs::msg::LaneletMapBin, universe_utils::polling_policy::Newest> + 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"}; + autoware::universe_utils::InterProcessPollingSubscriber odometry_subscriber_{ + this, "~/input/odometry"}; // publisher - rclcpp::Publisher::SharedPtr path_publisher_; + rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr<::path_generator::ParamListener> param_listener_; - autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; - autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr lanelet_map_bin_ptr_{nullptr}; - nav_msgs::msg::Odometry::ConstSharedPtr self_odometry_ptr_{nullptr}; + PlannerData planner_data_; - std::unique_ptr path_handler_; + void run(); - void takeData(); + InputData takeData(); - bool isDataReady(); + std::optional planPath(const InputData & input_data); - void run(); + bool updatePlannerData(const InputData & input_data, const ::path_generator::Params & param); + + void setRoute(const LaneletRoute::ConstSharedPtr & route_ptr); }; } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/path_handler.cpp b/planning/autoware_path_generator/src/path_handler.cpp deleted file mode 100644 index 5df16769478ac..0000000000000 --- a/planning/autoware_path_generator/src/path_handler.cpp +++ /dev/null @@ -1,420 +0,0 @@ -// 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/path_handler.hpp" - -#include "autoware/path_generator/utils.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace autoware::path_generator -{ -namespace -{ -template -bool exists(const std::vector & vec, const T & item) -{ - return std::find(vec.begin(), vec.end(), item) != vec.end(); -} -} // namespace - -PathWithLaneId PathHandler::generateCenterLinePath(const Pose & current_pose, const Params & param) -{ - if (!route_ptr_) { - return PathWithLaneId{}; - } - - lanelet::ConstLanelet current_lane; - if (!lanelet::utils::query::getClosestLanelet(preferred_lanelets_, current_pose, ¤t_lane)) { - return PathWithLaneId{}; - } - - const auto lanelet_sequence = getLaneletSequence( - current_lane, current_pose, param.backward_path_length, param.forward_path_length); - - auto centerline_path = generateCenterLinePath(lanelet_sequence, current_pose, param); - centerline_path.header = route_ptr_->header; - - return centerline_path; -} - -PathWithLaneId PathHandler::generateCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const Pose & current_pose, const Params & param) -{ - if (lanelet_sequence.empty() || !vehicle_info_) { - return PathWithLaneId{}; - } - - const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, current_pose); - const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates - const auto s_start = std::max(0., s - param.backward_path_length); - const auto s_end = [&]() { - auto s_end = s + param.forward_path_length; - - lanelet::ConstLanelet next_lanelet; - if (!getNextLaneletWithinRoute(lanelet_sequence.back(), next_lanelet)) { - const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); - s_end = std::clamp(s_end, 0.0, lane_length); - } - - if (exists(goal_lanelets_, lanelet_sequence.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence, route_ptr_->goal_pose); - s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); - } - - return s_end; - }(); - - const auto raw_path_with_lane_id = getCenterLinePath(lanelet_sequence, s_start, s_end); - const auto resampled_path_with_lane_id = autoware::motion_utils::resamplePath( - raw_path_with_lane_id, param.input_path_interval, param.enable_akima_spline_first); - - // convert centerline, which we consider as CoG center, to rear wheel center - if (param.enable_cog_on_centerline) { - const auto rear_to_cog = vehicle_info_->vehicle_length_m / 2 - vehicle_info_->rear_overhang_m; - return autoware::motion_utils::convertToRearWheelCenter( - resampled_path_with_lane_id, rear_to_cog); - } - - return resampled_path_with_lane_id; -} - -PathHandler & PathHandler::setRoute( - const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & lanelet_map_bin_ptr, - const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr) -{ - lanelet::utils::conversion::fromBinMsg( - *lanelet_map_bin_ptr, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - - if (!lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map_ptr_)) { - return *this; - } - route_ptr_ = route_ptr; - - route_lanelets_.clear(); - preferred_lanelets_.clear(); - start_lanelets_.clear(); - goal_lanelets_.clear(); - - if (!route_ptr_->segments.empty()) { - size_t primitive_size = 0; - for (const auto & route_section : route_ptr_->segments) { - primitive_size += route_section.primitives.size(); - } - route_lanelets_.reserve(primitive_size); - - for (const auto & route_section : route_ptr_->segments) { - for (const auto & primitive : route_section.primitives) { - const auto id = primitive.id; - const auto & lanelet = lanelet_map_ptr_->laneletLayer.get(id); - route_lanelets_.push_back(lanelet); - if (id == route_section.preferred_primitive.id) { - 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 = lanelet_map_ptr_->laneletLayer.get(primitive.id); - lanelets.push_back(lanelet); - } - }; - set_lanelets_from_segment(route_ptr_->segments.front(), start_lanelets_); - set_lanelets_from_segment(route_ptr_->segments.back(), goal_lanelets_); - } - - return *this; -} - -PathWithLaneId PathHandler::getCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end) -{ - if (!lanelet_map_ptr_ || !traffic_rules_ptr_) { - return PathWithLaneId{}; - } - - PathWithLaneId centerline_path{}; - auto & path_points = centerline_path.points; - - const auto waypoint_groups = utils::getWaypointGroups(lanelet_sequence, *lanelet_map_ptr_); - - double s = 0.; - for (const auto & lanelet : lanelet_sequence) { - std::vector reference_points; - const auto & centerline = lanelet.centerline(); - - std::optional 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 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; - } - } - - const auto speed_limit = traffic_rules_ptr_->speedLimit(lanelet).speedLimit.value(); - for (const auto & reference_point : reference_points) { - PathPointWithLaneId path_point{}; - path_point.point.pose.position = reference_point; - path_point.lane_ids.push_back(lanelet.id()); - path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); - path_points.push_back(path_point); - } - } - - utils::removeOverlappingPoints(centerline_path); - - // append a point if having only one point so that yaw calculation would work - if (path_points.size() == 1) { - const auto & lane_id = path_points.front().lane_ids.front(); - const auto & lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id); - const auto & point = path_points.front().point.pose.position; - const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); - - PathPointWithLaneId path_point{}; - path_point.lane_ids.push_back(lane_id); - constexpr double ds = 0.1; - path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); - path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); - path_point.point.pose.position.z = point.z; - path_points.push_back(path_point); - } - - // set yaw to each point - { - auto it = path_points.begin(); - for (; it != std::prev(path_points.end()); ++it) { - const auto angle = autoware::universe_utils::calcAzimuthAngle( - it->point.pose.position, std::next(it)->point.pose.position); - it->point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(angle); - } - path_points.back().point.pose.orientation = it->point.pose.orientation; - } - - return centerline_path; -} - -lanelet::ConstLanelets PathHandler::getLaneletSequence( - const lanelet::ConstLanelet & lanelet, const Pose & current_pose, - const double backward_path_length, const double forward_path_length) -{ - auto lanelet_sequence = [&]() { - const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); - if (arc_coordinate.length < backward_path_length) { - return getLaneletSequenceUpTo(lanelet, backward_path_length); - } - return lanelet::ConstLanelets{}; - }(); - const auto lanelet_sequence_forward = getLaneletSequenceAfter(lanelet, forward_path_length); - - // loop check - if ( - !lanelet_sequence_forward.empty() && !lanelet_sequence.empty() && - lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) { - return lanelet_sequence_forward; - } - - lanelet_sequence.push_back(lanelet); - std::move( - lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(), - std::back_inserter(lanelet_sequence)); - - return lanelet_sequence; -} - -lanelet::ConstLanelets PathHandler::getLaneletSequenceAfter( - const lanelet::ConstLanelet & lanelet, const double min_length) -{ - if (!routing_graph_ptr_) { - return lanelet::ConstLanelets{}; - } - - lanelet::ConstLanelets lanelet_sequence{}; - auto current_lanelet = lanelet; - auto length = 0.; - - while (rclcpp::ok() && length < min_length) { - lanelet::ConstLanelet next_lanelet; - if (!getNextLaneletWithinRoute(current_lanelet, next_lanelet)) { - const auto next_lanes = routing_graph_ptr_->following(current_lanelet); - if (next_lanes.empty()) { - break; - } - next_lanelet = next_lanes.front(); - } - - // loop check - if (lanelet.id() == next_lanelet.id()) { - break; - } - - lanelet_sequence.push_back(next_lanelet); - current_lanelet = next_lanelet; - length += lanelet::utils::getLaneletLength2d(next_lanelet); - } - - return lanelet_sequence; -} - -lanelet::ConstLanelets PathHandler::getLaneletSequenceUpTo( - const lanelet::ConstLanelet & lanelet, const double min_length) -{ - if (!routing_graph_ptr_) { - return lanelet::ConstLanelets{}; - } - - lanelet::ConstLanelets lanelet_sequence{}; - lanelet::ConstLanelets previous_lanelets; - auto current_lanelet = lanelet; - auto length = 0.; - - while (rclcpp::ok() && length < min_length) { - bool is_route_lanelets = getPreviousLaneletsWithinRoute(current_lanelet, previous_lanelets); - if (!is_route_lanelets) { - previous_lanelets = routing_graph_ptr_->previous(current_lanelet); - if (previous_lanelets.empty()) { - break; - } - } - - if ( - std::count_if(previous_lanelets.begin(), previous_lanelets.end(), [&](const auto & prev_llt) { - return lanelet.id() == prev_llt.id(); - }) >= (is_route_lanelets ? static_cast(previous_lanelets.size()) : 1)) { - break; - } - - for (const auto & prev_lanelet : previous_lanelets) { - if ( - lanelet.id() == prev_lanelet.id() || - std::any_of( - lanelet_sequence.begin(), lanelet_sequence.end(), - [&](const auto & llt) { return llt.id() == prev_lanelet.id(); }) || - exists(goal_lanelets_, prev_lanelet)) { - continue; - } - lanelet_sequence.push_back(prev_lanelet); - length += lanelet::utils::getLaneletLength2d(prev_lanelet); - current_lanelet = prev_lanelet; - break; - } - } - - std::reverse(lanelet_sequence.begin(), lanelet_sequence.end()); - return lanelet_sequence; -} - -bool PathHandler::getNextLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & next_lanelets) -{ - if (!routing_graph_ptr_ || preferred_lanelets_.empty()) { - return false; - } - - if (exists(goal_lanelets_, lanelet)) { - return false; - } - - next_lanelets.clear(); - for (const auto & lanelet : routing_graph_ptr_->following(lanelet)) { - if (preferred_lanelets_.front().id() != lanelet.id() && exists(route_lanelets_, lanelet)) { - next_lanelets.push_back(lanelet); - } - } - - return !next_lanelets.empty(); -} - -bool PathHandler::getNextLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet & next_lanelet) -{ - lanelet::ConstLanelets next_lanelets{}; - if (getNextLaneletsWithinRoute(lanelet, next_lanelets)) { - next_lanelet = next_lanelets.front(); - return true; - } - return false; -} - -bool PathHandler::getPreviousLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets & prev_lanelets) -{ - if (!routing_graph_ptr_) { - return false; - } - - if (exists(start_lanelets_, lanelet)) { - return false; - } - - prev_lanelets.clear(); - for (const auto & lanelet : routing_graph_ptr_->previous(lanelet)) { - if (exists(route_lanelets_, lanelet)) { - prev_lanelets.push_back(lanelet); - } - } - - return !prev_lanelets.empty(); -} -} // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 926b246f18d4a..5af5c53bb4c21 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -14,7 +14,12 @@ #include "autoware/path_generator/utils.hpp" +#include +#include +#include #include +#include +#include #include #include @@ -23,6 +28,377 @@ 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 generateCenterLinePath(const PlannerData & planner_data) +{ + lanelet::ConstLanelet current_lane; + if (!lanelet::utils::query::getClosestLanelet( + planner_data.preferred_lanelets, planner_data.current_pose, ¤t_lane)) { + return std::nullopt; + } + + const auto lanelet_sequence = getLaneletSequence(current_lane, planner_data); + if (!lanelet_sequence) { + return std::nullopt; + } + + const auto centerline_path = getCenterLinePath(*lanelet_sequence, planner_data); + if (!centerline_path) { + return std::nullopt; + } + + return centerline_path; +} + +std::optional getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const PlannerData & planner_data) +{ + if (lanelet_sequence.empty()) { + return std::nullopt; + } + + if (!planner_data.route_ptr) { + return std::nullopt; + } + + const auto arc_coordinates = + lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data.current_pose); + const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates + const auto s_start = std::max(0., s - planner_data.backward_path_length); + const auto s_end = [&]() { + auto s_end = s + planner_data.forward_path_length; + + if (!getNextLaneletWithinRoute(lanelet_sequence.back(), planner_data)) { + const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + s_end = std::clamp(s_end, 0.0, lane_length); + } + + if (exists(planner_data.goal_lanelets, lanelet_sequence.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data.route_ptr->goal_pose); + s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); + } + + return s_end; + }(); + + const auto raw_centerline_path = + getCenterLinePath(lanelet_sequence, s_start, s_end, planner_data); + if (!raw_centerline_path) { + return std::nullopt; + } + + auto centerline_path = autoware::motion_utils::resamplePath( + *raw_centerline_path, planner_data.input_path_interval, planner_data.enable_akima_spline_first); + + const auto current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + centerline_path.points, planner_data.current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + + centerline_path.points = autoware::motion_utils::cropPoints( + centerline_path.points, planner_data.current_pose.position, current_seg_idx, + planner_data.forward_path_length, + planner_data.backward_path_length + planner_data.input_path_interval); + + return centerline_path; +} + +std::optional getCenterLinePath( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, + const PlannerData & planner_data) +{ + if (!planner_data.lanelet_map_ptr || !planner_data.traffic_rules_ptr || !planner_data.route_ptr) { + return std::nullopt; + } + + PathWithLaneId centerline_path{}; + auto & path_points = centerline_path.points; + + const auto waypoint_groups = + utils::getWaypointGroups(lanelet_sequence, *planner_data.lanelet_map_ptr); + + double s = 0.; + for (const auto & lanelet : lanelet_sequence) { + std::vector reference_points; + const auto & centerline = lanelet.centerline(); + + std::optional 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 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; + } + } + + const auto speed_limit = planner_data.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); + for (const auto & reference_point : reference_points) { + PathPointWithLaneId path_point{}; + path_point.point.pose.position = reference_point; + path_point.lane_ids.push_back(lanelet.id()); + path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); + path_points.push_back(path_point); + } + } + + utils::removeOverlappingPoints(centerline_path); + + // append a point if having only one point so that yaw calculation would work + if (path_points.size() == 1) { + const auto & lane_id = path_points.front().lane_ids.front(); + const auto & lanelet = planner_data.lanelet_map_ptr->laneletLayer.get(lane_id); + const auto & point = path_points.front().point.pose.position; + const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); + + PathPointWithLaneId path_point{}; + path_point.lane_ids.push_back(lane_id); + constexpr double ds = 0.1; + path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); + path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); + path_point.point.pose.position.z = point.z; + path_points.push_back(path_point); + } + + // set yaw to each point + for (auto it = path_points.begin(); it != std::prev(path_points.end()); ++it) { + const auto angle = autoware::universe_utils::calcAzimuthAngle( + it->point.pose.position, std::next(it)->point.pose.position); + it->point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(angle); + } + path_points.back().point.pose.orientation = + std::prev(path_points.end(), 2)->point.pose.orientation; + + centerline_path.header = planner_data.route_ptr->header; + return centerline_path; +} + +std::optional getLaneletSequence( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + auto lanelet_sequence = [&]() -> std::optional { + const auto arc_coordinate = + lanelet::utils::getArcCoordinates({lanelet}, planner_data.current_pose); + if (arc_coordinate.length < planner_data.backward_path_length) { + return getLaneletSequenceUpTo(lanelet, planner_data); + } + return lanelet::ConstLanelets{}; + }(); + if (!lanelet_sequence) { + return std::nullopt; + } + + const auto lanelet_sequence_forward = getLaneletSequenceAfter(lanelet, planner_data); + if (!lanelet_sequence_forward) { + return std::nullopt; + } + + // loop check + if ( + !lanelet_sequence_forward->empty() && !lanelet_sequence->empty() && + lanelet_sequence->back().id() == lanelet_sequence_forward->front().id()) { + return lanelet_sequence_forward; + } + + lanelet_sequence->push_back(lanelet); + std::move( + lanelet_sequence_forward->begin(), lanelet_sequence_forward->end(), + std::back_inserter(*lanelet_sequence)); + + return lanelet_sequence; +} + +std::optional getLaneletSequenceAfter( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (!planner_data.routing_graph_ptr) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelet_sequence{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < planner_data.forward_path_length) { + auto next_lanelet = getNextLaneletWithinRoute(current_lanelet, planner_data); + if (!next_lanelet) { + const auto next_lanelets = planner_data.routing_graph_ptr->following(current_lanelet); + if (next_lanelets.empty()) { + break; + } + next_lanelet = next_lanelets.front(); + } + + // loop check + if (lanelet.id() == next_lanelet->id()) { + break; + } + + lanelet_sequence.push_back(*next_lanelet); + current_lanelet = *next_lanelet; + length += lanelet::utils::getLaneletLength2d(*next_lanelet); + } + + return lanelet_sequence; +} + +std::optional getLaneletSequenceUpTo( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (!planner_data.routing_graph_ptr) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelet_sequence{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < planner_data.backward_path_length) { + auto previous_lanelets = getPreviousLaneletsWithinRoute(current_lanelet, planner_data); + if (!previous_lanelets) { + previous_lanelets = planner_data.routing_graph_ptr->previous(current_lanelet); + if (previous_lanelets->empty()) { + break; + } + } + + if ( + std::count_if( + previous_lanelets->begin(), previous_lanelets->end(), [&](const auto & prev_llt) { + return lanelet.id() == prev_llt.id(); + }) >= (previous_lanelets ? static_cast(previous_lanelets->size()) : 1)) { + break; + } + + for (const auto & prev_lanelet : *previous_lanelets) { + if ( + lanelet.id() == prev_lanelet.id() || + std::any_of( + lanelet_sequence.begin(), lanelet_sequence.end(), + [&](const auto & llt) { return llt.id() == prev_lanelet.id(); }) || + exists(planner_data.goal_lanelets, prev_lanelet)) { + continue; + } + lanelet_sequence.push_back(prev_lanelet); + length += lanelet::utils::getLaneletLength2d(prev_lanelet); + current_lanelet = prev_lanelet; + break; + } + } + + std::reverse(lanelet_sequence.begin(), lanelet_sequence.end()); + return lanelet_sequence; +} + +std::optional getNextLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + const auto next_lanelets = getNextLaneletsWithinRoute(lanelet, planner_data); + + if (next_lanelets) { + return next_lanelets->front(); + } + return std::nullopt; +} + +std::optional getNextLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (!planner_data.routing_graph_ptr) { + return std::nullopt; + } + + if (planner_data.preferred_lanelets.empty()) { + return std::nullopt; + } + + if (exists(planner_data.goal_lanelets, lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelets next_lanelets{}; + for (const auto & lanelet : planner_data.routing_graph_ptr->following(lanelet)) { + if ( + planner_data.preferred_lanelets.front().id() != lanelet.id() && + exists(planner_data.route_lanelets, lanelet)) { + next_lanelets.push_back(lanelet); + } + } + + if (next_lanelets.empty()) { + return std::nullopt; + } + return next_lanelets; +} + +std::optional getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (!planner_data.routing_graph_ptr) { + return std::nullopt; + } + + if (exists(planner_data.start_lanelets, lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelets prev_lanelets{}; + for (const auto & lanelet : planner_data.routing_graph_ptr->previous(lanelet)) { + if (exists(planner_data.route_lanelets, lanelet)) { + prev_lanelets.push_back(lanelet); + } + } + + if (prev_lanelets.empty()) { + return std::nullopt; + } + return prev_lanelets; +} + std::vector>> getWaypointGroups( const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map) { From a6ef69cf10b5199f50ba017984a66bbdfd6fc60a Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 7 Nov 2024 15:47:32 +0900 Subject: [PATCH 05/34] remove unnecessary parameter Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/planner_data.hpp | 2 +- .../param/path_generator_parameters.yaml | 3 --- planning/autoware_path_generator/src/node.hpp | 6 +++--- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp b/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp index 9a93afda92c22..091ae7ef7e4b6 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp @@ -46,4 +46,4 @@ struct PlannerData }; } // namespace autoware::path_generator -#endif // AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ \ No newline at end of file +#endif // AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml index 87489f040087c..f6da9c468f1b1 100644 --- a/planning/autoware_path_generator/param/path_generator_parameters.yaml +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -11,9 +11,6 @@ path_generator: enable_akima_spline_first: type: bool - enable_cog_on_centerline: - type: bool - ego_nearest_dist_threshold: type: double diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 0487c8ca13868..99ae9629d3bfb 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__PATH_GENERATOR__NODE_HPP_ -#define AUTOWARE__PATH_GENERATOR__NODE_HPP_ +#ifndef NODE_HPP_ +#define NODE_HPP_ #include "autoware/path_generator/planner_data.hpp" @@ -76,4 +76,4 @@ class PathGenerator : public rclcpp::Node }; } // namespace autoware::path_generator -#endif // AUTOWARE__PATH_GENERATOR__NODE_HPP_ +#endif // NODE_HPP_ From d6358d64e924702dad6ce48d0e732b9915249bbb Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 14:58:10 +0900 Subject: [PATCH 06/34] reduce member variables Signed-off-by: mitukou1109 --- .../{planner_data.hpp => common_structs.hpp} | 23 +- .../include/autoware/path_generator/utils.hpp | 37 +- planning/autoware_path_generator/src/node.cpp | 323 ++++++++++++++---- planning/autoware_path_generator/src/node.hpp | 23 +- .../autoware_path_generator/src/utils.cpp | 231 ++----------- 5 files changed, 322 insertions(+), 315 deletions(-) rename planning/autoware_path_generator/include/autoware/path_generator/{planner_data.hpp => common_structs.hpp} (66%) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp similarity index 66% rename from planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp rename to planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp index 091ae7ef7e4b6..b9a0a6761d815 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/planner_data.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ -#define AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ +#ifndef AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_ +#define AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_ #include @@ -28,22 +28,13 @@ struct PlannerData 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; - - double forward_path_length; - double backward_path_length; - double input_path_interval; - double enable_akima_spline_first; - double ego_nearest_dist_threshold; - double ego_nearest_yaw_threshold; + lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets preferred_lanelets{}; + lanelet::ConstLanelets start_lanelets{}; + lanelet::ConstLanelets goal_lanelets{}; }; } // namespace autoware::path_generator -#endif // AUTOWARE__PATH_GENERATOR__PLANNER_DATA_HPP_ +#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 index 825207ccd8ec1..0ef1d7ed9b4fa 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PATH_GENERATOR__UTILS_HPP_ #define AUTOWARE__PATH_GENERATOR__UTILS_HPP_ -#include "autoware/path_generator/planner_data.hpp" +#include "autoware/path_generator/common_structs.hpp" #include @@ -28,37 +28,32 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace utils { -std::optional generateCenterLinePath(const PlannerData & planner_data); +std::optional get_lanelet_sequence( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const geometry_msgs::msg::Pose & current_pose, const double forward_path_length, + const double backward_path_length); -std::optional getCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const PlannerData & planner_data); +std::optional get_lanelet_sequence_after( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const double forward_path_length); -std::optional getCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, - const PlannerData & planner_data); +std::optional get_lanelet_sequence_up_to( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const double backward_path_length); -std::optional getLaneletSequence( +std::optional get_next_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); -std::optional getLaneletSequenceAfter( +std::optional get_next_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); -std::optional getLaneletSequenceUpTo( +std::optional get_previous_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); -std::optional getNextLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); - -std::optional getNextLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); - -std::optional getPreviousLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); - -std::vector>> getWaypointGroups( +std::vector>> get_waypoint_groups( const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map); -void removeOverlappingPoints(PathWithLaneId & path); +void remove_overlapping_points(PathWithLaneId & path); } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 7811d164b471a..ad4400dfac770 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -16,8 +16,14 @@ #include "autoware/path_generator/utils.hpp" +#include +#include +#include #include -#include +#include +#include + +#include namespace autoware::path_generator { @@ -27,7 +33,6 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) param_listener_ = std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface()); - // publisher path_publisher_ = create_publisher("~/output/path", 1); { @@ -39,17 +44,21 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) void PathGenerator::run() { - const auto input_data = takeData(); + const auto input_data = take_data(); + if (!is_data_ready(input_data)) { + return; + } - const auto path = planPath(input_data); + 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::takeData() +PathGenerator::InputData PathGenerator::take_data() { InputData input_data; @@ -75,13 +84,44 @@ PathGenerator::InputData PathGenerator::takeData() return input_data; } -std::optional PathGenerator::planPath(const InputData & input_data) +bool PathGenerator::is_data_ready(const InputData & input_data) { - if (!updatePlannerData(input_data, param_listener_->get_params())) { - return std::nullopt; + const auto missing = [this](const std::string & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for %s", name.c_str()); + return false; + }; + + if (!input_data.lanelet_map_bin_ptr && !planner_data_.lanelet_map_ptr) { + return missing("map"); + } + + if (!input_data.route_ptr && !planner_data_.route_ptr) { + return missing("route"); + } + + if (!input_data.odometry_ptr) { + return missing("odometry"); } - auto path = utils::generateCenterLinePath(planner_data_); + return true; +} + +std::optional PathGenerator::plan_path(const InputData & input_data) +{ + if (input_data.lanelet_map_bin_ptr) { + 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); + } + + const auto path = generate_centerline_path( + input_data.route_ptr, 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; @@ -93,87 +133,236 @@ std::optional PathGenerator::planPath(const InputData & input_da return path; } -bool PathGenerator::updatePlannerData( - const InputData & input_data, const ::path_generator::Params & param) +void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) { - if (!planner_data_.lanelet_map_ptr && !input_data.lanelet_map_bin_ptr) { - return false; + planner_data_.route_ptr = route_ptr; + if (route_ptr->segments.empty()) { + return; } - if (!planner_data_.route_ptr && !input_data.route_ptr) { - return false; - } + planner_data_.route_lanelets.clear(); + planner_data_.preferred_lanelets.clear(); + planner_data_.start_lanelets.clear(); + planner_data_.goal_lanelets.clear(); - if (input_data.lanelet_map_bin_ptr) { - 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); + size_t primitives_num = 0; + for (const auto & route_section : planner_data_.route_ptr->segments) { + primitives_num += route_section.primitives.size(); } + planner_data_.route_lanelets.reserve(primitives_num); - if (input_data.route_ptr) { - if (!lanelet::utils::route::isRouteValid( - *input_data.route_ptr, planner_data_.lanelet_map_ptr)) { - return false; + for (const auto & route_section : planner_data_.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); + } } - setRoute(input_data.route_ptr); } - if (input_data.odometry_ptr) { - planner_data_.current_pose = input_data.odometry_ptr->pose.pose; + 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( + planner_data_.route_ptr->segments.front(), planner_data_.start_lanelets); + set_lanelets_from_segment(planner_data_.route_ptr->segments.back(), planner_data_.goal_lanelets); +} + +std::optional PathGenerator::generate_centerline_path( + const LaneletRoute::ConstSharedPtr & route_ptr, 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; } - planner_data_.forward_path_length = param.forward_path_length; - planner_data_.backward_path_length = param.backward_path_length; - planner_data_.input_path_interval = param.input_path_interval; - planner_data_.enable_akima_spline_first = param.enable_akima_spline_first; - planner_data_.ego_nearest_dist_threshold = param.ego_nearest_dist_threshold; - planner_data_.ego_nearest_yaw_threshold = param.ego_nearest_yaw_threshold; + const auto lanelet_sequence = utils::get_lanelet_sequence( + current_lane, planner_data_, current_pose, params.forward_path_length, + params.backward_path_length); + if (!lanelet_sequence) { + return std::nullopt; + } - return true; + const auto centerline_path = + get_centerline_path(*lanelet_sequence, route_ptr, current_pose, params); + if (!centerline_path) { + return std::nullopt; + } + + return centerline_path; } -void PathGenerator::setRoute(const LaneletRoute::ConstSharedPtr & route_ptr) +std::optional PathGenerator::get_centerline_path( + const lanelet::ConstLanelets & lanelet_sequence, const LaneletRoute::ConstSharedPtr & route_ptr, + const geometry_msgs::msg::Pose & current_pose, const Params & params) const { - planner_data_.route_ptr = route_ptr; + if (lanelet_sequence.empty()) { + return std::nullopt; + } - planner_data_.route_lanelets.clear(); - planner_data_.preferred_lanelets.clear(); - planner_data_.start_lanelets.clear(); - planner_data_.goal_lanelets.clear(); + if (!route_ptr) { + return std::nullopt; + } + + const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, 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 (!planner_data_.route_ptr->segments.empty()) { - size_t primitives_num = 0; - for (const auto & route_section : planner_data_.route_ptr->segments) { - primitives_num += route_section.primitives.size(); + if (!utils::get_next_lanelet_within_route(lanelet_sequence.back(), planner_data_)) { + const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + s_end = std::clamp(s_end, 0.0, lane_length); } - planner_data_.route_lanelets.reserve(primitives_num); - - for (const auto & route_section : planner_data_.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); + + if ( + std::find( + planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), + lanelet_sequence.back()) != planner_data_.goal_lanelets.end()) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.route_ptr->goal_pose); + s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); + } + + return s_end; + }(); + + const auto raw_centerline_path = get_centerline_path(lanelet_sequence, s_start, s_end); + if (!raw_centerline_path) { + return std::nullopt; + } + + auto centerline_path = autoware::motion_utils::resamplePath( + *raw_centerline_path, params.input_path_interval, params.enable_akima_spline_first); + + const auto current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + centerline_path.points, current_pose, params.ego_nearest_dist_threshold, + params.ego_nearest_yaw_threshold); + + centerline_path.points = autoware::motion_utils::cropPoints( + centerline_path.points, current_pose.position, current_seg_idx, params.forward_path_length, + params.backward_path_length + params.input_path_interval); + + return centerline_path; +} + +std::optional PathGenerator::get_centerline_path( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end) const +{ + if ( + !planner_data_.lanelet_map_ptr || !planner_data_.traffic_rules_ptr || + !planner_data_.route_ptr) { + return std::nullopt; + } + + PathWithLaneId centerline_path{}; + auto & path_points = centerline_path.points; + + const auto waypoint_groups = + utils::get_waypoint_groups(lanelet_sequence, *planner_data_.lanelet_map_ptr); + + double s = 0.; + for (const auto & lanelet : lanelet_sequence) { + std::vector reference_points; + const auto & centerline = lanelet.centerline(); + + std::optional 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 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; } } - 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( - planner_data_.route_ptr->segments.front(), planner_data_.start_lanelets); - set_lanelets_from_segment( - planner_data_.route_ptr->segments.back(), planner_data_.goal_lanelets); + const auto speed_limit = + planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); + for (const auto & reference_point : reference_points) { + PathPointWithLaneId path_point{}; + path_point.point.pose.position = reference_point; + path_point.lane_ids.push_back(lanelet.id()); + path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); + path_points.push_back(path_point); + } } + + utils::remove_overlapping_points(centerline_path); + + // append a point if having only one point so that yaw calculation would work + if (path_points.size() == 1) { + const auto & lane_id = path_points.front().lane_ids.front(); + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(lane_id); + const auto & point = path_points.front().point.pose.position; + const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); + + PathPointWithLaneId path_point{}; + path_point.lane_ids.push_back(lane_id); + constexpr double ds = 0.1; + path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); + path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); + path_point.point.pose.position.z = point.z; + path_points.push_back(path_point); + } + + // set yaw to each point + for (auto it = path_points.begin(); it != std::prev(path_points.end()); ++it) { + const auto angle = autoware::universe_utils::calcAzimuthAngle( + it->point.pose.position, std::next(it)->point.pose.position); + it->point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(angle); + } + path_points.back().point.pose.orientation = + std::prev(path_points.end(), 2)->point.pose.orientation; + + centerline_path.header = planner_data_.route_ptr->header; + return centerline_path; } } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 99ae9629d3bfb..c1020b27d6a7d 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -15,7 +15,7 @@ #ifndef NODE_HPP_ #define NODE_HPP_ -#include "autoware/path_generator/planner_data.hpp" +#include "autoware/path_generator/common_structs.hpp" #include #include @@ -30,6 +30,7 @@ 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::PathWithLaneId; class PathGenerator : public rclcpp::Node @@ -66,13 +67,25 @@ class PathGenerator : public rclcpp::Node void run(); - InputData takeData(); + InputData take_data(); - std::optional planPath(const InputData & input_data); + bool is_data_ready(const InputData & input_data); - bool updatePlannerData(const InputData & input_data, const ::path_generator::Params & param); + std::optional plan_path(const InputData & input_data); - void setRoute(const LaneletRoute::ConstSharedPtr & route_ptr); + void set_route(const LaneletRoute::ConstSharedPtr & route_ptr); + + std::optional generate_centerline_path( + const LaneletRoute::ConstSharedPtr & route_ptr, const geometry_msgs::msg::Pose & current_pose, + const Params & params) const; + + std::optional get_centerline_path( + const lanelet::ConstLanelets & lanelet_sequence, const LaneletRoute::ConstSharedPtr & route_ptr, + const geometry_msgs::msg::Pose & current_pose, const Params & params) const; + + std::optional get_centerline_path( + const lanelet::ConstLanelets & lanelet_sequence, const double s_start, + const double s_end) const; }; } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 5af5c53bb4c21..44ebd9f87929a 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -14,12 +14,7 @@ #include "autoware/path_generator/utils.hpp" -#include -#include -#include #include -#include -#include #include #include @@ -37,194 +32,15 @@ bool exists(const std::vector & vec, const T & item) } } // namespace -std::optional generateCenterLinePath(const PlannerData & planner_data) -{ - lanelet::ConstLanelet current_lane; - if (!lanelet::utils::query::getClosestLanelet( - planner_data.preferred_lanelets, planner_data.current_pose, ¤t_lane)) { - return std::nullopt; - } - - const auto lanelet_sequence = getLaneletSequence(current_lane, planner_data); - if (!lanelet_sequence) { - return std::nullopt; - } - - const auto centerline_path = getCenterLinePath(*lanelet_sequence, planner_data); - if (!centerline_path) { - return std::nullopt; - } - - return centerline_path; -} - -std::optional getCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const PlannerData & planner_data) -{ - if (lanelet_sequence.empty()) { - return std::nullopt; - } - - if (!planner_data.route_ptr) { - return std::nullopt; - } - - const auto arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data.current_pose); - const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates - const auto s_start = std::max(0., s - planner_data.backward_path_length); - const auto s_end = [&]() { - auto s_end = s + planner_data.forward_path_length; - - if (!getNextLaneletWithinRoute(lanelet_sequence.back(), planner_data)) { - const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); - s_end = std::clamp(s_end, 0.0, lane_length); - } - - if (exists(planner_data.goal_lanelets, lanelet_sequence.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data.route_ptr->goal_pose); - s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); - } - - return s_end; - }(); - - const auto raw_centerline_path = - getCenterLinePath(lanelet_sequence, s_start, s_end, planner_data); - if (!raw_centerline_path) { - return std::nullopt; - } - - auto centerline_path = autoware::motion_utils::resamplePath( - *raw_centerline_path, planner_data.input_path_interval, planner_data.enable_akima_spline_first); - - const auto current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - centerline_path.points, planner_data.current_pose, planner_data.ego_nearest_dist_threshold, - planner_data.ego_nearest_yaw_threshold); - - centerline_path.points = autoware::motion_utils::cropPoints( - centerline_path.points, planner_data.current_pose.position, current_seg_idx, - planner_data.forward_path_length, - planner_data.backward_path_length + planner_data.input_path_interval); - - return centerline_path; -} - -std::optional getCenterLinePath( - const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, - const PlannerData & planner_data) -{ - if (!planner_data.lanelet_map_ptr || !planner_data.traffic_rules_ptr || !planner_data.route_ptr) { - return std::nullopt; - } - - PathWithLaneId centerline_path{}; - auto & path_points = centerline_path.points; - - const auto waypoint_groups = - utils::getWaypointGroups(lanelet_sequence, *planner_data.lanelet_map_ptr); - - double s = 0.; - for (const auto & lanelet : lanelet_sequence) { - std::vector reference_points; - const auto & centerline = lanelet.centerline(); - - std::optional 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 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; - } - } - - const auto speed_limit = planner_data.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); - for (const auto & reference_point : reference_points) { - PathPointWithLaneId path_point{}; - path_point.point.pose.position = reference_point; - path_point.lane_ids.push_back(lanelet.id()); - path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); - path_points.push_back(path_point); - } - } - - utils::removeOverlappingPoints(centerline_path); - - // append a point if having only one point so that yaw calculation would work - if (path_points.size() == 1) { - const auto & lane_id = path_points.front().lane_ids.front(); - const auto & lanelet = planner_data.lanelet_map_ptr->laneletLayer.get(lane_id); - const auto & point = path_points.front().point.pose.position; - const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); - - PathPointWithLaneId path_point{}; - path_point.lane_ids.push_back(lane_id); - constexpr double ds = 0.1; - path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); - path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); - path_point.point.pose.position.z = point.z; - path_points.push_back(path_point); - } - - // set yaw to each point - for (auto it = path_points.begin(); it != std::prev(path_points.end()); ++it) { - const auto angle = autoware::universe_utils::calcAzimuthAngle( - it->point.pose.position, std::next(it)->point.pose.position); - it->point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(angle); - } - path_points.back().point.pose.orientation = - std::prev(path_points.end(), 2)->point.pose.orientation; - - centerline_path.header = planner_data.route_ptr->header; - return centerline_path; -} - -std::optional getLaneletSequence( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +std::optional get_lanelet_sequence( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const geometry_msgs::msg::Pose & current_pose, const double forward_path_length, + const double backward_path_length) { auto lanelet_sequence = [&]() -> std::optional { - const auto arc_coordinate = - lanelet::utils::getArcCoordinates({lanelet}, planner_data.current_pose); - if (arc_coordinate.length < planner_data.backward_path_length) { - return getLaneletSequenceUpTo(lanelet, planner_data); + const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + if (arc_coordinate.length < backward_path_length) { + return get_lanelet_sequence_up_to(lanelet, planner_data, backward_path_length); } return lanelet::ConstLanelets{}; }(); @@ -232,7 +48,8 @@ std::optional getLaneletSequence( return std::nullopt; } - const auto lanelet_sequence_forward = getLaneletSequenceAfter(lanelet, planner_data); + const auto lanelet_sequence_forward = + get_lanelet_sequence_after(lanelet, planner_data, forward_path_length); if (!lanelet_sequence_forward) { return std::nullopt; } @@ -252,8 +69,9 @@ std::optional getLaneletSequence( return lanelet_sequence; } -std::optional getLaneletSequenceAfter( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +std::optional get_lanelet_sequence_after( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const double forward_path_length) { if (!planner_data.routing_graph_ptr) { return std::nullopt; @@ -263,8 +81,8 @@ std::optional getLaneletSequenceAfter( auto current_lanelet = lanelet; auto length = 0.; - while (rclcpp::ok() && length < planner_data.forward_path_length) { - auto next_lanelet = getNextLaneletWithinRoute(current_lanelet, planner_data); + while (rclcpp::ok() && length < forward_path_length) { + auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); if (!next_lanelet) { const auto next_lanelets = planner_data.routing_graph_ptr->following(current_lanelet); if (next_lanelets.empty()) { @@ -286,8 +104,9 @@ std::optional getLaneletSequenceAfter( return lanelet_sequence; } -std::optional getLaneletSequenceUpTo( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +std::optional get_lanelet_sequence_up_to( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const double backward_path_length) { if (!planner_data.routing_graph_ptr) { return std::nullopt; @@ -297,8 +116,8 @@ std::optional getLaneletSequenceUpTo( auto current_lanelet = lanelet; auto length = 0.; - while (rclcpp::ok() && length < planner_data.backward_path_length) { - auto previous_lanelets = getPreviousLaneletsWithinRoute(current_lanelet, planner_data); + while (rclcpp::ok() && length < backward_path_length) { + auto previous_lanelets = get_previous_lanelets_within_route(current_lanelet, planner_data); if (!previous_lanelets) { previous_lanelets = planner_data.routing_graph_ptr->previous(current_lanelet); if (previous_lanelets->empty()) { @@ -334,10 +153,10 @@ std::optional getLaneletSequenceUpTo( return lanelet_sequence; } -std::optional getNextLaneletWithinRoute( +std::optional get_next_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) { - const auto next_lanelets = getNextLaneletsWithinRoute(lanelet, planner_data); + const auto next_lanelets = get_next_lanelets_within_route(lanelet, planner_data); if (next_lanelets) { return next_lanelets->front(); @@ -345,7 +164,7 @@ std::optional getNextLaneletWithinRoute( return std::nullopt; } -std::optional getNextLaneletsWithinRoute( +std::optional get_next_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) { if (!planner_data.routing_graph_ptr) { @@ -375,7 +194,7 @@ std::optional getNextLaneletsWithinRoute( return next_lanelets; } -std::optional getPreviousLaneletsWithinRoute( +std::optional get_previous_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) { if (!planner_data.routing_graph_ptr) { @@ -399,7 +218,7 @@ std::optional getPreviousLaneletsWithinRoute( return prev_lanelets; } -std::vector>> getWaypointGroups( +std::vector>> get_waypoint_groups( const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map) { std::vector>> waypoint_groups; @@ -443,7 +262,7 @@ std::vector>> getWay return waypoint_groups; } -void removeOverlappingPoints(PathWithLaneId & path) +void remove_overlapping_points(PathWithLaneId & path) { auto & filtered_path_end = path.points.front(); for (auto it = std::next(path.points.begin()); it != path.points.end();) { From ea25b3a552ec8c8fd2eb3a3b33175d8d227f8db7 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 15:03:35 +0900 Subject: [PATCH 07/34] fix naming Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 10 ++++------ .../autoware_path_generator/src/utils.cpp | 20 +++++++++---------- 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 0ef1d7ed9b4fa..dde49bcd8e98a 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -30,16 +30,14 @@ namespace utils { std::optional get_lanelet_sequence( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const geometry_msgs::msg::Pose & current_pose, const double forward_path_length, - const double backward_path_length); + const geometry_msgs::msg::Pose & current_pose, const double forward_distance, + const double backward_distance); std::optional get_lanelet_sequence_after( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const double forward_path_length); + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); std::optional get_lanelet_sequence_up_to( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const double backward_path_length); + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); std::optional get_next_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 44ebd9f87929a..5bea7d0d94be2 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -34,13 +34,13 @@ bool exists(const std::vector & vec, const T & item) std::optional get_lanelet_sequence( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const geometry_msgs::msg::Pose & current_pose, const double forward_path_length, - const double backward_path_length) + const geometry_msgs::msg::Pose & current_pose, const double forward_distance, + const double backward_distance) { auto lanelet_sequence = [&]() -> std::optional { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); - if (arc_coordinate.length < backward_path_length) { - return get_lanelet_sequence_up_to(lanelet, planner_data, backward_path_length); + if (arc_coordinate.length < backward_distance) { + return get_lanelet_sequence_up_to(lanelet, planner_data, backward_distance); } return lanelet::ConstLanelets{}; }(); @@ -49,7 +49,7 @@ std::optional get_lanelet_sequence( } const auto lanelet_sequence_forward = - get_lanelet_sequence_after(lanelet, planner_data, forward_path_length); + get_lanelet_sequence_after(lanelet, planner_data, forward_distance); if (!lanelet_sequence_forward) { return std::nullopt; } @@ -70,8 +70,7 @@ std::optional get_lanelet_sequence( } std::optional get_lanelet_sequence_after( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const double forward_path_length) + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { if (!planner_data.routing_graph_ptr) { return std::nullopt; @@ -81,7 +80,7 @@ std::optional get_lanelet_sequence_after( auto current_lanelet = lanelet; auto length = 0.; - while (rclcpp::ok() && length < forward_path_length) { + while (rclcpp::ok() && length < distance) { auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); if (!next_lanelet) { const auto next_lanelets = planner_data.routing_graph_ptr->following(current_lanelet); @@ -105,8 +104,7 @@ std::optional get_lanelet_sequence_after( } std::optional get_lanelet_sequence_up_to( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const double backward_path_length) + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { if (!planner_data.routing_graph_ptr) { return std::nullopt; @@ -116,7 +114,7 @@ std::optional get_lanelet_sequence_up_to( auto current_lanelet = lanelet; auto length = 0.; - while (rclcpp::ok() && length < backward_path_length) { + while (rclcpp::ok() && length < distance) { auto previous_lanelets = get_previous_lanelets_within_route(current_lanelet, planner_data); if (!previous_lanelets) { previous_lanelets = planner_data.routing_graph_ptr->previous(current_lanelet); From 891f65ab16d35495dbed7ebab8c762caf4f8b934 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 15:19:32 +0900 Subject: [PATCH 08/34] include necessary headers Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 2 ++ planning/autoware_path_generator/src/node.cpp | 4 ++++ planning/autoware_path_generator/src/node.hpp | 1 + 3 files changed, 7 insertions(+) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index dde49bcd8e98a..e277238fad36f 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -20,6 +20,8 @@ #include #include +#include +#include namespace autoware::path_generator { diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index ad4400dfac770..7ff561ef14625 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -25,6 +25,10 @@ #include +#include +#include +#include + namespace autoware::path_generator { PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index c1020b27d6a7d..9ec6655726eab 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::path_generator { using autoware_map_msgs::msg::LaneletMapBin; From 509524b190ef40f316953fb67d71acb30ec6ce7b Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 15:50:03 +0900 Subject: [PATCH 09/34] remove unnecessary null check Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 10 ---------- planning/autoware_path_generator/src/utils.cpp | 16 ---------------- 2 files changed, 26 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 7ff561ef14625..cc19c958f39aa 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -215,10 +215,6 @@ std::optional PathGenerator::get_centerline_path( return std::nullopt; } - if (!route_ptr) { - return std::nullopt; - } - const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, 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); @@ -265,12 +261,6 @@ std::optional PathGenerator::get_centerline_path( std::optional PathGenerator::get_centerline_path( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end) const { - if ( - !planner_data_.lanelet_map_ptr || !planner_data_.traffic_rules_ptr || - !planner_data_.route_ptr) { - return std::nullopt; - } - PathWithLaneId centerline_path{}; auto & path_points = centerline_path.points; diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 5bea7d0d94be2..2fe24b4db046d 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -72,10 +72,6 @@ std::optional get_lanelet_sequence( std::optional get_lanelet_sequence_after( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { - if (!planner_data.routing_graph_ptr) { - return std::nullopt; - } - lanelet::ConstLanelets lanelet_sequence{}; auto current_lanelet = lanelet; auto length = 0.; @@ -106,10 +102,6 @@ std::optional get_lanelet_sequence_after( std::optional get_lanelet_sequence_up_to( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { - if (!planner_data.routing_graph_ptr) { - return std::nullopt; - } - lanelet::ConstLanelets lanelet_sequence{}; auto current_lanelet = lanelet; auto length = 0.; @@ -165,10 +157,6 @@ std::optional get_next_lanelet_within_route( std::optional get_next_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) { - if (!planner_data.routing_graph_ptr) { - return std::nullopt; - } - if (planner_data.preferred_lanelets.empty()) { return std::nullopt; } @@ -195,10 +183,6 @@ std::optional get_next_lanelets_within_route( std::optional get_previous_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) { - if (!planner_data.routing_graph_ptr) { - return std::nullopt; - } - if (exists(planner_data.start_lanelets, lanelet)) { return std::nullopt; } From b4c825ee7637e9a07f06b91e02cf9dfc65d75e18 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 16:20:01 +0900 Subject: [PATCH 10/34] remove route_ptr from member variables Signed-off-by: mitukou1109 --- .../path_generator/common_structs.hpp | 5 +- planning/autoware_path_generator/src/node.cpp | 104 +++++++++--------- planning/autoware_path_generator/src/node.hpp | 7 +- 3 files changed, 57 insertions(+), 59 deletions(-) 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 index b9a0a6761d815..a1b774e93afe7 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp @@ -20,6 +20,8 @@ #include #include +#include + namespace autoware::path_generator { struct PlannerData @@ -28,7 +30,8 @@ struct PlannerData lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr{nullptr}; lanelet::routing::RoutingGraphPtr routing_graph_ptr{nullptr}; - autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_ptr{nullptr}; + std::string route_frame_id{}; + geometry_msgs::msg::Pose goal_pose{}; lanelet::ConstLanelets route_lanelets{}; lanelet::ConstLanelets preferred_lanelets{}; diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index cc19c958f39aa..be4fa8eb38436 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -100,7 +100,7 @@ bool PathGenerator::is_data_ready(const InputData & input_data) return missing("map"); } - if (!input_data.route_ptr && !planner_data_.route_ptr) { + if (!input_data.route_ptr && planner_data_.route_lanelets.empty()) { return missing("route"); } @@ -123,8 +123,8 @@ std::optional PathGenerator::plan_path(const InputData & input_d set_route(input_data.route_ptr); } - const auto path = generate_centerline_path( - input_data.route_ptr, input_data.odometry_ptr->pose.pose, param_listener_->get_params()); + const auto path = + generate_centerline_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"); @@ -139,10 +139,8 @@ std::optional PathGenerator::plan_path(const InputData & input_d void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) { - planner_data_.route_ptr = route_ptr; - if (route_ptr->segments.empty()) { - return; - } + 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(); @@ -150,12 +148,12 @@ void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) planner_data_.goal_lanelets.clear(); size_t primitives_num = 0; - for (const auto & route_section : planner_data_.route_ptr->segments) { + 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 : planner_data_.route_ptr->segments) { + 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); @@ -176,14 +174,12 @@ void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) lanelets.push_back(lanelet); } }; - set_lanelets_from_segment( - planner_data_.route_ptr->segments.front(), planner_data_.start_lanelets); - set_lanelets_from_segment(planner_data_.route_ptr->segments.back(), planner_data_.goal_lanelets); + set_lanelets_from_segment(route_ptr->segments.front(), planner_data_.start_lanelets); + set_lanelets_from_segment(route_ptr->segments.back(), planner_data_.goal_lanelets); } std::optional PathGenerator::generate_centerline_path( - const LaneletRoute::ConstSharedPtr & route_ptr, const geometry_msgs::msg::Pose & current_pose, - const Params & params) const + const geometry_msgs::msg::Pose & current_pose, const Params & params) const { lanelet::ConstLanelet current_lane; if (!lanelet::utils::query::getClosestLanelet( @@ -198,8 +194,7 @@ std::optional PathGenerator::generate_centerline_path( return std::nullopt; } - const auto centerline_path = - get_centerline_path(*lanelet_sequence, route_ptr, current_pose, params); + const auto centerline_path = get_centerline_path(*lanelet_sequence, current_pose, params); if (!centerline_path) { return std::nullopt; } @@ -208,8 +203,8 @@ std::optional PathGenerator::generate_centerline_path( } std::optional PathGenerator::get_centerline_path( - const lanelet::ConstLanelets & lanelet_sequence, const LaneletRoute::ConstSharedPtr & route_ptr, - const geometry_msgs::msg::Pose & current_pose, const Params & params) const + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, + const Params & params) const { if (lanelet_sequence.empty()) { return std::nullopt; @@ -231,7 +226,7 @@ std::optional PathGenerator::get_centerline_path( planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), lanelet_sequence.back()) != planner_data_.goal_lanelets.end()) { const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.route_ptr->goal_pose); + lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.goal_pose); s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); } @@ -274,47 +269,47 @@ std::optional PathGenerator::get_centerline_path( std::optional 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 (s > s_end) { + break; + } + + 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)); } - } - if (!overlapped_waypoint_group_index) { - reference_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); + overlapped_waypoint_group_index = std::nullopt; } } - if (it == std::prev(centerline.end())) { - break; + 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 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; - } + const lanelet::Point3d next_point(*std::next(it)); + const auto distance = lanelet::geometry::distance2d(point, next_point); + std::optional 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; + 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; } const auto speed_limit = @@ -355,7 +350,8 @@ std::optional PathGenerator::get_centerline_path( path_points.back().point.pose.orientation = std::prev(path_points.end(), 2)->point.pose.orientation; - centerline_path.header = planner_data_.route_ptr->header; + centerline_path.header.frame_id = planner_data_.route_frame_id; + centerline_path.header.stamp = now(); return centerline_path; } } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 9ec6655726eab..4b3c994c7f6b9 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -77,12 +77,11 @@ class PathGenerator : public rclcpp::Node void set_route(const LaneletRoute::ConstSharedPtr & route_ptr); std::optional generate_centerline_path( - const LaneletRoute::ConstSharedPtr & route_ptr, const geometry_msgs::msg::Pose & current_pose, - const Params & params) const; + const geometry_msgs::msg::Pose & current_pose, const Params & params) const; std::optional get_centerline_path( - const lanelet::ConstLanelets & lanelet_sequence, const LaneletRoute::ConstSharedPtr & route_ptr, - const geometry_msgs::msg::Pose & current_pose, const Params & params) const; + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, + const Params & params) const; std::optional get_centerline_path( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, From dff7cd7bae4d26e6631c8db6888de897f091fe44 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 18:00:14 +0900 Subject: [PATCH 11/34] fix data handling Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 83 ++++++++++--------- planning/autoware_path_generator/src/node.hpp | 6 +- 2 files changed, 49 insertions(+), 40 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index be4fa8eb38436..57b1dd489e8a7 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -41,7 +41,7 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) { const auto planning_hz = declare_parameter("planning_hz"); - timer_ = rclcpp::create_timer( + timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(planning_hz).period(), std::bind(&PathGenerator::run, this)); } } @@ -49,6 +49,7 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) void PathGenerator::run() { const auto input_data = take_data(); + set_planner_data(input_data); if (!is_data_ready(input_data)) { return; } @@ -88,32 +89,10 @@ PathGenerator::InputData PathGenerator::take_data() return input_data; } -bool PathGenerator::is_data_ready(const InputData & input_data) -{ - const auto missing = [this](const std::string & name) { - RCLCPP_INFO_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting for %s", name.c_str()); - return false; - }; - - if (!input_data.lanelet_map_bin_ptr && !planner_data_.lanelet_map_ptr) { - return missing("map"); - } - - if (!input_data.route_ptr && planner_data_.route_lanelets.empty()) { - return missing("route"); - } - - if (!input_data.odometry_ptr) { - return missing("odometry"); - } - - return true; -} - -std::optional PathGenerator::plan_path(const InputData & 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); @@ -122,19 +101,6 @@ std::optional PathGenerator::plan_path(const InputData & input_d if (input_data.route_ptr) { set_route(input_data.route_ptr); } - - const auto path = - generate_centerline_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; } void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) @@ -178,6 +144,47 @@ void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) 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_centerline_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_centerline_path( const geometry_msgs::msg::Pose & current_pose, const Params & params) const { diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 4b3c994c7f6b9..8be018dcaaabd 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -70,12 +70,14 @@ class PathGenerator : public rclcpp::Node 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); - void set_route(const LaneletRoute::ConstSharedPtr & route_ptr); - std::optional generate_centerline_path( const geometry_msgs::msg::Pose & current_pose, const Params & params) const; From cd360d79008fd119483f4554fe1e69ff174356af Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 18:00:52 +0900 Subject: [PATCH 12/34] add planning_hz parameter to description Signed-off-by: mitukou1109 --- .../param/path_generator_parameters.yaml | 3 +++ planning/autoware_path_generator/src/node.cpp | 7 +++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml index f6da9c468f1b1..9696f98c61b27 100644 --- a/planning/autoware_path_generator/param/path_generator_parameters.yaml +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -1,4 +1,7 @@ path_generator: + planning_hz: + type: double + forward_path_length: type: double diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 57b1dd489e8a7..48c59c3514e68 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -39,11 +39,10 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) path_publisher_ = create_publisher("~/output/path", 1); - { - const auto planning_hz = declare_parameter("planning_hz"); + const auto params = param_listener_->get_params(); timer_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(planning_hz).period(), std::bind(&PathGenerator::run, this)); - } + this, get_clock(), rclcpp::Rate(params.planning_hz).period(), + std::bind(&PathGenerator::run, this)); } void PathGenerator::run() From a0bf9049ec56bcf20fccecbd3f94c3bc4fd01864 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 8 Nov 2024 18:01:11 +0900 Subject: [PATCH 13/34] add configuration example Signed-off-by: mitukou1109 --- .../config/path_generator.param.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 planning/autoware_path_generator/config/path_generator.param.yaml 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 0000000000000..f6d4cdbecd35d --- /dev/null +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + planning_hz: 10.0 + backward_path_length: 5.0 + forward_path_length: 300.0 + + enable_akima_spline_first: false + input_path_interval: 2.0 From 182f9134b1f84daa173681e5fdfdef8c65b5d12f Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 12 Nov 2024 15:00:31 +0900 Subject: [PATCH 14/34] set path bounds Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 48c59c3514e68..48acf52cdb291 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -327,6 +327,15 @@ std::optional PathGenerator::get_centerline_path( path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); path_points.push_back(path_point); } + + for (const auto & left_bound_point : lanelet.leftBound()) { + centerline_path.left_bound.push_back( + lanelet::utils::conversion::toGeomMsgPt(left_bound_point)); + } + for (const auto & right_bound_point : lanelet.rightBound()) { + centerline_path.right_bound.push_back( + lanelet::utils::conversion::toGeomMsgPt(right_bound_point)); + } } utils::remove_overlapping_points(centerline_path); From 1f148fe02a7ee53bd512b4eef6588201e28efff4 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 12 Nov 2024 17:46:46 +0900 Subject: [PATCH 15/34] use only route lanelets Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 9 ++++---- .../autoware_path_generator/src/utils.cpp | 23 +++++++++++-------- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 48acf52cdb291..54193462dedef 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -227,10 +227,11 @@ std::optional PathGenerator::get_centerline_path( s_end = std::clamp(s_end, 0.0, lane_length); } - if ( - std::find( - planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), - lanelet_sequence.back()) != planner_data_.goal_lanelets.end()) { + if (std::any_of( + planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), + [&](const auto & goal_lanelet) { + return lanelet_sequence.back().id() == goal_lanelet.id(); + })) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.goal_pose); s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 2fe24b4db046d..80850feccb443 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -37,6 +37,10 @@ std::optional get_lanelet_sequence( const geometry_msgs::msg::Pose & current_pose, const double forward_distance, const double backward_distance) { + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + auto lanelet_sequence = [&]() -> std::optional { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { @@ -72,6 +76,10 @@ std::optional get_lanelet_sequence( std::optional get_lanelet_sequence_after( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + lanelet::ConstLanelets lanelet_sequence{}; auto current_lanelet = lanelet; auto length = 0.; @@ -79,11 +87,7 @@ std::optional get_lanelet_sequence_after( while (rclcpp::ok() && length < distance) { auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); if (!next_lanelet) { - const auto next_lanelets = planner_data.routing_graph_ptr->following(current_lanelet); - if (next_lanelets.empty()) { - break; - } - next_lanelet = next_lanelets.front(); + break; } // loop check @@ -102,6 +106,10 @@ std::optional get_lanelet_sequence_after( std::optional get_lanelet_sequence_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 lanelet_sequence{}; auto current_lanelet = lanelet; auto length = 0.; @@ -109,10 +117,7 @@ std::optional get_lanelet_sequence_up_to( while (rclcpp::ok() && length < distance) { auto previous_lanelets = get_previous_lanelets_within_route(current_lanelet, planner_data); if (!previous_lanelets) { - previous_lanelets = planner_data.routing_graph_ptr->previous(current_lanelet); - if (previous_lanelets->empty()) { - break; - } + break; } if ( From 649690b5aedb76cba973cbe93a40b55ffde25923 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 13 Nov 2024 17:22:00 +0900 Subject: [PATCH 16/34] refactor utility functions Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 5 +- .../autoware_path_generator/src/utils.cpp | 95 +++++-------------- 2 files changed, 23 insertions(+), 77 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index e277238fad36f..49a50140b22b4 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -44,10 +44,7 @@ std::optional get_lanelet_sequence_up_to( std::optional get_next_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); -std::optional get_next_lanelets_within_route( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); - -std::optional get_previous_lanelets_within_route( +std::optional get_previous_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); std::vector>> get_waypoint_groups( diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 80850feccb443..1dbaba066ce64 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -41,34 +41,26 @@ std::optional get_lanelet_sequence( return std::nullopt; } - auto lanelet_sequence = [&]() -> std::optional { - const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); - if (arc_coordinate.length < backward_distance) { - return get_lanelet_sequence_up_to(lanelet, planner_data, backward_distance); - } - return lanelet::ConstLanelets{}; - }(); - if (!lanelet_sequence) { + const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + + const auto lanelet_sequence_backward = + get_lanelet_sequence_up_to(lanelet, planner_data, backward_distance - arc_coordinates.length); + if (!lanelet_sequence_backward) { return std::nullopt; } - const auto lanelet_sequence_forward = - get_lanelet_sequence_after(lanelet, planner_data, forward_distance); + const auto lanelet_sequence_forward = get_lanelet_sequence_after( + lanelet, planner_data, forward_distance - (lanelet_length - arc_coordinates.length)); if (!lanelet_sequence_forward) { return std::nullopt; } - // loop check - if ( - !lanelet_sequence_forward->empty() && !lanelet_sequence->empty() && - lanelet_sequence->back().id() == lanelet_sequence_forward->front().id()) { - return lanelet_sequence_forward; - } - - lanelet_sequence->push_back(lanelet); + lanelet::ConstLanelets lanelet_sequence(std::move(*lanelet_sequence_backward)); + lanelet_sequence.push_back(lanelet); std::move( lanelet_sequence_forward->begin(), lanelet_sequence_forward->end(), - std::back_inserter(*lanelet_sequence)); + std::back_inserter(lanelet_sequence)); return lanelet_sequence; } @@ -85,16 +77,11 @@ std::optional get_lanelet_sequence_after( auto length = 0.; while (rclcpp::ok() && length < distance) { - auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); + const auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); if (!next_lanelet) { break; } - // loop check - if (lanelet.id() == next_lanelet->id()) { - break; - } - lanelet_sequence.push_back(*next_lanelet); current_lanelet = *next_lanelet; length += lanelet::utils::getLaneletLength2d(*next_lanelet); @@ -115,33 +102,14 @@ std::optional get_lanelet_sequence_up_to( auto length = 0.; while (rclcpp::ok() && length < distance) { - auto previous_lanelets = get_previous_lanelets_within_route(current_lanelet, planner_data); - if (!previous_lanelets) { + const auto prev_lanelet = get_previous_lanelet_within_route(current_lanelet, planner_data); + if (!prev_lanelet) { break; } - if ( - std::count_if( - previous_lanelets->begin(), previous_lanelets->end(), [&](const auto & prev_llt) { - return lanelet.id() == prev_llt.id(); - }) >= (previous_lanelets ? static_cast(previous_lanelets->size()) : 1)) { - break; - } - - for (const auto & prev_lanelet : *previous_lanelets) { - if ( - lanelet.id() == prev_lanelet.id() || - std::any_of( - lanelet_sequence.begin(), lanelet_sequence.end(), - [&](const auto & llt) { return llt.id() == prev_lanelet.id(); }) || - exists(planner_data.goal_lanelets, prev_lanelet)) { - continue; - } - lanelet_sequence.push_back(prev_lanelet); - length += lanelet::utils::getLaneletLength2d(prev_lanelet); - current_lanelet = prev_lanelet; - break; - } + lanelet_sequence.push_back(*prev_lanelet); + current_lanelet = *prev_lanelet; + length += lanelet::utils::getLaneletLength2d(*prev_lanelet); } std::reverse(lanelet_sequence.begin(), lanelet_sequence.end()); @@ -150,17 +118,6 @@ std::optional get_lanelet_sequence_up_to( std::optional get_next_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) -{ - const auto next_lanelets = get_next_lanelets_within_route(lanelet, planner_data); - - if (next_lanelets) { - return next_lanelets->front(); - } - return std::nullopt; -} - -std::optional get_next_lanelets_within_route( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) { if (planner_data.preferred_lanelets.empty()) { return std::nullopt; @@ -170,39 +127,31 @@ std::optional get_next_lanelets_within_route( return std::nullopt; } - lanelet::ConstLanelets next_lanelets{}; for (const auto & lanelet : planner_data.routing_graph_ptr->following(lanelet)) { if ( planner_data.preferred_lanelets.front().id() != lanelet.id() && exists(planner_data.route_lanelets, lanelet)) { - next_lanelets.push_back(lanelet); + return lanelet; } } - if (next_lanelets.empty()) { - return std::nullopt; - } - return next_lanelets; + return std::nullopt; } -std::optional get_previous_lanelets_within_route( +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; } - lanelet::ConstLanelets prev_lanelets{}; for (const auto & lanelet : planner_data.routing_graph_ptr->previous(lanelet)) { if (exists(planner_data.route_lanelets, lanelet)) { - prev_lanelets.push_back(lanelet); + return lanelet; } } - if (prev_lanelets.empty()) { - return std::nullopt; - } - return prev_lanelets; + return std::nullopt; } std::vector>> get_waypoint_groups( From d0a2b0043a832023ea475e87ce0ce56127589561 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 13 Nov 2024 17:23:39 +0900 Subject: [PATCH 17/34] fix overlap removal function Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/utils.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 1dbaba066ce64..3cc2aecd2d06f 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -200,17 +200,18 @@ std::vector>> get_wa void remove_overlapping_points(PathWithLaneId & path) { - auto & filtered_path_end = path.points.front(); + auto filtered_path_end_it = path.points.begin(); for (auto it = std::next(path.points.begin()); it != path.points.end();) { constexpr auto min_interval = 0.001; if ( - autoware::universe_utils::calcDistance3d(filtered_path_end.point, it->point) < min_interval) { - filtered_path_end.lane_ids.push_back(it->lane_ids.front()); - filtered_path_end.point.longitudinal_velocity_mps = std::min( - it->point.longitudinal_velocity_mps, filtered_path_end.point.longitudinal_velocity_mps); + autoware::universe_utils::calcDistance3d(filtered_path_end_it->point, it->point) < + min_interval) { + filtered_path_end_it->lane_ids.push_back(it->lane_ids.front()); + filtered_path_end_it->point.longitudinal_velocity_mps = std::min( + filtered_path_end_it->point.longitudinal_velocity_mps, it->point.longitudinal_velocity_mps); it = path.points.erase(it); } else { - filtered_path_end = *it; + filtered_path_end_it = it; ++it; } } From b6a8b5b3305712974fedd8dcb5f09dbf387c65af Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 15 Nov 2024 10:17:05 +0900 Subject: [PATCH 18/34] refactor path generation function Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 54193462dedef..73eaed0225ec1 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -224,7 +224,7 @@ std::optional PathGenerator::get_centerline_path( if (!utils::get_next_lanelet_within_route(lanelet_sequence.back(), planner_data_)) { const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); - s_end = std::clamp(s_end, 0.0, lane_length); + s_end = std::min(s_end, lane_length); } if (std::any_of( @@ -234,27 +234,27 @@ std::optional PathGenerator::get_centerline_path( })) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.goal_pose); - s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length); + s_end = std::min(s_end, goal_arc_coordinates.length); } return s_end; }(); - const auto raw_centerline_path = get_centerline_path(lanelet_sequence, s_start, s_end); - if (!raw_centerline_path) { + auto centerline_path = get_centerline_path(lanelet_sequence, s_start, s_end); + if (!centerline_path) { return std::nullopt; } - auto centerline_path = autoware::motion_utils::resamplePath( - *raw_centerline_path, params.input_path_interval, params.enable_akima_spline_first); + centerline_path = autoware::motion_utils::resamplePath( + *centerline_path, params.input_path_interval, params.enable_akima_spline_first); const auto current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - centerline_path.points, current_pose, params.ego_nearest_dist_threshold, + centerline_path->points, current_pose, params.ego_nearest_dist_threshold, params.ego_nearest_yaw_threshold); - centerline_path.points = autoware::motion_utils::cropPoints( - centerline_path.points, current_pose.position, current_seg_idx, params.forward_path_length, + centerline_path->points = autoware::motion_utils::cropPoints( + centerline_path->points, current_pose.position, current_seg_idx, params.forward_path_length, params.backward_path_length + params.input_path_interval); return centerline_path; @@ -321,6 +321,7 @@ std::optional PathGenerator::get_centerline_path( const auto speed_limit = planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); + for (const auto & reference_point : reference_points) { PathPointWithLaneId path_point{}; path_point.point.pose.position = reference_point; From f67a2540da5592b08ccb204d92aa606770bee7c2 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 22 Nov 2024 18:39:13 +0900 Subject: [PATCH 19/34] use lanelet::LaneletSequence for lanelet sequence representation Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 11 +-- .../autoware_path_generator/src/utils.cpp | 86 +++++++++---------- 2 files changed, 45 insertions(+), 52 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 49a50140b22b4..6269f0776e691 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -30,15 +30,15 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace utils { -std::optional get_lanelet_sequence( +std::optional get_lanelet_sequence( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const geometry_msgs::msg::Pose & current_pose, const double forward_distance, const double backward_distance); -std::optional get_lanelet_sequence_after( +std::optional get_lanelets_after( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); -std::optional get_lanelet_sequence_up_to( +std::optional get_lanelets_up_to( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); std::optional get_next_lanelet_within_route( @@ -48,9 +48,10 @@ std::optional get_previous_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); std::vector>> get_waypoint_groups( - const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map); + const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map, + const double group_separation_threshold, const double interval_margin_ratio); -void remove_overlapping_points(PathWithLaneId & path); +void remove_overlapping_points(std::vector & path_points); } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 3cc2aecd2d06f..17d1f78b52b37 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -32,7 +32,7 @@ bool exists(const std::vector & vec, const T & item) } } // namespace -std::optional get_lanelet_sequence( +std::optional get_lanelet_sequence( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const geometry_msgs::msg::Pose & current_pose, const double forward_distance, const double backward_distance) @@ -44,35 +44,33 @@ std::optional get_lanelet_sequence( const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose); const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); - const auto lanelet_sequence_backward = - get_lanelet_sequence_up_to(lanelet, planner_data, backward_distance - arc_coordinates.length); - if (!lanelet_sequence_backward) { + const auto backward_lanelets = + get_lanelets_up_to(lanelet, planner_data, backward_distance - arc_coordinates.length); + if (!backward_lanelets) { return std::nullopt; } - const auto lanelet_sequence_forward = get_lanelet_sequence_after( + const auto forward_lanelets = get_lanelets_after( lanelet, planner_data, forward_distance - (lanelet_length - arc_coordinates.length)); - if (!lanelet_sequence_forward) { + if (!forward_lanelets) { return std::nullopt; } - lanelet::ConstLanelets lanelet_sequence(std::move(*lanelet_sequence_backward)); - lanelet_sequence.push_back(lanelet); - std::move( - lanelet_sequence_forward->begin(), lanelet_sequence_forward->end(), - std::back_inserter(lanelet_sequence)); + lanelet::ConstLanelets lanelets(std::move(*backward_lanelets)); + lanelets.push_back(lanelet); + std::move(forward_lanelets->begin(), forward_lanelets->end(), std::back_inserter(lanelets)); - return lanelet_sequence; + return lanelets; } -std::optional get_lanelet_sequence_after( +std::optional get_lanelets_after( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { if (!exists(planner_data.route_lanelets, lanelet)) { return std::nullopt; } - lanelet::ConstLanelets lanelet_sequence{}; + lanelet::ConstLanelets lanelets{}; auto current_lanelet = lanelet; auto length = 0.; @@ -82,22 +80,22 @@ std::optional get_lanelet_sequence_after( break; } - lanelet_sequence.push_back(*next_lanelet); + lanelets.push_back(*next_lanelet); current_lanelet = *next_lanelet; length += lanelet::utils::getLaneletLength2d(*next_lanelet); } - return lanelet_sequence; + return lanelets; } -std::optional get_lanelet_sequence_up_to( +std::optional get_lanelets_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 lanelet_sequence{}; + lanelet::ConstLanelets lanelets{}; auto current_lanelet = lanelet; auto length = 0.; @@ -107,13 +105,13 @@ std::optional get_lanelet_sequence_up_to( break; } - lanelet_sequence.push_back(*prev_lanelet); + lanelets.push_back(*prev_lanelet); current_lanelet = *prev_lanelet; length += lanelet::utils::getLaneletLength2d(*prev_lanelet); } - std::reverse(lanelet_sequence.begin(), lanelet_sequence.end()); - return lanelet_sequence; + std::reverse(lanelets.begin(), lanelets.end()); + return lanelets; } std::optional get_next_lanelet_within_route( @@ -155,53 +153,47 @@ std::optional get_previous_lanelet_within_route( } std::vector>> get_waypoint_groups( - const lanelet::ConstLanelets & lanelet_sequence, const lanelet::LaneletMap & lanelet_map) + const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map, + const double group_separation_threshold, const double interval_margin_ratio) { - std::vector>> waypoint_groups; + std::vector>> waypoint_groups{}; + + 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); + }; - double arc_length_offset = 0.; for (const auto & lanelet : lanelet_sequence) { if (!lanelet.hasAttribute("waypoints")) { - arc_length_offset += lanelet::utils::getLaneletLength2d(lanelet); continue; } const auto waypoints_id = lanelet.attribute("waypoints").asId().value(); const auto & waypoints = lanelet_map.lineStringLayer.get(waypoints_id); - const auto get_interval_bound = - [&](const lanelet::ConstPoint3d & point, const double lateral_distance_factor) { - const auto arc_coordinates = - lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), lanelet::utils::to2D(point)); - return arc_length_offset + arc_coordinates.length + - lateral_distance_factor * std::abs(arc_coordinates.distance); - }; - - constexpr auto new_group_threshold = 1.0; - constexpr auto lateral_distance_factor = 10.0; + if ( waypoint_groups.empty() || lanelet::geometry::distance2d(waypoint_groups.back().first.back(), waypoints.front()) > - new_group_threshold) { + group_separation_threshold) { waypoint_groups.emplace_back().second.first = - get_interval_bound(waypoints.front(), -lateral_distance_factor); + get_interval_bound(waypoints.front(), -interval_margin_ratio); } waypoint_groups.back().second.second = - get_interval_bound(waypoints.back(), lateral_distance_factor); - - for (const auto & waypoint : waypoints) { - waypoint_groups.back().first.push_back(waypoint); - } + get_interval_bound(waypoints.back(), interval_margin_ratio); - arc_length_offset += lanelet::utils::getLaneletLength2d(lanelet); + waypoint_groups.back().first.insert( + waypoint_groups.back().first.end(), waypoints.begin(), waypoints.end()); } return waypoint_groups; } -void remove_overlapping_points(PathWithLaneId & path) +void remove_overlapping_points(std::vector & path_points) { - auto filtered_path_end_it = path.points.begin(); - for (auto it = std::next(path.points.begin()); it != path.points.end();) { + auto filtered_path_end_it = path_points.begin(); + for (auto it = std::next(path_points.begin()); it != path_points.end();) { constexpr auto min_interval = 0.001; if ( autoware::universe_utils::calcDistance3d(filtered_path_end_it->point, it->point) < @@ -209,7 +201,7 @@ void remove_overlapping_points(PathWithLaneId & path) filtered_path_end_it->lane_ids.push_back(it->lane_ids.front()); filtered_path_end_it->point.longitudinal_velocity_mps = std::min( filtered_path_end_it->point.longitudinal_velocity_mps, it->point.longitudinal_velocity_mps); - it = path.points.erase(it); + it = path_points.erase(it); } else { filtered_path_end_it = it; ++it; From 8d29f416ad2eabdf5877f70142b74daadd103c66 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 22 Nov 2024 18:39:57 +0900 Subject: [PATCH 20/34] add parameters for handling waypoints Signed-off-by: mitukou1109 --- .../config/path_generator.param.yaml | 2 ++ .../param/path_generator_parameters.yaml | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml index f6d4cdbecd35d..a44bdaa516367 100644 --- a/planning/autoware_path_generator/config/path_generator.param.yaml +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -3,6 +3,8 @@ planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 + waypoint_group_separation_threshold: 1.0 + lateral_distance_factor: 10.0 enable_akima_spline_first: false input_path_interval: 2.0 diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml index 9696f98c61b27..7bfdc6e266201 100644 --- a/planning/autoware_path_generator/param/path_generator_parameters.yaml +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -8,6 +8,12 @@ path_generator: backward_path_length: type: double + waypoint_group_separation_threshold: + type: double + + waypoint_group_interval_margin_ratio: + type: double + input_path_interval: type: double From 271dafcda4b85442036ddcc0beeb858d9be608cb Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 22 Nov 2024 18:45:29 +0900 Subject: [PATCH 21/34] fix path generation function to work with waypoints properly Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 251 ++++++++++-------- planning/autoware_path_generator/src/node.hpp | 17 +- 2 files changed, 157 insertions(+), 111 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 73eaed0225ec1..50e8768a31c5c 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -29,6 +29,32 @@ #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; +} + +template +lanelet::BasicPoint3d get_interpolated_point(const T & start, const U & end, const double distance) +{ + lanelet::Point3d start_point, end_point; + start_point.x() = start.x(); + start_point.y() = start.y(); + start_point.z() = start.z(); + end_point.x() = end.x(); + end_point.y() = end.y(); + end_point.z() = end.z(); + + return lanelet::geometry::interpolatedPointAtDistance( + lanelet::ConstLineString3d(lanelet::InvalId, lanelet::Points3d{start_point, end_point}), + distance); +} +} // namespace + namespace autoware::path_generator { PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) @@ -171,7 +197,7 @@ bool PathGenerator::is_data_ready(const InputData & input_data) std::optional PathGenerator::plan_path(const InputData & input_data) { const auto path = - generate_centerline_path(input_data.odometry_ptr->pose.pose, param_listener_->get_params()); + 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"); @@ -184,7 +210,7 @@ std::optional PathGenerator::plan_path(const InputData & input_d return path; } -std::optional PathGenerator::generate_centerline_path( +std::optional PathGenerator::generate_path( const geometry_msgs::msg::Pose & current_pose, const Params & params) const { lanelet::ConstLanelet current_lane; @@ -200,148 +226,71 @@ std::optional PathGenerator::generate_centerline_path( return std::nullopt; } - const auto centerline_path = get_centerline_path(*lanelet_sequence, current_pose, params); - if (!centerline_path) { - return std::nullopt; - } - - return centerline_path; + return generate_path(*lanelet_sequence, current_pose, params); } -std::optional PathGenerator::get_centerline_path( - const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, +std::optional PathGenerator::generate_path( + const lanelet::LaneletSequence & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, const Params & params) const { if (lanelet_sequence.empty()) { return std::nullopt; } - const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, current_pose); + const auto arc_coordinates = + lanelet::utils::getArcCoordinates(lanelet_sequence.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(lanelet_sequence.back(), planner_data_)) { - const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + if (!utils::get_next_lanelet_within_route(lanelet_sequence.lanelets().back(), planner_data_)) { + const double lane_length = lanelet::geometry::length(lanelet_sequence.centerline2d()); s_end = std::min(s_end, lane_length); } if (std::any_of( planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), [&](const auto & goal_lanelet) { - return lanelet_sequence.back().id() == goal_lanelet.id(); + return lanelet_sequence.lanelets().back().id() == goal_lanelet.id(); })) { const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.goal_pose); + lanelet::utils::getArcCoordinates(lanelet_sequence.lanelets(), planner_data_.goal_pose); s_end = std::min(s_end, goal_arc_coordinates.length); } return s_end; }(); - auto centerline_path = get_centerline_path(lanelet_sequence, s_start, s_end); - if (!centerline_path) { + auto path = generate_path(lanelet_sequence, s_start, s_end, params); + if (!path) { return std::nullopt; } - centerline_path = autoware::motion_utils::resamplePath( - *centerline_path, params.input_path_interval, params.enable_akima_spline_first); + path = autoware::motion_utils::resamplePath( + *path, params.input_path_interval, params.enable_akima_spline_first); const auto current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - centerline_path->points, current_pose, params.ego_nearest_dist_threshold, + path->points, current_pose, params.ego_nearest_dist_threshold, params.ego_nearest_yaw_threshold); - centerline_path->points = autoware::motion_utils::cropPoints( - centerline_path->points, current_pose.position, current_seg_idx, params.forward_path_length, + path->points = autoware::motion_utils::cropPoints( + path->points, current_pose.position, current_seg_idx, params.forward_path_length, params.backward_path_length + params.input_path_interval); - return centerline_path; + return path; } -std::optional PathGenerator::get_centerline_path( - const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end) const +std::optional PathGenerator::generate_path( + const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const Params & params) const { - PathWithLaneId centerline_path{}; - auto & path_points = centerline_path.points; - - const auto waypoint_groups = - utils::get_waypoint_groups(lanelet_sequence, *planner_data_.lanelet_map_ptr); - - double s = 0.; - for (const auto & lanelet : lanelet_sequence) { - std::vector reference_points; - const auto & centerline = lanelet.centerline(); - - std::optional overlapped_waypoint_group_index = std::nullopt; - for (auto it = centerline.begin(); it != centerline.end(); ++it) { - if (s > s_end) { - break; - } - - 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 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; - } - - const auto speed_limit = - planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); - - for (const auto & reference_point : reference_points) { - PathPointWithLaneId path_point{}; - path_point.point.pose.position = reference_point; - path_point.lane_ids.push_back(lanelet.id()); - path_point.point.longitudinal_velocity_mps = static_cast(speed_limit); - path_points.push_back(path_point); - } - - for (const auto & left_bound_point : lanelet.leftBound()) { - centerline_path.left_bound.push_back( - lanelet::utils::conversion::toGeomMsgPt(left_bound_point)); - } - for (const auto & right_bound_point : lanelet.rightBound()) { - centerline_path.right_bound.push_back( - lanelet::utils::conversion::toGeomMsgPt(right_bound_point)); - } + auto path_points = generate_path_points(lanelet_sequence, s_start, s_end, params); + if (path_points.empty()) { + return std::nullopt; } - utils::remove_overlapping_points(centerline_path); - // append a point if having only one point so that yaw calculation would work if (path_points.size() == 1) { const auto & lane_id = path_points.front().lane_ids.front(); @@ -367,9 +316,101 @@ std::optional PathGenerator::get_centerline_path( path_points.back().point.pose.orientation = std::prev(path_points.end(), 2)->point.pose.orientation; - centerline_path.header.frame_id = planner_data_.route_frame_id; - centerline_path.header.stamp = now(); - return centerline_path; + PathWithLaneId path{}; + path.header.frame_id = planner_data_.route_frame_id; + path.header.stamp = now(); + path.points = std::move(path_points); + + for (const auto & left_bound_point : lanelet_sequence.leftBound()) { + path.left_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(left_bound_point)); + } + for (const auto & right_bound_point : lanelet_sequence.rightBound()) { + path.right_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(right_bound_point)); + } + + return path; +} + +std::vector PathGenerator::generate_path_points( + const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const Params & params) const +{ + lanelet::ConstPoints3d path_points = lanelet_sequence.centerline().constData()->points(); + + auto waypoint_groups = utils::get_waypoint_groups( + lanelet_sequence, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold, + params.waypoint_group_interval_margin_ratio); + + for (const auto & [waypoints, interval] : waypoint_groups) { + const auto overlap_start_it = std::find_if( + path_points.begin(), path_points.end(), [&](const lanelet::BasicPoint3d & point) { + return get_arc_length_along_centerline(lanelet_sequence, point) >= interval.first; + }); + const auto overlap_end_it = + std::find_if(overlap_start_it, path_points.end(), [&](const lanelet::BasicPoint3d & point) { + return get_arc_length_along_centerline(lanelet_sequence, point) > interval.second; + }); + if (overlap_start_it == overlap_end_it) { + continue; + } + const auto waypoint_start_it = path_points.erase(overlap_start_it, overlap_end_it); + path_points.insert(waypoint_start_it, waypoints.begin(), waypoints.end()); + } + + 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)); + }; + + for (auto it = path_points.begin(); it != path_points.end(); ++it) { + const auto & path_point = *it; + + const auto lanelet_it = std::find_if( + lanelet_sequence.begin(), lanelet_sequence.end(), [&](const lanelet::ConstLanelet & lanelet) { + return get_arc_length_along_centerline(lanelet_sequence, path_point) >= + get_arc_length_along_centerline(lanelet_sequence, lanelet.centerline().front()); + }); + if (lanelet_it == lanelet_sequence.end()) { + continue; + } + + const auto s = get_arc_length_along_centerline(lanelet_sequence, path_point); + + if (s < s_start) { + if (it == std::prev(path_points.end())) { + break; + } + const auto & next_path_point = *std::next(it); + const auto s_next = get_arc_length_along_centerline(lanelet_sequence, next_path_point); + if (s_next > s_start) { + add_path_point( + get_interpolated_point(path_point, next_path_point, s_start - s), *lanelet_it); + } + continue; + } + + if (s > s_end) { + if (it == path_points.begin()) { + break; + } + const auto & prev_path_point = *std::prev(it); + add_path_point(get_interpolated_point(path_point, prev_path_point, s - s_end), *lanelet_it); + break; + } + + add_path_point(path_point, *lanelet_it); + } + + utils::remove_overlapping_points(path_points_with_lane_id); + + return path_points_with_lane_id; } } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 8be018dcaaabd..a5d54b1874d82 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -32,6 +32,7 @@ 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; class PathGenerator : public rclcpp::Node @@ -78,16 +79,20 @@ class PathGenerator : public rclcpp::Node std::optional plan_path(const InputData & input_data); - std::optional generate_centerline_path( + std::optional generate_path( const geometry_msgs::msg::Pose & current_pose, const Params & params) const; - std::optional get_centerline_path( - const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, + std::optional generate_path( + const lanelet::LaneletSequence & lanelet_sequence, + const geometry_msgs::msg::Pose & current_pose, const Params & params) const; + + std::optional generate_path( + const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, const Params & params) const; - std::optional get_centerline_path( - const lanelet::ConstLanelets & lanelet_sequence, const double s_start, - const double s_end) const; + std::vector generate_path_points( + const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const Params & params) const; }; } // namespace autoware::path_generator From 3fc0a59bb6f6b5b3532ed961bd784e1862644555 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 26 Nov 2024 14:11:25 +0900 Subject: [PATCH 22/34] log error when multiple next lanelets are found Signed-off-by: mitukou1109 --- .../autoware_path_generator/src/utils.cpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 17d1f78b52b37..7baf1f40b27e2 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -125,15 +125,21 @@ std::optional get_next_lanelet_within_route( return std::nullopt; } - for (const auto & lanelet : planner_data.routing_graph_ptr->following(lanelet)) { - if ( - planner_data.preferred_lanelets.front().id() != lanelet.id() && - exists(planner_data.route_lanelets, lanelet)) { - return lanelet; - } + 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."); } - return std::nullopt; + if ( + planner_data.preferred_lanelets.front().id() == lanelet.id() || + !exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + return next_lanelets.front(); } std::optional get_previous_lanelet_within_route( From 2f44c46a47ca5dcb772830be65f7d3dc56c6161e Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 26 Nov 2024 14:21:59 +0900 Subject: [PATCH 23/34] add suffix to utility functions Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 6 +++--- planning/autoware_path_generator/src/node.cpp | 2 +- planning/autoware_path_generator/src/utils.cpp | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 6269f0776e691..9307aadc8e741 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -30,15 +30,15 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace utils { -std::optional get_lanelet_sequence( +std::optional get_lanelet_sequence_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const geometry_msgs::msg::Pose & current_pose, const double forward_distance, const double backward_distance); -std::optional get_lanelets_after( +std::optional get_lanelets_after_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); -std::optional get_lanelets_up_to( +std::optional get_lanelets_up_to_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); std::optional get_next_lanelet_within_route( diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 50e8768a31c5c..3973783de4266 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -219,7 +219,7 @@ std::optional PathGenerator::generate_path( return std::nullopt; } - const auto lanelet_sequence = utils::get_lanelet_sequence( + const auto lanelet_sequence = utils::get_lanelet_sequence_within_route( current_lane, planner_data_, current_pose, params.forward_path_length, params.backward_path_length); if (!lanelet_sequence) { diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 7baf1f40b27e2..490c133e1ef70 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -32,7 +32,7 @@ bool exists(const std::vector & vec, const T & item) } } // namespace -std::optional get_lanelet_sequence( +std::optional get_lanelet_sequence_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const geometry_msgs::msg::Pose & current_pose, const double forward_distance, const double backward_distance) @@ -44,13 +44,13 @@ std::optional get_lanelet_sequence( const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose); const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); - const auto backward_lanelets = - get_lanelets_up_to(lanelet, planner_data, backward_distance - arc_coordinates.length); + const auto backward_lanelets = get_lanelets_up_to_within_route( + lanelet, planner_data, backward_distance - arc_coordinates.length); if (!backward_lanelets) { return std::nullopt; } - const auto forward_lanelets = get_lanelets_after( + const auto forward_lanelets = get_lanelets_after_within_route( lanelet, planner_data, forward_distance - (lanelet_length - arc_coordinates.length)); if (!forward_lanelets) { return std::nullopt; @@ -63,7 +63,7 @@ std::optional get_lanelet_sequence( return lanelets; } -std::optional get_lanelets_after( +std::optional get_lanelets_after_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { if (!exists(planner_data.route_lanelets, lanelet)) { @@ -88,7 +88,7 @@ std::optional get_lanelets_after( return lanelets; } -std::optional get_lanelets_up_to( +std::optional get_lanelets_up_to_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) { if (!exists(planner_data.route_lanelets, lanelet)) { From 000629940efe2cf1d402266543679f118a4bf447 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 29 Nov 2024 16:18:16 +0900 Subject: [PATCH 24/34] change utility function signatures Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 16 ++-- .../autoware_path_generator/src/utils.cpp | 84 ++++++++++--------- 2 files changed, 54 insertions(+), 46 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 9307aadc8e741..5f23679ff34f0 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -30,25 +30,25 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace utils { -std::optional get_lanelet_sequence_within_route( +std::optional get_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const geometry_msgs::msg::Pose & current_pose, const double forward_distance, - const double backward_distance); + const geometry_msgs::msg::Pose & current_pose, const double backward_distance, + const double forward_distance); -std::optional get_lanelets_after_within_route( +std::optional get_lanelets_within_route_up_to( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); -std::optional get_lanelets_up_to_within_route( +std::optional get_lanelets_within_route_after( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); -std::optional get_next_lanelet_within_route( +std::optional get_previous_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); -std::optional get_previous_lanelet_within_route( +std::optional get_next_lanelet_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); std::vector>> get_waypoint_groups( - const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map, + const lanelet::ConstLanelets & lanelets, const lanelet::LaneletMap & lanelet_map, const double group_separation_threshold, const double interval_margin_ratio); void remove_overlapping_points(std::vector & path_points); diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 490c133e1ef70..879a75f01ddea 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -32,10 +32,10 @@ bool exists(const std::vector & vec, const T & item) } } // namespace -std::optional get_lanelet_sequence_within_route( +std::optional get_lanelets_within_route( const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, - const geometry_msgs::msg::Pose & current_pose, const double forward_distance, - const double backward_distance) + 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; @@ -44,13 +44,13 @@ std::optional get_lanelet_sequence_within_route( const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose); const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); - const auto backward_lanelets = get_lanelets_up_to_within_route( + 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_after_within_route( + 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; @@ -63,7 +63,7 @@ std::optional get_lanelet_sequence_within_route( return lanelets; } -std::optional get_lanelets_after_within_route( +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)) { @@ -75,20 +75,21 @@ std::optional get_lanelets_after_within_route( auto length = 0.; while (rclcpp::ok() && length < distance) { - const auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); - if (!next_lanelet) { + const auto prev_lanelet = get_previous_lanelet_within_route(current_lanelet, planner_data); + if (!prev_lanelet) { break; } - lanelets.push_back(*next_lanelet); - current_lanelet = *next_lanelet; - length += lanelet::utils::getLaneletLength2d(*next_lanelet); + 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_up_to_within_route( +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)) { @@ -100,20 +101,41 @@ std::optional get_lanelets_up_to_within_route( auto length = 0.; while (rclcpp::ok() && length < distance) { - const auto prev_lanelet = get_previous_lanelet_within_route(current_lanelet, planner_data); - if (!prev_lanelet) { + const auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); + if (!next_lanelet) { break; } - lanelets.push_back(*prev_lanelet); - current_lanelet = *prev_lanelet; - length += lanelet::utils::getLaneletLength2d(*prev_lanelet); + lanelets.push_back(*next_lanelet); + current_lanelet = *next_lanelet; + length += lanelet::utils::getLaneletLength2d(*next_lanelet); } - std::reverse(lanelets.begin(), lanelets.end()); 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) { @@ -134,35 +156,21 @@ std::optional get_next_lanelet_within_route( } if ( - planner_data.preferred_lanelets.front().id() == lanelet.id() || - !exists(planner_data.route_lanelets, lanelet)) { + 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::optional get_previous_lanelet_within_route( - const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) -{ - if (exists(planner_data.start_lanelets, lanelet)) { - return std::nullopt; - } - - for (const auto & lanelet : planner_data.routing_graph_ptr->previous(lanelet)) { - if (exists(planner_data.route_lanelets, lanelet)) { - return lanelet; - } - } - - return std::nullopt; -} - std::vector>> get_waypoint_groups( - const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map, + 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) { @@ -171,7 +179,7 @@ std::vector>> get_wa return arc_coordinates.length + lateral_distance_factor * std::abs(arc_coordinates.distance); }; - for (const auto & lanelet : lanelet_sequence) { + for (const auto & lanelet : lanelets) { if (!lanelet.hasAttribute("waypoints")) { continue; } From bfaab95596c591d5dfaea6256a38a558f334f1ae Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 4 Dec 2024 17:31:48 +0900 Subject: [PATCH 25/34] use autoware_trajectory for path handling Signed-off-by: mitukou1109 --- planning/autoware_path_generator/package.xml | 1 + planning/autoware_path_generator/src/node.cpp | 230 ++++++++---------- planning/autoware_path_generator/src/node.hpp | 12 +- 3 files changed, 103 insertions(+), 140 deletions(-) diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index b25c04e36f9ec..b094f61acd0ff 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -18,6 +18,7 @@ autoware_lanelet2_extension autoware_motion_utils + autoware_trajectory autoware_vehicle_info_utils generate_parameter_library rclcpp diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 3973783de4266..aa13a5b9d1cb9 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -219,144 +219,52 @@ std::optional PathGenerator::generate_path( return std::nullopt; } - const auto lanelet_sequence = utils::get_lanelet_sequence_within_route( - current_lane, planner_data_, current_pose, params.forward_path_length, - params.backward_path_length); - if (!lanelet_sequence) { + 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(*lanelet_sequence, current_pose, params); + return generate_path(*lanelets, current_pose, params); } std::optional PathGenerator::generate_path( - const lanelet::LaneletSequence & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & current_pose, const Params & params) const { - if (lanelet_sequence.empty()) { + if (lanelets.empty()) { return std::nullopt; } - const auto arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence.lanelets(), current_pose); + 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(lanelet_sequence.lanelets().back(), planner_data_)) { - const double lane_length = lanelet::geometry::length(lanelet_sequence.centerline2d()); - s_end = std::min(s_end, lane_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 lanelet_sequence.lanelets().back().id() == goal_lanelet.id(); - })) { + [&](const auto & goal_lanelet) { return lanelets.back().id() == goal_lanelet.id(); })) { const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(lanelet_sequence.lanelets(), planner_data_.goal_pose); + lanelet::utils::getArcCoordinates(lanelets, planner_data_.goal_pose); s_end = std::min(s_end, goal_arc_coordinates.length); } return s_end; }(); - auto path = generate_path(lanelet_sequence, s_start, s_end, params); - if (!path) { - return std::nullopt; - } - - path = autoware::motion_utils::resamplePath( - *path, params.input_path_interval, params.enable_akima_spline_first); - - const auto current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path->points, current_pose, params.ego_nearest_dist_threshold, - params.ego_nearest_yaw_threshold); - - path->points = autoware::motion_utils::cropPoints( - path->points, current_pose.position, current_seg_idx, params.forward_path_length, - params.backward_path_length + params.input_path_interval); - - return path; + return generate_path(lanelets, s_start, s_end, params); } std::optional PathGenerator::generate_path( - const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, - const Params & params) const -{ - auto path_points = generate_path_points(lanelet_sequence, s_start, s_end, params); - if (path_points.empty()) { - return std::nullopt; - } - - // append a point if having only one point so that yaw calculation would work - if (path_points.size() == 1) { - const auto & lane_id = path_points.front().lane_ids.front(); - const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(lane_id); - const auto & point = path_points.front().point.pose.position; - const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); - - PathPointWithLaneId path_point{}; - path_point.lane_ids.push_back(lane_id); - constexpr double ds = 0.1; - path_point.point.pose.position.x = point.x + ds * std::cos(lane_yaw); - path_point.point.pose.position.y = point.y + ds * std::sin(lane_yaw); - path_point.point.pose.position.z = point.z; - path_points.push_back(path_point); - } - - // set yaw to each point - for (auto it = path_points.begin(); it != std::prev(path_points.end()); ++it) { - const auto angle = autoware::universe_utils::calcAzimuthAngle( - it->point.pose.position, std::next(it)->point.pose.position); - it->point.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(angle); - } - path_points.back().point.pose.orientation = - std::prev(path_points.end(), 2)->point.pose.orientation; - - PathWithLaneId path{}; - path.header.frame_id = planner_data_.route_frame_id; - path.header.stamp = now(); - path.points = std::move(path_points); - - for (const auto & left_bound_point : lanelet_sequence.leftBound()) { - path.left_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(left_bound_point)); - } - for (const auto & right_bound_point : lanelet_sequence.rightBound()) { - path.right_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(right_bound_point)); - } - - return path; -} - -std::vector PathGenerator::generate_path_points( - const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end, const Params & params) const { - lanelet::ConstPoints3d path_points = lanelet_sequence.centerline().constData()->points(); - - auto waypoint_groups = utils::get_waypoint_groups( - lanelet_sequence, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold, - params.waypoint_group_interval_margin_ratio); - - for (const auto & [waypoints, interval] : waypoint_groups) { - const auto overlap_start_it = std::find_if( - path_points.begin(), path_points.end(), [&](const lanelet::BasicPoint3d & point) { - return get_arc_length_along_centerline(lanelet_sequence, point) >= interval.first; - }); - const auto overlap_end_it = - std::find_if(overlap_start_it, path_points.end(), [&](const lanelet::BasicPoint3d & point) { - return get_arc_length_along_centerline(lanelet_sequence, point) > interval.second; - }); - if (overlap_start_it == overlap_end_it) { - continue; - } - const auto waypoint_start_it = path_points.erase(overlap_start_it, overlap_end_it); - path_points.insert(waypoint_start_it, waypoints.begin(), waypoints.end()); - } - std::vector path_points_with_lane_id{}; const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) { @@ -369,48 +277,104 @@ std::vector PathGenerator::generate_path_points( path_points_with_lane_id.push_back(std::move(path_point_with_lane_id)); }; - for (auto it = path_points.begin(); it != path_points.end(); ++it) { - const auto & path_point = *it; + 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.; - const auto lanelet_it = std::find_if( - lanelet_sequence.begin(), lanelet_sequence.end(), [&](const lanelet::ConstLanelet & lanelet) { - return get_arc_length_along_centerline(lanelet_sequence, path_point) >= - get_arc_length_along_centerline(lanelet_sequence, lanelet.centerline().front()); - }); - if (lanelet_it == lanelet_sequence.end()) { + 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 auto s = get_arc_length_along_centerline(lanelet_sequence, path_point); + const lanelet::LaneletSequence extended_lanelet_sequence(extended_lanelets); + std::optional overlapping_waypoint_group_index = std::nullopt; - if (s < s_start) { - if (it == std::prev(path_points.end())) { - break; + 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; } - const auto & next_path_point = *std::next(it); - const auto s_next = get_arc_length_along_centerline(lanelet_sequence, next_path_point); - if (s_next > s_start) { - add_path_point( - get_interpolated_point(path_point, next_path_point, s_start - s), *lanelet_it); + + 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; } - continue; - } - if (s > s_end) { - if (it == path_points.begin()) { + 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; } - const auto & prev_path_point = *std::prev(it); - add_path_point(get_interpolated_point(path_point, prev_path_point, s - s_end), *lanelet_it); - break; + + if (overlapping_waypoint_group_index) { + continue; + } + add_path_point(*point_it, *lanelet_it); } + } - add_path_point(path_point, *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; } - utils::remove_overlapping_points(path_points_with_lane_id); + 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_points_with_lane_id; + return path; } } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index a5d54b1874d82..dd765294fad13 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -17,6 +17,7 @@ #include "autoware/path_generator/common_structs.hpp" +#include #include #include @@ -34,6 +35,7 @@ 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 { @@ -83,15 +85,11 @@ class PathGenerator : public rclcpp::Node const geometry_msgs::msg::Pose & current_pose, const Params & params) const; std::optional generate_path( - const lanelet::LaneletSequence & lanelet_sequence, - const geometry_msgs::msg::Pose & current_pose, const Params & params) const; - - std::optional generate_path( - const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & current_pose, const Params & params) const; - std::vector generate_path_points( - const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + std::optional generate_path( + const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end, const Params & params) const; }; } // namespace autoware::path_generator From 597cf3ee38f1d46906f21e87b69cbe3594e0c225 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 29 Nov 2024 16:26:26 +0900 Subject: [PATCH 26/34] remove unnecessary functions Signed-off-by: mitukou1109 --- .../include/autoware/path_generator/utils.hpp | 2 -- planning/autoware_path_generator/src/node.cpp | 16 ---------------- .../autoware_path_generator/src/utils.cpp | 19 ------------------- 3 files changed, 37 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 5f23679ff34f0..b582eda3a4ff0 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -50,8 +50,6 @@ std::optional get_next_lanelet_within_route( std::vector>> get_waypoint_groups( const lanelet::ConstLanelets & lanelets, const lanelet::LaneletMap & lanelet_map, const double group_separation_threshold, const double interval_margin_ratio); - -void remove_overlapping_points(std::vector & path_points); } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index aa13a5b9d1cb9..42424db6126aa 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -37,22 +37,6 @@ double get_arc_length_along_centerline(const T & lanelet, const U & point) return lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), lanelet::utils::to2D(point)) .length; } - -template -lanelet::BasicPoint3d get_interpolated_point(const T & start, const U & end, const double distance) -{ - lanelet::Point3d start_point, end_point; - start_point.x() = start.x(); - start_point.y() = start.y(); - start_point.z() = start.z(); - end_point.x() = end.x(); - end_point.y() = end.y(); - end_point.z() = end.z(); - - return lanelet::geometry::interpolatedPointAtDistance( - lanelet::ConstLineString3d(lanelet::InvalId, lanelet::Points3d{start_point, end_point}), - distance); -} } // namespace namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 879a75f01ddea..c9903c59998d8 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -203,24 +203,5 @@ std::vector>> get_wa return waypoint_groups; } - -void remove_overlapping_points(std::vector & path_points) -{ - auto filtered_path_end_it = path_points.begin(); - for (auto it = std::next(path_points.begin()); it != path_points.end();) { - constexpr auto min_interval = 0.001; - if ( - autoware::universe_utils::calcDistance3d(filtered_path_end_it->point, it->point) < - min_interval) { - filtered_path_end_it->lane_ids.push_back(it->lane_ids.front()); - filtered_path_end_it->point.longitudinal_velocity_mps = std::min( - filtered_path_end_it->point.longitudinal_velocity_mps, it->point.longitudinal_velocity_mps); - it = path_points.erase(it); - } else { - filtered_path_end_it = it; - ++it; - } - } -} } // namespace utils } // namespace autoware::path_generator From ea6a985470659516ef749d1f500f11d80c80fe00 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 29 Nov 2024 16:27:54 +0900 Subject: [PATCH 27/34] remove parameters Signed-off-by: mitukou1109 --- .../config/path_generator.param.yaml | 5 +---- .../param/path_generator_parameters.yaml | 16 ++-------------- 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml index a44bdaa516367..1a8d2c6c57b61 100644 --- a/planning/autoware_path_generator/config/path_generator.param.yaml +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -4,7 +4,4 @@ backward_path_length: 5.0 forward_path_length: 300.0 waypoint_group_separation_threshold: 1.0 - lateral_distance_factor: 10.0 - - enable_akima_spline_first: false - input_path_interval: 2.0 + waypoint_group_interval_margin_ratio: 10.0 diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml index 7bfdc6e266201..8d9f77a777d1c 100644 --- a/planning/autoware_path_generator/param/path_generator_parameters.yaml +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -2,10 +2,10 @@ path_generator: planning_hz: type: double - forward_path_length: + backward_path_length: type: double - backward_path_length: + forward_path_length: type: double waypoint_group_separation_threshold: @@ -13,15 +13,3 @@ path_generator: waypoint_group_interval_margin_ratio: type: double - - input_path_interval: - type: double - - enable_akima_spline_first: - type: bool - - ego_nearest_dist_threshold: - type: double - - ego_nearest_yaw_threshold: - type: double From f712f34a74bca5ff50bdd7b9d82f8b0e57e1bccd Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 29 Nov 2024 16:42:18 +0900 Subject: [PATCH 28/34] include necessary header Signed-off-by: mitukou1109 --- planning/autoware_path_generator/src/node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 42424db6126aa..16b5d774d92c8 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -27,6 +27,7 @@ #include #include +#include #include namespace From 409a7911fb8bd51eacf51145f7ad087c3110d804 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 23 Dec 2024 12:34:37 +0900 Subject: [PATCH 29/34] add README Signed-off-by: mitukou1109 --- planning/autoware_path_generator/README.md | 41 +++++++++++++++++++ ..._overlap_interval_determination.drawio.svg | 4 ++ 2 files changed, 45 insertions(+) create mode 100644 planning/autoware_path_generator/README.md create mode 100644 planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md new file mode 100644 index 0000000000000..9900fb21547c2 --- /dev/null +++ b/planning/autoware_path_generator/README.md @@ -0,0 +1,41 @@ +# Path Generator + +The `path_generator` node receives a route from `mission_planner` and converts the centerline 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 centerlines are concatenated to generate a path. + +If waypoints exist in the route, it replaces the overlapped segment of the centerline 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) + +## 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 + +| Name | Type | Description | +| :------------------------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------- | +| `planning_hz` | double | planning frequency | +| `backward_path_length` | double | length of the generated path behind vehicle | +| `forward_path_length` | double | length of the generated path in front of vehicle | +| `waypoint_group_separation_threshold` | double | maximum distance at which consecutive waypoints are considered to belong to the same group (see [here](#path-generation) for details) | +| `waypoint_group_interval_margin_ratio` | double | ratio for determining length of switching section from centerline to waypoints (see [here](#path-generation) for details) | 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 0000000000000..aed0a5c1feb6a --- /dev/null +++ b/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg @@ -0,0 +1,4 @@ + + + +
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 \ No newline at end of file From 0c9a70de564ad457ff3e01c0bccdbdf271295235 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 23 Dec 2024 03:37:45 +0000 Subject: [PATCH 30/34] style(pre-commit): autofix --- planning/autoware_path_generator/README.md | 10 +- ..._overlap_interval_determination.drawio.svg | 623 +++++++++++++++++- 2 files changed, 627 insertions(+), 6 deletions(-) diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md index 9900fb21547c2..b9f1c50c7c470 100644 --- a/planning/autoware_path_generator/README.md +++ b/planning/autoware_path_generator/README.md @@ -32,10 +32,10 @@ The overlap interval is determined as shown in the following figure. ## Parameters -| Name | Type | Description | -| :------------------------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------- | -| `planning_hz` | double | planning frequency | -| `backward_path_length` | double | length of the generated path behind vehicle | -| `forward_path_length` | double | length of the generated path in front of vehicle | +| Name | Type | Description | +| :------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | +| `planning_hz` | double | planning frequency | +| `backward_path_length` | double | length of the generated path behind vehicle | +| `forward_path_length` | double | length of the generated path in front of vehicle | | `waypoint_group_separation_threshold` | double | maximum distance at which consecutive waypoints are considered to belong to the same group (see [here](#path-generation) for details) | | `waypoint_group_interval_margin_ratio` | double | ratio for determining length of switching section from centerline to waypoints (see [here](#path-generation) for details) | 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 index aed0a5c1feb6a..64e5ffb8c1b69 100644 --- 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 @@ -1,4 +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 \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 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 + + + + + + + + From 3f08a34ca0d07628fe2c48923525a97640e27bca Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 23 Dec 2024 14:01:54 +0900 Subject: [PATCH 31/34] add json schema Signed-off-by: mitukou1109 --- planning/autoware_path_generator/README.md | 6 ++- .../schema/path_generator.schema.json | 50 +++++++++++++++++++ 2 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 planning/autoware_path_generator/schema/path_generator.schema.json diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md index b9f1c50c7c470..b7f79c601525a 100644 --- a/planning/autoware_path_generator/README.md +++ b/planning/autoware_path_generator/README.md @@ -32,10 +32,12 @@ The overlap interval is determined as shown in the following figure. ## Parameters +{{ json_to_markdown("planning/autoware_path_generator/schema/path_generator.schema.json") }} + | Name | Type | Description | | :------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | | `planning_hz` | double | planning frequency | -| `backward_path_length` | double | length of the generated path behind vehicle | -| `forward_path_length` | double | length of the generated path in front of vehicle | +| `backward_path_length` | double | length of generated path behind vehicle | +| `forward_path_length` | double | length of generated path in front of vehicle | | `waypoint_group_separation_threshold` | double | maximum distance at which consecutive waypoints are considered to belong to the same group (see [here](#path-generation) for details) | | `waypoint_group_interval_margin_ratio` | double | ratio for determining length of switching section from centerline to waypoints (see [here](#path-generation) for details) | 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 0000000000000..8bb9c005f79a4 --- /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 + } + } +} From 92fc51a8b7e0d9b62b639fbef947a36fb70e394c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 23 Dec 2024 05:06:20 +0000 Subject: [PATCH 32/34] style(pre-commit): autofix --- planning/autoware_path_generator/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md index b7f79c601525a..dc148a9ea7a12 100644 --- a/planning/autoware_path_generator/README.md +++ b/planning/autoware_path_generator/README.md @@ -37,7 +37,7 @@ The overlap interval is determined as shown in the following figure. | Name | Type | Description | | :------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | | `planning_hz` | double | planning frequency | -| `backward_path_length` | double | length of generated path behind vehicle | -| `forward_path_length` | double | length of generated path in front of vehicle | +| `backward_path_length` | double | length of generated path behind vehicle | +| `forward_path_length` | double | length of generated path in front of vehicle | | `waypoint_group_separation_threshold` | double | maximum distance at which consecutive waypoints are considered to belong to the same group (see [here](#path-generation) for details) | | `waypoint_group_interval_margin_ratio` | double | ratio for determining length of switching section from centerline to waypoints (see [here](#path-generation) for details) | From f4dfe9197e43b6c4a7182830ebb28c10f9c956a3 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 23 Dec 2024 15:16:56 +0900 Subject: [PATCH 33/34] add flowchart to README Signed-off-by: mitukou1109 --- planning/autoware_path_generator/README.md | 42 ++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md index dc148a9ea7a12..f4df1649da0ac 100644 --- a/planning/autoware_path_generator/README.md +++ b/planning/autoware_path_generator/README.md @@ -16,6 +16,48 @@ 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 + +```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 centerline 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 | From c639be547755edc7b38a10b21e172ec35221c8b4 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Mon, 23 Dec 2024 17:25:44 +0900 Subject: [PATCH 34/34] remove duplicate parameter list Signed-off-by: mitukou1109 --- planning/autoware_path_generator/README.md | 8 -------- 1 file changed, 8 deletions(-) diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md index f4df1649da0ac..282911bb270cd 100644 --- a/planning/autoware_path_generator/README.md +++ b/planning/autoware_path_generator/README.md @@ -75,11 +75,3 @@ stop ## Parameters {{ json_to_markdown("planning/autoware_path_generator/schema/path_generator.schema.json") }} - -| Name | Type | Description | -| :------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | -| `planning_hz` | double | planning frequency | -| `backward_path_length` | double | length of generated path behind vehicle | -| `forward_path_length` | double | length of generated path in front of vehicle | -| `waypoint_group_separation_threshold` | double | maximum distance at which consecutive waypoints are considered to belong to the same group (see [here](#path-generation) for details) | -| `waypoint_group_interval_margin_ratio` | double | ratio for determining length of switching section from centerline to waypoints (see [here](#path-generation) for details) |