diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt index 7f38e4387b452..85290c6c66730 100644 --- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -12,11 +12,18 @@ include_directories( ${PCL_INCLUDE_DIRS} ) +ament_auto_add_library(autoware_autonomous_emergency_braking_helpers SHARED + include/autoware/autonomous_emergency_braking/utils.hpp + src/utils.cpp +) + set(AEB_NODE ${PROJECT_NAME}_node) ament_auto_add_library(${AEB_NODE} SHARED + include/autoware/autonomous_emergency_braking/node.hpp src/node.cpp ) +target_link_libraries(${AEB_NODE} autoware_autonomous_emergency_braking_helpers) rclcpp_components_register_node(${AEB_NODE} PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" EXECUTABLE ${PROJECT_NAME} diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 4233f8b6bebcb..344d076197170 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -3,6 +3,8 @@ # Ego path calculation use_predicted_trajectory: true use_imu_path: false + use_pointcloud_data: true + use_predicted_object_data: true use_object_velocity_calculation: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 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 1096def70d483..2a1156adb41a0 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 @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -68,6 +69,9 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; + struct ObjectData { rclcpp::Time stamp; @@ -176,6 +180,13 @@ class CollisionDataKeeper std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { + // in case the object comes from predicted objects info, we reuse the speed. + if (closest_object.velocity > 0.0) { + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(closest_object.velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); + } + if (this->checkPreviousObjectDataExpired()) { this->setPreviousObjectData(closest_object); this->resetVelocityHistory(); @@ -243,6 +254,8 @@ class AEB : public rclcpp::Node autoware_universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; autoware_universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; + autoware_universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + this, "~/input/objects"}; autoware_universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher @@ -275,6 +288,10 @@ class AEB : public rclcpp::Node std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr); + void createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects); + void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); @@ -298,6 +315,7 @@ class AEB : public rclcpp::Node VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -313,6 +331,8 @@ class AEB : public rclcpp::Node bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; + bool use_pointcloud_data_; + bool use_predicted_object_data_; bool use_object_velocity_calculation_; double path_footprint_extra_margin_; double detection_range_min_height_; 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 new file mode 100644 index 0000000000000..b046acd9153e9 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -0,0 +1,88 @@ +// Copyright 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. + +#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ +#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; + +/** + * @brief Apply a transform to a predicted object + * @param input the predicted object + * @param transform_stamped the tf2 transform + */ +PredictedObject transformObjectFrame( + const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped); + +/** + * @brief Get the predicted objects polygon as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects cylindrical shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects bounding box shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param obj the object + */ +Polygon2d convertObjToPolygon(const PredictedObject & obj); +} // namespace autoware::motion::control::autonomous_emergency_braking::utils + +#endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index 75b1a9dcf822d..fe0df41ca89c7 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -5,6 +5,7 @@ + @@ -15,5 +16,6 @@ + diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 29ba9474b6e8e..e898ff4788da4 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/autonomous_emergency_braking/node.hpp" - +#include +#include #include #include #include @@ -21,7 +21,11 @@ #include #include +#include + #include +#include +#include #include #include @@ -46,6 +50,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { +using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -100,7 +105,7 @@ Polygon2d createPolygon( Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); - + bg::correct(hull_polygon); return hull_polygon; } @@ -124,6 +129,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); + use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); + use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); @@ -172,6 +179,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); + updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); @@ -298,13 +307,31 @@ bool AEB::fetchLatestData() return missing("ego velocity"); } - const auto pointcloud_ptr = sub_point_cloud_.takeData(); - if (!pointcloud_ptr) { - return missing("object pointcloud message"); + if (use_pointcloud_data_) { + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + + onPointCloud(pointcloud_ptr); + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + } else { + obstacle_ros_pointcloud_ptr_.reset(); + } + + if (use_predicted_object_data_) { + predicted_objects_ptr_ = predicted_objects_sub_.takeData(); + if (!predicted_objects_ptr_) { + return missing("predicted objects"); + } + } else { + predicted_objects_ptr_.reset(); } - onPointCloud(pointcloud_ptr); - if (!obstacle_ros_pointcloud_ptr_) { - return missing("object pointcloud"); + + if (!obstacle_ros_pointcloud_ptr_ && !predicted_objects_ptr_) { + return missing("object detection method (pointcloud or predicted objects)"); } const auto imu_ptr = sub_imu_.takeData(); @@ -381,27 +408,33 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto & path, const colorTuple & debug_colors, const std::string & debug_ns, pcl::PointCloud::Ptr filtered_objects) { - // Crop out Pointcloud using an extra wide ego path - const auto expanded_ego_polys = - generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); - cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); - // Check which points of the cropped point cloud are on the ego path, and get the closest one - std::vector objects_from_point_clusters; const auto ego_polys = generatePathFootprint(path, expand_width_); - const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - createObjectDataUsingPointCloudClusters( - path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + auto objects = std::invoke([&]() { + std::vector objects; + // Crop out Pointcloud using an extra wide ego path + if (use_pointcloud_data_) { + const auto expanded_ego_polys = + generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + createObjectDataUsingPointCloudClusters( + path, ego_polys, current_time, objects, filtered_objects); + } + if (use_predicted_object_data_) { + createObjectDataUsingPredictedObjects(path, ego_polys, objects); + } + return objects; + }); // Get only the closest object and calculate its speed const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = std::min_element( - objects_from_point_clusters.begin(), objects_from_point_clusters.end(), - [](const auto & o1, const auto & o2) { + const auto closest_object_point_itr = + std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) { return o1.distance_to_object < o2.distance_to_object; }); - if (closest_object_point_itr == objects_from_point_clusters.end()) { + if (closest_object_point_itr == objects.end()) { return std::nullopt; } const auto closest_object_speed = (use_object_velocity_calculation_) @@ -419,8 +452,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects_from_point_clusters, - closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, + color_b, color_a, debug_ns, debug_markers); } // check collision using rss distance return (closest_object_point.has_value()) @@ -462,9 +495,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { - const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared(); - pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_); - pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_); + const auto filtered_objects_ros_pointcloud_ptr = std::make_shared(); + pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr); + pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr); } return has_collision; } @@ -577,6 +610,93 @@ std::vector AEB::generatePathFootprint( return polygons; } +void AEB::createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & object_data_vector) +{ + if (predicted_objects_ptr_->objects.empty()) return; + + const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; + const auto & objects = predicted_objects_ptr_->objects; + const auto & stamp = predicted_objects_ptr_->header.stamp; + + // Ego position + const auto current_p = [&]() { + const auto & first_point_of_path = ego_path.front(); + const auto & p = first_point_of_path.position; + return autoware_universe_utils::createPoint(p.x, p.y, p.z); + }(); + + auto get_object_tangent_velocity = + [&](const PredictedObject & predicted_object, const auto & obj_pose) { + const double obj_vel_norm = std::hypot( + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + + const auto obj_yaw = tf2::getYaw(obj_pose.orientation); + const auto obj_idx = autoware_motion_utils::findNearestIndex(ego_path, obj_pose.position); + const auto path_yaw = (current_ego_speed > 0.0) + ? tf2::getYaw(ego_path.at(obj_idx).orientation) + : tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI; + return obj_vel_norm * std::cos(obj_yaw - path_yaw); + }; + + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_objects_ptr_->header.frame_id, stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + // Check which objects collide with the ego footprints + std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { + // get objects in base_link frame + const auto t_predicted_object = + utils::transformObjectFrame(predicted_object, transform_stamped); + const auto & obj_pose = t_predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = convertObjToPolygon(t_predicted_object); + const double obj_tangent_velocity = get_object_tangent_velocity(t_predicted_object, obj_pose); + + for (const auto & ego_poly : ego_polys) { + // check collision with 2d polygon + std::vector collision_points_bg; + bg::intersection(ego_poly, obj_poly, collision_points_bg); + if (collision_points_bg.empty()) continue; + + // Create an object for each intersection point + bool collision_points_added{false}; + for (const auto & collision_point : collision_points_bg) { + const auto obj_position = + autoware_universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + const double obj_arc_length = + autoware_motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + if (std::isnan(obj_arc_length)) continue; + + // If the object is behind the ego, we need to use the backward long offset. The + // distance should be a positive number in any case + const bool is_object_in_front_of_ego = obj_arc_length > 0.0; + const double dist_ego_to_object = + (is_object_in_front_of_ego) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m + : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; + + ObjectData obj; + obj.stamp = stamp; + obj.position = obj_position; + obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0; + obj.distance_to_object = std::abs(dist_ego_to_object); + object_data_vector.push_back(obj); + collision_points_added = true; + } + // The ego polygons are in order, so the first intersection points found are the closest + // points. It is not necessary to continue iterating the ego polys for the same object. + if (collision_points_added) break; + } + }); +} + void AEB::createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr) diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp new file mode 100644 index 0000000000000..81f6afee31172 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -0,0 +1,150 @@ +// Copyright 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 + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; + +PredictedObject transformObjectFrame( + const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped) +{ + PredictedObject output = input; + const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; + const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular; + const auto & pose = input.kinematics.initial_pose_with_covariance.pose; + + geometry_msgs::msg::Pose t_pose; + Vector3 t_linear_twist; + Vector3 t_angular_twist; + + tf2::doTransform(pose, t_pose, transform_stamped); + tf2::doTransform(linear_twist, t_linear_twist, transform_stamped); + tf2::doTransform(angular_twist, t_angular_twist, transform_stamped); + + output.kinematics.initial_pose_with_covariance.pose = t_pose; + output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist; + output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist; + return output; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} +} // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index a61f29014dcce..1cf3cb5b44656 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -148,6 +148,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/velocity", "/vehicle/status/velocity_status"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/objects", "/perception/object_recognition/objects"), ( "~/input/predicted_trajectory", "/control/trajectory_follower/lateral/predicted_trajectory",