Skip to content

Commit

Permalink
perf(out_of_lane): use rtree to get stop lines and trajectory lanelets (
Browse files Browse the repository at this point in the history 
#1461)

perf(out_of_lane): use rtree to get stop lines and trajectory lanelets (autowarefoundation#8439)

Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem
maxime-clem authored Aug 16, 2024
1 parent c00012b commit 6d0db53
Show file tree
Hide file tree
Showing 6 changed files with 121 additions and 50 deletions.
63 changes: 33 additions & 30 deletions ...city_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,20 +15,23 @@
#include "filter_predicted_objects.hpp"

#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/geometry/boost_geometry.hpp>
#include <traffic_light_utils/traffic_light_utils.hpp>

#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/index/predicates.hpp>

#include <lanelet2_core/geometry/LaneletMap.h>
#include <lanelet2_core/primitives/BasicRegulatoryElements.h>

#include <algorithm>
#include <vector>

namespace autoware::motion_velocity_planner::out_of_lane
{
void cut_predicted_path_beyond_line(
autoware_perception_msgs::msg::PredictedPath & predicted_path,
const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
const universe_utils::LineString2d & stop_line, const double object_front_overhang)
{
if (predicted_path.path.empty() || stop_line.size() < 2) return;

Expand Down Expand Up @@ -58,43 +61,43 @@ void cut_predicted_path_beyond_line(
}
}

std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
const autoware_perception_msgs::msg::PredictedPath & path,
const std::shared_ptr<const PlannerData> planner_data)
std::optional<universe_utils::LineString2d> find_next_stop_line(
const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data)
{
lanelet::ConstLanelets lanelets;
lanelet::BasicLineString2d query_line;
for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y);
const auto query_results = lanelet::geometry::findWithin2d(
planner_data->route_handler->getLaneletMapPtr()->laneletLayer, query_line);
for (const auto & r : query_results) lanelets.push_back(r.second);
for (const auto & ll : lanelets) {
for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) {
const auto traffic_signal_stamped = planner_data->get_traffic_signal(element->id());
if (
traffic_signal_stamped.has_value() && element->stopLine().has_value() &&
traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) {
lanelet::BasicLineString2d stop_line;
for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y());
return stop_line;
universe_utils::LineString2d query_path;
for (const auto & p : path.path) query_path.emplace_back(p.position.x, p.position.y);
std::vector<StopLineNode> query_results;
ego_data.stop_lines_rtree.query(
boost::geometry::index::intersects(query_path), std::back_inserter(query_results));
auto earliest_intersecting_index = query_path.size();
std::optional<universe_utils::LineString2d> earliest_stop_line;
universe_utils::Segment2d path_segment;
for (const auto & [_, stop_line] : query_results) {
for (auto index = 0UL; index + 1 < query_path.size(); ++index) {
path_segment.first = query_path[index];
path_segment.second = query_path[index + 1];
if (boost::geometry::intersects(path_segment, stop_line.stop_line)) {
bool within_stop_line_lanelet =
std::any_of(stop_line.lanelets.begin(), stop_line.lanelets.end(), [&](const auto & ll) {
return boost::geometry::within(path_segment.first, ll.polygon2d().basicPolygon());
});
if (within_stop_line_lanelet) {
earliest_intersecting_index = std::min(index, earliest_intersecting_index);
earliest_stop_line = stop_line.stop_line;
}
}
}
}
return std::nullopt;
return earliest_stop_line;
}

void cut_predicted_path_beyond_red_lights(
autoware_perception_msgs::msg::PredictedPath & predicted_path,
const std::shared_ptr<const PlannerData> planner_data, const double object_front_overhang)
autoware_perception_msgs::msg::PredictedPath & predicted_path, const EgoData & ego_data,
const double object_front_overhang)
{
const auto stop_line = find_next_stop_line(predicted_path, planner_data);
const auto stop_line = find_next_stop_line(predicted_path, ego_data);
if (stop_line) {
// we use a longer stop line to also cut predicted paths that slightly go around the stop line
auto longer_stop_line = *stop_line;
const auto diff = stop_line->back() - stop_line->front();
longer_stop_line.front() -= diff * 0.5;
longer_stop_line.back() += diff * 0.5;
cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang);
cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
}
}

