diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index c589624e56f61..927793f713433 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -224,7 +224,7 @@ ament_auto_add_library(scene_module_occlusion_spot SHARED src/scene_module/occlusion_spot/manager.cpp src/scene_module/occlusion_spot/debug.cpp src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp - src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp + src/scene_module/occlusion_spot/scene_occlusion_spot.cpp src/scene_module/occlusion_spot/occlusion_spot_utils.cpp src/scene_module/occlusion_spot/risk_predictive_braking.cpp ) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 7ccda0468bac6..a70d94e7e3e54 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: "predicted_object" # [-] 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 68945d2ded3b0..6464226c788b0 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 @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface const char * getModuleName() override { return "occlusion_spot"; } private: - enum class ModuleID { PRIVATE, PUBLIC }; + enum class ModuleID { OCCUPANCY, OBJECT }; 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_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp similarity index 87% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp rename to planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index fed53518eb1ca..147c1e4396201 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ #include #include @@ -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 = "occupancy"; 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, @@ -82,4 +82,4 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index 869084e7ed410..073d3bb6aea86 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -43,7 +43,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface public: struct DebugData { - std::string road_type = "public"; + std::string road_type = "object"; std::vector detection_areas; std::vector parked_vehicle_point; std::vector possible_collisions; 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 4cae4d7b92a80..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 @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -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/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index 6e7d31e4b5705..8e8b408082e78 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 @@ -16,6 +16,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -181,13 +182,14 @@ void toQuantizedImage( for (int y = height - 1; y >= 0; y--) { const int idx = (height - 1 - y) + (width - 1 - x) * height; int8_t intensity = occupancy_grid.data.at(idx); - if (0 <= intensity && intensity < param.free_space_max) { + if (0 <= intensity && intensity <= param.free_space_max) { intensity = grid_utils::occlusion_cost_value::FREE_SPACE; - } else if ( // NOLINT - intensity == occlusion_cost_value::NO_INFORMATION || intensity < param.occupied_min) { + } else if (param.free_space_max < intensity && intensity < param.occupied_min) { intensity = grid_utils::occlusion_cost_value::UNKNOWN; - } else { + } else if (param.occupied_min <= intensity) { intensity = grid_utils::occlusion_cost_value::OCCUPIED; + } else { + std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); } cv_image->at(y, x) = intensity; } 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 1ad0f5970c3f2..9c57cff00f35d 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 @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include @@ -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); - 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_)); + if (path.points.empty()) return; + const int64_t module_id = static_cast(ModuleID::OCCUPANCY); + const int64_t public_road_module_id = static_cast(ModuleID::OBJECT); + // 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,8 @@ 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) { + return false; }; } } // namespace behavior_velocity_planner 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 e2427b90ae055..6c5b9f10d218b 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 @@ -47,7 +47,7 @@ void applySafeVelocityConsideringPossibleCollision( // min allowed velocity : min allowed velocity consider maximum allowed braking const double v_slow_down = - (l_obs < 0) + (l_obs < 0 && v0 <= v_safe) ? v_safe : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs); // compare safe velocity consider EBS, minimum allowed velocity and original velocity diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp similarity index 87% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp rename to planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 4ce2b34b60c60..26aaaaae02df0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include @@ -27,12 +27,11 @@ namespace behavior_velocity_planner { -using occlusion_spot_utils::ROAD_TYPE::PRIVATE; 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, @@ -42,12 +41,11 @@ 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"; if (path->points.size() < 2) { return true; } @@ -83,9 +81,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); @@ -94,16 +89,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 15b5029d39b3f..3a3d6013b49b6 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 @@ -30,7 +30,6 @@ namespace behavior_velocity_planner { using occlusion_spot_utils::PossibleCollisionInfo; -using occlusion_spot_utils::ROAD_TYPE::PUBLIC; namespace utils = occlusion_spot_utils; OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( @@ -47,7 +46,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); - debug_data_.road_type = "public"; if (path->points.size() < 2) { return true; } @@ -81,9 +79,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); @@ -96,7 +91,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); diff --git a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py index fc7b9323ad183..9b550c359a9ab 100644 --- a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py +++ b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py @@ -61,7 +61,7 @@ def add_launch_arg(name: str, default_value=None): "max_height": 2.0, "angle_min": -3.141592, # -M_PI "angle_max": 3.141592, # M_PI - "angle_increment": 0.00436332222, # 0.25*M_PI/180.0 + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 "scan_time": 0.0, "range_min": 1.0, "range_max": 200.0,