chore(behavior_velocity): add system delay parameter and minor update

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Apr 23, 2022 · 3f250d0 · 3f250d0
1 parent 32502db
commit 3f250d0
Show file tree

Hide file tree

Showing 9 changed files with 25 additions and 14 deletions.
diff --git a/...anning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/...anning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml
@@ -5,6 +5,7 @@
       pass_judge: "smooth_velocity"         # [-] candidate is "smooth_velocity" or "current_velocity"
       filter_occupancy_grid: true           # [-] whether to filter occupancy grid by morphologyEx or not
       use_object_info: true                 # [-] whether to reflect object info to occupancy grid map or not
+      use_moving_object_ray_cast: true      # [-] whether to reflect moving object ray_cast to occupancy grid map or not
       use_partition_lanelet: true           # [-] whether to use partition lanelet map data
       pedestrian_vel: 1.5                   # [m/s] assume pedestrian is dashing from occlusion at this velocity
       pedestrian_radius: 0.3                # [m] assume pedestrian width(0.2m) + margin(0.1m)
@@ -30,5 +31,5 @@
         min_longitudinal_offset: 1.0     # [m] detection area safety buffer from front bumper.
         max_lateral_distance: 6.0        # [m] buffer around the ego path used to build the detection area.
       grid:
-        free_space_max: 40  # [-] maximum value of a free space cell in the occupancy grid
-        occupied_min: 60    # [-] minimum value of an occupied cell in the occupancy grid
+        free_space_max: 43  # [-] maximum value of a free space cell in the occupancy grid
+        occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
@@ -31,5 +31,5 @@
         min_longitudinal_offset: 1.0     # [m] detection area safety buffer from front bumper.
         max_lateral_distance: 6.0        # [m] buffer around the ego path used to build the detection area.
       grid:
-        free_space_max: 40  # [-] maximum value of a free space cell in the occupancy grid
-        occupied_min: 60    # [-] minimum value of an occupied cell in the occupancy grid
+        free_space_max: 43  # [-] maximum value of a free space cell in the occupancy grid
+        occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp
@@ -62,6 +62,7 @@ struct PlannerData
     max_stop_acceleration_threshold = node.declare_parameter(
       "max_accel", -5.0);  // TODO(someone): read min_acc in velocity_controller.param.yaml?
     max_stop_jerk_threshold = node.declare_parameter("max_jerk", -5.0);
+    system_delay = node.declare_parameter("system_delay", 0.50);
     delay_response_time = node.declare_parameter("delay_response_time", 0.50);
   }
   // tf
@@ -97,6 +98,7 @@ struct PlannerData
   // additional parameters
   double max_stop_acceleration_threshold;
   double max_stop_jerk_threshold;
+  double system_delay;
   double delay_response_time;
   double stop_line_extend_length;
 

diff --git a/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
@@ -218,7 +218,9 @@ void clipPathByLength(
 //!< @brief extract target vehicles
 bool isStuckVehicle(const PredictedObject & obj, const double min_vel);
 bool isMovingVehicle(const PredictedObject & obj, const double min_vel);
-std::vector<PredictedObject> extractVehicles(const PredictedObjects::ConstSharedPtr objects_ptr);
+std::vector<PredictedObject> extractVehicles(
+  const PredictedObjects::ConstSharedPtr objects_ptr, const Point ego_position,
+  const double distance);
 std::vector<PredictedObject> filterVehiclesByDetectionArea(
   const std::vector<PredictedObject> & objs, const Polygons2d & polys);
 bool isVehicle(const ObjectClassification & obj_class);

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp
@@ -488,11 +488,10 @@ void denoiseOccupancyGridCV(
     cv::Scalar(grid_utils::occlusion_cost_value::OCCUPIED));
   toQuantizedImage(occupancy_grid, &cv_image, param);
 
-  //! show orignal occupancy grid to compare difference
+  //! show original occupancy grid to compare difference
   if (is_show_debug_window) {
     cv::namedWindow("original", cv::WINDOW_NORMAL);
     cv::imshow("original", cv_image);
-    cv::waitKey(1);
   }
 
   //! raycast object shadow using vehicle
@@ -503,7 +502,6 @@ void denoiseOccupancyGridCV(
     if (is_show_debug_window) {
       cv::namedWindow("object ray shadow", cv::WINDOW_NORMAL);
       cv::imshow("object ray shadow", cv_image);
-      cv::waitKey(1);
     }
   }
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp
@@ -59,8 +59,8 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
     }
   }
   pp.filter_occupancy_grid = node.declare_parameter(ns + ".filter_occupancy_grid", false);
