fix(avoidance): unintentional path cut (autowarefoundation#5887)

* fix(avoidance): unintentional path cut

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp_common): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp_side_shift): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp_avoidance): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp_lane_change): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp_goal_planner): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp_start_planner): nouse pointer

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: karishma <karishma@interpl.ai>

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -220,7 +220,7 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
 
   // lanelet info
   data.current_lanelets = utils::avoidance::getCurrentLanesFromPath(
-    *getPreviousModuleOutput().reference_path, planner_data_);
+    getPreviousModuleOutput().reference_path, planner_data_);
 
   data.extend_lanelets =
     utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
@@ -233,8 +233,8 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     });
 
   // calc drivable bound
-  const auto shorten_lanes =
-    utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes);
+  auto tmp_path = getPreviousModuleOutput().path;
+  const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
   data.left_bound = toLineString3d(utils::calcBound(
     planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings,
     parameters_->use_intersection_areas, true));
@@ -244,9 +244,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
 
   // reference path
   if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
-    data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path);
+    data.reference_path_rough = extendBackwardLength(getPreviousModuleOutput().path);
   } else {
-    data.reference_path_rough = *getPreviousModuleOutput().path;
+    data.reference_path_rough = getPreviousModuleOutput().path;
     RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path.");
   }
 
@@ -910,17 +910,17 @@ BehaviorModuleOutput AvoidanceModule::plan()
   }
 
   if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
-    output.path = std::make_shared<PathWithLaneId>(spline_shift_path.path);
+    output.path = spline_shift_path.path;
   } else {
     output.path = getPreviousModuleOutput().path;
     RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing.");
   }
 
   output.reference_path = getPreviousModuleOutput().reference_path;
-  path_reference_ = getPreviousModuleOutput().reference_path;
+  path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
 
-  const size_t ego_idx = planner_data_->findEgoIndex(output.path->points);
-  utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters);
+  const size_t ego_idx = planner_data_->findEgoIndex(output.path.points);
+  utils::clipPathLength(output.path, ego_idx, planner_data_->parameters);
 
   // Drivable area generation.
   {
@@ -995,7 +995,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval()
   }
 
   path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
-  path_reference_ = getPreviousModuleOutput().reference_path;
+  path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
 
   return out;
 }
@@ -1169,11 +1169,11 @@ void AvoidanceModule::updateData()
   helper_->setData(planner_data_);
 
   if (!helper_->isInitialized()) {
-    helper_->setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path));
-    helper_->setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path));
-    helper_->setPreviousReferencePath(*getPreviousModuleOutput().path);
+    helper_->setPreviousSplineShiftPath(toShiftedPath(getPreviousModuleOutput().path));
+    helper_->setPreviousLinearShiftPath(toShiftedPath(getPreviousModuleOutput().path));
+    helper_->setPreviousReferencePath(getPreviousModuleOutput().path);
     helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath(
-      *getPreviousModuleOutput().reference_path, planner_data_));
+      getPreviousModuleOutput().reference_path, planner_data_));
   }
 
   debug_data_ = DebugData();

planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp

@@ -33,9 +33,8 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan(
   BehaviorModuleOutput output =
     // use planner previous module reference path
     getPreviousModuleOutput();
-  const PathWithLaneId smoothed_path =
-    modifyPathForSmoothGoalConnection(*(output.path), planner_data);
-  output.path = std::make_shared<PathWithLaneId>(smoothed_path);
+  const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(output.path, planner_data);
+  output.path = smoothed_path;
   output.reference_path = getPreviousModuleOutput().reference_path;
   return output;
 }

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -282,7 +282,7 @@ void GoalPlannerModule::updateData()
 
   resetPathCandidate();
   resetPathReference();
-  path_reference_ = getPreviousModuleOutput().reference_path;
+  path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
 
   updateOccupancyGrid();
 
@@ -761,7 +761,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const
     // because it takes time for the trajectory to be reflected
     auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
     keepStoppedWithCurrentPath(current_path);
-    output.path = std::make_shared<PathWithLaneId>(current_path);
+    output.path = current_path;
   }
 
   setModifiedGoal(output);
