Skip to content

Commit

Permalink
fix(behavior_path_planner): reduce obj indices call in lane change (#…
Browse files Browse the repository at this point in the history 
…2726)

* fix(behavior_path_planner): reduce obj indices call in lane change

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* remove unused functions

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* add continue for the intersect in current

* replace target and current lanes

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* change isLaneChangePathSafe arguments

Signed-off-by: Muhammad Zulfaqar <[email protected]>

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Signed-off-by: Muhammad Zulfaqar <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 authored Jan 24, 2023
1 parent 43a80d5 commit 7d26232
Show file tree
Hide file tree
Showing 6 changed files with 101 additions and 104 deletions.
2 changes: 1 addition & 1 deletion ...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ bool selectSafePath(
std::unordered_map<std::string, CollisionCheckDebug> & debug_data);

bool isLaneChangePathSafe(
const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
const size_t current_seg_idx, const Twist & current_twist,
Expand Down
12 changes: 0 additions & 12 deletions planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -289,14 +289,6 @@ double calcLongitudinalDistanceFromEgoToObjects(
const Pose & ego_pose, double base_link2front, double base_link2rear,
const PredictedObjects & dynamic_objects);

/**
* @brief Get index of the obstacles inside the lanelets with start and end length
* @return Indices corresponding to the obstacle inside the lanelets
*/
std::vector<size_t> filterObjectIndicesByLanelets(
const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets,
const double start_arc_length, const double end_arc_length);

/**
* @brief Separate index of the obstacles into two part based on whether the object is within
* lanelet.
Expand All @@ -313,10 +305,6 @@ std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanel
std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);

std::vector<size_t> filterObjectsIndicesByPath(
const PredictedObjects & objects, const std::vector<size_t> & object_indices,
const PathWithLaneId & ego_path, const double vehicle_width);

PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v);

PredictedObjects filterObjectsByVelocity(
Expand Down
5 changes: 2 additions & 3 deletions ...ehavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -698,9 +698,8 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_
path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
common_parameters.ego_nearest_yaw_threshold);
return lane_change_utils::isLaneChangePathSafe(
path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx,
current_twist, common_parameters, *parameters_,
common_parameters.expected_front_deceleration_for_abort,
path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort,
common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data,
false, status_.lane_change_path.acceleration);
}
Expand Down
5 changes: 2 additions & 3 deletions planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -694,9 +694,8 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons
path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
common_parameters.ego_nearest_yaw_threshold);
return lane_change_utils::isLaneChangePathSafe(
path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx,
current_twist, common_parameters, *parameters_,
common_parameters.expected_front_deceleration_for_abort,
path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort,
common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data,
false, status_.lane_change_path.acceleration);
}
Expand Down
115 changes: 96 additions & 19 deletions planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "behavior_path_planner/parameters.hpp"
#include "behavior_path_planner/path_utilities.hpp"
#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
#include "behavior_path_planner/utilities.hpp"

Expand All @@ -34,6 +35,85 @@
#include <string>
#include <vector>

namespace
{

using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using behavior_path_planner::util::calcObjectPolygon;
using behavior_path_planner::util::getHighestProbLabel;
using geometry_msgs::msg::Pose;
using tier4_autoware_utils::LineString2d;
using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;

void filterObjectIndices(
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & ego_path,
const Pose & current_pose, const double forward_path_length, const double filter_width,
std::vector<size_t> & current_lane_obj_indices, std::vector<size_t> & target_lane_obj_indices,
std::vector<size_t> & others_obj_indices, const bool ignore_unknown_obj = false)
{
// Reserve maximum amount possible
current_lane_obj_indices.reserve(objects.objects.size());
target_lane_obj_indices.reserve(objects.objects.size());
others_obj_indices.reserve(objects.objects.size());

const auto get_basic_polygon =
[](const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) {
const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist);
return lanelet::utils::to2D(polygon_3d).basicPolygon();
};
const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
const auto current_polygon =
get_basic_polygon(current_lanes, arc.length, arc.length + forward_path_length);
const auto target_polygon =
get_basic_polygon(target_lanes, 0.0, std::numeric_limits<double>::max());
LineString2d ego_path_linestring;
ego_path_linestring.reserve(ego_path.points.size());
for (const auto & pt : ego_path.points) {
const auto & position = pt.point.pose.position;
boost::geometry::append(ego_path_linestring, Point2d(position.x, position.y));
}

for (size_t i = 0; i < objects.objects.size(); ++i) {
const auto & obj = objects.objects.at(i);

if (ignore_unknown_obj) {
const auto t = getHighestProbLabel(obj.classification);
if (t == ObjectClassification::UNKNOWN) {
continue;
}
}

Polygon2d obj_polygon;
if (!calcObjectPolygon(obj, &obj_polygon)) {
RCLCPP_ERROR_STREAM(
rclcpp::get_logger("behavior_path_planner").get_child("lane_change"),
"Failed to calcObjectPolygon...!!!");
continue;
}

if (boost::geometry::intersects(current_polygon, obj_polygon)) {
const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring);

if (distance < filter_width) {
current_lane_obj_indices.push_back(i);
continue;
}
}

const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon);
if (is_intersect_with_target) {
target_lane_obj_indices.push_back(i);
} else {
others_obj_indices.push_back(i);
}
}
}
} // namespace

namespace behavior_path_planner::lane_change_utils
{
using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
Expand Down Expand Up @@ -337,7 +417,7 @@ bool selectSafePath(
common_parameters.ego_nearest_yaw_threshold);
Pose ego_pose_before_collision;
if (isLaneChangePathSafe(
path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx,
path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx,
current_twist, common_parameters, ros_parameters,
common_parameters.expected_front_deceleration,
common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true,
Expand Down Expand Up @@ -403,7 +483,7 @@ bool hasEnoughDistance(
}

bool isLaneChangePathSafe(
const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
const size_t current_seg_idx, const Twist & current_twist,
Expand All @@ -417,6 +497,7 @@ bool isLaneChangePathSafe(
return true;
}

const auto & path = lane_change_path.path;
if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) {
return false;
}
Expand All @@ -435,24 +516,20 @@ bool isLaneChangePathSafe(
const auto prepare_phase_ignore_target_speed_thresh =
lane_change_parameters.prepare_phase_ignore_target_speed_thresh;

const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose);

// find obstacle in lane change target lanes
// retrieve lanes that are merging target lanes as well
const auto [target_lane_object_indices, others] =
util::separateObjectIndicesByLanelets(*dynamic_objects, target_lanes);

// find objects in current lane
const double check_distance = common_parameters.forward_path_length;
const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets(
*dynamic_objects, current_lanes, arc.length, arc.length + check_distance);

const double lateral_buffer = (use_buffer) ? 0.5 : 0.0;
const auto & vehicle_info = common_parameters.vehicle_info;
const auto & vehicle_width = common_parameters.vehicle_width;
const auto current_lane_object_indices = util::filterObjectsIndicesByPath(
*dynamic_objects, current_lane_object_indices_lanelet, path,
vehicle_width / 2 + lateral_buffer);

std::vector<size_t> current_lane_object_indices{};
std::vector<size_t> target_lane_object_indices{};
std::vector<size_t> other_lane_object_indices{};
{
const auto lateral_buffer = (use_buffer) ? 0.5 : 0.0;
const auto current_obj_filtering_buffer = lateral_buffer + common_parameters.vehicle_width / 2;

filterObjectIndices(
*dynamic_objects, lane_change_path.reference_lanelets, lane_change_path.target_lanelets, path,
current_pose, common_parameters.forward_path_length, current_obj_filtering_buffer,
current_lane_object_indices, target_lane_object_indices, other_lane_object_indices, true);
}

const auto assignDebugData = [](const PredictedObject & obj) {
CollisionCheckDebug debug;
Expand Down
66 changes: 0 additions & 66 deletions planning/behavior_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -490,48 +490,6 @@ double calcLongitudinalDistanceFromEgoToObjects(
return min_distance;
}

// only works with consecutive lanes
std::vector<size_t> filterObjectIndicesByLanelets(
const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
const double start_arc_length, const double end_arc_length)
{
std::vector<size_t> indices;
if (target_lanelets.empty()) {
return {};
}
const auto polygon =
lanelet::utils::getPolygonFromArcLength(target_lanelets, start_arc_length, end_arc_length);
const auto polygon2d = lanelet::utils::to2D(polygon).basicPolygon();
if (polygon2d.empty()) {
// no lanelet polygon
return {};
}

for (size_t i = 0; i < objects.objects.size(); i++) {
const auto & obj = objects.objects.at(i);
// create object polygon
Polygon2d obj_polygon;
if (!calcObjectPolygon(obj, &obj_polygon)) {
continue;
}
// create lanelet polygon
Polygon2d lanelet_polygon;
lanelet_polygon.outer().reserve(polygon2d.size() + 1);
for (const auto & lanelet_point : polygon2d) {
lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y());
}

lanelet_polygon.outer().push_back(lanelet_polygon.outer().front());

// check the object does not intersect the lanelet
if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) {
indices.push_back(i);
continue;
}
}
return indices;
}

std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
{
Expand Down Expand Up @@ -674,30 +632,6 @@ std::vector<double> calcObjectsDistanceToPath(
return distance_array;
}

std::vector<size_t> filterObjectsIndicesByPath(
const PredictedObjects & objects, const std::vector<size_t> & object_indices,
const PathWithLaneId & ego_path, const double vehicle_width)
{
std::vector<size_t> indices;
const auto ego_path_point_array = convertToGeometryPointArray(ego_path);
for (const auto & i : object_indices) {
Polygon2d obj_polygon;
if (!calcObjectPolygon(objects.objects.at(i), &obj_polygon)) {
continue;
}
LineString2d ego_path_line;
ego_path_line.reserve(ego_path_point_array.size());
for (const auto & ego_path_point : ego_path_point_array) {
boost::geometry::append(ego_path_line, Point2d(ego_path_point.x, ego_path_point.y));
}
const double distance = boost::geometry::distance(obj_polygon, ego_path_line);
if (distance < vehicle_width) {
indices.push_back(i);
}
}
return indices;
}

PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path)
{
PathWithLaneId filtered_path;
Expand Down

0 comments on commit 7d26232

Please sign in to comment.