-  pp.use_object_info = node.declare_parameter(ns + ".use_object_info", false);
-  pp.use_moving_object_ray_cast = node.declare_parameter(ns + ".use_moving_object_ray_cast", false);
+  pp.use_object_info = node.declare_parameter(ns + ".use_object_info", true);
+  pp.use_moving_object_ray_cast = node.declare_parameter(ns + ".use_moving_object_ray_cast", true);
   pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false);
   pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.5);
   pp.pedestrian_radius = node.declare_parameter(ns + ".pedestrian_radius", 0.5);

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
@@ -216,11 +216,17 @@ bool isMovingVehicle(const PredictedObject & obj, const double min_vel)
   return std::abs(obj_vel) > min_vel;
 }
 
-std::vector<PredictedObject> extractVehicles(const PredictedObjects::ConstSharedPtr objects_ptr)
+std::vector<PredictedObject> extractVehicles(
+  const PredictedObjects::ConstSharedPtr objects_ptr, const Point ego_position,
+  const double distance)
 {
   std::vector<PredictedObject> vehicles;
   for (const auto & obj : objects_ptr->objects) {
     if (occlusion_spot_utils::isVehicle(obj)) {
+      const auto & o = obj.kinematics.initial_pose_with_covariance.pose.position;
+      const auto & p = ego_position;
+      // Don't consider far vehicle
+      if (std::hypot(p.x - o.x, p.y - o.y) > distance) continue;
       vehicles.emplace_back(obj);
     }
   }

diff --git a/...ing/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/...ing/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp
@@ -48,6 +48,8 @@ void applySafeVelocityConsideringPossibleCollision(
       (l_obs < 0 && v0 <= v_safe)
         ? v_safe
         : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs);
+    // skip non effective velocity insertion
+    if (original_vel < v_safe) continue;
     // compare safe velocity consider EBS, minimum allowed velocity and original velocity
     const double safe_velocity = calculateInsertVelocity(v_slow_down, v_safe, v_min, original_vel);
     possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity;

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
@@ -92,8 +92,7 @@ bool OcclusionSpotModule::modifyPathVelocity(
     param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold;
     param_.v.v_ego = planner_data_->current_velocity->twist.linear.x;
     param_.v.a_ego = planner_data_->current_accel.get();
-    // introduce delay ratio until system delay param will introduce
-    param_.v.delay_time = 0.5;
+    param_.v.delay_time = planner_data_->system_delay;
     const double detection_area_offset = 5.0;  // for visualization and stability
     param_.detection_area_max_length =
       planning_utils::calcJudgeLineDistWithJerkLimit(
@@ -137,7 +136,8 @@ bool OcclusionSpotModule::modifyPathVelocity(
   }
   DEBUG_PRINT(show_time, "extract[ms]: ", stop_watch_.toc("processing_time", true));
   const auto objects_ptr = planner_data_->predicted_objects;
-  const auto vehicles = utils::extractVehicles(objects_ptr);
+  const auto vehicles =
+    utils::extractVehicles(objects_ptr, ego_pose.position, param_.detection_area_length);
   const std::vector<PredictedObject> filtered_vehicles =
     utils::filterVehiclesByDetectionArea(vehicles, debug_data_.detection_area_polygons);
   DEBUG_PRINT(show_time, "filter obj[ms]: ", stop_watch_.toc("processing_time", true));