From 14cdb34f33f6fccb3a0ab033a3a443fe4b28f232 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 18 Dec 2023 20:04:29 +0900 Subject: [PATCH] update Signed-off-by: Yuki Takagi --- .../src/scene.cpp | 29 ++++++++++--------- .../src/scene.hpp | 8 +++-- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index a3b64b6d1c4ae..92516e7b4424b 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -115,8 +115,14 @@ bool RunOutModule::modifyPathVelocity( elapsed_obstacle_creation.count() / 1000.0); // detect collision with dynamic obstacles using velocity planning of ego + const auto crosswalk_lanelets = planner_param_.run_out.suppress_on_crosswalk + ? getCrosswalksOnPath( + planner_data_->current_odometry->pose, *path, + planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->route_handler_->getOverallGraphPtr()) + : std::vector>(); const auto dynamic_obstacle = - detectCollision(partition_excluded_obstacles, extended_smoothed_path, *path); + detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets); // record time for collision check const auto t_collision_check = std::chrono::system_clock::now(); @@ -161,7 +167,7 @@ bool RunOutModule::modifyPathVelocity( std::optional RunOutModule::detectCollision( const std::vector & dynamic_obstacles, const PathWithLaneId & path, - const PathWithLaneId & raw_path) + const std::vector> & crosswalk_lanelets) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -195,7 +201,7 @@ std::optional RunOutModule::detectCollision( debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0); auto obstacles_collision = - checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, raw_path); + checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets); if (obstacles_collision.empty()) { continue; } @@ -316,7 +322,7 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, std::vector poly, const float travel_time, - PathWithLaneId raw_path) const + const std::vector> & crosswalk_lanelets) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -346,7 +352,7 @@ std::vector RunOutModule::checkCollisionWithObstacles( std::vector collision_points; const bool collision_detected = checkCollisionWithShape( - bg_poly_vehicle, pose_with_range, obstacle.shape, raw_path, collision_points); + bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points); if (!collision_detected) { continue; @@ -405,7 +411,8 @@ std::optional RunOutModule::calcPredictedObstaclePose( bool RunOutModule::checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, - const PathWithLaneId raw_path, std::vector & collision_points) const + const std::vector> & crosswalk_lanelets, + std::vector & collision_points) const { bool collision_detected = false; switch (shape.type) { @@ -427,16 +434,11 @@ bool RunOutModule::checkCollisionWithShape( break; } - if (!collision_points.empty() && planner_param_.run_out.suppress_on_crosswalk) { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, raw_path, - planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->route_handler_->getOverallGraphPtr()); - + if (!collision_points.empty()) { for (const auto & crosswalk : crosswalk_lanelets) { const auto poly = crosswalk.second.polygon2d().basicPolygon(); for (auto it = collision_points.begin(); it != collision_points.end();) { - if (boost::geometry::within(lanelet::BasicPoint2d((*it).x, (*it).y), poly)) { + if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) { it = collision_points.erase(it); } else { ++it; @@ -448,6 +450,7 @@ bool RunOutModule::checkCollisionWithShape( } collision_detected = !collision_points.empty(); } + return collision_detected; } diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index f9ee815874a49..def90f036c440 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -68,7 +69,7 @@ class RunOutModule : public SceneModuleInterface std::optional detectCollision( const std::vector & dynamic_obstacles, const PathWithLaneId & path, - const PathWithLaneId & raw_path); + const std::vector> & crosswalk_lanelets); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -79,7 +80,7 @@ class RunOutModule : public SceneModuleInterface std::vector checkCollisionWithObstacles( const std::vector & dynamic_obstacles, std::vector poly, const float travel_time, - PathWithLaneId path) const; + const std::vector> & crosswalk_lanelets) const; std::optional findNearestCollisionObstacle( const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, @@ -91,7 +92,8 @@ class RunOutModule : public SceneModuleInterface bool checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, - const PathWithLaneId path, std::vector & collision_points) const; + const std::vector> & crosswalk_lanelets, + std::vector & collision_points) const; bool checkCollisionWithCylinder( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius,