Merge pull request #107 from tier4/fix/intersection

fix: hotfix to 0.5.2

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

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<std::string> class_names_{"CAR",     "TRUCK",     "BUS",       "TRAILER",
-                                        "BICYCLE", "MOTORBIKE", "PEDESTRIAN"};
+  std::vector<std::string> class_names_{"UNKNWON", "CAR",     "TRUCK",     "BUS",
+                                        "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"};
   bool rename_car_to_truck_and_bus_;
 
   // Number of width and height cells

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

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);

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;

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;

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::IRuntime>(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);

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

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;

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

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

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<lanelet::ConstLanelets> * 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<lanelet::CompoundPolygon3d> detection_areas,
   const std::shared_ptr<const PlannerData> & 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
 

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;

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<bool>(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;

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;
   }