From 02a48c158e512331a74673ee4f764647b2b233ab Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Tue, 20 Feb 2024 21:22:57 +0900
Subject: [PATCH 1/6] add function to cut a predicted path beyond a red light
stop line
TODO: check perf, take object footprint size into account when cutting,
add param
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../package.xml | 1 +
.../src/filter_predicted_objects.hpp | 106 +++++++++++++++++-
.../src/scene_out_of_lane.cpp | 4 +-
.../src/types.hpp | 6 +-
4 files changed, 108 insertions(+), 9 deletions(-)
diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml
index 43dea2f2153ff..b61441132969c 100644
--- a/planning/behavior_velocity_out_of_lane_module/package.xml
+++ b/planning/behavior_velocity_out_of_lane_module/package.xml
@@ -27,6 +27,7 @@
<depend>tf2</depend>
<depend>tier4_autoware_utils</depend>
<depend>tier4_planning_msgs</depend>
+ <depend>traffic_light_utils</depend>
<depend>vehicle_info_util</depend>
<depend>visualization_msgs</depend>
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
index 7bad4974ea9af..96aa474380afb 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -18,23 +18,112 @@
#include "types.hpp"
#include <motion_utils/trajectory/trajectory.hpp>
+#include <traffic_light_utils/traffic_light_utils.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
+
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+
+#include <algorithm>
+#include <optional>
#include <string>
+#include <vector>
+
+// for writing the svg file
+#include <fstream>
+#include <iostream>
+// for the geometry types
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+// for the svg mapper
+#include <boost/geometry/io/svg/svg_mapper.hpp>
+#include <boost/geometry/io/svg/write.hpp>
namespace behavior_velocity_planner::out_of_lane
{
+void cut_predicted_path_beyond_line(
+ autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+ const lanelet::BasicLineString2d & stop_line)
+{
+ lanelet::BasicSegment2d path_segment;
+ path_segment.first.x() = predicted_path.path.front().position.x;
+ path_segment.first.y() = predicted_path.path.front().position.y;
+ for (auto i = 1; i < predicted_path.path.size(); ++i) {
+ path_segment.second.x() = predicted_path.path[i].position.x;
+ path_segment.second.y() = predicted_path.path[i].position.y;
+ if (boost::geometry::intersects(stop_line, path_segment)) {
+ predicted_path.path.resize(i - std::min(i, 10));
+ return;
+ }
+ path_segment.first = path_segment.second;
+ }
+}
+
+std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
+ const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data,
+ const double max_length)
+{
+ lanelet::ConstLanelets lanelets;
+ lanelet::BasicPoint2d query_point;
+ for (const auto & p : path.path) {
+ query_point.x() = p.position.x;
+ query_point.y() = p.position.y;
+ const auto results = lanelet::geometry::findWithin2d(
+ planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_point);
+ for (const auto & r : 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.getTrafficSignal(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;
+ }
+ }
+ }
+ return std::nullopt;
+}
+
+/// @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
+void cut_predicted_path_beyond_red_lights(
+ autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+ const PlannerData & planner_data)
+{
+ // Declare a stream and an SVG mapper
+ std::ofstream svg("/home/mclement/Pictures/image.svg"); // /!\ CHANGE PATH
+ boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> mapper(svg, 400, 400);
+ const lanelet::BasicPoint2d first_point = {
+ predicted_path.path.front().position.x, predicted_path.path.front().position.y};
+ const auto stop_line = find_next_stop_line(
+ predicted_path, planner_data,
+ motion_utils::calcSignedArcLength(predicted_path.path, 0, predicted_path.path.size() - 1));
+ lanelet::BasicLineString2d path_ls;
+ for (const auto & p : predicted_path.path) path_ls.emplace_back(p.position.x, p.position.y);
+ if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line);
+ lanelet::BasicLineString2d cut_path_ls;
+ for (const auto & p : predicted_path.path) cut_path_ls.emplace_back(p.position.x, p.position.y);
+ mapper.add(cut_path_ls);
+ mapper.map(first_point, "opacity:1.0;fill:red;stroke:green;stroke-width:2", 2);
+ mapper.map(path_ls, "opacity:0.3;fill:black;stroke:black;stroke-width:2");
+ mapper.map(cut_path_ls, "opacity:0.3;fill:red;stroke:red;stroke-width:2");
+}
+
/// @brief filter predicted objects and their predicted paths
-/// @param [in] objects predicted objects to filter
+/// @param [in] planner_data planner data
/// @param [in] ego_data ego data
/// @param [in] params parameters
/// @return filtered predicted objects
autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
- const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data,
- const PlannerParam & params)
+ const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params)
{
autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
- filtered_objects.header = objects.header;
- for (const auto & object : objects.objects) {
+ filtered_objects.header = planner_data.predicted_objects->header;
+ for (const auto & object : planner_data.predicted_objects->objects) {
const auto is_pedestrian =
std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
@@ -60,7 +149,14 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
const auto new_end =
std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
predicted_paths.erase(new_end, predicted_paths.end());
+ if (true || params.objects_cut_predicted_paths_beyond_red_lights)
+ for (auto & predicted_path : predicted_paths)
+ cut_predicted_path_beyond_red_lights(predicted_path, planner_data);
+ predicted_paths.erase(
+ std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path),
+ predicted_paths.end());
}
+
if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty())
filtered_objects.objects.push_back(filtered_object);
}
diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
index ef4434e235244..6ce8338401e22 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
@@ -113,7 +113,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
DecisionInputs inputs;
inputs.ranges = ranges;
inputs.ego_data = ego_data;
- inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_);
+ inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_);
inputs.route_handler = planner_data_->route_handler_;
inputs.lanelets = other_lanelets;
const auto decisions = calculate_decisions(inputs, params_, logger_);
@@ -157,7 +157,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
}
if (point_to_insert) {
prev_inserted_point_ = point_to_insert;
- RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
+ RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
debug_data_.slowdowns = {*point_to_insert};
auto path_idx = motion_utils::findNearestSegmentIndex(
path->points, point_to_insert->point.point.pose.position) +
diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp
index 34bd52634ce7b..d482870e5ce18 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp
@@ -47,8 +47,10 @@ struct PlannerParam
double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range
bool objects_use_predicted_paths; // whether to use the objects' predicted paths
- double objects_min_vel; // [m/s] objects lower than this velocity will be ignored
- double objects_min_confidence; // minimum confidence to consider a predicted path
+ bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red
+ // lights' stop lines
+ double objects_min_vel; // [m/s] objects lower than this velocity will be ignored
+ double objects_min_confidence; // minimum confidence to consider a predicted path
double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in
// the other lane
From aa75a5b44321fffeb7b4c90943bbe55c1f4d7471 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Thu, 22 Feb 2024 15:14:33 +0900
Subject: [PATCH 2/6] account for predicted object front overhang+dont
extrapolate pred paths
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../src/decisions.cpp | 13 +++-
.../src/filter_predicted_objects.hpp | 62 +++++++++++--------
.../src/scene_out_of_lane.cpp | 9 ++-
3 files changed, 53 insertions(+), 31 deletions(-)
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
index be72109db4548..8c2466bf9cd37 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
@@ -73,6 +73,7 @@ std::optional<std::pair<double, double>> object_time_to_range(
if (!object_is_incoming(object_point, route_handler, range.lane)) return {};
const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer;
+ const auto max_lon_deviation = object.shape.dimensions.x / 2.0;
auto worst_enter_time = std::optional<double>();
auto worst_exit_time = std::optional<double>();
@@ -121,7 +122,17 @@ std::optional<std::pair<double, double>> object_time_to_range(
max_deviation);
continue;
}
- // else we rely on the interpolation to estimate beyond the end of the predicted path
+ const auto is_last_predicted_path_point =
+ (exit_segment_idx + 2 == unique_path_points.size() ||
+ enter_segment_idx + 2 == unique_path_points.size());
+ const auto does_not_reach_overlap =
+ is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation);
+ if (does_not_reach_overlap) {
+ RCLCPP_DEBUG(
+ logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n",
+ std::min(exit_offset, enter_offset), max_lon_deviation);
+ continue;
+ }
const auto same_driving_direction_as_ego = enter_time < exit_time;
if (same_driving_direction_as_ego) {
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
index 96aa474380afb..19b6af66db5fb 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -43,35 +43,41 @@ namespace behavior_velocity_planner::out_of_lane
{
void cut_predicted_path_beyond_line(
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
- const lanelet::BasicLineString2d & stop_line)
+ const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
{
+ auto stop_line_idx = 0UL;
+ bool found = false;
lanelet::BasicSegment2d path_segment;
path_segment.first.x() = predicted_path.path.front().position.x;
path_segment.first.y() = predicted_path.path.front().position.y;
- for (auto i = 1; i < predicted_path.path.size(); ++i) {
- path_segment.second.x() = predicted_path.path[i].position.x;
- path_segment.second.y() = predicted_path.path[i].position.y;
+ for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) {
+ path_segment.second.x() = predicted_path.path[stop_line_idx].position.x;
+ path_segment.second.y() = predicted_path.path[stop_line_idx].position.y;
if (boost::geometry::intersects(stop_line, path_segment)) {
- predicted_path.path.resize(i - std::min(i, 10));
- return;
+ found = true;
+ break;
}
path_segment.first = path_segment.second;
}
+ auto cut_idx = stop_line_idx;
+ double arc_length = 0;
+ while (cut_idx > 0 && arc_length < object_front_overhang) {
+ arc_length += tier4_autoware_utils::calcDistance2d(
+ predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
+ --cut_idx;
+ }
+ predicted_path.path.resize(cut_idx);
}
std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
- const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data,
- const double max_length)
+ const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data)
{
lanelet::ConstLanelets lanelets;
- lanelet::BasicPoint2d query_point;
- for (const auto & p : path.path) {
- query_point.x() = p.position.x;
- query_point.y() = p.position.y;
- const auto results = lanelet::geometry::findWithin2d(
- planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_point);
- for (const auto & r : results) lanelets.push_back(r.second);
- }
+ 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.getTrafficSignal(element->id());
@@ -92,24 +98,22 @@ std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
/// @param [in] planner_data planner data to get the map and traffic light information
void cut_predicted_path_beyond_red_lights(
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
- const PlannerData & planner_data)
+ const PlannerData & planner_data, const double object_front_overhang, int i)
{
// Declare a stream and an SVG mapper
- std::ofstream svg("/home/mclement/Pictures/image.svg"); // /!\ CHANGE PATH
+ std::ofstream svg(
+ "/home/mclement/Pictures/image" + std::to_string(i) + ".svg"); // /!\ CHANGE PATH
boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> mapper(svg, 400, 400);
- const lanelet::BasicPoint2d first_point = {
- predicted_path.path.front().position.x, predicted_path.path.front().position.y};
- const auto stop_line = find_next_stop_line(
- predicted_path, planner_data,
- motion_utils::calcSignedArcLength(predicted_path.path, 0, predicted_path.path.size() - 1));
+ const auto stop_line = find_next_stop_line(predicted_path, planner_data);
lanelet::BasicLineString2d path_ls;
for (const auto & p : predicted_path.path) path_ls.emplace_back(p.position.x, p.position.y);
- if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line);
+ if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
lanelet::BasicLineString2d cut_path_ls;
for (const auto & p : predicted_path.path) cut_path_ls.emplace_back(p.position.x, p.position.y);
mapper.add(cut_path_ls);
- mapper.map(first_point, "opacity:1.0;fill:red;stroke:green;stroke-width:2", 2);
+ if (stop_line) mapper.add(*stop_line);
mapper.map(path_ls, "opacity:0.3;fill:black;stroke:black;stroke-width:2");
+ if (stop_line) mapper.map(*stop_line, "opacity:0.5;fill:black;stroke:red;stroke-width:2");
mapper.map(cut_path_ls, "opacity:0.3;fill:red;stroke:red;stroke-width:2");
}
@@ -149,11 +153,15 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
const auto new_end =
std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
predicted_paths.erase(new_end, predicted_paths.end());
+ auto i = 0;
if (true || params.objects_cut_predicted_paths_beyond_red_lights)
for (auto & predicted_path : predicted_paths)
- cut_predicted_path_beyond_red_lights(predicted_path, planner_data);
+ cut_predicted_path_beyond_red_lights(
+ predicted_path, planner_data, filtered_object.shape.dimensions.x, i++);
predicted_paths.erase(
- std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path),
+ std::remove_if(
+ predicted_paths.begin(), predicted_paths.end(),
+ [](const auto & p) { return p.path.empty(); }),
predicted_paths.end());
}
diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
index 6ce8338401e22..47020fb1cb623 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
@@ -109,13 +109,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_);
const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges");
// Calculate stop and slowdown points
- stopwatch.tic("calculate_decisions");
DecisionInputs inputs;
inputs.ranges = ranges;
inputs.ego_data = ego_data;
+ stopwatch.tic("filter_predicted_objects");
inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_);
+ const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects");
inputs.route_handler = planner_data_->route_handler_;
inputs.lanelets = other_lanelets;
+ stopwatch.tic("calculate_decisions");
const auto decisions = calculate_decisions(inputs, params_, logger_);
const auto calculate_decisions_us = stopwatch.toc("calculate_decisions");
stopwatch.tic("calc_slowdown_points");
@@ -187,12 +189,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
"\tcalculate_lanelets = %2.0fus\n"
"\tcalculate_path_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_path_footprints_us,
- calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us,
- insert_slowdown_points_us);
+ calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us,
+ calc_slowdown_points_us, insert_slowdown_points_us);
return true;
}
From 9813c1285d1849b63227b3a9a9dca96b554b0143 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Thu, 22 Feb 2024 15:29:35 +0900
Subject: [PATCH 3/6] add param and remove debug svg
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../config/out_of_lane.param.yaml | 1 +
.../src/filter_predicted_objects.hpp | 45 +++++--------------
.../src/manager.cpp | 2 +
3 files changed, 15 insertions(+), 33 deletions(-)
diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
index 092f1d32b27b3..1b483b95510ed 100644
--- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
+++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
@@ -18,6 +18,7 @@
# if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value.
distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
+ cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
overlap:
minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
index 19b6af66db5fb..ed3f893887ece 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -30,15 +30,6 @@
#include <string>
#include <vector>
-// for writing the svg file
-#include <fstream>
-#include <iostream>
-// for the geometry types
-#include <tier4_autoware_utils/geometry/geometry.hpp>
-// for the svg mapper
-#include <boost/geometry/io/svg/svg_mapper.hpp>
-#include <boost/geometry/io/svg/write.hpp>
-
namespace behavior_velocity_planner::out_of_lane
{
void cut_predicted_path_beyond_line(
@@ -59,14 +50,16 @@ void cut_predicted_path_beyond_line(
}
path_segment.first = path_segment.second;
}
- auto cut_idx = stop_line_idx;
- double arc_length = 0;
- while (cut_idx > 0 && arc_length < object_front_overhang) {
- arc_length += tier4_autoware_utils::calcDistance2d(
- predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
- --cut_idx;
+ if(found) {
+ auto cut_idx = stop_line_idx;
+ double arc_length = 0;
+ while (cut_idx > 0 && arc_length < object_front_overhang) {
+ arc_length += tier4_autoware_utils::calcDistance2d(
+ predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
+ --cut_idx;
+ }
+ predicted_path.path.resize(cut_idx);
}
- predicted_path.path.resize(cut_idx);
}
std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
@@ -98,23 +91,10 @@ std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
/// @param [in] planner_data planner data to get the map and traffic light information
void cut_predicted_path_beyond_red_lights(
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
- const PlannerData & planner_data, const double object_front_overhang, int i)
+ const PlannerData & planner_data, const double object_front_overhang)
{
- // Declare a stream and an SVG mapper
- std::ofstream svg(
- "/home/mclement/Pictures/image" + std::to_string(i) + ".svg"); // /!\ CHANGE PATH
- boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> mapper(svg, 400, 400);
const auto stop_line = find_next_stop_line(predicted_path, planner_data);
- lanelet::BasicLineString2d path_ls;
- for (const auto & p : predicted_path.path) path_ls.emplace_back(p.position.x, p.position.y);
if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
- lanelet::BasicLineString2d cut_path_ls;
- for (const auto & p : predicted_path.path) cut_path_ls.emplace_back(p.position.x, p.position.y);
- mapper.add(cut_path_ls);
- if (stop_line) mapper.add(*stop_line);
- mapper.map(path_ls, "opacity:0.3;fill:black;stroke:black;stroke-width:2");
- if (stop_line) mapper.map(*stop_line, "opacity:0.5;fill:black;stroke:red;stroke-width:2");
- mapper.map(cut_path_ls, "opacity:0.3;fill:red;stroke:red;stroke-width:2");
}
/// @brief filter predicted objects and their predicted paths
@@ -153,11 +133,10 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
const auto new_end =
std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
predicted_paths.erase(new_end, predicted_paths.end());
- auto i = 0;
- if (true || params.objects_cut_predicted_paths_beyond_red_lights)
+ 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, i++);
+ predicted_path, planner_data, filtered_object.shape.dimensions.x);
predicted_paths.erase(
std::remove_if(
predicted_paths.begin(), predicted_paths.end(),
diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp
index 23de809e429ec..8055f5c9106ef 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp
@@ -51,6 +51,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
pp.objects_min_confidence =
getOrDeclareParameter<double>(node, ns + ".objects.predicted_path_min_confidence");
pp.objects_dist_buffer = getOrDeclareParameter<double>(node, ns + ".objects.distance_buffer");
+ pp.objects_cut_predicted_paths_beyond_red_lights =
+ getOrDeclareParameter<bool>(node, ns + ".objects.cut_predicted_paths_beyond_red_lights");
pp.overlap_min_dist = getOrDeclareParameter<double>(node, ns + ".overlap.minimum_distance");
pp.overlap_extra_length = getOrDeclareParameter<double>(node, ns + ".overlap.extra_length");
From 7a9c4adc90750d83641ffa8fa43ee824b30a8f5b Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Fri, 1 Mar 2024 14:34:05 +0900
Subject: [PATCH 4/6] skip pred paths with only 1 non-overlapping point to
avoid error msgs
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
planning/behavior_velocity_out_of_lane_module/src/decisions.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
index 8c2466bf9cd37..d68c7c97df7d2 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
@@ -79,6 +79,7 @@ std::optional<std::pair<double, double>> object_time_to_range(
for (const auto & predicted_path : object.kinematics.predicted_paths) {
const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
+ if(unique_path_points.size() < 2) continue;
const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
const auto enter_point =
geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
From 3f17ebf2a57129c3bc4aea21205f1f66383ac768 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Fri, 1 Mar 2024 14:35:18 +0900
Subject: [PATCH 5/6] fix lint
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
planning/behavior_velocity_out_of_lane_module/src/decisions.cpp | 2 +-
.../src/filter_predicted_objects.hpp | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
index d68c7c97df7d2..e948bd74eba45 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
@@ -79,7 +79,7 @@ std::optional<std::pair<double, double>> object_time_to_range(
for (const auto & predicted_path : object.kinematics.predicted_paths) {
const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
- if(unique_path_points.size() < 2) continue;
+ if (unique_path_points.size() < 2) continue;
const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
const auto enter_point =
geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
index ed3f893887ece..a5da98c614ff7 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -50,7 +50,7 @@ void cut_predicted_path_beyond_line(
}
path_segment.first = path_segment.second;
}
- if(found) {
+ if (found) {
auto cut_idx = stop_line_idx;
double arc_length = 0;
while (cut_idx > 0 && arc_length < object_front_overhang) {
From 5d121853426aece5a68fadf6ca4766101307536e Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Mon, 4 Mar 2024 13:10:42 +0900
Subject: [PATCH 6/6] split filter_predicted_objects in hpp/cpp and add
docstrings
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../src/filter_predicted_objects.cpp | 139 ++++++++++++++++++
.../src/filter_predicted_objects.hpp | 126 ++--------------
2 files changed, 153 insertions(+), 112 deletions(-)
create mode 100644 planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp
new file mode 100644
index 0000000000000..4a76ef2078d63
--- /dev/null
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp
@@ -0,0 +1,139 @@
+// Copyright 2023-2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "filter_predicted_objects.hpp"
+
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <traffic_light_utils/traffic_light_utils.hpp>
+
+#include <boost/geometry/algorithms/intersects.hpp>
+
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+
+#include <algorithm>
+
+namespace behavior_velocity_planner::out_of_lane
+{
+void cut_predicted_path_beyond_line(
+ autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+ const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
+{
+ auto stop_line_idx = 0UL;
+ bool found = false;
+ lanelet::BasicSegment2d path_segment;
+ path_segment.first.x() = predicted_path.path.front().position.x;
+ path_segment.first.y() = predicted_path.path.front().position.y;
+ for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) {
+ path_segment.second.x() = predicted_path.path[stop_line_idx].position.x;
+ path_segment.second.y() = predicted_path.path[stop_line_idx].position.y;
+ if (boost::geometry::intersects(stop_line, path_segment)) {
+ found = true;
+ break;
+ }
+ path_segment.first = path_segment.second;
+ }
+ if (found) {
+ auto cut_idx = stop_line_idx;
+ double arc_length = 0;
+ while (cut_idx > 0 && arc_length < object_front_overhang) {
+ arc_length += tier4_autoware_utils::calcDistance2d(
+ predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
+ --cut_idx;
+ }
+ predicted_path.path.resize(cut_idx);
+ }
+}
+
+std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
+ const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_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.getTrafficSignal(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;
+ }
+ }
+ }
+ return std::nullopt;
+}
+
+void cut_predicted_path_beyond_red_lights(
+ autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+ const PlannerData & planner_data, const double object_front_overhang)
+{
+ const auto stop_line = find_next_stop_line(predicted_path, planner_data);
+ if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
+}
+
+autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
+ const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params)
+{
+ autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
+ filtered_objects.header = planner_data.predicted_objects->header;
+ for (const auto & object : planner_data.predicted_objects->objects) {
+ const auto is_pedestrian =
+ std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
+ return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
+ }) != object.classification.end();
+ if (is_pedestrian) continue;
+
+ auto filtered_object = object;
+ const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
+ const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
+ const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path);
+ if (no_overlap_path.size() <= 1) return true;
+ const auto lat_offset_to_current_ego =
+ std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position));
+ const auto is_crossing_ego =
+ lat_offset_to_current_ego <=
+ object.shape.dimensions.y / 2.0 + std::max(
+ params.left_offset + params.extra_left_offset,
+ params.right_offset + params.extra_right_offset);
+ return is_low_confidence || is_crossing_ego;
+ };
+ if (params.objects_use_predicted_paths) {
+ auto & predicted_paths = filtered_object.kinematics.predicted_paths;
+ const auto new_end =
+ std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
+ predicted_paths.erase(new_end, predicted_paths.end());
+ 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_paths.erase(
+ std::remove_if(
+ predicted_paths.begin(), predicted_paths.end(),
+ [](const auto & p) { return p.path.empty(); }),
+ predicted_paths.end());
+ }
+
+ if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty())
+ filtered_objects.objects.push_back(filtered_object);
+ }
+ return filtered_objects;
+}
+
+} // namespace behavior_velocity_planner::out_of_lane
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
index a5da98c614ff7..bb6b5f4d00005 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -17,85 +17,33 @@
#include "types.hpp"
-#include <motion_utils/trajectory/trajectory.hpp>
-#include <traffic_light_utils/traffic_light_utils.hpp>
+#include <behavior_velocity_planner_common/planner_data.hpp>
-#include <boost/geometry/algorithms/intersects.hpp>
-
-#include <lanelet2_core/geometry/LaneletMap.h>
-#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
-
-#include <algorithm>
#include <optional>
-#include <string>
-#include <vector>
namespace behavior_velocity_planner::out_of_lane
{
+/// @brief cut a predicted path beyond the given stop line
+/// @param [inout] predicted_path predicted path to cut
+/// @param [in] stop_line stop line used for cutting
+/// @param [in] object_front_overhang extra distance to cut ahead of the stop line
void cut_predicted_path_beyond_line(
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
- const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
-{
- auto stop_line_idx = 0UL;
- bool found = false;
- lanelet::BasicSegment2d path_segment;
- path_segment.first.x() = predicted_path.path.front().position.x;
- path_segment.first.y() = predicted_path.path.front().position.y;
- for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) {
- path_segment.second.x() = predicted_path.path[stop_line_idx].position.x;
- path_segment.second.y() = predicted_path.path[stop_line_idx].position.y;
- if (boost::geometry::intersects(stop_line, path_segment)) {
- found = true;
- break;
- }
- path_segment.first = path_segment.second;
- }
- if (found) {
- auto cut_idx = stop_line_idx;
- double arc_length = 0;
- while (cut_idx > 0 && arc_length < object_front_overhang) {
- arc_length += tier4_autoware_utils::calcDistance2d(
- predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
- --cut_idx;
- }
- predicted_path.path.resize(cut_idx);
- }
-}
+ const lanelet::BasicLineString2d & 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
+/// @return the first red light stop line found along the path (if any)
std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
- const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_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.getTrafficSignal(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;
- }
- }
- }
- return std::nullopt;
-}
+ const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_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
void cut_predicted_path_beyond_red_lights(
autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
- const PlannerData & planner_data, const double object_front_overhang)
-{
- const auto stop_line = find_next_stop_line(predicted_path, planner_data);
- if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
-}
+ const PlannerData & planner_data, const double object_front_overhang);
/// @brief filter predicted objects and their predicted paths
/// @param [in] planner_data planner data
@@ -103,53 +51,7 @@ void cut_predicted_path_beyond_red_lights(
/// @param [in] params parameters
/// @return filtered predicted objects
autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
- const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params)
-{
- autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
- filtered_objects.header = planner_data.predicted_objects->header;
- for (const auto & object : planner_data.predicted_objects->objects) {
- const auto is_pedestrian =
- std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
- return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
- }) != object.classification.end();
- if (is_pedestrian) continue;
-
- auto filtered_object = object;
- const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
- const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
- const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path);
- if (no_overlap_path.size() <= 1) return true;
- const auto lat_offset_to_current_ego =
- std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position));
- const auto is_crossing_ego =
- lat_offset_to_current_ego <=
- object.shape.dimensions.y / 2.0 + std::max(
- params.left_offset + params.extra_left_offset,
- params.right_offset + params.extra_right_offset);
- return is_low_confidence || is_crossing_ego;
- };
- if (params.objects_use_predicted_paths) {
- auto & predicted_paths = filtered_object.kinematics.predicted_paths;
- const auto new_end =
- std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
- predicted_paths.erase(new_end, predicted_paths.end());
- 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_paths.erase(
- std::remove_if(
- predicted_paths.begin(), predicted_paths.end(),
- [](const auto & p) { return p.path.empty(); }),
- predicted_paths.end());
- }
-
- if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty())
- filtered_objects.objects.push_back(filtered_object);
- }
- return filtered_objects;
-}
-
+ const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params);
} // namespace behavior_velocity_planner::out_of_lane
#endif // FILTER_PREDICTED_OBJECTS_HPP_