Expand Down Expand Up @@ -141,7 +144,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
if (params.objects_cut_predicted_paths_beyond_red_lights)
for (auto & predicted_path : predicted_paths)
cut_predicted_path_beyond_red_lights(
predicted_path, planner_data, filtered_object.shape.dimensions.x);
predicted_path, ego_data, filtered_object.shape.dimensions.x);
predicted_paths.erase(
std::remove_if(
predicted_paths.begin(), predicted_paths.end(),
Expand Down
15 changes: 7 additions & 8 deletions ...city_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,22 +30,21 @@ namespace autoware::motion_velocity_planner::out_of_lane
/// @param [in] object_front_overhang extra distance to cut ahead of the stop line
void cut_predicted_path_beyond_line(
autoware_perception_msgs::msg::PredictedPath & predicted_path,
const lanelet::BasicLineString2d & stop_line, const double object_front_overhang);
const universe_utils::LineString2d & stop_line, const double object_front_overhang);

/// @brief find the next red light stop line along the given path
/// @param [in] path predicted path to check for a stop line
/// @param [in] planner_data planner data with stop line information
/// @param [in] ego_data ego data with the stop lines information
/// @return the first red light stop line found along the path (if any)
std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
const autoware_perception_msgs::msg::PredictedPath & path,
const std::shared_ptr<const PlannerData> planner_data);
std::optional<universe_utils::LineString2d> find_next_stop_line(
const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data);

/// @brief cut predicted path beyond stop lines of red lights
/// @param [inout] predicted_path predicted path to cut
/// @param [in] planner_data planner data to get the map and traffic light information
/// @param [in] ego_data ego data with the stop lines information
void cut_predicted_path_beyond_red_lights(
autoware_perception_msgs::msg::PredictedPath & predicted_path,
const std::shared_ptr<const PlannerData> planner_data, const double object_front_overhang);
autoware_perception_msgs::msg::PredictedPath & predicted_path, const EgoData & ego_data,
const double object_front_overhang);

/// @brief filter predicted objects and their predicted paths
/// @param [in] planner_data planner data
Expand Down
13 changes: 9 additions & 4 deletions ...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@

#include <autoware/universe_utils/geometry/geometry.hpp>

#include <boost/geometry/algorithms/disjoint.hpp>

#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/geometry/LaneletMap.h>
#include <lanelet2_routing/RoutingGraph.h>

Expand Down Expand Up @@ -71,10 +74,12 @@ lanelet::ConstLanelets calculate_trajectory_lanelets(
lanelet::BasicLineString2d trajectory_ls;
for (const auto & p : ego_data.trajectory_points)
trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y);
for (const auto & dist_lanelet :
lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, trajectory_ls)) {
if (!contains_lanelet(trajectory_lanelets, dist_lanelet.second.id()))
trajectory_lanelets.push_back(dist_lanelet.second);
const auto candidates =
lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls));
for (const auto & ll : candidates) {
if (!boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) {
trajectory_lanelets.push_back(ll);
}
}
const auto missing_lanelets =
get_missing_lane_change_lanelets(trajectory_lanelets, route_handler);
Expand Down
52 changes: 50 additions & 2 deletions ...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,19 @@

#include <autoware/motion_utils/trajectory/interpolation.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/motion_velocity_planner_common/planner_data.hpp>
#include <autoware/universe_utils/geometry/boost_geometry.hpp>
#include <autoware/universe_utils/ros/parameter.hpp>
#include <autoware/universe_utils/ros/update_param.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <traffic_light_utils/traffic_light_utils.hpp>

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <lanelet2_core/geometry/LaneletMap.h>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/geometry/Polygon.h>
#include <lanelet2_core/primitives/BasicRegulatoryElements.h>

#include <map>
#include <memory>
Expand Down Expand Up @@ -155,6 +161,42 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset);
}

void prepare_stop_lines_rtree(
out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance)
{
std::vector<out_of_lane::StopLineNode> rtree_nodes;
const auto bbox = lanelet::BoundingBox2d(
lanelet::BasicPoint2d{
ego_data.pose.position.x - search_distance, ego_data.pose.position.y - search_distance},
lanelet::BasicPoint2d{
ego_data.pose.position.x + search_distance, ego_data.pose.position.y + search_distance});
out_of_lane::StopLineNode stop_line_node;
for (const auto & ll :
planner_data.route_handler->getLaneletMapPtr()->laneletLayer.search(bbox)) {
for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) {
const auto traffic_signal_stamped = planner_data.get_traffic_signal(element->id());
if (
traffic_signal_stamped.has_value() && element->stopLine().has_value() &&
traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) {
stop_line_node.second.stop_line.clear();
for (const auto & p : element->stopLine()->basicLineString()) {
stop_line_node.second.stop_line.emplace_back(p.x(), p.y());
}
// use a longer stop line to also cut predicted paths that slightly go around the stop line
const auto diff =
stop_line_node.second.stop_line.back() - stop_line_node.second.stop_line.front();
stop_line_node.second.stop_line.front() -= diff * 0.5;
stop_line_node.second.stop_line.back() += diff * 0.5;
stop_line_node.second.lanelets = planner_data.route_handler->getPreviousLanelets(ll);
stop_line_node.first =
boost::geometry::return_envelope<universe_utils::Box2d>(stop_line_node.second.stop_line);
rtree_nodes.push_back(stop_line_node);
}
}
}
ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()};
}

