diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp index c4355a04e49d9..f4b3354eeafb7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp @@ -39,17 +39,22 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC private: CrosswalkModule::PlannerParam crosswalk_planner_param_; - WalkwayModule::PlannerParam walkway_planner_param_; - std::vector getCrosswalksOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs); + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class WalkwayModuleManager : public SceneModuleManagerInterface +{ +public: + explicit WalkwayModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "walkway"; } - std::set getCrosswalkIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs); +private: + WalkwayModule::PlannerParam walkway_planner_param_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ae8fca491cdc5..3d1bd430296db 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -166,6 +166,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager if (this->declare_parameter("launch_crosswalk", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_traffic_light", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 063b1cb820c4c..dba00ffbfdb11 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -19,32 +19,10 @@ #include #include -namespace behavior_velocity_planner +namespace { -CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) -{ - const std::string ns(getModuleName()); - - // for crosswalk parameters - auto & cp = crosswalk_planner_param_; - cp.stop_line_distance = node.declare_parameter(ns + ".crosswalk.stop_line_distance", 1.5); - cp.stop_margin = node.declare_parameter(ns + ".crosswalk.stop_margin", 1.0); - cp.slow_margin = node.declare_parameter(ns + ".crosswalk.slow_margin", 2.0); - cp.slow_velocity = node.declare_parameter(ns + ".crosswalk.slow_velocity", 5.0 / 3.6); - cp.stop_predicted_object_prediction_time_margin = - node.declare_parameter(ns + ".crosswalk.stop_predicted_object_prediction_time_margin", 3.0); - cp.external_input_timeout = node.declare_parameter(ns + ".crosswalk.external_input_timeout", 1.0); - - // for walkway parameters - auto & wp = walkway_planner_param_; - wp.stop_margin = node.declare_parameter(ns + ".walkway.stop_margin", 1.0); - wp.stop_line_distance = node.declare_parameter(ns + ".walkway.stop_line_distance", 1.0); - wp.stop_duration_sec = node.declare_parameter(ns + ".walkway.stop_duration_sec", 1.0); - wp.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); -} - -std::vector CrosswalkModuleManager::getCrosswalksOnPath( +std::vector getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) @@ -53,14 +31,13 @@ std::vector CrosswalkModuleManager::getCrosswalksOnPath( std::set unique_lane_ids; auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( - path.points, planner_data_->current_pose.pose, std::numeric_limits::max(), M_PI_2); + path.points, current_pose, std::numeric_limits::max(), M_PI_2); // Add current lane id lanelet::ConstLanelets current_lanes; if ( lanelet::utils::query::getCurrentLanelets( - lanelet::utils::query::laneletLayer(lanelet_map), planner_data_->current_pose.pose, - ¤t_lanes) && + lanelet::utils::query::laneletLayer(lanelet_map), current_pose, ¤t_lanes) && nearest_segment_idx) { for (const auto & ll : current_lanes) { if ( @@ -91,40 +68,55 @@ std::vector CrosswalkModuleManager::getCrosswalksOnPath( return crosswalks; } -std::set CrosswalkModuleManager::getCrosswalkIdSetOnPath( +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::set crosswalk_id_set; - for (const auto & crosswalk : getCrosswalksOnPath(path, lanelet_map, overall_graphs)) { + for (const auto & crosswalk : + getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { crosswalk_id_set.insert(crosswalk.id()); } return crosswalk_id_set; } +} // namespace + +namespace behavior_velocity_planner +{ +CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC(node, getModuleName()) +{ + const std::string ns(getModuleName()); + + // for crosswalk parameters + auto & cp = crosswalk_planner_param_; + cp.stop_line_distance = node.declare_parameter(ns + ".crosswalk.stop_line_distance", 1.5); + cp.stop_margin = node.declare_parameter(ns + ".crosswalk.stop_margin", 1.0); + cp.slow_margin = node.declare_parameter(ns + ".crosswalk.slow_margin", 2.0); + cp.slow_velocity = node.declare_parameter(ns + ".crosswalk.slow_velocity", 5.0 / 3.6); + cp.stop_predicted_object_prediction_time_margin = + node.declare_parameter(ns + ".crosswalk.stop_predicted_object_prediction_time_margin", 3.0); + cp.external_input_timeout = node.declare_parameter(ns + ".crosswalk.external_input_timeout", 1.0); +} void CrosswalkModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - for (const auto & crosswalk : - getCrosswalksOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) { + for (const auto & crosswalk : getCrosswalksOnPath( + planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr())) { const auto module_id = crosswalk.id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, crosswalk, crosswalk_planner_param_, logger_.get_child("crosswalk_module"), clock_)); - if ( - crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) == - lanelet::AttributeValueString::Walkway) { - registerModule(std::make_shared( - module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"), - clock_)); - } - generateUUID(module_id); } + generateUUID(module_id); } } @@ -133,12 +125,55 @@ CrosswalkModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto crosswalk_id_set = - getCrosswalkIdSetOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + const auto crosswalk_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } +WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + + // for walkway parameters + auto & wp = walkway_planner_param_; + wp.stop_margin = node.declare_parameter(ns + ".walkway.stop_margin", 1.0); + wp.stop_line_distance = node.declare_parameter(ns + ".walkway.stop_line_distance", 1.0); + wp.stop_duration_sec = node.declare_parameter(ns + ".walkway.stop_duration_sec", 1.0); + wp.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); +} + +void WalkwayModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto rh = planner_data_->route_handler_; + for (const auto & crosswalk : getCrosswalksOnPath( + planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr())) { + const auto module_id = crosswalk.id(); + if ( + !isModuleRegistered(module_id) && + crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) == + lanelet::AttributeValueString::Walkway) { + registerModule(std::make_shared( + module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"), clock_)); + } + } +} + +std::function &)> +WalkwayModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto rh = planner_data_->route_handler_; + const auto walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + return [walkway_id_set](const std::shared_ptr & scene_module) { + return walkway_id_set.count(scene_module->getModuleId()) == 0; + }; +} } // namespace behavior_velocity_planner