Skip to content

Commit

Permalink
add Object struct and big refactoring
Browse files Browse the repository at this point in the history 
Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem
maxime-clem committed Jan 22, 2025
1 parent a87d818 commit dffa14a
Show file tree
Hide file tree
Showing 7 changed files with 164 additions and 169 deletions.
40 changes: 17 additions & 23 deletions planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/collision.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,24 +91,22 @@ inline FootprintIntersection calculate_footprint_intersection(

inline std::vector<std::vector<FootprintIntersection>>
calculate_footprint_intersections_per_predicted_path(
const FootprintSegmentRtree & footprint_rtree,
const autoware_perception_msgs::msg::PredictedObject & object,
const FootprintSegmentRtree & footprint_rtree, const Object & object,
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory)
{
std::vector<std::vector<FootprintIntersection>> intersections_per_predicted_path;
for (const auto & path : object.kinematics.predicted_paths) {
for (const auto & corner_footprint : object.corner_footprints) {
std::vector<FootprintIntersection> intersections;
const auto time_step = rclcpp::Duration(path.time_step).seconds();
const auto half_length = object.shape.dimensions.x * 0.5;
for (auto i = 0UL; i + 1 < path.path.size(); ++i) {
const auto object_polygon1 = universe_utils::toFootprint(
path.path[i], half_length, half_length, object.shape.dimensions.y);
const auto object_polygon2 = universe_utils::toFootprint(
path.path[i + 1], half_length, half_length, object.shape.dimensions.y);
for (auto i = 0UL; i + 1 < corner_footprint.front_left_ls.size(); ++i) {
universe_utils::Segment2d segment;
for (auto corner_idx = 0UL; corner_idx < 4; ++corner_idx) {
segment.first = object_polygon1.outer()[corner_idx];
segment.second = object_polygon2.outer()[corner_idx];
for (auto & corner_ls : {
corner_footprint.front_left_ls,
corner_footprint.front_right_ls,
corner_footprint.rear_right_ls,
corner_footprint.rear_left_ls,
}) {
segment.first = corner_ls[i];
segment.second = corner_ls[i + 1];
std::vector<FootprintSegmentNode> query_results;
footprint_rtree.query(
boost::geometry::index::intersects(segment), std::back_inserter(query_results));
Expand All @@ -118,7 +116,8 @@ calculate_footprint_intersections_per_predicted_path(
if (intersection) {
intersections.push_back(calculate_footprint_intersection(
segment, *intersection, query_result.second, ego_trajectory,
{time_step * static_cast<double>(i), time_step * (static_cast<double>(i) + 1)}));
{corner_footprint.time_step * static_cast<double>(i),
corner_footprint.time_step * (static_cast<double>(i) + 1)}));
}
}
}
Expand Down Expand Up @@ -167,27 +166,22 @@ inline Collision calculate_collision(
params};
}

inline std::vector<run_out::CollisionsWithObject> calculate_collisions_per_object(
const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
const run_out::FootprintSegmentRtree & ego_footprint_rtree,
inline void calculate_collisions(
std::vector<Object> & objects, const run_out::FootprintSegmentRtree & ego_footprint_rtree,
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory,
const run_out::Parameters & params)
{
std::vector<run_out::CollisionsWithObject> collisions_per_object;
for (const auto & object : objects) {
collisions_per_object.emplace_back();
collisions_per_object.back().object_uuid = universe_utils::toHexString(object.object_id);
for (auto & object : objects) {
const auto intersections_per_predicted_path =
run_out::calculate_footprint_intersections_per_predicted_path(
ego_footprint_rtree, object, ego_trajectory);
for (const auto & intersections : intersections_per_predicted_path) {
if (intersections.empty()) {
continue;
}
collisions_per_object.back().push_back(run_out::calculate_collision(intersections, params));
object.collisions.push_back(run_out::calculate_collision(intersections, params));
}
}
return collisions_per_object;
}
} // namespace autoware::motion_velocity_planner::run_out

Expand Down
113 changes: 56 additions & 57 deletions planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <ios>
#include <iostream>
#include <vector>

namespace autoware::motion_velocity_planner::run_out
Expand All @@ -38,8 +38,7 @@ using visualization_msgs::msg::Marker;
using visualization_msgs::msg::MarkerArray;

inline MarkerArray make_debug_footprint_markers(
const run_out::TrajectoryCornerFootprint & ego,
const std::vector<run_out::ObjectFootprint> & objects)
const run_out::TrajectoryCornerFootprint & ego, const std::vector<run_out::Object> & objects)
{
MarkerArray markers;
Marker m;
Expand Down Expand Up @@ -76,66 +75,66 @@ inline MarkerArray make_debug_footprint_markers(
m.ns = "objects_footprints";
m.color.r = 1.0;
for (const auto & object : objects) {
const auto & o = object.corner_footprint;
for (auto i = 0UL; i + 1 < o.front_left_ls.size(); ++i) {
m.points.push_back(
universe_utils::createPoint(o.front_left_ls[i].x(), o.front_left_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.front_left_ls[i + 1].x(), o.front_left_ls[i + 1].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.front_right_ls[i].x(), o.front_right_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.front_right_ls[i + 1].x(), o.front_right_ls[i + 1].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.rear_left_ls[i].x(), o.rear_left_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.rear_left_ls[i + 1].x(), o.rear_left_ls[i + 1].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.rear_right_ls[i].x(), o.rear_right_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(o.rear_right_ls[i + 1].x(), o.rear_right_ls[i + 1].y(), 0.0));
}
for (const auto & f : object.corner_footprints)
for (auto i = 0UL; i + 1 < f.front_left_ls.size(); ++i) {
m.points.push_back(
universe_utils::createPoint(f.front_left_ls[i].x(), f.front_left_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(f.front_left_ls[i + 1].x(), f.front_left_ls[i + 1].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(f.front_right_ls[i].x(), f.front_right_ls[i].y(), 0.0));
m.points.push_back(universe_utils::createPoint(
f.front_right_ls[i + 1].x(), f.front_right_ls[i + 1].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(f.rear_left_ls[i].x(), f.rear_left_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(f.rear_left_ls[i + 1].x(), f.rear_left_ls[i + 1].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(f.rear_right_ls[i].x(), f.rear_right_ls[i].y(), 0.0));
m.points.push_back(
universe_utils::createPoint(f.rear_right_ls[i + 1].x(), f.rear_right_ls[i + 1].y(), 0.0));
}
}
markers.markers.push_back(m);
return markers;
}

inline MarkerArray make_debug_collisions_markers(
const std::vector<CollisionsWithObject> & collisions_per_object)
{
MarkerArray markers;
Marker m;
m.header.frame_id = "base_link";
m.ns = "collisions";
m.type = Marker::TEXT_VIEW_FACING;
m.color = universe_utils::createMarkerColor(1.0, 1.0, 0.0, 0.75);
m.scale = universe_utils::createMarkerScale(0.1, 0.0, 0.5);
std::stringstream ss;
ss << std::setprecision(4);
ss << std::fixed;
ss << std::left;
ss << std::setw(25) << "collision";
ss << "|";
ss << std::setw(20) << "ego_time_interval";
ss << "|";
ss << std::setw(20) << "object_time_interval";
ss << "\n";
for (const auto & collisions : collisions_per_object) {
for (const auto & collision : collisions) {
ss << std::setw(25) << magic_enum::enum_name(collision.type);
ss << "|";
ss << std::setw(20) << collision.ego_time_interval;
ss << "|";
ss << std::setw(20) << collision.object_time_interval;
ss << "\n";
}
}
m.text = ss.str();
std::cout << ss.str() << std::endl;
markers.markers.push_back(m);
// inline MarkerArray make_debug_collisions_markers(
// const std::vector<CollisionsWithObject> & collisions_per_object)
// {
// MarkerArray markers;
// Marker m;
// m.header.frame_id = "base_link";
// m.ns = "collisions";
// m.type = Marker::TEXT_VIEW_FACING;
// m.color = universe_utils::createMarkerColor(1.0, 1.0, 0.0, 0.75);
// m.scale = universe_utils::createMarkerScale(0.1, 0.0, 0.5);
// std::stringstream ss;
// ss << std::setprecision(4);
// ss << std::fixed;
// ss << std::left;
// ss << std::setw(25) << "collision";
// ss << "|";
// ss << std::setw(20) << "ego_time_interval";
// ss << "|";
// ss << std::setw(20) << "object_time_interval";
// ss << "\n";
// for (const auto & collisions : collisions_per_object) {
// for (const auto & collision : collisions) {
// ss << std::setw(25) << magic_enum::enum_name(collision.type);
// ss << "|";
// ss << std::setw(20) << collision.ego_time_interval;
// ss << "|";
// ss << std::setw(20) << collision.object_time_interval;
// ss << "\n";
// }
// }
// m.text = ss.str();
// std::cout << ss.str() << std::endl;
// markers.markers.push_back(m);

return markers;
}
// return markers;
// }

inline MarkerArray make_debug_decisions_markers(const ObjectDecisionsTracker & decisions_tracker)
{
Expand Down
14 changes: 6 additions & 8 deletions planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/decision.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,20 +95,18 @@ inline DecisionType calculate_decision_type(
}

inline void calculate_decisions(
ObjectDecisionsTracker & decisions_tracker,
const std::vector<CollisionsWithObject> & collisions_per_object, const rclcpp::Time & now,
const Parameters & params)
ObjectDecisionsTracker & decisions_tracker, const std::vector<Object> & objects,
const rclcpp::Time & now, const Parameters & params)
{
for (const auto & collisions : collisions_per_object) {
for (const auto & object : objects) {
std::optional<Decision> object_decision;
const auto & decision_history = decisions_tracker.history_per_object[collisions.object_uuid];
for (const auto & collision : collisions) {
auto & decision_history = decisions_tracker.history_per_object[object.uuid];
for (const auto & collision : object.collisions) {
Decision d(collision, calculate_decision_type(collision.type, decision_history, now, params));
update_decision(object_decision, d);
}
if (object_decision) {
decisions_tracker.history_per_object[collisions.object_uuid].add_decision(
now, *object_decision);
decision_history.add_decision(now, *object_decision);
}
}
decisions_tracker.remove_outdated(now, params.max_history_duration);
Expand Down
24 changes: 0 additions & 24 deletions planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/footprints.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,30 +34,6 @@

namespace autoware::motion_velocity_planner::run_out
{
inline std::vector<run_out::ObjectFootprint> calculate_object_footprints(
const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects)
{
std::vector<run_out::ObjectFootprint> footprints;
// TODO(Maxime): handle objects with no shape if needed
for (const auto & object : objects) {
const auto uuid = universe_utils::toHexString(object.object_id);
const auto half_length = object.shape.dimensions.x * 0.5;
for (const auto & path : object.kinematics.predicted_paths) {
run_out::ObjectFootprint footprint(uuid);
for (const auto & p : path.path) {
const auto polygon =
universe_utils::toFootprint(p, half_length, half_length, object.shape.dimensions.y);
footprint.corner_footprint.front_left_ls.push_back(polygon.outer()[0]);
footprint.corner_footprint.front_right_ls.push_back(polygon.outer()[1]);
footprint.corner_footprint.rear_right_ls.push_back(polygon.outer()[2]);
footprint.corner_footprint.rear_left_ls.push_back(polygon.outer()[3]);
}
footprints.push_back(footprint);
}
}
return footprints;
}

inline run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint(
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
autoware::vehicle_info_utils::VehicleInfo vehicle_info, const run_out::Parameters & params)
Expand Down
Loading

0 comments on commit dffa14a

Please sign in to comment.