@@ -777,14 +777,14 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
 {
   if (prev_data_.found_path || !prev_data_.stop_path) {
     // safe -> not_safe or no prev_stop_path: generate new stop_path
-    output.path = std::make_shared<PathWithLaneId>(generateStopPath());
+    output.path = generateStopPath();
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
   } else {
     // not_safe -> not_safe: use previous stop path
-    output.path = prev_data_.stop_path;
+    output.path = *prev_data_.stop_path;
     // stop_pose_ is removed in manager every loop, so need to set every loop.
-    stop_pose_ = utils::getFirstStopPoseFromPath(*output.path);
+    stop_pose_ = utils::getFirstStopPoseFromPath(output.path);
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
   }
@@ -800,20 +800,19 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output
         current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop,
         parameters_->maximum_jerk_for_stop);
     if (stop_path) {
-      output.path = std::make_shared<PathWithLaneId>(*stop_path);
+      output.path = *stop_path;
       RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path");
     } else {
-      output.path =
-        std::make_shared<PathWithLaneId>(thread_safe_data_.get_pull_over_path()->getCurrentPath());
+      output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
       RCLCPP_WARN_THROTTLE(
         getLogger(), *clock_, 5000,
         "Collision detected, no feasible stop path found, cannot stop.");
     }
   } else {
     // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path
-    output.path = prev_data_.stop_path_after_approval;
+    output.path = *prev_data_.stop_path_after_approval;
     // stop_pose_ is removed in manager every loop, so need to set every loop.
-    stop_pose_ = utils::getFirstStopPoseFromPath(*output.path);
+    stop_pose_ = utils::getFirstStopPoseFromPath(output.path);
     RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path");
   }
 }
@@ -826,7 +825,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
   } else {
     const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
-      *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters);
+      output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters);
 
     DrivableAreaInfo current_drivable_area_info;
     current_drivable_area_info.drivable_lanes = target_drivable_lanes;
@@ -853,9 +852,9 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const
 {
   const auto original_signal = getPreviousModuleOutput().turn_signal_info;
   const auto new_signal = calcTurnSignalInfo();
-  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
+  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
   output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
-    *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
+    output.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
     planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
 }
@@ -942,11 +941,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate()
   BehaviorModuleOutput output{};
   const BehaviorModuleOutput pull_over_output = planPullOverAsOutput();
   output.modified_goal = pull_over_output.modified_goal;
-  output.path = std::make_shared<PathWithLaneId>(generateStopPath());
+  output.path = generateStopPath();
   output.reference_path = getPreviousModuleOutput().reference_path;
 
   const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
-    *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters);
+    output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters);
 
   DrivableAreaInfo current_drivable_area_info{};
   current_drivable_area_info.drivable_lanes = target_drivable_lanes;
@@ -1054,7 +1053,7 @@ void GoalPlannerModule::postProcess()
 void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
 {
   if (prev_data_.found_path || !prev_data_.stop_path) {
-    prev_data_.stop_path = output.path;
+    prev_data_.stop_path = std::make_shared<PathWithLaneId>(output.path);
   }
 
   // for the next loop setOutput().
@@ -1086,7 +1085,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
   if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) {
     return;
   }
