From 1bf149d500a7b4576972e62f939235a28e302a9e Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 15 Dec 2023 18:51:58 +0900 Subject: [PATCH 1/8] fix(avoidance): unintentional path cut Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 783877e9c753d..0ea261ffb4130 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -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)); From 8ee8ca3ebba98fdd65e27fc70d7e2be86b738dc2 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 17 Dec 2023 21:07:12 +0900 Subject: [PATCH 2/8] fix(bpp): nouse pointer Signed-off-by: satoshi-ota --- .../src/behavior_path_planner_node.cpp | 3 +- .../src/planner_manager.cpp | 16 +++++----- .../dynamic_avoidance_module.cpp | 32 +++++++++---------- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 4828a0d62e7f6..85abf774d159e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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(output.path) + : planner_data->prev_output_path; path->header = planner_data->route_handler->getRouteHeader(); path->header.stamp = this->now(); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c867bffc8e9bd..36d5c7773fd24 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -198,7 +198,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da void PlannerManager::generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const { - if (!output.path || output.path->points.empty()) { + if (output.path.points.empty()) { RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!"); return; } @@ -206,20 +206,20 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); + const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path.points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (epsilon < std::abs(di.drivable_margin)) { // for single free space pull over utils::generateDrivableArea( - *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); + output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, + output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, is_driving_forward); } else { - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); const auto & dp = data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -228,19 +228,19 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( - *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, + output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, is_driving_forward); } // extract obstacles from drivable area - utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles); + utils::extractObstaclesFromDrivableArea(output.path, di.obstacles); } std::vector PlannerManager::getRequestModules( const BehaviorModuleOutput & previous_module_output) const { - if (!previous_module_output.path) { + if (previous_module_output.path.points.empty()) { RCLCPP_ERROR_STREAM( logger_, "Current module output is null. Skip candidate module check." << "\n - Approved module list: " << getNames(approved_module_ptrs_) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index de76166e068d3..99d7931dff907 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -264,12 +264,12 @@ bool DynamicAvoidanceModule::isExecutionRequested() const RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); const auto input_path = getPreviousModuleOutput().path; - if (!input_path || input_path->points.size() < 2) { + if (input_path.points.size() < 2) { return false; } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(input_path->points); + const auto is_driving_forward = motion_utils::isDrivingForward(input_path.points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -401,7 +401,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto input_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + const auto input_ref_path_points = getPreviousModuleOutput().reference_path.points; const auto prev_objects = target_objects_manager_.getValidObjects(); // 1. Rough filtering of target objects @@ -427,7 +427,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(input_path->points, predicted_object); + projectObstacleVelocityToTrajectory(input_path.points, predicted_object); if ( std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { @@ -435,7 +435,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleBetweenPaths(input_path->points, obj_path); + const double obj_angle = calcDiffAngleBetweenPaths(input_path.points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -455,7 +455,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.e. check if object lateral offset to ego's path is small enough - const double obj_dist_to_path = calcDistanceToPath(input_path->points, obj_pose.position); + const double obj_dist_to_path = calcDistanceToPath(input_path.points, obj_pose.position); const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { RCLCPP_INFO_EXPRESSION( @@ -499,7 +499,7 @@ void DynamicAvoidanceModule::updateTargetObjects() [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); // 2.a. check if object is not to be followed by ego - const double obj_angle = calcDiffAngleAgainstPath(input_path->points, object.pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, object.pose); const bool is_object_aligned_to_path = std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); @@ -513,13 +513,13 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.b. calculate which side object exists against ego's path - const bool is_object_left = isLeft(input_path->points, object.pose.position); + const bool is_object_left = isLeft(input_path.points, object.pose.position); const auto lat_lon_offset = - getLateralLongitudinalOffset(input_path->points, object.pose, object.shape); + getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); // 2.c. check if object will not cut in const bool will_object_cut_in = - willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset); + willObjectCutIn(input_path.points, obj_path, object.vel, lat_lon_offset); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -537,7 +537,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2.e. check time to collision const double time_to_collision = - calcTimeToCollision(input_path->points, object.pose, object.vel, lat_lon_offset); + calcTimeToCollision(input_path.points, object.pose, object.vel, lat_lon_offset); if ( (0 <= object.vel && parameters_->max_time_to_collision_overtaking_object < time_to_collision) || @@ -561,13 +561,13 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); const bool is_collision_left = - future_obj_pose ? isLeft(input_path->points, future_obj_pose->position) : is_object_left; + future_obj_pose ? isLeft(input_path.points, future_obj_pose->position) : is_object_left; // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path->points); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - input_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -898,7 +898,7 @@ double DynamicAvoidanceModule::calcValidStartLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const { - const auto input_path_points = getPreviousModuleOutput().path->points; + const auto input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); @@ -1027,7 +1027,7 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( return std::nullopt; } - auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + auto input_ref_path_points = getPreviousModuleOutput().reference_path.points; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_ref_path_points, object.pose.position); From e63a1ab30fa054191f24f7bfd7bed76770b3db24 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 17 Dec 2023 21:07:26 +0900 Subject: [PATCH 3/8] fix(bpp_common): nouse pointer Signed-off-by: satoshi-ota --- .../include/behavior_path_planner_common/data_manager.hpp | 4 ++-- .../interface/scene_module_interface.hpp | 2 +- .../behavior_path_planner_common/src/utils/path_utils.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 3dcfdcc2bdcef..d52906ef4684f 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -119,10 +119,10 @@ struct BehaviorModuleOutput BehaviorModuleOutput() = default; // path planed by module - PlanResult path{}; + PathWithLaneId path{}; // reference path planed by module - PlanResult reference_path{}; + PathWithLaneId reference_path{}; TurnSignalInfo turn_signal_info{}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index a5e2c3245f3c3..0ad30520815e8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -405,7 +405,7 @@ class SceneModuleInterface virtual BehaviorModuleOutput planWaitingApproval() { path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return getPreviousModuleOutput(); } diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index 994a7ba1a19d5..57a5743a5963b 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -642,8 +642,8 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); BehaviorModuleOutput output; - output.path = std::make_shared(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; @@ -692,8 +692,8 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; From a11fbe71e3bdabc8ce59b5bae1f5369d1008927c Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 17 Dec 2023 21:07:45 +0900 Subject: [PATCH 4/8] fix(bpp_side_shift): nouse pointer Signed-off-by: satoshi-ota --- .../behavior_path_side_shift_module/src/scene.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp index 69df77672cd96..6df8c1ec629c9 100644 --- a/planning/behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_side_shift_module/src/scene.cpp @@ -203,17 +203,17 @@ void SideShiftModule::updateData() ? planner_data_->self_odometry->pose.pose : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); if (prev_reference_.points.empty()) { - prev_reference_ = *getPreviousModuleOutput().path; + prev_reference_ = getPreviousModuleOutput().path; } - if (!getPreviousModuleOutput().reference_path) { + if (getPreviousModuleOutput().reference_path.points.empty()) { return; } const auto centerline_path = utils::calcCenterLinePath( planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); + getPreviousModuleOutput().reference_path); constexpr double resample_interval = 1.0; - const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path); + const auto backward_extened_path = extendBackwardLength(getPreviousModuleOutput().path); reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval); path_shifter_.setPath(reference_path_); @@ -286,7 +286,7 @@ BehaviorModuleOutput SideShiftModule::plan() output.reference_path = getPreviousModuleOutput().reference_path; prev_output_ = shifted_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); debug_data_.path_shifter = std::make_shared(path_shifter_); @@ -329,7 +329,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); prev_output_ = shifted_path; @@ -409,7 +409,8 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat { // for new architecture // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be // assigned without combining. - out.path = std::make_shared(output_path); + out.path = output_path; + out.reference_path = getPreviousModuleOutput().reference_path; out.drivable_area_info.drivable_lanes = expanded_lanes; out.drivable_area_info.is_already_expanded = true; } From 28548485fee3f24823444e549a02eabf01e3b476 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 18 Dec 2023 10:31:25 +0900 Subject: [PATCH 5/8] fix(bpp_avoidance): nouse pointer Signed-off-by: satoshi-ota --- .../src/scene.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 0ea261ffb4130..ebe42f73d538b 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/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,7 +233,7 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat }); // calc drivable bound - auto tmp_path = *getPreviousModuleOutput().path; + 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, @@ -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(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(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(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(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(); From 7b20507706abf5859b71a22b6e10ed0cfbdc44bc Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 18 Dec 2023 10:31:39 +0900 Subject: [PATCH 6/8] fix(bpp_lane_change): nouse pointer Signed-off-by: satoshi-ota --- .../utils/base_class.hpp | 11 ++++----- .../src/interface.cpp | 16 ++++++------- .../src/scene.cpp | 24 +++++++++---------- 3 files changed, 25 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index a7a35f07e5f30..4bfd461a0815f 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/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 & prev_module_reference_path, - const std::shared_ptr & 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; } }; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 1e80067842152..4a8eb34b088af 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/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(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(*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(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}, diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 21a600d11d83d..3adef17fc3b4d 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/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(abort_path_->path); - output.reference_path = std::make_shared(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(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(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); From 572d8638cb9d0166fb4793c5c45483c326d09dfc Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 18 Dec 2023 10:48:02 +0900 Subject: [PATCH 7/8] fix(bpp_goal_planner): nouse pointer Signed-off-by: satoshi-ota --- .../src/default_fixed_goal_planner.cpp | 5 ++- .../src/goal_planner_module.cpp | 33 +++++++++---------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index f028b8aff8b98..cbbe5f585dbe2 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/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(smoothed_path); + const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(output.path, planner_data); + output.path = smoothed_path; output.reference_path = getPreviousModuleOutput().reference_path; return output; } diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index b50387b3f6b0e..2ea6f1e100e91 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/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(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(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(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(*stop_path); + output.path = *stop_path; RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { - output.path = - std::make_shared(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(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(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(*output.path); + auto stop_path = std::make_shared(output.path); for (auto & point : stop_path->points) { point.point.longitudinal_velocity_mps = 0.0; } From 67dac3fecc9c460f4f2fa5a333c73e95472e6d60 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 18 Dec 2023 10:57:38 +0900 Subject: [PATCH 8/8] fix(bpp_start_planner): nouse pointer Signed-off-by: satoshi-ota --- .../src/start_planner_module.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 0d3892cde8023..c9fa15d855268 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 @@ -402,11 +402,11 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - output.path = std::make_shared(path); + output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -512,11 +512,11 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - output.path = std::make_shared(stop_path); + output.path = stop_path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_unique(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -1182,7 +1182,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; const PathWithLaneId stop_path = generateStopPath(); - output.path = std::make_shared(stop_path); + output.path = stop_path; setDrivableAreaInfo(output); @@ -1201,7 +1201,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() } path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return output; } @@ -1258,7 +1258,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(*output.path), + output.path, generateDrivableLanes(output.path), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info;