Skip to content

Commit ea4915f

Browse files
delete unused function
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 9ed4a21 commit ea4915f

File tree

2 files changed

+0
-32
lines changed

2 files changed

+0
-32
lines changed

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -207,7 +207,6 @@ class StartPlannerModule : public SceneModuleInterface
207207

208208
bool isModuleRunning() const;
209209
bool isCurrentPoseOnMiddleOfTheRoad() const;
210-
bool isOverlapWithCenterLane() const;
211210
bool isPreventingRearVehicleFromPassingThrough() const;
212211

213212
bool isCloseToOriginalStartPose() const;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

-31
Original file line numberDiff line numberDiff line change
@@ -423,37 +423,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
423423
gap_between_ego_and_lane_border.value();
424424
}
425425

426-
bool StartPlannerModule::isOverlapWithCenterLane() const
427-
{
428-
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
429-
const auto current_lanes = utils::getCurrentLanes(planner_data_);
430-
const auto local_vehicle_footprint = vehicle_info_.createFootprint();
431-
const auto vehicle_footprint =
432-
transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose));
433-
for (const auto & current_lane : current_lanes) {
434-
std::vector<geometry_msgs::msg::Point> centerline_points;
435-
for (const auto & point : current_lane.centerline()) {
436-
geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point);
437-
centerline_points.push_back(center_point);
438-
}
439-
440-
for (size_t i = 0; i < centerline_points.size() - 1; ++i) {
441-
const auto & p1 = centerline_points.at(i);
442-
const auto & p2 = centerline_points.at(i + 1);
443-
for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) {
444-
const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d());
445-
const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d());
446-
const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4);
447-
448-
if (intersection.has_value()) {
449-
return true;
450-
}
451-
}
452-
}
453-
}
454-
return false;
455-
}
456-
457426
bool StartPlannerModule::isCloseToOriginalStartPose() const
458427
{
459428
const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();

0 commit comments

Comments
 (0)