diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml index a2f16ee825927..3ea4bb66ea990 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/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/README.md b/planning/behavior_velocity_planner/README.md index 19c3a1d72264c..a7feea30a3017 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -52,4 +52,5 @@ So for example, in order to stop at a stop line with the vehicles' front on the | `forward_path_length` | double | forward path length | | `backward_path_length` | double | backward path length | | `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | +| `system_delay` | double | (to be a global parameter) delay time until output control command | | `delay_response_time` | double | (to be a global parameter) delay time of the vehicle's response to control commands | diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 298d93447fde2..3ea4bb66ea990 100644 --- 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 index a182711c27604..1e19a0d4f256b 100644 --- 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/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 4c639bcb5ab1e..8c86a954ef7c8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/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 extractVehicles(const PredictedObjects::ConstSharedPtr objects_ptr); +std::vector extractVehicles( + const PredictedObjects::ConstSharedPtr objects_ptr, const Point ego_position, + const double distance); std::vector filterVehiclesByDetectionArea( const std::vector & 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 index 91927807e70ba..af94ef8c00c85 100644 --- 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 index aacd7ff01aa2b..ebbeed5483db0 100644 --- 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 index 55abac57cd83a..df42a8fd0ab76 100644 --- 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 extractVehicles(const PredictedObjects::ConstSharedPtr objects_ptr) +std::vector extractVehicles( + const PredictedObjects::ConstSharedPtr objects_ptr, const Point ego_position, + const double distance) { std::vector 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/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 845d4867048b5..2e2636215eca7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/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); if(original_vel < safe_velocity) continue; 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 index e97499c2f0f32..d0a0e37eb7c0d 100644 --- 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 filtered_vehicles = utils::filterVehiclesByDetectionArea(vehicles, debug_data_.detection_area_polygons); DEBUG_PRINT(show_time, "filter obj[ms]: ", stop_watch_.toc("processing_time", true));