Skip to content

Commit 765e17f

Browse files
solve bugs when merging with lane
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent e5f6309 commit 765e17f

File tree

2 files changed

+40
-4
lines changed

2 files changed

+40
-4
lines changed

planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -205,11 +205,9 @@ class SamplingPlannerModule : public SceneModuleInterface
205205
const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
206206
const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
207207
const double length_to_goal = std::abs(goal_arc.length - ego_arc.length);
208-
const double min_target_length = *std::min_element(
209-
internal_params_->sampling.target_lengths.begin(),
210-
internal_params_->sampling.target_lengths.end());
211208

212-
if (length_to_goal < min_target_length) return isReferencePathSafe();
209+
constexpr double epsilon = 1E-5;
210+
if (length_to_goal < epsilon) return isReferencePathSafe();
213211

214212
const auto nearest_index =
215213
motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);

planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp

+38
Original file line numberDiff line numberDiff line change
@@ -172,6 +172,44 @@ SamplingPlannerModule::SamplingPlannerModule(
172172

173173
bool SamplingPlannerModule::isExecutionRequested() const
174174
{
175+
const auto & p = planner_data_->parameters;
176+
const auto ego_pose = planner_data_->self_odometry->pose.pose;
177+
const auto goal_pose = planner_data_->route_handler->getGoalPose();
178+
179+
lanelet::ConstLanelet current_lane;
180+
181+
if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, &current_lane)) {
182+
RCLCPP_ERROR(
183+
rclcpp::get_logger("behavior_path_planner").get_child("utils"),
184+
"failed to find closest lanelet within route!!!");
185+
return false;
186+
}
187+
const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
188+
current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
189+
// expand drivable lanes
190+
std::vector<DrivableLanes> drivable_lanes{};
191+
192+
std::for_each(
193+
current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
194+
drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
195+
});
196+
197+
lanelet::ConstLanelets current_lanes;
198+
199+
for (auto & d : drivable_lanes) {
200+
current_lanes.push_back(d.right_lane);
201+
current_lanes.push_back(d.left_lane);
202+
current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
203+
}
204+
lanelet::ConstLanelet closest_lanelet_to_ego;
205+
lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
206+
lanelet::ConstLanelet closest_lanelet_to_goal;
207+
lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
208+
const bool ego_and_goal_on_same_lanelet =
209+
closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id();
210+
211+
if (!ego_and_goal_on_same_lanelet) return false;
212+
175213
if (getPreviousModuleOutput().reference_path.points.empty()) {
176214
return false;
177215
}

0 commit comments

Comments
 (0)