From 1cf58eb72b037ca95c3f3bf4b5e41eced8648e3a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 20 Sep 2024 23:58:08 +0900 Subject: [PATCH] feat(autonomous_emergency_braking): make hull markers 3d (#8930) make hull markers 3d Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 3 ++- .../autonomous_emergency_braking/utils.hpp | 9 +++++++++ .../src/node.cpp | 18 ++++++++++++++---- .../src/utils.cpp | 17 +++++++++++++++++ 4 files changed, 42 insertions(+), 5 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 9addc059ad0af..bc45e6049abd7 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -65,6 +65,7 @@ using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -496,7 +497,7 @@ class AEB : public rclcpp::Node * @param debug_markers Marker array for debugging */ void addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); /** diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 5a2dacad2dfc9..18f3716b755ee 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -42,6 +42,7 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; @@ -105,6 +106,14 @@ std::optional getTransform( */ void fillMarkerFromPolygon( const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon3d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); } // namespace autoware::motion::control::autonomous_emergency_braking::utils #endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 1f8fd6f4137a2..48d9ea4c1b458 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -59,6 +59,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -71,6 +72,16 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +void appendPointToPolygon(Polygon3d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point3d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + point.z() = geom_point.z; + + bg::append(polygon.outer(), point); +} + Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) @@ -580,7 +591,6 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object // collision happens ObjectData collision_data = closest_object; collision_data.rss = rss_dist; - collision_data.distance_to_object = closest_object.distance_to_object; collision_data_keeper_.setCollisionData(collision_data); return true; } @@ -787,7 +797,7 @@ void AEB::getPointsBelongingToClusterHulls( ec.extract(cluster_idx); return cluster_idx; }); - std::vector hull_polygons; + std::vector hull_polygons; for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); bool cluster_surpasses_threshold_height{false}; @@ -806,7 +816,7 @@ void AEB::getPointsBelongingToClusterHulls( std::vector polygons; PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); - Polygon2d hull_polygon; + Polygon3d hull_polygon; for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); if (publish_debug_markers_) { @@ -900,7 +910,7 @@ void AEB::cropPointCloudWithEgoFootprintPath( } void AEB::addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 9c8bbe388fdc5..5d9782ada5fbd 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -186,4 +186,21 @@ void fillMarkerFromPolygon( } } +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z()); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + } // namespace autoware::motion::control::autonomous_emergency_braking::utils