refactor(obstacle_avoidance_planner): move the elastic band smoothing to a new package

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -4,7 +4,8 @@
   <arg name="vehicle_param_file"/>
   <arg name="use_pointcloud_container"/>
   <arg name="pointcloud_container_name"/>
-  <arg name="smoother_type"/>
+  <arg name="path_smoother_type"/>
+  <arg name="velocity_smoother_type"/>
   <arg name="vehicle_param_file"/>
   <arg name="use_pointcloud_container"/>
   <arg name="pointcloud_container_name"/>
@@ -43,7 +44,7 @@
   <arg name="obstacle_cruise_planner_param_path"/>
   <!-- motion velocity smoother -->
   <arg name="motion_velocity_smoother_param_path"/>
-  <arg name="smoother_type_param_path"/>
+  <arg name="velocity_smoother_type_param_path"/>
   <!-- planning validator -->
   <arg name="planning_validator_param_path"/>
 

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -21,6 +21,7 @@
 from launch.conditions import LaunchConfigurationEquals
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PythonExpression
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
@@ -41,6 +42,39 @@ def launch_setup(context, *args, **kwargs):
     with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f:
         nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    # path smoother
+    smoother_output_path_topic = "path_smoother/path"
+    with open(LaunchConfiguration("elastic_band_smoother_param_path").perform(context), "r") as f:
+        elastic_band_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    elastic_band_smoother_component = ComposableNode(
+        package="path_smoothing",
+        plugin="path_smoothing::ElasticBandSmoother",
+        name="elastic_band_smoother",
+        namespace="",
+        remappings=[
+            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/output/path", smoother_output_path_topic),
+        ],
+        parameters=[
+            nearest_search_param,
+            elastic_band_smoother_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    planner_input_path_topic = PythonExpression(
+        [
+            "'",
+            LaunchConfiguration("input_path_topic"),
+            "' if '",
+            LaunchConfiguration("path_smoother_type"),
+            "' == 'none'",
+            " else '",
+            smoother_output_path_topic,
+            "'",
+        ]
+    )
     # obstacle avoidance planner
     with open(
         LaunchConfiguration("obstacle_avoidance_planner_param_path").perform(context), "r"
@@ -52,7 +86,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_avoidance_planner",
         namespace="",
         remappings=[
-            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/path", planner_input_path_topic),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/output/path", "obstacle_avoidance_planner/trajectory"),
         ],
@@ -73,7 +107,7 @@ def launch_setup(context, *args, **kwargs):
         name="path_sampler",
         namespace="",
         remappings=[
-            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/path", planner_input_path_topic),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/output/path", "obstacle_avoidance_planner/trajectory"),
         ],
@@ -252,6 +286,12 @@ def launch_setup(context, *args, **kwargs):
         condition=LaunchConfigurationEquals("cruise_planner_type", "none"),
     )
 
