diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 7ccda0468bac6..b31449fe466e2 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: occlusion_spot: + method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" debug: false # [-] whether to publish debug markers. Note Default should be false for performance pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity threshold: @@ -18,7 +19,7 @@ safe_margin: 1.0 # [m] maximum safety distance for any error detection_area: min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. + slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. max_lateral_distance: 4.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 diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp index 2489506c27815..5c434114bfc40 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp @@ -45,7 +45,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface const char * getModuleName() override { return "occlusion_spot"; } private: - enum class ModuleID { PRIVATE, PUBLIC }; + enum class ModuleID { DEFAULT, PUBLIC }; using PlannerParam = occlusion_spot_utils::PlannerParam; PlannerParam planner_param_; 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 dc6349dd12793..3a3f13d709338 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 @@ -65,6 +65,7 @@ using DetectionAreaIdx = boost::optional>; namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; +enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; struct DetectionArea { @@ -96,6 +97,7 @@ struct LatLon struct PlannerParam { + METHOD method; bool debug; // [-] // parameters in yaml double detection_area_length; // [m] diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index 18d82fb103618..e877223a94f5b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -37,7 +37,7 @@ namespace behavior_velocity_planner { -class OcclusionSpotInPrivateModule : public SceneModuleInterface +class OcclusionSpotModule : public SceneModuleInterface { using PlannerParam = occlusion_spot_utils::PlannerParam; @@ -45,7 +45,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface struct DebugData { double z; - std::string road_type = "private"; + std::string road_type = "general"; std::vector detection_areas; std::vector possible_collisions; std::vector occlusion_points; @@ -53,7 +53,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface PathWithLaneId interp_path; }; - OcclusionSpotInPrivateModule( + OcclusionSpotModule( const int64_t module_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 8a01f7111e8ed..595d97d766a94 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -269,7 +269,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar return debug_marker_array; } -visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMarkerArray() +visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray() { const auto current_time = this->clock_->now(); 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 c84b5b3032e45..de0f3a5d05e6f 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 @@ -25,50 +25,7 @@ namespace behavior_velocity_planner { -namespace -{ -std::vector getLaneletsOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - std::vector lanelets; - - for (const auto & p : path.points) { - const auto lane_id = p.lane_ids.at(0); - lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); - } - return lanelets; -} - -bool hasPublicRoadOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { - // Is public load ? - const std::string location = ll.attributeOr("location", "else"); - if (location == "urban" || location == "public") { - return true; - } - } - return false; -} - -bool hasPrivateRoadOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { - // Is private load ? - const std::string location = ll.attributeOr("location", "else"); - if (location == "private") { - return true; - } - } - return false; -} - -} // namespace +using occlusion_spot_utils::METHOD; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -80,6 +37,14 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & pp = planner_param_; // assume pedestrian coming out from occlusion spot with this velocity + const std::string method = node.declare_parameter(ns + ".method", "occupancy_grid"); + if (method == "occupancy_grid") { + pp.method = METHOD::OCCUPANCY_GRID; + } else if (method == "predicted_object") { + pp.method = METHOD::PREDICTED_OBJECT; + } else { + throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"}; + } pp.debug = node.declare_parameter(ns + ".debug", false); pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); @@ -117,19 +82,20 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) void OcclusionSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const int64_t private_road_module_id = static_cast(ModuleID::PRIVATE); + if (path.points.empty()) return; + const int64_t module_id = static_cast(ModuleID::DEFAULT); const int64_t public_road_module_id = static_cast(ModuleID::PUBLIC); - // private - if (!isModuleRegistered(private_road_module_id)) { - if (hasPrivateRoadOnPath(path, planner_data_->lanelet_map)) { - registerModule(std::make_shared( - private_road_module_id, planner_data_, planner_param_, - logger_.get_child("occlusion_spot_in_private_module"), clock_, pub_debug_occupancy_grid_)); + // general + if (!isModuleRegistered(module_id)) { + if (planner_param_.method == METHOD::OCCUPANCY_GRID) { + registerModule(std::make_shared( + module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), + clock_, pub_debug_occupancy_grid_)); } } // public if (!isModuleRegistered(public_road_module_id)) { - if (hasPublicRoadOnPath(path, planner_data_->lanelet_map)) { + if (planner_param_.method == METHOD::PREDICTED_OBJECT) { registerModule(std::make_shared( public_road_module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_in_public_module"), clock_)); @@ -141,17 +107,14 @@ std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const bool has_public_road = hasPublicRoadOnPath(path, planner_data_->lanelet_map); - const bool has_private_road = hasPrivateRoadOnPath(path, planner_data_->lanelet_map); - return [has_private_road, - has_public_road](const std::shared_ptr & scene_module) { - if (scene_module->getModuleId() == static_cast(ModuleID::PRIVATE)) { - return !has_private_road; - } - if (scene_module->getModuleId() == static_cast(ModuleID::PUBLIC)) { - return !has_public_road; - } - return true; + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + // expire if no path points + if (path.points.empty()) return true; + // expire if there are path points + else if (path.points.size() > 0) + return false; + else + return true; }; } } // namespace behavior_velocity_planner 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 f2552bda9ed77..f0ac84af0a14e 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 @@ -31,7 +31,7 @@ namespace bg = boost::geometry; namespace lg = lanelet::geometry; namespace utils = occlusion_spot_utils; -OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( +OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, @@ -41,12 +41,12 @@ OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( param_ = planner_param; } -bool OcclusionSpotInPrivateModule::modifyPathVelocity( +bool OcclusionSpotModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); - debug_data_.road_type = "private"; + debug_data_.road_type = "general"; if (path->points.size() < 2) { return true; } @@ -82,9 +82,6 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( } // return if ego is final point of interpolated path if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - DetectionAreaIdx focus_area = - extractTargetRoadArcLength(lanelet_map_ptr, max_range, *path, PRIVATE); - if (!focus_area) return true; nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); @@ -93,16 +90,12 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( std::vector detection_area_polygons; utils::buildDetectionAreaPolygon( detection_area_polygons, interp_path, offset_from_start_to_ego, param_); - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego); std::vector possible_collisions; // Note: Don't consider offset from path start to ego here utils::createPossibleCollisionsInDetectionArea( detection_area_polygons, possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_.occlusion_points); if (detection_area_polygons.empty()) return true; - utils::filterCollisionByRoadType(possible_collisions, focus_area); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 99aebf5ad60d8..f9326f3bda012 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -80,9 +80,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( } // return if ego is final point of interpolated path if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - DetectionAreaIdx focus_area = - extractTargetRoadArcLength(lanelet_map_ptr, param_.detection_area_length, *path, PUBLIC); - if (!focus_area) return true; std::vector obj = utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); @@ -95,7 +92,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( std::vector possible_collisions = utils::generatePossibleCollisionBehindParkedVehicle( interp_path, param_, offset_from_start_to_ego, filtered_obj); - utils::filterCollisionByRoadType(possible_collisions, focus_area); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0);