VelocityPlanningResult OutOfLaneModule::plan(
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
const std::shared_ptr<const PlannerData> planner_data)
Expand All @@ -169,6 +211,10 @@ VelocityPlanningResult OutOfLaneModule::plan(
autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
stopwatch.tic("preprocessing");
prepare_stop_lines_rtree(ego_data, *planner_data, 100.0);
const auto preprocessing_us = stopwatch.toc("preprocessing");

stopwatch.tic("calculate_trajectory_footprints");
const auto current_ego_footprint =
out_of_lane::calculate_current_ego_footprint(ego_data, params_, true);
Expand Down Expand Up @@ -289,21 +335,23 @@ VelocityPlanningResult OutOfLaneModule::plan(
RCLCPP_DEBUG(
logger_,
"Total time = %2.2fus\n"
"\tpreprocessing = %2.0fus\n"
"\tcalculate_lanelets = %2.0fus\n"
"\tcalculate_trajectory_footprints = %2.0fus\n"
"\tcalculate_overlapping_ranges = %2.0fus\n"
"\tfilter_pred_objects = %2.0fus\n"
"\tcalculate_decisions = %2.0fus\n"
"\tcalc_slowdown_points = %2.0fus\n"
"\tinsert_slowdown_points = %2.0fus\n",
total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
preprocessing_us, total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us,
calc_slowdown_points_us, insert_slowdown_points_us);
debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
virtual_wall_marker_creator.add_virtual_walls(
out_of_lane::debug::create_virtual_walls(debug_data_, params_));
virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
std::map<std::string, double> processing_times;
processing_times["preprocessing"] = preprocessing_us / 1000;
processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000;
processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000;
processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000;
Expand Down
26 changes: 21 additions & 5 deletions planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,17 @@
#define TYPES_HPP_

#include <autoware/route_handler/route_handler.hpp>
#include <autoware/universe_utils/geometry/boost_geometry.hpp>

#include <autoware_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <lanelet2_core/Forward.h>
#include <lanelet2_core/LaneletMap.h>

#include <algorithm>
Expand Down Expand Up @@ -172,6 +177,16 @@ struct OtherLane
}
};

namespace bgi = boost::geometry::index;
struct StopLine
{
universe_utils::LineString2d stop_line;
lanelet::ConstLanelets lanelets;
};
using StopLineNode = std::pair<universe_utils::Box2d, StopLine>;
using StopLinesRtree = bgi::rtree<StopLineNode, bgi::rstar<16>>;
using OutAreaRtree = bgi::rtree<std::pair<universe_utils::Box2d, size_t>, bgi::rstar<16>>;

/// @brief data related to the ego vehicle
struct EgoData
{
Expand All @@ -180,16 +195,17 @@ struct EgoData
double velocity{}; // [m/s]
double max_decel{}; // [m/s²]
geometry_msgs::msg::Pose pose{};
StopLinesRtree stop_lines_rtree;
};

/// @brief data needed to make decisions
struct DecisionInputs
{
OverlapRanges ranges{};
EgoData ego_data{};
autoware_perception_msgs::msg::PredictedObjects objects{};
std::shared_ptr<const route_handler::RouteHandler> route_handler{};
lanelet::ConstLanelets lanelets{};
OverlapRanges ranges;
EgoData ego_data;
autoware_perception_msgs::msg::PredictedObjects objects;
std::shared_ptr<const route_handler::RouteHandler> route_handler;
lanelet::ConstLanelets lanelets;
};

/// @brief debug data
Expand Down
2 changes: 1 addition & 1 deletion ...lanner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ TEST(TestCollisionDistance, CutPredictedPathBeyondLine)
{
using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line;
autoware_perception_msgs::msg::PredictedPath predicted_path;
lanelet::BasicLineString2d stop_line;
autoware::universe_utils::LineString2d stop_line;
double object_front_overhang = 0.0;
const auto eps = 1e-9;

Expand Down

0 comments on commit 6d0db53

Please sign in to comment.