+    smoother_loader = LoadComposableNodes(
+        composable_node_descriptions=[elastic_band_smoother_component],
+        target_container=container,
+        condition=LaunchConfigurationEquals("path_smoother_type", "elastic_band"),
+    )
+
     obstacle_avoidance_planner_loader = LoadComposableNodes(
         composable_node_descriptions=[obstacle_avoidance_planner_component],
         target_container=container,
@@ -273,6 +313,7 @@ def launch_setup(context, *args, **kwargs):
     group = GroupAction(
         [
             container,
+            smoother_loader,
             obstacle_avoidance_planner_loader,
             path_sampler_loader,
             obstacle_stop_planner_loader,
@@ -310,6 +351,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg(
         "path_planner_type", "obstacle_avoidance_planner"
     )  # select from "obstacle_avoidance_planner", "path_sampler"
+    add_launch_arg("path_smoother_type", "elastic_band")  # select from "elastic_band", "none"
 
     add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -27,11 +27,11 @@
       <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
       <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
       <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
-        <arg name="smoother_type" value="$(var smoother_type)"/>
+        <arg name="velocity_smoother_type" value="$(var velocity_smoother_type)"/>
         <arg name="common_param_path" value="$(var common_param_path)"/>
         <arg name="nearest_search_param_path" value="$(var nearest_search_param_path)"/>
         <arg name="param_path" value="$(var motion_velocity_smoother_param_path)"/>
-        <arg name="smoother_param_path" value="$(var smoother_type_param_path)"/>
+        <arg name="velocity_smoother_param_path" value="$(var velocity_smoother_type_param_path)"/>
       </include>
     </group>
   </group>

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -94,6 +94,11 @@ struct LaneChangePhaseInfo
   double lane_changing{0.0};
 
   [[nodiscard]] double sum() const { return prepare + lane_changing; }
+
+  LaneChangePhaseInfo(const double _prepare, const double _lane_changing)
+  : prepare(_prepare), lane_changing(_lane_changing)
+  {
+  }
 };
 
 struct LaneChangeTargetObjectIndices

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp

@@ -36,9 +36,12 @@ struct LaneChangePath
   Pose lane_changing_end{};
   ShiftedPath shifted_path{};
   ShiftLine shift_line{};
-  double acceleration{0.0};
-  LaneChangePhaseInfo length{};
-  LaneChangePhaseInfo duration{};
+
+  // longitudinal acceleration applied on the prepare and lane-changing phase
+  LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0};
+
+  LaneChangePhaseInfo length{0.0, 0.0};
+  LaneChangePhaseInfo duration{0.0, 0.0};
   PathWithLaneId prev_path{};
 };
 using LaneChangePaths = std::vector<LaneChangePath>;

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -90,19 +90,19 @@ std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const std::vector<std::vector<int64_t>> & sorted_lane_ids, const double longitudinal_acceleration,
-  const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length,
-  const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity,
-  const LaneChangePhaseInfo lane_change_time);
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+  const LaneChangePhaseInfo longitudinal_acceleration, const double lateral_acceleration,
+  const LaneChangePhaseInfo lane_change_length, const LaneChangePhaseInfo lane_change_velocity,
+  const double terminal_lane_changing_velocity, const LaneChangePhaseInfo lane_change_time);
 
 PathSafetyStatus isLaneChangePathSafe(
   const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects,
   const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose,
   const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter,
   const behavior_path_planner::LaneChangeParameters & lane_change_parameter,
   const double front_decel, const double rear_decel,
-  std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const double prepare_acc = 0.0,
-  const double lane_changing_acc = 0.0);
+  std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const double prepare_acc,
+  const double lane_changing_acc);
 
 bool isObjectIndexIncluded(
   const size_t & index, const std::vector<size_t> & dynamic_objects_indices);

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -578,11 +578,13 @@ bool NormalLaneChange::getLaneChangePaths(
       minimum_lane_changing_velocity);
 
     // compute actual longitudinal acceleration
-    const double longitudinal_acc = (prepare_velocity - current_velocity) / prepare_duration;
+    const double longitudinal_acc_on_prepare =
+      (prepare_velocity - current_velocity) / prepare_duration;
 
     // get path on original lanes
     const double prepare_length = std::max(
-      current_velocity * prepare_duration + 0.5 * longitudinal_acc * std::pow(prepare_duration, 2),
+      current_velocity * prepare_duration +
+        0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2),
       minimum_prepare_length);
 
     if (prepare_length < target_length) {
@@ -629,14 +631,15 @@ bool NormalLaneChange::getLaneChangePaths(
          lateral_acc += lateral_acc_resolution) {
       const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
         shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc);
-      const double lane_changing_lon_acc = utils::lane_change::calcLaneChangingAcceleration(
-        initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
-        sampled_longitudinal_acc);
+      const double longitudinal_acc_on_lane_changing =
+        utils::lane_change::calcLaneChangingAcceleration(
+          initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
+          sampled_longitudinal_acc);
       const auto lane_changing_length =
         initial_lane_changing_velocity * lane_changing_time +
-        0.5 * lane_changing_lon_acc * lane_changing_time * lane_changing_time;
+        0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
       const auto terminal_lane_changing_velocity =
-        initial_lane_changing_velocity + lane_changing_lon_acc * lane_changing_time;
+        initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
       utils::lane_change::setPrepareVelocity(
         prepare_segment, current_velocity, terminal_lane_changing_velocity);
 
@@ -701,7 +704,8 @@ bool NormalLaneChange::getLaneChangePaths(
 
       const auto candidate_path = utils::lane_change::constructCandidatePath(
         prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets,
-        target_lanelets, sorted_lane_ids, lane_changing_lon_acc, lateral_acc, lc_length,
+        target_lanelets, sorted_lane_ids,
+        {longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}, lateral_acc, lc_length,
         lc_velocity, terminal_lane_changing_velocity, lc_time);
 
       if (!candidate_path) {
@@ -752,8 +756,8 @@ bool NormalLaneChange::getLaneChangePaths(
       const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe(
         *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(),
         common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration,
-        common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc,
-        lane_changing_lon_acc);
+        common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc_on_prepare,
+        longitudinal_acc_on_lane_changing);
 
       if (is_safe) {
         return true;
@@ -790,7 +794,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
     path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters,
     *lane_change_parameters_, common_parameters.expected_front_deceleration_for_abort,
     common_parameters.expected_rear_deceleration_for_abort, debug_data,
-    status_.lane_change_path.acceleration);
+    status_.lane_change_path.longitudinal_acceleration.prepare,
+    status_.lane_change_path.longitudinal_acceleration.lane_changing);
 
   return safety_status;
 }

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -112,7 +112,12 @@ bool StartPlannerModule::isExecutionRequested() const
     tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose));
 
   // Check if ego is not out of lanes
