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"),
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_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;
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");