From 1e1d2da0518531c38bc1d62512a5b4d346723b80 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 31 Jan 2023 18:12:17 +0900 Subject: [PATCH] feat(behavior_path_planner): pull over freespace parking Signed-off-by: kosuke55 --- .../behavior_planning.launch.py | 4 + .../scenario_planning/parking.launch.py | 2 +- planning/behavior_path_planner/CMakeLists.txt | 1 + .../behavior_path_planner_node.hpp | 2 + .../behavior_path_planner/data_manager.hpp | 1 + .../behavior_path_planner/path_utilities.hpp | 21 ++ .../pull_over/freespace_pull_over.hpp | 58 ++++ .../pull_over/pull_over_module.hpp | 41 ++- .../pull_over/pull_over_planner_base.hpp | 1 + .../behavior_path_planner/utilities.hpp | 4 + planning/behavior_path_planner/package.xml | 1 + .../src/behavior_path_planner_node.cpp | 8 + .../src/path_utilities.cpp | 153 +++++++++++ .../pull_over/freespace_pull_over.cpp | 195 ++++++++++++++ .../pull_over/pull_over_module.cpp | 248 ++++++++++++++---- .../pull_over/shift_pull_over.cpp | 57 +--- .../behavior_path_planner/src/utilities.cpp | 137 ++++++++++ .../abstract_algorithm.hpp | 24 ++ .../astar_search.hpp | 1 + .../freespace_planning_algorithms/package.xml | 1 + .../src/astar_search.cpp | 24 +- .../src/rrtstar.cpp | 2 +- 22 files changed, 864 insertions(+), 122 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/pull_over/freespace_pull_over.cpp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index aa2d31ff15745..44c9283b2ef99 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -68,6 +68,10 @@ def launch_setup(context, *args, **kwargs): ("~/input/vector_map", LaunchConfiguration("map_topic_name")), ("~/input/perception", "/perception/object_recognition/objects"), ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), + ( + "~/input/costmap", + "/planning/scenario_planning/parking/costmap_generator/occupancy_grid", + ), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/accel", "/localization/acceleration"), ("~/input/scenario", "/planning/scenario_planning/scenario"), diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index acf0b7546e27f..98932dfc88e2f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -63,7 +63,7 @@ def launch_setup(context, *args, **kwargs): "vehicle_frame": "base_link", "map_frame": "map", "update_rate": 10.0, - "activate_by_scenario": True, + "activate_by_scenario": False, "grid_min_value": 0.0, "grid_max_value": 1.0, "grid_resolution": 0.2, diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 7a31aed99f160..0e3233462b101 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -30,6 +30,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/pull_over/util.cpp src/scene_module/pull_over/shift_pull_over.cpp src/scene_module/pull_over/geometric_pull_over.cpp + src/scene_module/pull_over/freespace_pull_over.cpp src/scene_module/pull_over/goal_searcher.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 07d5621ec7fe9..71688a5cbbcd8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -99,6 +99,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; + rclcpp::Subscription::SharedPtr costmap_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; @@ -147,6 +148,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); + void onCostMap(const OccupancyGrid::ConstSharedPtr msg); void onExternalApproval(const ApprovalMsg::ConstSharedPtr msg); void onForceApproval(const PathChangeModule::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index ccc4b8e183fd5..adfb583ee624d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -73,6 +73,7 @@ struct PlannerData AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{}; PredictedObjects::ConstSharedPtr dynamic_object{}; OccupancyGrid::ConstSharedPtr occupancy_grid{}; + OccupancyGrid::ConstSharedPtr costmap{}; PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; lanelet::ConstLanelets current_lanes{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index 09269e8cb7456..b9a9a44c27721 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -70,6 +71,26 @@ std::pair getPathTurnSignal( const ShiftLine & shift_line, const Pose & pose, const double & velocity, const BehaviorPathPlannerParameters & common_parameter); +PathWithLaneId convertWayPointsToPathWithLaneId( + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + +std::vector getReversingIndices(const PathWithLaneId & path); + +std::vector devidePath( + const PathWithLaneId & path, const std::vector indices); + +void correctDevidedPathVelocity(std::vector & devided_paths); + +bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold); + +// only two points is supported +std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + +std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); + } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__PATH_UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp new file mode 100644 index 0000000000000..d43a79b2a894b --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__FREESPACE_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__FREESPACE_PULL_OVER_HPP_ + +#include "behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + +class FreespacePullOver : public PullOverPlannerBase +{ +public: + FreespacePullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } + + boost::optional plan(const Pose & goal_pose) override; + +protected: + PlannerCommonParam getCommonParam(rclcpp::Node & node) const; + AstarParam getAstarParam(rclcpp::Node & node) const; + RRTStarParam getRRTStarParam(rclcpp::Node & node) const; + + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 4d463c9c3fa09..64f9a7b8ea9d0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ +#include "behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp" #include "behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp" #include "behavior_path_planner/scene_module/pull_over/goal_searcher.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp" @@ -23,6 +24,8 @@ #include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" #include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" +#include +#include #include #include @@ -44,17 +47,27 @@ using nav_msgs::msg::OccupancyGrid; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStar; +using freespace_planning_algorithms::RRTStarParam; + enum PathType { NONE = 0, SHIFT, ARC_FORWARD, ARC_BACKWARD, + FREESPACE, }; struct PUllOverStatus { - PullOverPath pull_over_path{}; + std::shared_ptr pull_over_path{}; + std::shared_ptr lane_parking_pull_over_path{}; size_t current_path_idx{0}; + bool require_increment_{true}; std::shared_ptr prev_stop_path{nullptr}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_over_lanes{}; @@ -65,6 +78,8 @@ struct PUllOverStatus bool has_decided_velocity{false}; bool has_requested_approval{false}; std::optional stop_pose{}; + bool is_ready{false}; + bool during_freespace_parking{false}; }; class PullOverModule : public SceneModuleInterface @@ -99,8 +114,9 @@ class PullOverModule : public SceneModuleInterface PullOverParameters parameters_; std::vector> pull_over_planners_; + std::unique_ptr freespace_planner_; std::shared_ptr goal_searcher_; - + // std::unique_ptr freespace_planner_; PullOverPath shift_parking_path_; vehicle_info_util::VehicleInfo vehicle_info_; @@ -116,13 +132,16 @@ class PullOverModule : public SceneModuleInterface std::optional closest_start_pose_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; - std::deque odometry_buffer_; + std::deque odometry_buffer_stopped_; + std::deque odometry_buffer_stuck_; + tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::unique_ptr last_received_time_; std::unique_ptr last_approved_time_; + std::unique_ptr last_increment_time_; std::unique_ptr last_approved_pose_; - void incrementPathIndex(); + bool incrementPathIndex(); PathWithLaneId getCurrentPath() const; Pose calcRefinedGoal(const Pose & goal_pose) const; ParallelParkingParameters getGeometricPullOverParameters() const; @@ -131,6 +150,8 @@ class PullOverModule : public SceneModuleInterface PathWithLaneId generateEmergencyStopPath(); bool isStopped(); + bool isStopped( + std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); bool hasFinishedPullOver(); void updateOccupancyGrid(); @@ -141,15 +162,23 @@ class PullOverModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo() const; + bool planFreespacePath(); + bool returnToLaneParking(); + bool isStuck(); + // timer for generating pull over path candidates void onTimer(); - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::CallbackGroup::SharedPtr timer_cb_group_; + rclcpp::TimerBase::SharedPtr lane_parking_timer_; + rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; std::mutex mutex_; // debug void setDebugData(); void printParkingPositionError() const; + + void onFreespaceParkingTimer(); + rclcpp::TimerBase::SharedPtr freespace_parking_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp index 92d4d872a4010..9bc2fa2d3ec79 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp @@ -39,6 +39,7 @@ enum class PullOverPlannerType { SHIFT, ARC_FORWARD, ARC_BACKWARD, + FREESPACE, }; struct PullOverPath diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 4f9b8300ab87b..9c11ac60e0570 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -329,6 +329,10 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward = true); +void generateDrivableArea( + PathWithLaneId & path, const double vehicle_length, const double vehicle_width, + const double margin, const bool is_driving_forward = true); + lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1b50220431362..d96610d775ffc 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -47,6 +47,7 @@ autoware_auto_tf2 autoware_auto_vehicle_msgs behaviortree_cpp_v3 + freespace_planning_algorithms geometry_msgs interpolation lane_departure_checker diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9847287793725..e57ed3c37984b 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -90,6 +90,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod occupancy_grid_subscriber_ = create_subscription( "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); + costmap_subscriber_ = create_subscription( + "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), + createSubscriptionOptions(this)); scenario_subscriber_ = create_subscription( "~/input/scenario", 1, [this](const Scenario::ConstSharedPtr msg) { @@ -1037,6 +1040,11 @@ void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPt const std::lock_guard lock(mutex_pd_); planner_data_->occupancy_grid = msg; } +void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) +{ + const std::lock_guard lock(mutex_pd_); + planner_data_->costmap = msg; +} void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg) { const std::lock_guard lock(mutex_pd_); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index a2a3343986576..568a238d8b079 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -313,4 +313,157 @@ std::pair getPathTurnSignal( return std::make_pair(turn_signal, max_distance); } +PathWithLaneId convertWayPointsToPathWithLaneId( + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) +{ + PathWithLaneId path; + path.header = waypoints.header; + for (const auto & waypoint : waypoints.waypoints) { + PathPointWithLaneId point{}; + point.point.pose = waypoint.pose.pose; + // point.lane_id = // todo + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; + path.points.push_back(point); + } + return path; +} + +std::vector getReversingIndices(const PathWithLaneId & path) +{ + std::vector indices; + + for (size_t i = 0; i < path.points.size() - 1; ++i) { + if ( + path.points.at(i).point.longitudinal_velocity_mps * + path.points.at(i + 1).point.longitudinal_velocity_mps < + 0) { + indices.push_back(i); + } + } + + return indices; +} + +std::vector devidePath( + const PathWithLaneId & path, const std::vector indices) +{ + std::vector divided_paths; + + if (indices.empty()) { + divided_paths.push_back(path); + return divided_paths; + } + + for (size_t i = 0; i < indices.size(); ++i) { + PathWithLaneId divided_path; + divided_path.header = path.header; + if (i == 0) { + divided_path.points.insert( + divided_path.points.end(), path.points.begin(), path.points.begin() + indices.at(i) + 1); + } else { + // include the point at indices.at(i - 1) and indices.at(i) + divided_path.points.insert( + divided_path.points.end(), path.points.begin() + indices.at(i - 1), + path.points.begin() + indices.at(i) + 1); + } + divided_paths.push_back(divided_path); + } + + PathWithLaneId divided_path; + divided_path.header = path.header; + divided_path.points.insert( + divided_path.points.end(), path.points.begin() + indices.back(), path.points.end()); + divided_paths.push_back(divided_path); + + return divided_paths; +} + +void correctDevidedPathVelocity(std::vector & devided_paths) +{ + for (auto & path : devided_paths) { + const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + if (!is_driving_forward) { + continue; + } + + if (*is_driving_forward) { + for (auto & point : path.points) { + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + } else { + for (auto & point : path.points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + } + path.points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold) +{ + for (const auto & point : path.points) { + const auto & p = point.point.pose.position; + const double dist = std::hypot(pose.position.x - p.x, pose.position.y - p.y); + if (dist < distance_threshold) { + return true; + } + } + return false; +} + +// only two points is supported +std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + const double h = base_s.at(1) - base_s.at(0); + + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); + } + + return res; +} + +std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval) +{ + std::vector interpolated_poses{}; // output + + const std::vector base_s{ + 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const std::vector base_x{start_pose.position.x, end_pose.position.x}; + const std::vector base_y{start_pose.position.y, end_pose.position.y}; + std::vector new_s; + + constexpr double eps = 0.3; // prevent overlapping + for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + new_s.push_back(s); + } + + const std::vector interpolated_x = splineTwoPoints( + base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), + std::cos(tf2::getYaw(end_pose.orientation)), new_s); + const std::vector interpolated_y = splineTwoPoints( + base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), + std::sin(tf2::getYaw(end_pose.orientation)), new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + Pose pose{}; + pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + pose.position.x = interpolated_x.at(i); + pose.position.y = interpolated_y.at(i); + pose.position.z = end_pose.position.z; + interpolated_poses.push_back(pose); + } + + return interpolated_poses; +} + } // namespace behavior_path_planner::util diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/freespace_pull_over.cpp new file mode 100644 index 0000000000000..146a63bb540af --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_over/freespace_pull_over.cpp @@ -0,0 +1,195 @@ +// Copyright 2023 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 "behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/pull_over/util.hpp" + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOver::FreespacePullOver( + rclcpp::Node & node, const PullOverParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOverPlannerBase{node, parameters} +{ + velocity_ = node.declare_parameter("pull_over.freespace_parking.velocity", 1.0); + + const double vehicle_shape_margin = + node.declare_parameter("pull_over.freespace_parking.vehicle_shape_margin", 1.0); + freespace_planning_algorithms::VehicleShape vehicle_shape(vehicle_info, vehicle_shape_margin); + + const auto algorithm = + node.declare_parameter("pull_over.freespace_parking.planning_algorithm", "astar"); + if (algorithm == "astar") { + const auto astar_param = getAstarParam(node); + use_back_ = astar_param.use_back; + planner_ = std::make_unique(getCommonParam(node), vehicle_shape, astar_param); + } else if (algorithm == "rrtstar") { + use_back_ = true; // no opition for disabling back in rrtstar + planner_ = + std::make_unique(getCommonParam(node), vehicle_shape, getRRTStarParam(node)); + } +} + +PlannerCommonParam FreespacePullOver::getCommonParam(rclcpp::Node & node) const +{ + const auto dp = [&node](const std::string & str, auto def_val) { + std::string name = "pull_over.freespace_parking." + str; + return node.declare_parameter(name, def_val); + }; + + PlannerCommonParam p{}; + + // search configs + p.time_limit = dp("time_limit", 5000.0); + p.minimum_turning_radius = dp("minimum_turning_radius", 0.5); + p.maximum_turning_radius = dp("maximum_turning_radius", 6.0); + p.turning_radius_size = dp("turning_radius_size", 11); + p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); + p.turning_radius_size = std::max(p.turning_radius_size, 1); + + p.theta_size = dp("theta_size", 48); + p.angle_goal_range = dp("angle_goal_range", 6.0); + + p.curve_weight = dp("curve_weight", 1.2); + p.reverse_weight = dp("reverse_weight", 2.00); + p.lateral_goal_range = dp("lateral_goal_range", 0.5); + p.longitudinal_goal_range = dp("longitudinal_goal_range", 2.0); + + // costmap configs + p.obstacle_threshold = dp("obstacle_threshold", 100); + + return p; +} + +AstarParam FreespacePullOver::getAstarParam(rclcpp::Node & node) const +{ + const auto dp = [&node](const std::string & str, auto def_val) { + std::string name = "pull_over.freespace_parking.astar." + str; + return node.declare_parameter(name, def_val); + }; + + AstarParam p{}; + + p.only_behind_solutions = dp("only_behind_solutions", false); + p.use_back = dp("use_back", true); + p.distance_heuristic_weight = dp("distance_heuristic_weight", 1.0); + + return p; +} + +RRTStarParam FreespacePullOver::getRRTStarParam(rclcpp::Node & node) const +{ + const auto dp = [&node](const std::string & str, auto def_val) { + std::string name = "pull_over.freespace_parking.rrtstar." + str; + return node.declare_parameter(name, def_val); + }; + + RRTStarParam p; + + p.enable_update = dp("enable_update", true); + p.use_informed_sampling = dp("use_informed_sampling", true); + p.max_planning_time = dp("max_planning_time", 150.0); + p.neighbour_radius = dp("neighbour_radius", 8.0); + p.margin = dp("margin", 0.1); + + return p; +} + +boost::optional FreespacePullOver::plan(const Pose & goal_pose) +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + planner_->setMap(*planner_data_->costmap); + + // offset goal pose to make staright path near goal for improving parking precision + // todo: support straight path when using back + constexpr double straight_distance = 1.0; + const Pose end_pose = + use_back_ ? goal_pose + : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); + const bool found_path = planner_->makePlan(current_pose, end_pose); + if (!found_path) { + return {}; + } + + PathWithLaneId path = util::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + const auto reverse_indices = util::getReversingIndices(path); + std::vector partial_paths = util::devidePath(path, reverse_indices); + + // remove points which are near the goal + PathWithLaneId & last_path = partial_paths.back(); + const double th_goal_disntace = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_goal_disntace) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // add PathPointWithLaneId to last path + auto addPose = [&last_path](const Pose & pose) { + PathPointWithLaneId p = last_path.points.back(); + p.point.pose = pose; + last_path.points.push_back(p); + }; + + if (use_back_) { + addPose(end_pose); + } else { + // add interpolated poses + auto addInterpolatedPoses = [&addPose](const Pose & pose1, const Pose & pose2) { + constexpr double interval = 0.5; + std::vector interpolated_poses = util::interpolatePose(pose1, pose2, interval); + for (const auto & pose : interpolated_poses) { + addPose(pose); + } + }; + addInterpolatedPoses(last_path.points.back().point.pose, end_pose); + addPose(end_pose); + addInterpolatedPoses(end_pose, goal_pose); + addPose(goal_pose); + } + + util::correctDevidedPathVelocity(partial_paths); + + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + for (auto & path : partial_paths) { + const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + if (!is_driving_forward) { + // path points is less than 2 + return {}; + } + util::generateDrivableArea( + path, planner_data_->parameters.vehicle_length, planner_data_->parameters.vehicle_width, + drivable_area_margin, *is_driving_forward); + } + + PullOverPath pull_over_path{}; + pull_over_path.partial_paths = partial_paths; + pull_over_path.start_pose = current_pose; + pull_over_path.end_pose = goal_pose; + pull_over_path.type = getPlannerType(); + + return pull_over_path; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 1cbdae1f5731f..71d231e8e0fca 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -80,6 +80,8 @@ PullOverModule::PullOverModule( RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } + freespace_planner_ = std::make_unique(node, parameters, vehicle_info_); + // set selected goal searcher // currently there is only one goal_searcher_type const auto & vehicle_footprint = @@ -90,11 +92,21 @@ PullOverModule::PullOverModule( // for collision check with objects vehicle_footprint_ = createVehicleFootprint(vehicle_info_); - // timer callback for generating candidate paths - const auto period_ns = rclcpp::Rate(1.0).period(); - timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - timer_ = rclcpp::create_timer( - &node, clock_, period_ns, std::bind(&PullOverModule::onTimer, this), timer_cb_group_); + // timer callback for generating lane parking candidate paths + const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); + lane_parking_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + lane_parking_timer_ = rclcpp::create_timer( + &node, clock_, lane_parking_period_ns, std::bind(&PullOverModule::onTimer, this), + lane_parking_timer_cb_group_); + + // timer callback for freespace parking + const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); + freespace_parking_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_parking_timer_ = rclcpp::create_timer( + &node, clock_, freespace_parking_period_ns, + std::bind(&PullOverModule::onFreespaceParkingTimer, this), freespace_parking_timer_cb_group_); resetStatus(); } @@ -182,6 +194,22 @@ void PullOverModule::onTimer() mutex_.unlock(); } +void PullOverModule::onFreespaceParkingTimer() +{ + if (!planner_data_) { + return; + } + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } +} + BehaviorModuleOutput PullOverModule::run() { current_state_ = BT::NodeStatus::RUNNING; @@ -252,8 +280,6 @@ void PullOverModule::onEntry() } last_received_time_ = std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); - - // Use refined goal as modified goal when disabling goal research } void PullOverModule::onExit() @@ -377,6 +403,58 @@ BT::NodeStatus PullOverModule::updateState() return current_state_; } +bool PullOverModule::planFreespacePath() +{ + goal_searcher_->update(goal_candidates_); + for (const auto & goal_candidate : goal_candidates_) { + if (!goal_candidate.is_safe) { + continue; + } + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(goal_candidate.goal_pose); + freespace_path->goal_id = goal_candidate.id; + if (!freespace_path) { + continue; + } + mutex_.lock(); + status_.pull_over_path = std::make_shared(*freespace_path); + status_.current_path_idx = 0; + status_.is_safe = true; + modified_goal_pose_ = goal_candidate; + mutex_.unlock(); + return true; + } + return false; +} + +bool PullOverModule::returnToLaneParking() +{ + if (!status_.lane_parking_pull_over_path) { + return false; + } + + const PathWithLaneId path = status_.lane_parking_pull_over_path->getFullPath(); + if (checkCollision(path)) { + return false; + } + + const Point & current_point = planner_data_->self_odometry->pose.pose.position; + constexpr double th_distance = 0.5; + const bool is_close_to_path = + std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; + if (!is_close_to_path) { + return false; + } + + mutex_.lock(); + status_.has_decided_path = false; + status_.pull_over_path = status_.lane_parking_pull_over_path; + status_.current_path_idx = 0; + mutex_.unlock(); + + return true; +} + BehaviorModuleOutput PullOverModule::plan() { const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -391,7 +469,7 @@ BehaviorModuleOutput PullOverModule::plan() // Check if it needs to decide path if (status_.is_safe) { const auto dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, current_pose, status_.pull_over_path.start_pose.position, + getCurrentPath().points, current_pose, status_.pull_over_path->start_pose.position, std::numeric_limits::max(), M_PI_2); if (*dist_to_parking_start_pose < parameters_.decide_path_distance) { @@ -417,7 +495,7 @@ BehaviorModuleOutput PullOverModule::plan() // decide velocity to guarantee turn signal lighting time if (!status_.has_decided_velocity) { - auto & first_path = status_.pull_over_path.partial_paths.front(); + auto & first_path = status_.pull_over_path->partial_paths.front(); const auto vel = static_cast(std::max( planner_data_->self_odometry->twist.twist.linear.x, parameters_.pull_over_minimum_velocity)); @@ -434,11 +512,13 @@ BehaviorModuleOutput PullOverModule::plan() const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > planner_data_->parameters.turn_signal_search_time; - if (hasFinishedCurrentPath() && has_passed_enough_time) { - incrementPathIndex(); + if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { + if (incrementPathIndex()) { + last_increment_time_ = std::make_unique(clock_->now()); + } } } - } else { + } else if (!pull_over_path_candidates_.empty()) { // select safe path from pull over path candidates goal_searcher_->setPlannerData(planner_data_); mutex_.lock(); @@ -464,14 +544,18 @@ BehaviorModuleOutput PullOverModule::plan() } status_.is_safe = true; - status_.pull_over_path = pull_over_path; + mutex_.lock(); + status_.pull_over_path = std::make_shared(pull_over_path); + status_.current_path_idx = 0; + status_.lane_parking_pull_over_path = status_.pull_over_path; modified_goal_pose_ = *goal_candidate_it; + mutex_.unlock(); break; } // Decelerate before the minimum shift distance from the goal search area. if (status_.is_safe) { - auto & first_path = status_.pull_over_path.partial_paths.front(); + auto & first_path = status_.pull_over_path->partial_paths.front(); const auto search_start_pose = calcLongitudinalOffsetPose( first_path.points, refined_goal_pose_.position, -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); @@ -484,7 +568,7 @@ BehaviorModuleOutput PullOverModule::plan() } // generate drivable area for each partial path - for (auto & path : status_.pull_over_path.partial_paths) { + for (auto & path : status_.pull_over_path->partial_paths) { const size_t ego_idx = findEgoIndex(path.points); util::clipPathLength(path, ego_idx, planner_data_->parameters); const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); @@ -500,8 +584,29 @@ BehaviorModuleOutput PullOverModule::plan() if (status_.is_safe) { // safe: use pull over path status_.stop_pose.reset(); - output.path = std::make_shared(getCurrentPath()); - path_candidate_ = std::make_shared(status_.pull_over_path.getFullPath()); + + auto current_path = getCurrentPath(); + + // keep stop if not enough time passed + constexpr double keep_stop_time = 2.0; + constexpr double keep_current_idx_buffer_time = 2.0; + if (last_increment_time_) { + const auto time_diff = (clock_->now() - *last_increment_time_).seconds(); + if (time_diff < keep_stop_time) { + status_.require_increment_ = false; + for (auto & p : current_path.points) { + p.point.longitudinal_velocity_mps = 0.0; + } + } else if (time_diff > keep_stop_time + keep_current_idx_buffer_time) { + // require incremnet only when the time passed is enough + // to prevent immediate increment before driving + // when the end of the next path is close to the current pose + status_.require_increment_ = true; + } + } + + output.path = std::make_shared(current_path); + path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); } else { // not safe: use stop_path if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { @@ -510,8 +615,11 @@ BehaviorModuleOutput PullOverModule::plan() status_.prev_stop_path = output.path; // set stop path as pull over path PullOverPath pull_over_path{}; - status_.pull_over_path = pull_over_path; - status_.pull_over_path.partial_paths.push_back(*output.path); + mutex_.lock(); + status_.pull_over_path = std::make_shared(pull_over_path); + status_.current_path_idx = 0; + status_.pull_over_path->partial_paths.push_back(*output.path); + mutex_.unlock(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { @@ -523,6 +631,16 @@ BehaviorModuleOutput PullOverModule::plan() } status_.prev_is_safe = status_.is_safe; + // return to lane parking if it is possible + if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + // return only before starting free space parking + if (!isStopped() || status_.during_freespace_parking) { + status_.during_freespace_parking = true; + } else if (returnToLaneParking()) { + RCLCPP_INFO(getLogger(), "return to lane parking"); + } + } + // set hazard and turn signal if (status_.has_decided_path) { output.turn_signal_info = calcTurnSignalInfo(); @@ -543,6 +661,8 @@ BehaviorModuleOutput PullOverModule::plan() modified_goal.header = planner_data_->route_handler->getRouteHeader(); output.modified_goal = modified_goal; prev_goal_id_ = modified_goal_pose_->id; + } else { + output.modified_goal = {}; } const uint16_t steering_factor_direction = std::invoke([this]() { @@ -557,7 +677,7 @@ BehaviorModuleOutput PullOverModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_over_path.start_pose, modified_goal_pose_->goal_pose}, + {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, steering_factor_direction, SteeringFactor::TURNING, ""); @@ -581,7 +701,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); path_candidate_ = status_.is_safe - ? std::make_shared(status_.pull_over_path.getFullPath()) + ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); @@ -597,7 +717,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() }); steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_over_path.start_pose, modified_goal_pose_->goal_pose}, + {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, steering_factor_direction, SteeringFactor::APPROACHING, ""); waitApproval(); @@ -607,10 +727,10 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() std::pair PullOverModule::calcDistanceToPathChange() const { - const auto & full_path = status_.pull_over_path.getFullPath(); + const auto & full_path = status_.pull_over_path->getFullPath(); const auto dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose, - status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); + status_.pull_over_path->start_pose.position, std::numeric_limits::max(), M_PI_2); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, modified_goal_pose_->goal_pose.position); @@ -657,7 +777,7 @@ PathWithLaneId PullOverModule::generateStopPath() return generateEmergencyStopPath(); } const Pose stop_pose = - status_.is_safe ? status_.pull_over_path.start_pose + status_.is_safe ? status_.pull_over_path->start_pose : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible @@ -753,31 +873,35 @@ PathWithLaneId PullOverModule::generateEmergencyStopPath() return stop_path; } -void PullOverModule::incrementPathIndex() +bool PullOverModule::incrementPathIndex() { - status_.current_path_idx = - std::min(status_.current_path_idx + 1, status_.pull_over_path.partial_paths.size() - 1); + if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { + return false; + } + status_.current_path_idx = status_.current_path_idx + 1; + return true; } PathWithLaneId PullOverModule::getCurrentPath() const { - return status_.pull_over_path.partial_paths.at(status_.current_path_idx); + return status_.pull_over_path->partial_paths.at(status_.current_path_idx); } -bool PullOverModule::isStopped() +bool PullOverModule::isStopped( + std::deque & odometry_buffer, const double time) { - odometry_buffer_.push_back(planner_data_->self_odometry); + odometry_buffer.push_back(planner_data_->self_odometry); // Delete old data in buffer while (rclcpp::ok()) { - const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - - rclcpp::Time(odometry_buffer_.front()->header.stamp); - if (time_diff.seconds() < parameters_.th_stopped_time) { + const auto time_diff = rclcpp::Time(odometry_buffer.back()->header.stamp) - + rclcpp::Time(odometry_buffer.front()->header.stamp); + if (time_diff.seconds() < time) { break; } - odometry_buffer_.pop_front(); + odometry_buffer.pop_front(); } bool is_stopped = true; - for (const auto & odometry : odometry_buffer_) { + for (const auto & odometry : odometry_buffer) { const double ego_vel = util::l2Norm(odometry->twist.twist.linear); if (ego_vel > parameters_.th_stopped_velocity) { is_stopped = false; @@ -787,6 +911,20 @@ bool PullOverModule::isStopped() return is_stopped; } +bool PullOverModule::isStopped() +{ + return isStopped(odometry_buffer_stopped_, parameters_.th_stopped_time); +} + +bool PullOverModule::isStuck() +{ + if (!status_.pull_over_path) { + return false; + } + constexpr double stuck_time = 5.0; + return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); +} + bool PullOverModule::hasFinishedCurrentPath() { const auto & current_path_end = getCurrentPath().points.back(); @@ -812,9 +950,9 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.pull_over_path.start_pose; - const auto & end_pose = status_.pull_over_path.end_pose; - const auto full_path = status_.pull_over_path.getFullPath(); + const auto & start_pose = status_.pull_over_path->start_pose; + const auto & end_pose = status_.pull_over_path->end_pose; + const auto full_path = status_.pull_over_path->getFullPath(); // calc TurnIndicatorsCommand { @@ -871,27 +1009,45 @@ void PullOverModule::setDebugData() // Visualize path and related pose if (status_.is_safe) { add(createPoseMarkerArray( - status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add(createPathMarkerArray(status_.pull_over_path.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + status_.pull_over_path->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add( + createPathMarkerArray(status_.pull_over_path->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + + // visualize each partial path + for (size_t i = 0; i < status_.pull_over_path->partial_paths.size(); ++i) { + const auto & partial_path = status_.pull_over_path->partial_paths.at(i); + add( + createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); + } } // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; + const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.99)); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = modified_goal_pose_->goal_pose; - marker.text = magic_enum::enum_name(status_.pull_over_path.type); + marker.text = magic_enum::enum_name(status_.pull_over_path->type); + marker.text += " " + std::to_string(status_.current_path_idx) + "/" + + std::to_string(status_.pull_over_path->partial_paths.size() - 1); + if (isStuck()) { + marker.text += " stuck"; + } else if (isStopped()) { + marker.text += " stopped"; + } + planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } // Visualize debug poses - const auto & debug_poses = status_.pull_over_path.debug_poses; + const auto & debug_poses = status_.pull_over_path->debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index cc079ec42e3da..6e318a113973f 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -143,7 +143,7 @@ boost::optional ShiftPullOver::generatePullOverPath( // interpolate between shift end pose to goal pose std::vector interpolated_poses = - interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); + util::interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); for (const auto & pose : interpolated_poses) { PathPointWithLaneId p = shifted_path.path.points.back(); p.point.pose = pose; @@ -242,59 +242,4 @@ double ShiftPullOver::calcBeforeShiftedArcLength( return before_arc_length; } - -// only two points is supported -std::vector ShiftPullOver::splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s) -{ - const double h = base_s.at(1) - base_s.at(0); - - const double c = begin_diff; - const double d = base_x.at(0); - const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); - const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); - - std::vector res; - for (const auto & s : new_s) { - const double ds = s - base_s.at(0); - res.push_back(d + (c + (b + a * ds) * ds) * ds); - } - - return res; -} - -std::vector ShiftPullOver::interpolatePose( - const Pose & start_pose, const Pose & end_pose, const double resample_interval) -{ - std::vector interpolated_poses{}; // output - - const std::vector base_s{ - 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; - const std::vector base_x{start_pose.position.x, end_pose.position.x}; - const std::vector base_y{start_pose.position.y, end_pose.position.y}; - std::vector new_s; - - constexpr double eps = 0.3; // prevent overlapping - for (double s = eps; s < base_s.back() - eps; s += resample_interval) { - new_s.push_back(s); - } - - const std::vector interpolated_x = splineTwoPoints( - base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), - std::cos(tf2::getYaw(end_pose.orientation)), new_s); - const std::vector interpolated_y = splineTwoPoints( - base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), - std::sin(tf2::getYaw(end_pose.orientation)), new_s); - for (size_t i = 0; i < interpolated_x.size(); ++i) { - Pose pose{}; - pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); - pose.position.x = interpolated_x.at(i); - pose.position.y = interpolated_y.at(i); - pose.position.z = end_pose.position.z; - interpolated_poses.push_back(pose); - } - - return interpolated_poses; -} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 156bca488cc58..deed3109dfbd5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1206,6 +1206,143 @@ void generateDrivableArea( } } +// generate drivable area by expandin path for freespace +void generateDrivableArea( + PathWithLaneId & path, const double vehicle_length, const double vehicle_width, + const double margin, const bool is_driving_forward) +{ + using tier4_autoware_utils::calcOffsetPose; + + // remove path points which is close to the previous point + PathWithLaneId resampled_path{}; + const double resample_interval = 2.0; + for (size_t i = 0; i < path.points.size(); ++i) { + if (i == 0) { + resampled_path.points.push_back(path.points.at(i)); + } else { + const auto & prev_point = resampled_path.points.back().point.pose.position; + const auto & curr_point = path.points.at(i).point.pose.position; + const double signed_arc_length = + motion_utils::calcSignedArcLength(path.points, prev_point, curr_point); + if (signed_arc_length > resample_interval) { + resampled_path.points.push_back(path.points.at(i)); + } + } + } + // add last point of path if enogh far from the one of resampled path + constexpr double th_last_point_distance = 0.3; + if ( + tier4_autoware_utils::calcDistance2d( + resampled_path.points.back().point.pose.position, path.points.back().point.pose.position) > + th_last_point_distance) { + resampled_path.points.push_back(path.points.back()); + } + + // create bound point by calculating offset point + std::vector left_bound; + std::vector right_bound; + for (const auto & point : resampled_path.points) { + const auto & pose = point.point.pose; + + const auto left_point = calcOffsetPose(pose, 0, vehicle_width / 2.0 + margin, 0); + const auto right_point = calcOffsetPose(pose, 0, -vehicle_width / 2.0 - margin, 0); + + left_bound.push_back(left_point.position); + right_bound.push_back(right_point.position); + } + + if (is_driving_forward) { + // add backward offset point to bound + const Pose first_point = + calcOffsetPose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.insert(left_bound.begin(), left_first_point.position); + right_bound.insert(right_bound.begin(), right_first_point.position); + + // add forward offset point to bound + const Pose last_point = + calcOffsetPose(resampled_path.points.back().point.pose, vehicle_length, 0, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.push_back(left_last_point.position); + right_bound.push_back(right_last_point.position); + } else { + // add forward offset point to bound + const Pose first_point = + calcOffsetPose(resampled_path.points.front().point.pose, vehicle_length, 0, 0); + const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.insert(left_bound.begin(), left_first_point.position); + right_bound.insert(right_bound.begin(), right_first_point.position); + + // add backward offset point to bound + const Pose last_point = + calcOffsetPose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0); + const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0); + const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0); + left_bound.push_back(left_last_point.position); + right_bound.push_back(right_last_point.position); + } + + if (left_bound.empty() || right_bound.empty()) { + return; + } + + // fix intersected bound + // if bound is intersected, remove them and insert intersection point + typedef boost::geometry::model::d2::point_xy BoostPoint; + typedef boost::geometry::model::linestring LineString; + auto modify_bound_intersection = [](const std::vector & bound) { + const double intersection_check_distance = 10.0; + std::vector modified_bound; + size_t i = 0; + while (i < bound.size() - 1) { + BoostPoint p1(bound.at(i).x, bound.at(i).y); + BoostPoint p2(bound.at(i + 1).x, bound.at(i + 1).y); + LineString p_line; + p_line.push_back(p1); + p_line.push_back(p2); + bool intersection_found = false; + for (size_t j = i + 2; j < bound.size() - 1; j++) { + const double distance = tier4_autoware_utils::calcDistance2d(bound.at(i), bound.at(j)); + if (distance > intersection_check_distance) { + break; + } + LineString q_line; + BoostPoint q1(bound.at(j).x, bound.at(j).y); + BoostPoint q2(bound.at(j + 1).x, bound.at(j + 1).y); + q_line.push_back(q1); + q_line.push_back(q2); + std::vector intersection_points; + boost::geometry::intersection(p_line, q_line, intersection_points); + if (intersection_points.size() > 0) { + modified_bound.push_back(bound.at(i)); + Point intersection_point; + intersection_point.x = intersection_points.at(0).x(); + intersection_point.y = intersection_points.at(0).y(); + modified_bound.push_back(intersection_point); + i = j + 1; + intersection_found = true; + break; + } + } + if (!intersection_found) { + modified_bound.push_back(bound.at(i)); + i++; + } + } + modified_bound.push_back(bound.back()); + return modified_bound; + }; + std::vector modified_left_bound = modify_bound_intersection(left_bound); + std::vector modified_right_bound = modify_bound_intersection(right_bound); + + // set bound to path + path.left_bound = modified_left_bound; + path.right_bound = modified_right_bound; +} + double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index b59888fa52281..32d84813db880 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -16,6 +16,7 @@ #define FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #include +#include #include #include @@ -59,6 +60,22 @@ struct VehicleShape double length; // X [m] double width; // Y [m] double base2back; // base_link to rear [m] + + VehicleShape() = default; + + VehicleShape(double length, double width, double base2back) + { + length = length; + width = width; + base2back = base2back; + } + + VehicleShape(const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + { + length = vehicle_info.vehicle_length_m + margin; + width = vehicle_info.vehicle_width_m + margin; + base2back = vehicle_info.rear_overhang_m + margin / 2.0; + } }; struct PlannerCommonParam @@ -106,6 +123,13 @@ class AbstractPlanningAlgorithm { } + AbstractPlanningAlgorithm( + const PlannerCommonParam & planner_common_param, + const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) + { + } + virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); virtual bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 9ccec85bfc185..6f1810fabec88 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -144,6 +144,7 @@ class AstarSearch : public AbstractPlanningAlgorithm bool setGoalNode(); double estimateCost(const geometry_msgs::msg::Pose & pose) const; bool isGoal(const AstarNode & node) const; + geometry_msgs::msg::Pose node2pose(const AstarNode & node) const; AstarNode * getNodeRef(const IndexXYT & index) { diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index ea0813c5b57ea..544b0d19c74b1 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -24,6 +24,7 @@ tf2 tf2_geometry_msgs tier4_autoware_utils + vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 8d419c3daa13f..5686d7eee26e0 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -61,18 +61,6 @@ geometry_msgs::msg::Pose calcRelativePose( return transformed.pose; } -geometry_msgs::msg::Pose node2pose(const AstarNode & node) -{ - geometry_msgs::msg::Pose pose_local; - - pose_local.position.x = node.x; - pose_local.position.y = node.y; - pose_local.position.z = 0; - pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); - - return pose_local; -} - AstarSearch::TransitionTable createTransitionTable( const double minimum_turning_radius, const double maximum_turning_radius, const int turning_radius_size, const double theta_size, const bool use_back) @@ -356,4 +344,16 @@ bool AstarSearch::isGoal(const AstarNode & node) const return true; } +geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const +{ + geometry_msgs::msg::Pose pose_local; + + pose_local.position.x = node.x; + pose_local.position.y = node.y; + pose_local.position.z = goal_pose_.position.z; + pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); + + return pose_local; +} + } // namespace freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/freespace_planning_algorithms/src/rrtstar.cpp index d710b74111b1a..f03dc81faba05 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar.cpp @@ -141,7 +141,7 @@ void RRTStar::setRRTPath(const std::vector & waypoints) geometry_msgs::msg::Pose pose_local; pose_local.position.x = pt.x; pose_local.position.y = pt.y; - pose_local.position.z = 0.0; + pose_local.position.z = goal_pose_.position.z; tf2::Quaternion quat; quat.setRPY(0, 0, pt.yaw); tf2::convert(quat, pose_local.orientation);