From c71df55b88f1143c3feddfa32b4ad57a23b7f639 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 10 Aug 2022 20:46:52 +0900 Subject: [PATCH 1/3] fix: update trt api (#1472) * fix: update trt api Signed-off-by: Daisuke Nishimatsu * compatible build with tensorrt 8.2 Signed-off-by: Daisuke Nishimatsu * revert changes for caffe parser Signed-off-by: Daisuke Nishimatsu --- .../lidar_apollo_instance_segmentation/CMakeLists.txt | 5 +++++ .../lidar_apollo_instance_segmentation/src/detector.cpp | 6 ++++-- .../lidar_centerpoint/lib/network/tensorrt_wrapper.cpp | 4 ++++ perception/tensorrt_yolo/lib/src/trt_yolo.cpp | 4 ++++ perception/traffic_light_classifier/utils/trt_common.cpp | 8 +++++--- perception/traffic_light_classifier/utils/trt_common.hpp | 1 - .../traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp | 4 ++++ 7 files changed, 26 insertions(+), 6 deletions(-) diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 8fff790a64790..53b0d6ba6a352 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -134,6 +134,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${PCL_LIBRARIES} ) + target_compile_options(tensorrt_apollo_cnn_lib + PUBLIC + -Wno-deprecated-declarations + ) + ament_auto_add_library(lidar_apollo_instance_segmentation SHARED src/node.cpp src/detector.cpp diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index af8b29f55f340..27e4377f404b9 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -60,9 +60,11 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * RCLCPP_ERROR(node_->get_logger(), "can not find output named %s", output_node.c_str()); } network->markOutput(*output); - const int batch_size = 1; - builder->setMaxBatchSize(batch_size); +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, 1 << 30); +#else config->setMaxWorkspaceSize(1 << 30); +#endif nvinfer1::IHostMemory * plan = builder->buildSerializedNetwork(*network, *config); assert(plan != nullptr); std::ofstream outfile(engine_file, std::ofstream::binary); diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 81e48a98adbf0..a175079ab0348 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -79,7 +79,11 @@ bool TensorRTWrapper::parseONNX( std::cout << "Fail to create config" << std::endl; return false; } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); +#else config->setMaxWorkspaceSize(workspace_size); +#endif if (precision == "fp16") { if (builder->platformHasFastFp16()) { std::cout << "use TensorRT FP16 Inference" << std::endl; diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp index 748680266ca9c..65fb298aff388 100644 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp @@ -161,7 +161,11 @@ Net::Net( if (fp16 || int8) { config->setFlag(nvinfer1::BuilderFlag::kFP16); } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); +#else config->setMaxWorkspaceSize(workspace_size); +#endif // Parse ONNX FCN std::cout << "Building " << precision << " core model..." << std::endl; diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp index cae9f0cd1682a..4aae3e0ece5e1 100644 --- a/perception/traffic_light_classifier/utils/trt_common.cpp +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -42,8 +42,7 @@ TrtCommon::TrtCommon(std::string model_path, std::string precision) precision_(precision), input_name_("input_0"), output_name_("output_0"), - is_initialized_(false), - max_batch_size_(1) + is_initialized_(false) { runtime_ = UniquePtr(nvinfer1::createInferRuntime(logger_)); } @@ -106,8 +105,11 @@ bool TrtCommon::buildEngineFromOnnx(std::string onnx_file_path, std::string outp return false; } - builder->setMaxBatchSize(max_batch_size_); +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, 16 << 20); +#else config->setMaxWorkspaceSize(16 << 20); +#endif if (precision_ == "fp16") { config->setFlag(nvinfer1::BuilderFlag::kFP16); diff --git a/perception/traffic_light_classifier/utils/trt_common.hpp b/perception/traffic_light_classifier/utils/trt_common.hpp index 7c172b6719678..9577dfd2262a6 100644 --- a/perception/traffic_light_classifier/utils/trt_common.hpp +++ b/perception/traffic_light_classifier/utils/trt_common.hpp @@ -136,7 +136,6 @@ class TrtCommon std::string input_name_; std::string output_name_; bool is_initialized_; - size_t max_batch_size_; }; } // namespace Tn diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index ba9f94b014513..d810eb5275058 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -92,7 +92,11 @@ Net::Net( if (fp16 || int8) { config->setFlag(nvinfer1::BuilderFlag::kFP16); } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); +#else config->setMaxWorkspaceSize(workspace_size); +#endif // Parse ONNX FCN std::cout << "Building " << precision << " core model..." << std::endl; From e05e16cb9ffb450fbe43282a6f81f0c20b030002 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 5 Sep 2022 19:41:21 +0900 Subject: [PATCH 2/3] fix(heatmap_visualizer): add unknown class (#1781) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../include/heatmap_visualizer/heatmap_visualizer_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp index 88adf1d3d94b6..9ffb3349435e5 100644 --- a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp +++ b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp @@ -66,8 +66,8 @@ class HeatmapVisualizerNode : public rclcpp::Node float map_length_; float map_resolution_; bool use_confidence_; - std::vector class_names_{"CAR", "TRUCK", "BUS", "TRAILER", - "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; + std::vector class_names_{"UNKNWON", "CAR", "TRUCK", "BUS", + "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; bool rename_car_to_truck_and_bus_; // Number of width and height cells From 3f5f6016ebcdfdcff6081efca9546536dc30a981 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 31 Aug 2022 00:40:44 +0900 Subject: [PATCH 3/3] feat(intersection): continue detection after pass judge (#1719) Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura --- .../intersection.param.yaml | 2 + .../config/intersection.param.yaml | 2 + .../intersection/scene_intersection.hpp | 4 + .../scene_module/intersection/util.hpp | 32 +++++- .../src/scene_module/intersection/manager.cpp | 2 + .../intersection/scene_intersection.cpp | 66 +++++++----- .../scene_merge_from_private_road.cpp | 14 +-- .../src/scene_module/intersection/util.cpp | 101 ++++++++++++++---- 8 files changed, 167 insertions(+), 56 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 2c49ef72e8ff6..21b14a2c103d8 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 2c49ef72e8ff6..21b14a2c103d8 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 8dd55bbda686b..957cadc98fdcd 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -104,6 +104,10 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double keep_detection_line_margin; //! distance (toward path end) from generated stop line. + //! keep detection if ego is before this line and ego.vel < + //! keep_detection_vel_thr + double keep_detection_vel_thr; double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 576ec92165268..d628f16e89d2e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -47,7 +47,7 @@ geometry_msgs::msg::Pose getAheadPose( bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx); @@ -62,6 +62,14 @@ bool getObjectiveLanelets( std::vector * objective_lanelets_with_margin_result, const rclcpp::Logger logger); +struct StopLineIdx +{ + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; +}; + /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, * read it from the map; otherwise, generate a stop line at a position where it will not collide. @@ -76,9 +84,10 @@ bool getObjectiveLanelets( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger); /** * @brief If use_stuck_stopline is true, a stop line is generated before the intersection. @@ -129,6 +138,23 @@ double calcArcLengthFromPath( lanelet::ConstLanelet generateOffsetLanelet( const lanelet::ConstLanelet lanelet, double right_margin, double left_margin); +geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p); + +/** + * @brief check if ego is over the target_idx. If the index is same, compare the exact pose + * @param path path + * @param closest_idx ego's closest index on the path + * @param current_pose ego's exact pose + * @return true if ego is over the target_idx + */ +bool isOverTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx); + +bool isBeforeTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 8959beebdfba0..92d4cf75187e4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -33,6 +33,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + ip.keep_detection_line_margin = node.declare_parameter(ns + ".keep_detection_line_margin", 1.0); + ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index c21dc31e2e3d4..6de8a16e16709 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -120,17 +120,19 @@ bool IntersectionModule::modifyPathVelocity( debug_data_.detection_area_with_margin = detection_areas_with_margin; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int pass_judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path, - &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs, + logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); return false; } + const int stop_line_idx = stop_line_idxs.stop_line_idx; + const int pass_judge_line_idx = stop_line_idxs.pass_judge_line_idx; + const int keep_detection_line_idx = stop_line_idxs.keep_detection_line_idx; if (stop_line_idx <= 0) { RCLCPP_DEBUG(logger_, "stop line line is at path[0], ignore planning."); RCLCPP_DEBUG(logger_, "===== plan end ====="); @@ -147,20 +149,32 @@ bool IntersectionModule::modifyPathVelocity( return false; } - /* if current_state = GO, and current_pose is in front of stop_line, ignore planning. */ - bool is_over_pass_judge_line = static_cast(closest_idx > pass_judge_line_idx); - if (closest_idx == pass_judge_line_idx) { - geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; - is_over_pass_judge_line = util::isAheadOf(current_pose.pose, pass_judge_line); - } - if (is_go_out_ && is_over_pass_judge_line && !external_stop) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx).point.pose.position)); - return true; // no plan needed. + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); + if (is_over_pass_judge_line) { + /* + in case ego could not stop exactly before the stop line, but with some overshoot, keep + detection within some margin and low velocity threshold + */ + const bool is_before_keep_detection_line = + util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); + if ( + is_before_keep_detection_line && std::fabs(planner_data_->current_velocity->twist.linear.x) < + planner_param_.keep_detection_vel_thr) { + RCLCPP_DEBUG( + logger_, + "over the pass judge line, but before keep detection line and low speed, " + "continue planning"); + // no return here, keep planning + } else if (is_go_out_ && !external_stop) { + RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx).point.pose.position)); + return true; // no plan needed. + } } /* get dynamic object */ @@ -190,21 +204,23 @@ bool IntersectionModule::modifyPathVelocity( if (!isActivated()) { constexpr double v = 0.0; is_go_out_ = false; + int stop_line_idx_stop = stop_line_idx; + int pass_judge_line_idx_stop = pass_judge_line_idx; if (planner_param_.use_stuck_stopline && is_stuck) { int stuck_stop_line_idx = -1; int stuck_pass_judge_line_idx = -1; if (util::generateStopLineBeforeIntersection( lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx, &stuck_pass_judge_line_idx, logger_.get_child("util"))) { - stop_line_idx = stuck_stop_line_idx; - pass_judge_line_idx = stuck_pass_judge_line_idx; + stop_line_idx_stop = stuck_stop_line_idx; + pass_judge_line_idx_stop = stuck_pass_judge_line_idx; } } - util::setVelocityFrom(stop_line_idx, v, path); + util::setVelocityFrom(stop_line_idx_stop, v, path); debug_data_.stop_required = true; - debug_data_.stop_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path); - debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose; + debug_data_.stop_wall_pose = util::getAheadPose(stop_line_idx_stop, base_link2front, *path); + debug_data_.stop_point_pose = path->points.at(stop_line_idx_stop).point.pose; + debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_stop).point.pose; /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 7bfbcea3f2eb1..dc0dd1725833d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -83,27 +83,27 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( debug_data_.detection_area = conflicting_areas; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, - private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + 0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - if (stop_line_idx <= 0 || judge_line_idx <= 0) { - RCLCPP_DEBUG(logger_, "stop line or judge line is at path[0], ignore planning."); + const int stop_line_idx = stop_line_idxs.stop_line_idx; + if (stop_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; } debug_data_.virtual_wall_pose = util::getAheadPose( stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + const int first_idx_inside_lane = stop_line_idxs.first_idx_inside_lane; if (first_idx_inside_lane != -1) { debug_data_.first_collision_point = path->points.at(first_idx_inside_lane).point.pose.position; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index c16e482757606..a2bc300cf9b70 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -120,7 +120,7 @@ bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, return false; } -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx) { @@ -163,9 +163,10 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -179,7 +180,8 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(stop_line_margin / interval); + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int keep_detection_line_margin_idx_dist = std::ceil(keep_detection_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); @@ -190,6 +192,10 @@ bool generateStopLine( return false; } + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; /* generate stop point */ // If a stop_line is defined in lanelet_map, use it. // else, generates a local stop_line with considering the lane conflicts. @@ -206,31 +212,49 @@ bool generateStopLine( } // only for visualization const auto first_inside_point = path_ip.points.at(first_idx_ip_inside_lane).point.pose; - planning_utils::calcClosestIndex( - *original_path, first_inside_point, *first_idx_inside_lane, 10.0); - if (*first_idx_inside_lane == 0) { + + const auto first_idx_inside_lane_opt = + motion_utils::findNearestIndex(original_path->points, first_inside_point, 10.0, M_PI_4); + if (*first_idx_inside_lane_opt) { + first_idx_inside_lane = first_idx_inside_lane_opt.get(); + } + + if (first_idx_inside_lane == 0) { RCLCPP_DEBUG( logger, "path[0] is already in the detection area. This happens if you have " "already crossed the stop line or are very far from the intersection. Ignore computation."); - *stop_line_idx = 0; - *pass_judge_line_idx = 0; + stop_line_idxs->first_idx_inside_lane = 0; + stop_line_idxs->pass_judge_line_idx = 0; + stop_line_idxs->stop_line_idx = 0; + stop_line_idxs->keep_detection_line_idx = 0; return true; } - stop_idx_ip = std::max(first_idx_ip_inside_lane - 1 - margin_idx_dist - base2front_idx_dist, 0); + stop_idx_ip = + std::max(first_idx_ip_inside_lane - 1 - stop_line_margin_idx_dist - base2front_idx_dist, 0); + } + + /* insert keep_detection_line */ + const int keep_detection_idx_ip = std::min( + stop_idx_ip + keep_detection_line_margin_idx_dist, static_cast(path_ip.points.size()) - 1); + const auto inserted_keep_detection_point = path_ip.points.at(keep_detection_idx_ip).point.pose; + if (!util::getDuplicatedPointIdx( + *original_path, inserted_keep_detection_point.position, &keep_detection_line_idx)) { + keep_detection_line_idx = util::insertPoint(inserted_keep_detection_point, original_path); } /* insert stop_point */ const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as stop_line_idx - if (!util::hasDuplicatedPoint(*original_path, inserted_stop_point.position, stop_line_idx)) { - *stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + if (!util::getDuplicatedPointIdx(*original_path, inserted_stop_point.position, &stop_line_idx)) { + stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + ++keep_detection_line_idx; // the index is incremented by judge stop line insertion } /* if another stop point exist before intersection stop_line, disable judge_line. */ bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { + for (int i = 0; i < stop_line_idx; ++i) { if (std::fabs(original_path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { has_prior_stopline = true; break; @@ -241,23 +265,29 @@ bool generateStopLine( const int pass_judge_idx_ip = std::min( static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { - *pass_judge_line_idx = *stop_line_idx; + pass_judge_line_idx = stop_line_idx; } else { const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( - *original_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { - *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); - ++(*stop_line_idx); // stop index is incremented by judge line insertion + if (!util::getDuplicatedPointIdx( + *original_path, inserted_pass_judge_point.position, &pass_judge_line_idx)) { + pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); + ++stop_line_idx; // stop index is incremented by judge line insertion + ++keep_detection_line_idx; // same. } } + stop_line_idxs->first_idx_inside_lane = first_idx_inside_lane; + stop_line_idxs->pass_judge_line_idx = pass_judge_line_idx; + stop_line_idxs->stop_line_idx = stop_line_idx; + stop_line_idxs->keep_detection_line_idx = keep_detection_line_idx; + RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %d, pass_judge_idx = %d, stop_idx_ip = " "%d, pass_judge_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); + stop_line_idx, pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); return true; } @@ -561,7 +591,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as *stuck_stop_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_stop_point.position, stuck_stop_line_idx)) { *stuck_stop_line_idx = util::insertPoint(inserted_stop_point, output_path); } @@ -585,7 +615,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, output_path); ++(*stuck_stop_line_idx); // stop index is incremented by judge line insertion @@ -604,5 +634,34 @@ bool generateStopLineBeforeIntersection( return false; } +geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) +{ + geometry_msgs::msg::Pose pose; + pose.position = p; + return pose; +} + +bool isOverTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(current_pose, target_pose); + } + return static_cast(closest_idx > target_idx); +} + +bool isBeforeTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(target_pose, current_pose); + } + return static_cast(target_idx > closest_idx); +} + } // namespace util } // namespace behavior_velocity_planner