-  const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  // const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  const auto current_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
   auto lanes = current_lanes;
   lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
@@ -305,7 +310,12 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     return output;
   }
 
-  const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  // const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  const auto current_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
   auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
   const auto drivable_lanes =
@@ -542,7 +552,10 @@ void StartPlannerModule::updatePullOutStatus()
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();
 
-  status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  status_.current_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
   status_.pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
 
   // combine road and shoulder lanes

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -210,15 +210,15 @@ std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const std::vector<std::vector<int64_t>> & sorted_lane_ids, const double longitudinal_acceleration,
-  const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length,
-  const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity,
-  const LaneChangePhaseInfo lane_change_time)
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+  const LaneChangePhaseInfo longitudinal_acceleration, const double lateral_acceleration,
+  const LaneChangePhaseInfo lane_change_length, const LaneChangePhaseInfo lane_change_velocity,
+  const double terminal_lane_changing_velocity, const LaneChangePhaseInfo lane_change_time)
 {
   PathShifter path_shifter;
   path_shifter.setPath(target_lane_reference_path);
   path_shifter.addShiftLine(shift_line);
-  path_shifter.setLongitudinalAcceleration(longitudinal_acceleration);
+  path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing);
   ShiftedPath shifted_path;
 
   // offset front side
@@ -238,7 +238,7 @@ std::optional<LaneChangePath> constructCandidatePath(
   const auto & lane_changing_length = lane_change_length.lane_changing;
 
   LaneChangePath candidate_path;
-  candidate_path.acceleration = longitudinal_acceleration;
+  candidate_path.longitudinal_acceleration = longitudinal_acceleration;
   candidate_path.length.prepare = prepare_length;
   candidate_path.length.lane_changing = lane_changing_length;
   candidate_path.duration.prepare = lane_change_time.prepare;

planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp

@@ -40,7 +40,11 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
   PullOutPath output;
 
   // combine road lane and shoulder lane
-  const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
+  const auto road_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   const auto shoulder_lanes = getPullOutLanes(planner_data_);
   auto lanes = road_lanes;
   lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -44,12 +44,16 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
   const auto & route_handler = planner_data_->route_handler;
   const auto & common_parameters = planner_data_->parameters;
   const auto & dynamic_objects = planner_data_->dynamic_object;
-  const auto & road_lanes = utils::getExtendedCurrentLanes(planner_data_);
-  const auto & shoulder_lanes = getPullOutLanes(planner_data_);
+  const auto shoulder_lanes = getPullOutLanes(planner_data_);
   if (shoulder_lanes.empty()) {
     return boost::none;
   }
 
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
+  const auto road_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   // find candidate paths
   auto pull_out_paths = calcPullOutPaths(
     *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters,
@@ -135,6 +139,7 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   }
 
   // rename parameter
+  const double forward_path_length = common_parameter.forward_path_length;
   const double backward_path_length = common_parameter.backward_path_length;
   const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance;
   const double lateral_jerk = parameter.lateral_jerk;
@@ -150,13 +155,13 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const auto arc_position_start = getArcCoordinates(road_lanes, start_pose);
   const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0);
   const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose);
-  const double road_lanes_length = std::accumulate(
-    road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) {
-      return sum + lanelet::utils::getLaneletLength2d(lane);
-    });
-  // if goal is behind start pose,
+
+  // if goal is behind start pose, use path with forward_path_length
   const bool goal_is_behind = arc_position_goal.length < s_start;
-  const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length;
+  const double s_forward_length = s_start + forward_path_length;
+  const double s_end =
+    goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length);
+
   constexpr double RESAMPLE_INTERVAL = 1.0;
   PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
     route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);