Skip to content

Commit 7778c7b

Browse files
committed
fix(avoidance): limit drivable lane only when the ego in on original lane
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 3a10282 commit 7778c7b

File tree

3 files changed

+44
-5
lines changed

3 files changed

+44
-5
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj);
3636
double calcShiftLength(
3737
const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
3838

39+
bool isWithinLanes(
40+
const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data);
41+
3942
bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);
4043

4144
bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);

planning/behavior_path_avoidance_module/src/scene.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,6 @@
3232
#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
3333
#include <tier4_planning_msgs/msg/detail/avoidance_debug_factor__struct.hpp>
3434

35-
#include <boost/geometry/algorithms/centroid.hpp>
36-
3735
#include <algorithm>
3836
#include <limits>
3937
#include <memory>
@@ -223,12 +221,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
223221
utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
224222

225223
// expand drivable lanes
226-
const auto has_shift_point = !path_shifter_.getShiftLines().empty();
227-
const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted();
224+
const auto is_within_current_lane =
225+
utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_);
228226
std::for_each(
229227
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
230228
data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
231-
lanelet, planner_data_, parameters_, in_avoidance_maneuver));
229+
lanelet, planner_data_, parameters_, !is_within_current_lane));
232230
});
233231

234232
// calc drivable bound

planning/behavior_path_avoidance_module/src/utils.cpp

+38
Original file line numberDiff line numberDiff line change
@@ -943,6 +943,44 @@ double calcShiftLength(
943943
return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
944944
}
945945

946+
bool isWithinLanes(
947+
const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data)
948+
{
949+
const auto & rh = planner_data->route_handler;
950+
const auto & ego_pose = planner_data->self_odometry->pose.pose;
951+
const auto transform = tier4_autoware_utils::pose2transform(ego_pose);
952+
const auto footprint = tier4_autoware_utils::transformVector(
953+
planner_data->parameters.vehicle_info.createFootprint(), transform);
954+
955+
lanelet::ConstLanelet closest_lanelet{};
956+
if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) {
957+
return true;
958+
}
959+
960+
lanelet::ConstLanelets concat_lanelets{};
961+
962+
// push previous lanelet
963+
lanelet::ConstLanelets prev_lanelet;
964+
if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) {
965+
concat_lanelets.push_back(prev_lanelet.front());
966+
}
967+
968+
// push nearest lanelet
969+
{
970+
concat_lanelets.push_back(closest_lanelet);
971+
}
972+
973+
// push next lanelet
974+
lanelet::ConstLanelet next_lanelet;
975+
if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) {
976+
concat_lanelets.push_back(next_lanelet);
977+
}
978+
979+
const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets);
980+
981+
return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon());
982+
}
983+
946984
bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length)
947985
{
948986
/**

0 commit comments

Comments
 (0)