-  auto stop_path = std::make_shared<PathWithLaneId>(*output.path);
+  auto stop_path = std::make_shared<PathWithLaneId>(output.path);
   for (auto & point : stop_path->points) {
     point.point.longitudinal_velocity_mps = 0.0;
   }

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -107,14 +107,13 @@ class LaneChangeBase
   virtual bool specialExpiredCheck() const { return false; }
 
   virtual void setPreviousModulePaths(
-    const std::shared_ptr<PathWithLaneId> & prev_module_reference_path,
-    const std::shared_ptr<PathWithLaneId> & prev_module_path)
+    const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path)
   {
-    if (prev_module_reference_path) {
-      prev_module_reference_path_ = *prev_module_reference_path;
+    if (!prev_module_reference_path.points.empty()) {
+      prev_module_reference_path_ = prev_module_reference_path;
     }
-    if (prev_module_path) {
-      prev_module_path_ = *prev_module_path;
+    if (!prev_module_path.points.empty()) {
+      prev_module_path_ = prev_module_path;
     }
   };
 

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -201,8 +201,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
   module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   auto output = module_type_->generateOutput();
-  path_reference_ = output.reference_path;
-  *prev_approved_path_ = *getPreviousModuleOutput().path;
+  path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
+  *prev_approved_path_ = getPreviousModuleOutput().path;
 
   stop_pose_ = module_type_->getStopPose();
 
@@ -219,12 +219,12 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 
 BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 {
-  *prev_approved_path_ = *getPreviousModuleOutput().path;
+  *prev_approved_path_ = getPreviousModuleOutput().path;
   module_type_->insertStopPoint(
     module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_);
 
   BehaviorModuleOutput out;
-  out.path = std::make_shared<PathWithLaneId>(*prev_approved_path_);
+  out.path = *prev_approved_path_;
   out.reference_path = getPreviousModuleOutput().reference_path;
   out.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
@@ -240,9 +240,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   }
 
   // change turn signal when the vehicle reaches at the end of the path for waiting lane change
-  out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info);
+  out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
 
-  path_reference_ = getPreviousModuleOutput().reference_path;
+  path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
 
   stop_pose_ = module_type_->getStopPose();
 
@@ -366,9 +366,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o
   const auto current_position = module_type_->getEgoPosition();
   const auto status = module_type_->getLaneChangeStatus();
   const auto start_distance = motion_utils::calcSignedArcLength(
-    output.path->points, current_position, status.lane_change_path.info.shift_line.start.position);
+    output.path.points, current_position, status.lane_change_path.info.shift_line.start.position);
   const auto finish_distance = motion_utils::calcSignedArcLength(
-    output.path->points, current_position, status.lane_change_path.info.shift_line.end.position);
+    output.path.points, current_position, status.lane_change_path.info.shift_line.end.position);
 
   steering_factor_interface_ptr_->updateSteeringFactor(
     {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end},

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -145,38 +145,38 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
   BehaviorModuleOutput output;
 
   if (isAbortState() && abort_path_) {
-    output.path = std::make_shared<PathWithLaneId>(abort_path_->path);
-    output.reference_path = std::make_shared<PathWithLaneId>(prev_module_reference_path_);
+    output.path = abort_path_->path;
+    output.reference_path = prev_module_reference_path_;
     output.turn_signal_info = prev_turn_signal_info_;
-    insertStopPoint(status_.current_lanes, *output.path);
+    insertStopPoint(status_.current_lanes, output.path);
   } else {
-    output.path = std::make_shared<PathWithLaneId>(getLaneChangePath().path);
+    output.path = getLaneChangePath().path;
 
     const auto found_extended_path = extendPath();
     if (found_extended_path) {
-      *output.path = utils::combinePath(*output.path, *found_extended_path);
+      output.path = utils::combinePath(output.path, *found_extended_path);
     }
-    output.reference_path = std::make_shared<PathWithLaneId>(getReferencePath());
+    output.reference_path = getReferencePath();
     output.turn_signal_info = updateOutputTurnSignal();
 
     if (isStopState()) {
       const auto current_velocity = getEgoVelocity();
       const auto current_dist = calcSignedArcLength(
-        output.path->points, output.path->points.front().point.pose.position, getEgoPosition());
+        output.path.points, output.path.points.front().point.pose.position, getEgoPosition());
       const auto stop_dist =
         -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
-      const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
+      const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path);
       setStopPose(stop_point.point.pose);
     } else {
-      insertStopPoint(status_.target_lanes, *output.path);
+      insertStopPoint(status_.target_lanes, output.path);
     }
   }
 
   extendOutputDrivableArea(output);
 
-  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
+  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
   output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
-    *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
+    output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
     planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
 
@@ -189,7 +189,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
 
   const auto drivable_lanes = utils::lane_change::generateDrivableLanes(
     *getRouteHandler(), status_.current_lanes, status_.target_lanes);
-  const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes);
+  const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes);
   const auto expanded_lanes = utils::expandLanelets(
     shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
     dp.drivable_area_types_to_skip);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -738,7 +738,8 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
 {
   // TODO(Horibe) do some error handling when path is not available.
 
-  auto path = output.path ? output.path : planner_data->prev_output_path;
+  auto path = !output.path.points.empty() ? std::make_shared<PathWithLaneId>(output.path)
+                                          : planner_data->prev_output_path;
   path->header = planner_data->route_handler->getRouteHeader();
   path->header.stamp = this->now();