Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(behavior_path_planner): add empty guard to combinePath #5147

Merged
merged 1 commit into from
Sep 27, 2023
Merged

fix(behavior_path_planner): add empty guard to combinePath #5147

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,6 @@ using geometry_msgs::msg::Twist;
using visualization_msgs::msg::Marker;
using visualization_msgs::msg::MarkerArray;

// TODO(sugahara) move to util
PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
lanelet::ConstLanelets getPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance);
Expand Down
2 changes: 0 additions & 2 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,6 @@ std::vector<std::vector<int64_t>> getSortedLaneIds(
const RouteHandler & route_handler, const Pose & current_pose,
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes);

PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);

lanelet::ConstLanelets getTargetPreferredLanes(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const Direction & direction,
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,8 @@ PathWithLaneId calcCenterLinePath(
const std::shared_ptr<const PlannerData> & planner_data, const Pose & ref_pose,
const double longest_dist_to_shift_line,
const std::optional<PathWithLaneId> & prev_module_path = std::nullopt);

PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
} // namespace behavior_path_planner::utils

#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_
1 change: 0 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,6 @@ using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
using route_handler::RouteHandler;

PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2);
PathWithLaneId getBackwardPath(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const Pose & backed_pose, const double velocity);
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()

const auto found_extended_path = extendPath();
if (found_extended_path) {
*output.path = utils::lane_change::combineReferencePath(*output.path, *found_extended_path);
*output.path = utils::combinePath(*output.path, *found_extended_path);
}
extendOutputDrivableArea(output);
output.reference_path = std::make_shared<PathWithLaneId>(getReferencePath());
Expand Down
10 changes: 0 additions & 10 deletions planning/behavior_path_planner/src/utils/goal_planner/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,16 +44,6 @@ namespace behavior_path_planner
{
namespace goal_planner_utils
{
PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2)
{
PathWithLaneId path;
path.points.insert(path.points.end(), path1.points.begin(), path1.points.end());

// skip overlapping point
path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end());

return path;
}

lanelet::ConstLanelets getPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
Expand Down
13 changes: 1 addition & 12 deletions planning/behavior_path_planner/src/utils/lane_change/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,17 +180,6 @@ std::vector<double> getAccelerationValues(
return sampled_values;
}

PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2)
{
PathWithLaneId path;
path.points.insert(path.points.end(), path1.points.begin(), path1.points.end());

// skip overlapping point
path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end());

return path;
}

lanelet::ConstLanelets getTargetPreferredLanes(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const Direction & direction,
Expand Down Expand Up @@ -355,7 +344,7 @@ std::optional<LaneChangePath> constructCandidatePath(
return std::nullopt;
}

candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path);
candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path);
candidate_path.shifted_path = shifted_path;

return std::optional<LaneChangePath>{candidate_path};
Expand Down
21 changes: 21 additions & 0 deletions planning/behavior_path_planner/src/utils/path_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -561,4 +561,25 @@ PathWithLaneId calcCenterLinePath(

return centerline_path;
}

PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2)
{
if (path1.points.empty()) {
return path2;
}
if (path2.points.empty()) {
return path1;
}

PathWithLaneId path{};
path.points.insert(path.points.end(), path1.points.begin(), path1.points.end());

// skip overlapping point
path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end());

PathWithLaneId filtered_path = path;
filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points);
return filtered_path;
}

} // namespace behavior_path_planner::utils
3 changes: 1 addition & 2 deletions planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,7 @@ boost::optional<PullOutPath> FreespacePullOut::plan(const Pose & start_pose, con

const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
last_path = utils::resamplePathWithSpline(
start_planner_utils::combineReferencePath(last_path, road_center_line_path),
parameters_.center_line_path_interval);
utils::combinePath(last_path, road_center_line_path), parameters_.center_line_path_interval);

const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps;
utils::correctDividedPathVelocity(partial_paths);
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp"

#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner/utils/path_utils.hpp"
#include "behavior_path_planner/utils/start_planner/util.hpp"
#include "behavior_path_planner/utils/utils.hpp"

Expand All @@ -26,7 +27,6 @@ using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
namespace behavior_path_planner
{
using start_planner_utils::combineReferencePath;
using start_planner_utils::getPullOutLanes;

GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters)
Expand Down Expand Up @@ -106,7 +106,7 @@ boost::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, con
std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path)));
} else {
const auto partial_paths = planner_.getPaths();
const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1));
const auto combined_path = utils::combinePath(partial_paths.at(0), partial_paths.at(1));
output.partial_paths.push_back(combined_path);

// Calculate the acceleration required to reach the forward parking velocity at the center of
Expand Down
1 change: 0 additions & 1 deletion planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
namespace behavior_path_planner
{
using start_planner_utils::combineReferencePath;
using start_planner_utils::getPullOutLanes;

ShiftPullOut::ShiftPullOut(
Expand Down
13 changes: 0 additions & 13 deletions planning/behavior_path_planner/src/utils/start_planner/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,19 +38,6 @@

namespace behavior_path_planner::start_planner_utils
{
PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2)
{
PathWithLaneId path;
path.points.insert(path.points.end(), path1.points.begin(), path1.points.end());

// skip overlapping point
path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end());

PathWithLaneId filtered_path = path;
filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points);
return filtered_path;
}

PathWithLaneId getBackwardPath(
const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes,
const Pose & current_pose, const Pose & backed_pose, const double velocity)
Expand Down