From 94357d27c4003384d76aacbe24dd8718b2e69d6a Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 20 Jan 2024 15:07:26 +0900 Subject: [PATCH 1/3] feat(behavior_velocity_planner): add enable_all_modules_auto_mode argument to launch files for behavior velocity planner modules (#6094) * set default value for enable_all_modules_auto_mode Signed-off-by: kyoichi-sugahara * fix enable_rtc configuration in scene_module_manager_interface.hpp Signed-off-by: kyoichi-sugahara * Refactor scene module managers to use getEnableRTC function Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../behavior_planning.launch.xml | 1 + .../src/manager.cpp | 5 +---- .../src/manager.cpp | 3 +-- .../src/manager.cpp | 3 +-- .../src/manager.cpp | 5 ++--- .../src/manager.cpp | 3 +-- .../scene_module_interface.hpp | 17 +++++++++++++++++ .../src/manager.cpp | 3 +-- 8 files changed, 25 insertions(+), 15 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index d7da81c969da2..a297697234ef2 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -237,6 +237,7 @@ + diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 09d1c5a7c3270..d114ab0c65da9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -16,7 +16,6 @@ #include #include -#include #include @@ -28,12 +27,10 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.use_pass_judge_line = diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index b8190cb6124e7..dc3ce32be8505 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index e7ec6b37f7f20..834ffc03e5dde 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0f9faaaa901c1..d2311d1c0c992 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -35,11 +35,10 @@ using tier4_autoware_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - getOrDeclareParameter( - node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index d5e9eeddb6377..c4192750af1d5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index fcde16d8a871c..d8b042ec880e4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -52,6 +53,7 @@ using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; +using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -251,6 +253,21 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index fa1a516c107b0..89817f3342dbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); From bbf678de4b9b61b06dc8b2f6e94512a1f1a03f28 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 20 Jan 2024 15:09:01 +0900 Subject: [PATCH 2/3] refactor(start_planner): calculate drivable lanes efficiently (#6105) * Refactor ShiftPullOut class and update expanded drivable lanes Signed-off-by: kyoichi-sugahara * Refactor drivable lane functions Signed-off-by: kyoichi-sugahara * Fix lane departure check in ShiftPullOut::plan() Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../shift_pull_out.hpp | 7 ++++ .../start_planner_module.hpp | 2 + .../src/shift_pull_out.cpp | 23 +---------- .../src/start_planner_module.cpp | 38 +++++++++++++++++++ 4 files changed, 49 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 2d50ae189dc13..fef9a4aa8ebfc 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -50,6 +50,11 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); + void setDrivableLanes(const lanelet::ConstLanelets & drivable_lanes) + { + drivable_lanes_ = drivable_lanes; + } + std::shared_ptr lane_departure_checker_; private: @@ -58,6 +63,8 @@ class ShiftPullOut : public PullOutPlannerBase double calcPullOutLongitudinalDistance( const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; + + lanelet::ConstLanelets drivable_lanes_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 47cfa408162ca..5bbde1c2fc523 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -242,6 +242,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + void updateDrivableLanes(); + lanelet::ConstLanelets createDrivableLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 36b8b1dc347a1..3b4d08b65923c 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -49,11 +49,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - if (pull_out_lanes.empty()) { - return std::nullopt; - } - const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); @@ -80,20 +75,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [&route_handler](const auto & pull_out_lane) { - return route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); - // crop backward path // removes points which are out of lanes up to the start pose. // this ensures that the backward_path stays within the drivable area when starting from a @@ -107,7 +88,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -121,7 +102,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) { continue; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 1b54b655fc424..7857462e87320 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -128,6 +128,7 @@ void StartPlannerModule::initVariables() debug_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); + updateDrivableLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -560,6 +561,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; + updateDrivableLanes(); } void StartPlannerModule::incrementPathIndex() @@ -1325,6 +1327,42 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +void StartPlannerModule::updateDrivableLanes() +{ + const auto drivable_lanes = createDrivableLanes(); + for (auto & planner : start_planners_) { + auto shift_pull_out = std::dynamic_pointer_cast(planner); + + if (shift_pull_out) { + shift_pull_out->setDrivableLanes(drivable_lanes); + } + } +} + +lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const +{ + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + if (pull_out_lanes.empty()) { + return lanelet::ConstLanelets{}; + } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [this](const auto & pull_out_lane) { + return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); + }); + const auto drivable_lanes = utils::transformToLanelets( + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); + return drivable_lanes; +} + void StartPlannerModule::setDebugData() { using marker_utils::addFootprintMarker; From 4194028f5561a3bf948c5ced9b6338cddb898f7f Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Sat, 20 Jan 2024 18:22:21 +0900 Subject: [PATCH 3/3] chore(pointcloud_container): move glog_component to autoware_launch (#6114) --- .../ground_segmentation/ground_segmentation.launch.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index b54172acd4afc..66a0bb4d0fb63 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -474,13 +474,7 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, def launch_setup(context, *args, **kwargs): pipeline = GroundSegmentationPipeline(context) - glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - - components = [glog_component] + components = [] components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"),