From 06427777a91f057a436ba3f0f1ad04d739769d0d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 18:14:26 +0900 Subject: [PATCH] feat(autonomous_emergency_braking): add some tests to aeb (#8126) * add initial tests Signed-off-by: Daniel Sanchez * add more tests Signed-off-by: Daniel Sanchez * more tests Signed-off-by: Daniel Sanchez * WIP add publishing and test subscription Signed-off-by: Daniel Sanchez * add more tests Signed-off-by: Daniel Sanchez * fix lint cmake Signed-off-by: Daniel Sanchez * WIP tf topic Signed-off-by: Daniel Sanchez * Revert "WIP tf topic" This reverts commit b5ef11b499e719b2cdbe0464bd7de7778de54e76. Signed-off-by: Daniel Sanchez * add path crop test Signed-off-by: Daniel Sanchez * add test for transform object Signed-off-by: Daniel Sanchez * add briefs Signed-off-by: Daniel Sanchez * delete repeated test Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../CMakeLists.txt | 7 +- .../autonomous_emergency_braking/node.hpp | 192 ++++++++++- ...re_autonomous_emergency_braking.launch.xml | 1 - .../package.xml | 8 +- .../src/node.cpp | 3 +- .../src/utils.cpp | 3 + .../test/test.cpp | 326 ++++++++++++++++++ .../test/test.hpp | 116 +++++++ 8 files changed, 642 insertions(+), 14 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/test/test.cpp create mode 100644 control/autoware_autonomous_emergency_braking/test/test.hpp diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt index 85290c6c66730..9f7a64c18da9a 100644 --- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -30,8 +30,11 @@ rclcpp_components_register_node(${AEB_NODE} ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_aeb + test/test.cpp) + +target_link_libraries(test_aeb ${AEB_NODE}) + endif() ament_auto_package( 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 a85a0f890f0cd..229ea49d89102 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 @@ -71,6 +71,9 @@ using Vector3 = geometry_msgs::msg::Vector3; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +/** + * @brief Struct to store object data + */ struct ObjectData { rclcpp::Time stamp; @@ -80,22 +83,44 @@ struct ObjectData double distance_to_object{0.0}; }; +/** + * @brief Class to manage collision data + */ class CollisionDataKeeper { public: + /** + * @brief Constructor for CollisionDataKeeper + * @param clock Shared pointer to the clock + */ explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + /** + * @brief Set timeout values for collision and obstacle data + * @param collision_keep_time Time to keep collision data + * @param previous_obstacle_keep_time Time to keep previous obstacle data + */ void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) { collision_keep_time_ = collision_keep_time; previous_obstacle_keep_time_ = previous_obstacle_keep_time; } + /** + * @brief Get timeout values for collision and obstacle data + * @return Pair of collision and obstacle data timeout values + */ std::pair getTimeout() { return {collision_keep_time_, previous_obstacle_keep_time_}; } + /** + * @brief Check if object data has expired + * @param data Optional reference to the object data + * @param timeout Timeout value to check against + * @return True if object data has expired, false otherwise + */ bool checkObjectDataExpired(std::optional & data, const double timeout) { if (!data.has_value()) return true; @@ -109,38 +134,70 @@ class CollisionDataKeeper return false; } + /** + * @brief Check if collision data has expired + * @return True if collision data has expired, false otherwise + */ bool checkCollisionExpired() { return this->checkObjectDataExpired(closest_object_, collision_keep_time_); } + /** + * @brief Check if previous object data has expired + * @return True if previous object data has expired, false otherwise + */ bool checkPreviousObjectDataExpired() { return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); } + /** + * @brief Get the closest object data + * @return Object data of the closest object + */ [[nodiscard]] ObjectData get() const { return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); } + /** + * @brief Get the previous closest object data + * @return Object data of the previous closest object + */ [[nodiscard]] ObjectData getPreviousObjectData() const { return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); } + /** + * @brief Set collision data + * @param data Object data to set + */ void setCollisionData(const ObjectData & data) { closest_object_ = std::make_optional(data); } + /** + * @brief Set previous object data + * @param data Object data to set + */ void setPreviousObjectData(const ObjectData & data) { prev_closest_object_ = std::make_optional(data); } + /** + * @brief Reset the obstacle velocity history + */ void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + /** + * @brief Update the velocity history with current object velocity + * @param current_object_velocity Current object velocity + * @param current_object_velocity_time_stamp Timestamp of the current object velocity + */ void updateVelocityHistory( const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) { @@ -158,6 +215,10 @@ class CollisionDataKeeper current_object_velocity, current_object_velocity_time_stamp); } + /** + * @brief Get the median obstacle velocity from history + * @return Optional median obstacle velocity + */ [[nodiscard]] std::optional getMedianObstacleVelocity() const { if (obstacle_velocity_history_.empty()) return std::nullopt; @@ -177,6 +238,13 @@ class CollisionDataKeeper return (vel1 + vel2) / 2.0; } + /** + * @brief Calculate object speed from velocity history + * @param closest_object Closest object data + * @param path Ego vehicle path + * @param current_ego_speed Current ego vehicle speed + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { @@ -241,9 +309,16 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +/** + * @brief Autonomous Emergency Braking (AEB) node + */ class AEB : public rclcpp::Node { public: + /** + * @brief Constructor for AEB + * @param node_options Options for the node + */ explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber @@ -267,53 +342,156 @@ class AEB : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // callback + /** + * @brief Callback for point cloud messages + * @param input_msg Shared pointer to the point cloud message + */ void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + + /** + * @brief Callback for IMU messages + * @param input_msg Shared pointer to the IMU message + */ void onImu(const Imu::ConstSharedPtr input_msg); + + /** + * @brief Timer callback function + */ void onTimer(); + + /** + * @brief Callback for parameter updates + * @param parameters Vector of updated parameters + * @return Set parameters result + */ rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); + /** + * @brief Fetch the latest data from subscribers + * @return True if data fetch was successful, false otherwise + */ bool fetchLatestData(); - // main function + /** + * @brief Diagnostic check for collisions + * @param stat Diagnostic status wrapper + */ void onCheckCollision(DiagnosticStatusWrapper & stat); + + /** + * @brief Check for collisions + * @param debug_markers Marker array for debugging + * @return True if a collision is detected, false otherwise + */ bool checkCollision(MarkerArray & debug_markers); + + /** + * @brief Check if there is a collision with the closest object + * @param current_v Current velocity of the ego vehicle + * @param closest_object Data of the closest object + * @return True if a collision is detected, false otherwise + */ bool hasCollision(const double current_v, const ObjectData & closest_object); + /** + * @brief Generate the ego vehicle path + * @param curr_v Current velocity of the ego vehicle + * @param curr_w Current angular velocity of the ego vehicle + * @return Generated ego path + */ Path generateEgoPath(const double curr_v, const double curr_w); + + /** + * @brief Generate the ego vehicle path from the predicted trajectory + * @param predicted_traj Predicted trajectory of the ego vehicle + * @return Optional generated ego path + */ std::optional generateEgoPath(const Trajectory & predicted_traj); + + /** + * @brief Generate the footprint of the path with extra width margin + * @param path Ego vehicle path + * @param extra_width_margin Extra width margin for the footprint + * @return Vector of polygons representing the path footprint + */ std::vector generatePathFootprint(const Path & path, const double extra_width_margin); + /** + * @brief Create object data using point cloud clusters + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param stamp Timestamp of the data + * @param objects Vector to store the created object data + * @param obstacle_points_ptr Pointer to the point cloud of obstacles + */ void createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr); + /** + * @brief Create object data using predicted objects + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param objects Vector to store the created object data + */ void createObjectDataUsingPredictedObjects( const Path & ego_path, const std::vector & ego_polys, std::vector & objects); + /** + * @brief Crop the point cloud with the ego vehicle footprint path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param filtered_objects Pointer to the filtered point cloud of obstacles + */ void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects); - void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); - + /** + * @brief Add a marker for debugging + * @param current_time Current time + * @param path Ego vehicle path + * @param polygons Polygons representing the ego vehicle footprint + * @param objects Vector of object data + * @param closest_object Optional data of the closest object + * @param color_r Red color component + * @param color_g Green color component + * @param color_b Blue color component + * @param color_a Alpha (transparency) component + * @param ns Namespace for the marker + * @param debug_markers Marker array for debugging + */ void addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, const std::vector & objects, const std::optional & closest_object, const double color_r, const double color_g, const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers); + /** + * @brief Add a collision marker for debugging + * @param data Data of the collision object + * @param debug_markers Marker array for debugging + */ void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + /** + * @brief Add an info marker stop wall in front of the ego vehicle + * @param markers Data of the closest object + */ void addVirtualStopWallMarker(MarkerArray & markers); + /** + * @brief Calculate object speed from history + * @param closest_object Data of the closest object + * @param path Ego vehicle path + * @param current_ego_speed Current speed of the ego vehicle + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed); + // Member variables PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; @@ -330,7 +508,7 @@ class AEB : public rclcpp::Node // diag Updater updater_{this}; - // member variables + // Member variables bool publish_debug_pointcloud_; bool publish_debug_markers_; bool use_predicted_trajectory_; 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 fe0df41ca89c7..34d0492799577 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 @@ -3,7 +3,6 @@ - diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 0c64b9498486a..248d3689072fa 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -14,11 +14,16 @@ ament_cmake autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils autoware_control_msgs autoware_motion_utils autoware_planning_msgs autoware_system_msgs + autoware_test_utils autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -38,9 +43,6 @@ tf2_ros visualization_msgs - ament_lint_auto - autoware_lint_common - ament_cmake diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index e2a81abc724a8..e8281a2adae46 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -507,7 +507,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }; // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects(new PointCloud); + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); // evaluate if there is a collision for both paths const bool has_collision = has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index d6f21916a238f..0267ee7f6a8d8 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -51,6 +51,9 @@ PredictedObject transformObjectFrame( Polygon2d convertPolygonObjectToGeometryPolygon( const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { + if (obj_shape.footprint.points.empty()) { + return {}; + } Polygon2d object_polygon; tf2::Transform tf_map2obj; fromMsg(current_pose, tf_map2obj); diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp new file mode 100644 index 0000000000000..42054e718d09c --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -0,0 +1,326 @@ +// Copyright 2024 TIER IV +// +// 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 "test.hpp" + +#include "autoware/autonomous_emergency_braking/node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; +using std_msgs::msg::Header; + +Header get_header(const char * const frame_id, rclcpp::Time t) +{ + std_msgs::msg::Header header; + header.stamp = t; + header.frame_id = frame_id; + return header; +}; + +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z) +{ + Imu imu_msg; + imu_msg.header = header; + imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + imu_msg.angular_velocity.z = angular_velocity_z; + imu_msg.linear_acceleration.x = ax; + imu_msg.linear_acceleration.y = ay; + return imu_msg; +}; + +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate) +{ + VelocityReport velocity_msg; + velocity_msg.header = header; + velocity_msg.lateral_velocity = lat_velocity; + velocity_msg.longitudinal_velocity = long_velocity; + velocity_msg.heading_rate = heading_rate; + return velocity_msg; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto aeb_dir = + ament_index_cpp::get_package_share_directory("autoware_autonomous_emergency_braking"); + const auto vehicle_info_util_dir = + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); + + node_options.arguments( + {"--ros-args", "--params-file", aeb_dir + "/config/autonomous_emergency_braking.param.yaml", + "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + return std::make_shared(node_options); +}; + +std::shared_ptr generatePubSubNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments({"--ros-args"}); + return std::make_shared(node_options); +}; + +PubSubNode::PubSubNode(const rclcpp::NodeOptions & node_options) +: Node("test_aeb_pubsub", node_options) +{ + rclcpp::QoS qos{1}; + qos.transient_local(); + + pub_imu_ = create_publisher("~/input/imu", qos); + pub_point_cloud_ = create_publisher("~/input/pointcloud", qos); + pub_velocity_ = create_publisher("~/input/velocity", qos); + pub_predicted_traj_ = create_publisher("~/input/predicted_trajectory", qos); + pub_predicted_objects_ = create_publisher("~/input/objects", qos); + pub_autoware_state_ = create_publisher("autoware/state", qos); +} + +TEST_F(TestAEB, checkCollision) +{ + constexpr double longitudinal_velocity = 3.0; + ObjectData object_collision; + object_collision.distance_to_object = 0.5; + object_collision.velocity = 0.1; + ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); + + ObjectData object_no_collision; + object_no_collision.distance_to_object = 10.0; + object_no_collision.velocity = 0.1; + ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision)); +} + +TEST_F(TestAEB, checkImuPathGeneration) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); + + const auto stamp = rclcpp::Time(); + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + const double x_start{0.0}; + const double y_start{0.0}; + + for (size_t i = 0; i < 15; ++i) { + pcl::PointXYZ p1( + x_start + static_cast(i / 100.0), y_start - static_cast(i / 100.0), 0.5); + pcl::PointXYZ p2( + x_start + static_cast((i + 10) / 100.0), y_start - static_cast(i / 100.0), + 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + } + std::vector objects; + aeb_node_->createObjectDataUsingPointCloudClusters( + imu_path, footprint, stamp, objects, obstacle_points_ptr); + ASSERT_FALSE(objects.empty()); +} + +TEST_F(TestAEB, checkIncompleteImuPathGeneration) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double slow_velocity = min_generated_path_length / (2.0 * horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + ASSERT_TRUE(autoware::motion_utils::calcArcLength(imu_path) >= min_generated_path_length); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + +TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) +{ + const double velocity = 0.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(velocity, yaw_rate); + ASSERT_EQ(imu_path.size(), 1); +} + +TEST_F(TestAEB, checkParamUpdate) +{ + std::vector parameters{rclcpp::Parameter("param")}; + const auto result = aeb_node_->onParameter(parameters); + ASSERT_TRUE(result.successful); +} + +TEST_F(TestAEB, checkEmptyFetchData) +{ + ASSERT_FALSE(aeb_node_->fetchLatestData()); +} + +TEST_F(TestAEB, checkConvertObjectToPolygon) +{ + using autoware_perception_msgs::msg::Shape; + PredictedObject obj_cylinder; + obj_cylinder.shape.type = Shape::CYLINDER; + obj_cylinder.shape.dimensions.x = 1.0; + Pose obj_cylinder_pose; + obj_cylinder_pose.position.x = 1.0; + obj_cylinder_pose.position.y = 1.0; + obj_cylinder.kinematics.initial_pose_with_covariance.pose = obj_cylinder_pose; + const auto cylinder_polygon = utils::convertObjToPolygon(obj_cylinder); + ASSERT_FALSE(cylinder_polygon.outer().empty()); + + PredictedObject obj_box; + obj_box.shape.type = Shape::BOUNDING_BOX; + obj_box.shape.dimensions.x = 1.0; + obj_box.shape.dimensions.y = 2.0; + Pose obj_box_pose; + obj_box_pose.position.x = 1.0; + obj_box_pose.position.y = 1.0; + obj_box.kinematics.initial_pose_with_covariance.pose = obj_box_pose; + const auto box_polygon = utils::convertObjToPolygon(obj_box); + ASSERT_FALSE(box_polygon.outer().empty()); + + geometry_msgs::msg::TransformStamped tf_stamped; + geometry_msgs::msg::Transform transform; + + constexpr double yaw{0.0}; + transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw); + geometry_msgs::msg::Vector3 translation; + translation.x = 1.0; + translation.y = 0.0; + translation.z = 0.0; + transform.translation = translation; + tf_stamped.set__transform(transform); + const auto t_obj_box = utils::transformObjectFrame(obj_box, tf_stamped); + const auto t_pose = t_obj_box.kinematics.initial_pose_with_covariance.pose; + Pose expected_pose; + expected_pose.position.x = obj_box_pose.position.x + translation.x; + expected_pose.position.y = obj_box_pose.position.y + translation.y; + expected_pose.position.z = obj_box_pose.position.z + translation.z; + + ASSERT_DOUBLE_EQ(expected_pose.position.x, t_pose.position.x); + ASSERT_DOUBLE_EQ(expected_pose.position.y, t_pose.position.y); + ASSERT_DOUBLE_EQ(expected_pose.position.z, t_pose.position.z); +} + +TEST_F(TestAEB, CollisionDataKeeper) +{ + using namespace std::literals::chrono_literals; + constexpr double collision_keeping_sec{1.0}, previous_obstacle_keep_time{1.0}; + CollisionDataKeeper collision_data_keeper_(aeb_node_->get_clock()); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); + ASSERT_TRUE(collision_data_keeper_.checkPreviousObjectDataExpired()); + + ObjectData obj; + obj.stamp = aeb_node_->now(); + obj.velocity = 0.0; + obj.position.x = 0.0; + rclcpp::sleep_for(100ms); + + ObjectData obj2; + obj2.stamp = aeb_node_->now(); + obj2.velocity = 0.0; + obj2.position.x = 0.1; + rclcpp::sleep_for(100ms); + + constexpr double ego_longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(ego_longitudinal_velocity, yaw_rate); + + const auto speed_null = + collision_data_keeper_.calcObjectSpeedFromHistory(obj, imu_path, ego_longitudinal_velocity); + ASSERT_FALSE(speed_null.has_value()); + + const auto median_velocity = + collision_data_keeper_.calcObjectSpeedFromHistory(obj2, imu_path, ego_longitudinal_velocity); + ASSERT_TRUE(median_velocity.has_value()); + + // object speed is 1.0 m/s greater than ego's = 0.1 [m] / 0.1 [s] + longitudinal_velocity + ASSERT_TRUE(std::abs(median_velocity.value() - 4.0) < 1e-2); + rclcpp::sleep_for(1100ms); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); +} + +TEST_F(TestAEB, TestCropPointCloud) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + constexpr size_t n_points{15}; + // Create n_points inside the path and 1 point outside. + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + constexpr double x_start{0.0}; + constexpr double y_start{0.0}; + + for (size_t i = 0; i < n_points; ++i) { + const double offset_1 = static_cast(i / 100.0); + const double offset_2 = static_cast((i + 10) / 100.0); + pcl::PointXYZ p1(x_start + offset_1, y_start - offset_1, 0.5); + pcl::PointXYZ p2(x_start + offset_2, y_start - offset_1, 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + pcl::PointXYZ p_out(x_start + 100.0, y_start + 100, 0.5); + obstacle_points_ptr->push_back(p_out); + } + aeb_node_->obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*obstacle_points_ptr, *aeb_node_->obstacle_ros_pointcloud_ptr_); + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); + aeb_node_->cropPointCloudWithEgoFootprintPath(footprint, filtered_objects); + // Check if the point outside the path was excluded + ASSERT_TRUE(filtered_objects->points.size() == 2 * n_points); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking::test diff --git a/control/autoware_autonomous_emergency_braking/test/test.hpp b/control/autoware_autonomous_emergency_braking/test/test.hpp new file mode 100644 index 0000000000000..86edbf73a2a12 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV +// +// 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 TEST_HPP_ +#define TEST_HPP_ + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +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; +using std_msgs::msg::Header; + +std::shared_ptr generateNode(); +Header get_header(const char * const frame_id, rclcpp::Time t); +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z); +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate); +class PubSubNode : public rclcpp::Node +{ +public: + explicit PubSubNode(const rclcpp::NodeOptions & node_options); + // publisher + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_predicted_traj_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + rclcpp::Publisher::SharedPtr pub_autoware_state_; + // timer + // rclcpp::TimerBase::SharedPtr timer_; + void publishDefaultTopicsNoSpin() + { + const auto header = get_header("base_link", now()); + const auto imu_msg = make_imu_message(header, 0.0, 0.0, 0.0, 0.05); + const auto velocity_msg = make_velocity_report_msg(header, 0.0, 3.0, 0.0); + + pub_imu_->publish(imu_msg); + pub_velocity_->publish(velocity_msg); + }; +}; + +std::shared_ptr generatePubSubNode(); + +class TestAEB : public ::testing::Test +{ +public: + TestAEB() {} + TestAEB(const TestAEB &) = delete; + TestAEB(TestAEB &&) = delete; + TestAEB & operator=(const TestAEB &) = delete; + TestAEB & operator=(TestAEB &&) = delete; + ~TestAEB() override = default; + + void SetUp() override + { + rclcpp::init(0, nullptr); + pub_sub_node_ = generatePubSubNode(); + aeb_node_ = generateNode(); + } + + void TearDown() override + { + aeb_node_.reset(); + pub_sub_node_.reset(); + rclcpp::shutdown(); + } + + std::shared_ptr pub_sub_node_; + std::shared_ptr aeb_node_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking::test + +#endif // TEST_HPP_