From d1877225c65f8f29e03a91712c141966c122bc8c Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 13 Nov 2024 08:03:53 +0900 Subject: [PATCH] test(costmap_generator): unit test implementation for costmap generator (#9149) * modify costmap generator directory structure Signed-off-by: mohammad alqudah * rename class CostmapGenerator to CostmapGeneratorNode Signed-off-by: mohammad alqudah * unit test for object_map_utils Signed-off-by: mohammad alqudah * catch error from lookupTransform Signed-off-by: mohammad alqudah * use polling subscriber in costmap generator node Signed-off-by: mohammad alqudah * add test for costmap generator node Signed-off-by: mohammad alqudah * add test for isActive() Signed-off-by: mohammad alqudah * revert unnecessary changes Signed-off-by: mohammad alqudah * remove commented out line Signed-off-by: mohammad alqudah * minor fix Signed-off-by: mohammad alqudah * Update planning/autoware_costmap_generator/src/costmap_generator.cpp Co-authored-by: Kosuke Takeuchi --------- Signed-off-by: mohammad alqudah Co-authored-by: Kosuke Takeuchi --- .../test_map/overlap_map.osm | 557 +++++++++++------- .../autoware_costmap_generator/CMakeLists.txt | 34 +- .../costmap_generator}/costmap_generator.hpp | 44 +- .../utils}/object_map_utils.hpp | 12 +- .../utils}/objects_to_costmap.hpp | 6 +- .../utils}/points_to_costmap.hpp | 6 +- .../autoware_costmap_generator/package.xml | 1 + .../costmap_generator.cpp} | 116 ++-- .../utils}/object_map_utils.cpp | 13 +- .../utils}/objects_to_costmap.cpp | 2 +- .../utils}/points_to_costmap.cpp | 2 +- .../test/test_costmap_generator.cpp | 170 ++++++ .../test/test_object_map_utils.cpp | 93 +++ .../test/test_objects_to_costmap.cpp | 151 ++++- .../test/test_points_to_costmap.cpp | 2 +- 15 files changed, 844 insertions(+), 365 deletions(-) rename planning/autoware_costmap_generator/include/{autoware_costmap_generator => autoware/costmap_generator}/costmap_generator.hpp (83%) rename planning/autoware_costmap_generator/include/{autoware_costmap_generator => autoware/costmap_generator/utils}/object_map_utils.hpp (80%) rename planning/autoware_costmap_generator/include/{autoware_costmap_generator => autoware/costmap_generator/utils}/objects_to_costmap.hpp (96%) rename planning/autoware_costmap_generator/include/{autoware_costmap_generator => autoware/costmap_generator/utils}/points_to_costmap.hpp (96%) rename planning/autoware_costmap_generator/{nodes/autoware_costmap_generator/costmap_generator_node.cpp => src/costmap_generator.cpp} (85%) rename planning/autoware_costmap_generator/{nodes/autoware_costmap_generator => src/utils}/object_map_utils.cpp (79%) rename planning/autoware_costmap_generator/{nodes/autoware_costmap_generator => src/utils}/objects_to_costmap.cpp (99%) rename planning/autoware_costmap_generator/{nodes/autoware_costmap_generator => src/utils}/points_to_costmap.cpp (98%) create mode 100644 planning/autoware_costmap_generator/test/test_costmap_generator.cpp create mode 100644 planning/autoware_costmap_generator/test/test_object_map_utils.cpp diff --git a/common/autoware_test_utils/test_map/overlap_map.osm b/common/autoware_test_utils/test_map/overlap_map.osm index 27823db79b19c..7333b45a0e9ab 100644 --- a/common/autoware_test_utils/test_map/overlap_map.osm +++ b/common/autoware_test_utils/test_map/overlap_map.osm @@ -1,684 +1,691 @@ - - - + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -916,6 +923,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_costmap_generator/CMakeLists.txt b/planning/autoware_costmap_generator/CMakeLists.txt index 4978f7e54df6a..59ec264ea3d1e 100644 --- a/planning/autoware_costmap_generator/CMakeLists.txt +++ b/planning/autoware_costmap_generator/CMakeLists.txt @@ -16,9 +16,9 @@ include_directories( ) ament_auto_add_library(costmap_generator_lib SHARED - nodes/autoware_costmap_generator/points_to_costmap.cpp - nodes/autoware_costmap_generator/objects_to_costmap.cpp - nodes/autoware_costmap_generator/object_map_utils.cpp + src/utils/points_to_costmap.cpp + src/utils/objects_to_costmap.cpp + src/utils/object_map_utils.cpp ) target_link_libraries(costmap_generator_lib ${PCL_LIBRARIES} @@ -37,7 +37,7 @@ generate_parameter_library(costmap_generator_node_parameters ) ament_auto_add_library(costmap_generator_node SHARED - nodes/autoware_costmap_generator/costmap_generator_node.cpp + src/costmap_generator.cpp ) target_link_libraries(costmap_generator_node ${PCL_LIBRARIES} @@ -56,25 +56,29 @@ rclcpp_components_register_node(costmap_generator_node if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) - ament_add_ros_isolated_gtest(test_points_to_costmap + ament_add_ros_isolated_gtest(test_costmap_generator_lib test/test_points_to_costmap.cpp + test/test_objects_to_costmap.cpp + test/test_object_map_utils.cpp ) - - target_link_libraries(test_points_to_costmap + target_link_libraries(test_costmap_generator_lib costmap_generator_lib ) - ament_add_ros_isolated_gtest(test_objects_to_costmap - test/test_objects_to_costmap.cpp + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_costmap_generator.cpp ) - - target_link_libraries(test_objects_to_costmap - costmap_generator_lib + target_link_libraries(test_${PROJECT_NAME} + costmap_generator_node + costmap_generator_node_parameters + ) + target_compile_options(test_${PROJECT_NAME} PUBLIC + -Wno-error=deprecated-declarations ) endif() ament_auto_package( - INSTALL_TO_SHARE - launch - config + INSTALL_TO_SHARE + launch + config ) diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp similarity index 83% rename from planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp rename to planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index a26589775ff3e..94906cdf2725a 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -42,13 +42,14 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#define AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#ifndef AUTOWARE__COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#define AUTOWARE__COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#include "autoware_costmap_generator/objects_to_costmap.hpp" -#include "autoware_costmap_generator/points_to_costmap.hpp" +#include "autoware/costmap_generator/utils/objects_to_costmap.hpp" +#include "autoware/costmap_generator/utils/points_to_costmap.hpp" #include "costmap_generator_node_parameters.hpp" +#include #include #include #include @@ -72,8 +73,12 @@ #include #include +class TestCostmapGenerator; + namespace autoware::costmap_generator { +using autoware_perception_msgs::msg::PredictedObjects; + class CostmapGenerator : public rclcpp::Node { public: @@ -82,9 +87,10 @@ class CostmapGenerator : public rclcpp::Node private: std::shared_ptr<::costmap_generator_node::ParamListener> param_listener_; std::shared_ptr<::costmap_generator_node::Params> param_; + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; lanelet::LaneletMapPtr lanelet_map_; - autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; + PredictedObjects::ConstSharedPtr objects_; sensor_msgs::msg::PointCloud2::ConstSharedPtr points_; grid_map::GridMap costmap_; @@ -95,10 +101,13 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_processing_time_; rclcpp::Publisher::SharedPtr pub_processing_time_ms_; - rclcpp::Subscription::SharedPtr sub_points_; - rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; - rclcpp::Subscription::SharedPtr sub_scenario_; + autoware::universe_utils::InterProcessPollingSubscriber + sub_points_{this, "~/input/points_no_ground", autoware::universe_utils::SingleDepthSensorQoS()}; + autoware::universe_utils::InterProcessPollingSubscriber sub_objects_{ + this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_scenario_{this, "~/input/scenario"}; rclcpp::TimerBase::SharedPtr timer_; @@ -126,17 +135,9 @@ class CostmapGenerator : public rclcpp::Node /// \brief callback for loading lanelet2 map void onLaneletMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); - /// \brief callback for DynamicObjectArray - /// \param[in] in_objects input DynamicObjectArray usually from prediction or perception - /// component - void onObjects(const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void update_data(); - /// \brief callback for sensor_msgs::PointCloud2 - /// \param[in] in_points input sensor_msgs::PointCloud2. Assuming ground-filtered pointcloud - /// by default - void onPoints(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - - void onScenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr msg); + void set_current_pose(); void onTimer(); @@ -170,8 +171,7 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost from DynamicObjectArray /// \param[in] in_objects: subscribed DynamicObjectArray - grid_map::Matrix generateObjectsCostmap( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + grid_map::Matrix generateObjectsCostmap(const PredictedObjects::ConstSharedPtr in_objects); /// \brief calculate cost from lanelet2 map grid_map::Matrix generatePrimitivesCostmap(); @@ -181,7 +181,9 @@ class CostmapGenerator : public rclcpp::Node /// \brief measure processing time autoware::universe_utils::StopWatch stop_watch; + + friend class ::TestCostmapGenerator; }; } // namespace autoware::costmap_generator -#endif // AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#endif // AUTOWARE__COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/object_map_utils.hpp similarity index 80% rename from planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp rename to planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/object_map_utils.hpp index 737af55aa0b92..e68162f18efbe 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/object_map_utils.hpp @@ -30,8 +30,8 @@ * */ -#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ -#define AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#ifndef AUTOWARE__COSTMAP_GENERATOR__UTILS__OBJECT_MAP_UTILS_HPP_ +#define AUTOWARE__COSTMAP_GENERATOR__UTILS__OBJECT_MAP_UTILS_HPP_ #include #include @@ -55,16 +55,12 @@ namespace autoware::costmap_generator::object_map * @param[in] in_grid_layer_name Name to assign to the layer * @param[in] in_layer_background_value Empty state value * @param[in] in_fill_value Value to fill inside the given polygons - * @param[in] in_tf_target_frame Target frame to transform the points - * @param[in] in_tf_source_frame Source frame, where the points are located - * @param[in] in_tf_listener Valid listener to obtain the transformation */ void fill_polygon_areas( grid_map::GridMap & out_grid_map, const std::vector & in_polygons, const std::string & in_grid_layer_name, const float in_layer_background_value, - const float in_fill_value, const std::string & in_tf_target_frame, - const std::string & in_tf_source_frame, const tf2_ros::Buffer & in_tf_buffer); + const float in_fill_value); } // namespace autoware::costmap_generator::object_map -#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#endif // AUTOWARE__COSTMAP_GENERATOR__UTILS__OBJECT_MAP_UTILS_HPP_ diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/objects_to_costmap.hpp similarity index 96% rename from planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/objects_to_costmap.hpp index a04e139fdaa6e..955c39169e962 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/objects_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ -#define AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE__COSTMAP_GENERATOR__UTILS__OBJECTS_TO_COSTMAP_HPP_ +#define AUTOWARE__COSTMAP_GENERATOR__UTILS__OBJECTS_TO_COSTMAP_HPP_ #include @@ -126,4 +126,4 @@ class ObjectsToCostmap grid_map::GridMap & objects_costmap); }; } // namespace autoware::costmap_generator -#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#endif // AUTOWARE__COSTMAP_GENERATOR__UTILS__OBJECTS_TO_COSTMAP_HPP_ diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/points_to_costmap.hpp similarity index 96% rename from planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/points_to_costmap.hpp index 0e3abbd69ec20..cda96ea9b3283 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/utils/points_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ -#define AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE__COSTMAP_GENERATOR__UTILS__POINTS_TO_COSTMAP_HPP_ +#define AUTOWARE__COSTMAP_GENERATOR__UTILS__POINTS_TO_COSTMAP_HPP_ #include @@ -120,4 +120,4 @@ class PointsToCostmap }; } // namespace autoware::costmap_generator -#endif // AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#endif // AUTOWARE__COSTMAP_GENERATOR__UTILS__POINTS_TO_COSTMAP_HPP_ diff --git a/planning/autoware_costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml index db2ad24c0c773..8779c1e706123 100644 --- a/planning/autoware_costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -19,6 +19,7 @@ autoware_lanelet2_extension autoware_map_msgs autoware_perception_msgs + autoware_test_utils autoware_universe_utils generate_parameter_library grid_map_ros diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp similarity index 85% rename from planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp rename to planning/autoware_costmap_generator/src/costmap_generator.cpp index 0a85f7520b317..6fa3db119d02e 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -42,8 +42,9 @@ * OF private_node SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "autoware_costmap_generator/costmap_generator.hpp" -#include "autoware_costmap_generator/object_map_utils.hpp" +#include "autoware/costmap_generator/costmap_generator.hpp" + +#include "autoware/costmap_generator/utils/object_map_utils.hpp" #include #include @@ -135,6 +136,19 @@ pcl::PointCloud getTransformedPointCloud( return transformed_pointcloud; } +std::vector getTransformedPrimitives( + const std::vector & in_polygons, + const geometry_msgs::msg::TransformStamped & transform) +{ + std::vector out_polygons; + for (const auto & polygon : in_polygons) { + geometry_msgs::msg::Polygon transformed_polygon; + tf2::doTransform(polygon, transformed_polygon, transform); + out_polygons.emplace_back(transformed_polygon); + } + return out_polygons; +} + } // namespace namespace autoware::costmap_generator @@ -146,30 +160,10 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) this->get_node_parameters_interface()); param_ = std::make_shared<::costmap_generator_node::Params>(param_listener_->get_params()); - // Wait for first tf - // We want to do this before creating subscriptions - while (rclcpp::ok()) { - try { - tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero); - break; - } catch (const tf2::TransformException & ex) { - RCLCPP_INFO(this->get_logger(), "waiting for initial pose..."); - } - rclcpp::sleep_for(std::chrono::milliseconds(5000)); - } - - // Subscribers - using std::placeholders::_1; - sub_objects_ = this->create_subscription( - "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1)); - sub_points_ = this->create_subscription( - "~/input/points_no_ground", rclcpp::SensorDataQoS(), - std::bind(&CostmapGenerator::onPoints, this, _1)); + // Lanelet map subscriber sub_lanelet_bin_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&CostmapGenerator::onLaneletMapBin, this, _1)); - sub_scenario_ = this->create_subscription( - "~/input/scenario", 1, std::bind(&CostmapGenerator::onScenario, this, _1)); + std::bind(&CostmapGenerator::onLaneletMapBin, this, std::placeholders::_1)); // Publishers pub_costmap_ = this->create_publisher("~/output/grid_map", 1); @@ -249,26 +243,27 @@ void CostmapGenerator::onLaneletMapBin( } } -void CostmapGenerator::onObjects( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) -{ - objects_ = msg; -} - -void CostmapGenerator::onPoints(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +void CostmapGenerator::update_data() { - points_ = msg; + objects_ = sub_objects_.takeData(); + points_ = sub_points_.takeData(); + scenario_ = sub_scenario_.takeData(); } -void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr msg) +void CostmapGenerator::set_current_pose() { - scenario_ = msg; + current_pose_ = getCurrentPose(tf_buffer_, this->get_logger()); } void CostmapGenerator::onTimer() { + update_data(); + autoware::universe_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_); stop_watch.tic(); + + if (!param_->activate_by_scenario) set_current_pose(); + if (!isActive()) { // Publish ProcessingTime tier4_debug_msgs::msg::Float64Stamped processing_time_msg; @@ -326,19 +321,15 @@ bool CostmapGenerator::isActive() } if (param_->activate_by_scenario) { - if (scenario_) { - const auto & s = scenario_->activating_scenarios; - if ( - std::find(std::begin(s), std::end(s), tier4_planning_msgs::msg::Scenario::PARKING) != - std::end(s)) { - return true; - } - } - return false; + if (!scenario_) return false; + const auto & s = scenario_->activating_scenarios; + return std::any_of(s.begin(), s.end(), [](const auto scen) { + return scen == tier4_planning_msgs::msg::Scenario::PARKING; + }); } - const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger()); - if (!current_pose_wrt_map) return false; - return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose); + + if (!current_pose_) return false; + return isInParkingLot(lanelet_map_, current_pose_->pose); } void CostmapGenerator::initGridmap() @@ -374,12 +365,11 @@ grid_map::Matrix CostmapGenerator::generatePointsCostmap( return points_costmap; } -autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( - const tf2_ros::Buffer & tf_buffer, - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, +PredictedObjects::ConstSharedPtr transformObjects( + const tf2_ros::Buffer & tf_buffer, const PredictedObjects::ConstSharedPtr in_objects, const std::string & target_frame_id, const std::string & src_frame_id) { - auto objects = new autoware_perception_msgs::msg::PredictedObjects(); + auto objects = std::make_shared(); *objects = *in_objects; objects->header.frame_id = target_frame_id; @@ -398,11 +388,11 @@ autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; } - return autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); + return PredictedObjects::ConstSharedPtr(objects); } grid_map::Matrix CostmapGenerator::generateObjectsCostmap( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const PredictedObjects::ConstSharedPtr in_objects) { const auto object_frame = in_objects->header.frame_id; const auto transformed_objects = @@ -417,11 +407,25 @@ grid_map::Matrix CostmapGenerator::generateObjectsCostmap( grid_map::Matrix CostmapGenerator::generatePrimitivesCostmap() { grid_map::GridMap lanelet2_costmap = costmap_; - if (!primitives_polygons_.empty()) { - object_map::fill_polygon_areas( - lanelet2_costmap, primitives_polygons_, LayerName::primitives, param_->grid_max_value, - param_->grid_min_value, param_->costmap_frame, param_->map_frame, tf_buffer_); + if (primitives_polygons_.empty()) { + return lanelet2_costmap[LayerName::primitives]; + } + + geometry_msgs::msg::TransformStamped primitives2costmap; + try { + primitives2costmap = + tf_buffer_.lookupTransform(param_->costmap_frame, param_->map_frame, tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR(rclcpp::get_logger("costmap_generator"), "%s", ex.what()); } + + const auto transformed_primitives = + getTransformedPrimitives(primitives_polygons_, primitives2costmap); + + object_map::fill_polygon_areas( + lanelet2_costmap, transformed_primitives, LayerName::primitives, param_->grid_max_value, + param_->grid_min_value); + return lanelet2_costmap[LayerName::primitives]; } diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp b/planning/autoware_costmap_generator/src/utils/object_map_utils.cpp similarity index 79% rename from planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp rename to planning/autoware_costmap_generator/src/utils/object_map_utils.cpp index 1b8764e4d3f1c..e4dcaccb4ed6d 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp +++ b/planning/autoware_costmap_generator/src/utils/object_map_utils.cpp @@ -30,7 +30,7 @@ * */ -#include "autoware_costmap_generator/object_map_utils.hpp" +#include "autoware/costmap_generator/utils/object_map_utils.hpp" #include #include @@ -46,23 +46,16 @@ namespace autoware::costmap_generator::object_map void fill_polygon_areas( grid_map::GridMap & out_grid_map, const std::vector & in_polygons, const std::string & in_grid_layer_name, const float in_layer_background_value, - const float in_fill_value, const std::string & in_tf_target_frame, - const std::string & in_tf_source_frame, const tf2_ros::Buffer & in_tf_buffer) + const float in_fill_value) { if (!out_grid_map.exists(in_grid_layer_name)) { out_grid_map.add(in_grid_layer_name); } out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value); - const geometry_msgs::msg::TransformStamped transform = - in_tf_buffer.lookupTransform(in_tf_target_frame, in_tf_source_frame, tf2::TimePointZero); - for (const auto & poly : in_polygons) { - // transform from Map to GridMap coordinates - geometry_msgs::msg::Polygon transformed_poly; - tf2::doTransform(poly, transformed_poly, transform); grid_map::Polygon grid_map_poly; - for (const auto & p : transformed_poly.points) { + for (const auto & p : poly.points) { grid_map_poly.addVertex({p.x, p.y}); } for (grid_map_utils::PolygonIterator it(out_grid_map, grid_map_poly); !it.isPastEnd(); ++it) { diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp similarity index 99% rename from planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp rename to planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp index efa0ea6651632..bcd2e15fe9ca7 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp @@ -42,7 +42,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "autoware_costmap_generator/objects_to_costmap.hpp" +#include "autoware/costmap_generator/utils/objects_to_costmap.hpp" #include #include diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp b/planning/autoware_costmap_generator/src/utils/points_to_costmap.cpp similarity index 98% rename from planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp rename to planning/autoware_costmap_generator/src/utils/points_to_costmap.cpp index 723c27201ac8a..6b2dd2b20d672 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/src/utils/points_to_costmap.cpp @@ -42,7 +42,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "autoware_costmap_generator/points_to_costmap.hpp" +#include "autoware/costmap_generator/utils/points_to_costmap.hpp" #include #include diff --git a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp new file mode 100644 index 0000000000000..00c35e2514f43 --- /dev/null +++ b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp @@ -0,0 +1,170 @@ +// 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 "autoware/costmap_generator/costmap_generator.hpp" + +#include +#include +#include + +#include + +using autoware::costmap_generator::CostmapGenerator; +using tier4_planning_msgs::msg::Scenario; + +class TestCostmapGenerator : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + set_up_node(); + setup_lanelet_map(); + } + + void set_up_node() + { + auto node_options = rclcpp::NodeOptions{}; + const auto costmap_generator_dir = + ament_index_cpp::get_package_share_directory("autoware_costmap_generator"); + node_options.arguments( + {"--ros-args", "--params-file", + costmap_generator_dir + "/config/costmap_generator.param.yaml"}); + costmap_generator_ = std::make_shared(node_options); + } + + void setup_lanelet_map() + { + const auto lanelet2_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "overlap_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, 5.0); + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(map_bin_msg, lanelet_map_); + } + + [[nodiscard]] double get_grid_length_x() { return costmap_generator_->costmap_.getLength()[0]; } + + [[nodiscard]] double get_grid_length_y() { return costmap_generator_->costmap_.getLength()[1]; } + + [[nodiscard]] double get_grid_resolution() + { + return costmap_generator_->costmap_.getResolution(); + } + + [[nodiscard]] std::vector get_grid_layers() + { + return costmap_generator_->costmap_.getLayers(); + } + + size_t test_load_road_areas() + { + std::vector road_areas; + costmap_generator_->loadRoadAreasFromLaneletMap(lanelet_map_, road_areas); + return road_areas.size(); + } + + size_t test_load_parking_areas() + { + std::vector parking_areas; + costmap_generator_->loadParkingAreasFromLaneletMap(lanelet_map_, parking_areas); + return parking_areas.size(); + } + + bool test_is_active_by_pos( + const bool is_within_parking_lot = true, const bool no_lanelet_map = false) + { + costmap_generator_->lanelet_map_.reset(); + if (no_lanelet_map) { + return costmap_generator_->isActive(); + } + + costmap_generator_->lanelet_map_ = lanelet_map_; + costmap_generator_->param_->activate_by_scenario = false; + + geometry_msgs::msg::PoseStamped::SharedPtr p(new geometry_msgs::msg::PoseStamped()); + p->pose.position.x = 3697.07; + p->pose.position.y = 73735.49; + if (!is_within_parking_lot) { + p->pose.position.x += 10.0; + } + + costmap_generator_->current_pose_ = p; + return costmap_generator_->isActive(); + } + + bool test_is_active_by_scenario( + const bool is_parking_scenario = true, const bool no_lanelet_map = false) + { + costmap_generator_->lanelet_map_.reset(); + if (no_lanelet_map) { + return costmap_generator_->isActive(); + } + + costmap_generator_->lanelet_map_ = lanelet_map_; + costmap_generator_->param_->activate_by_scenario = true; + + Scenario scenario; + scenario.current_scenario = is_parking_scenario ? Scenario::PARKING : Scenario::LANEDRIVING; + if (is_parking_scenario) { + scenario.activating_scenarios.push_back(Scenario::PARKING); + } + + costmap_generator_->scenario_ = std::make_shared(scenario); + return costmap_generator_->isActive(); + } + + void TearDown() override + { + rclcpp::shutdown(); + costmap_generator_ = nullptr; + lanelet_map_ = nullptr; + } + + std::shared_ptr costmap_generator_; + lanelet::LaneletMapPtr lanelet_map_; +}; + +TEST_F(TestCostmapGenerator, testInitializeGridmap) +{ + const double grid_resolution = get_grid_resolution(); + const double grid_length_x = get_grid_length_x(); + const double grid_length_y = get_grid_length_y(); + EXPECT_FLOAT_EQ(grid_resolution, 0.3); + EXPECT_TRUE(70.0 - grid_length_x < grid_resolution); + EXPECT_TRUE(70.0 - grid_length_y < grid_resolution); + + const auto grid_layers = get_grid_layers(); + + EXPECT_TRUE(std::find(grid_layers.begin(), grid_layers.end(), "points") != grid_layers.end()); + EXPECT_TRUE(std::find(grid_layers.begin(), grid_layers.end(), "objects") != grid_layers.end()); + EXPECT_TRUE(std::find(grid_layers.begin(), grid_layers.end(), "primitives") != grid_layers.end()); + EXPECT_TRUE(std::find(grid_layers.begin(), grid_layers.end(), "combined") != grid_layers.end()); +} + +TEST_F(TestCostmapGenerator, testLoadAreasFromLaneletMap) +{ + EXPECT_EQ(test_load_road_areas(), 12ul); + EXPECT_EQ(test_load_parking_areas(), 10ul); +} + +TEST_F(TestCostmapGenerator, testIsActive) +{ + EXPECT_TRUE(test_is_active_by_pos()); + EXPECT_FALSE(test_is_active_by_pos(false)); + EXPECT_FALSE(test_is_active_by_pos(true, true)); + + EXPECT_TRUE(test_is_active_by_scenario()); + EXPECT_FALSE(test_is_active_by_scenario(false)); + EXPECT_FALSE(test_is_active_by_scenario(true, true)); +} diff --git a/planning/autoware_costmap_generator/test/test_object_map_utils.cpp b/planning/autoware_costmap_generator/test/test_object_map_utils.cpp new file mode 100644 index 0000000000000..a20c2326d2757 --- /dev/null +++ b/planning/autoware_costmap_generator/test/test_object_map_utils.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 +#include + +#include + +namespace +{ +grid_map::GridMap construct_gridmap( + const double length_x, const double length_y, const double resolution, const double position_x, + const double position_y) +{ + grid_map::GridMap gm; + + gm.setFrameId("map"); + // set gridmap size,resolution + gm.setGeometry(grid_map::Length(length_x, length_y), resolution); + // center of grid to position p in map frame + gm.setPosition(grid_map::Position(position_x, position_y)); + + // set initial value + gm.add("primitives", 0); + + return gm; +} + +geometry_msgs::msg::Polygon get_primitive_polygon( + const double min_x, const double min_y, const double max_x, const double max_y) +{ + const auto get_point = [](const double x, const double y) { + geometry_msgs::msg::Point32 point; + point.x = x; + point.y = y; + point.z = 0.0; + return point; + }; + geometry_msgs::msg::Polygon polygon; + polygon.points.push_back(get_point(min_x, min_y)); + polygon.points.push_back(get_point(min_x, max_y)); + polygon.points.push_back(get_point(max_x, max_y)); + polygon.points.push_back(get_point(max_x, min_y)); + return polygon; +} +} // namespace + +namespace autoware::costmap_generator +{ +TEST(ObjectMapUtilsTest, testFillPolygonAreas) +{ + const double grid_length_x = 21; + const double grid_length_y = 21; + const double grid_resolution = 1.0; + const double grid_position_x = 0.0; + const double grid_position_y = 0.0; + grid_map::GridMap gridmap = construct_gridmap( + grid_length_x, grid_length_y, grid_resolution, grid_position_x, grid_position_y); + + std::vector primitives_polygons; + primitives_polygons.emplace_back(get_primitive_polygon(-15.0, -2.0, 15.0, 2.0)); + primitives_polygons.emplace_back(get_primitive_polygon(-5.0, -5.0, 5.0, 5.0)); + + const double min_value = 0.0; + const double max_value = 1.0; + + object_map::fill_polygon_areas(gridmap, primitives_polygons, "primitives", max_value, min_value); + + const auto costmap = gridmap["primitives"]; + + int empty_grid_cell_num = 0; + for (int i = 0; i < costmap.rows(); i++) { + for (int j = 0; j < costmap.cols(); j++) { + if (costmap(i, j) == min_value) { + empty_grid_cell_num += 1; + } + } + } + + EXPECT_EQ(empty_grid_cell_num, 144); +} +} // namespace autoware::costmap_generator diff --git a/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp index 8d437865b8793..4ab5a32e77028 100644 --- a/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp @@ -12,14 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include #include #include +#include + +namespace +{ +geometry_msgs::msg::Point32 toPoint32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 point; + point.x = pose.position.x; + point.y = pose.position.y; + point.z = pose.position.z; + return point; +} +} // namespace namespace autoware::costmap_generator { using LABEL = autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; class ObjectsToCostMapTest : public ::testing::Test { @@ -30,6 +47,10 @@ class ObjectsToCostMapTest : public ::testing::Test grid_map::GridMap construct_gridmap(); + [[nodiscard]] PredictedObject get_object( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 dimension, + const bool box_type = true) const; + public: double grid_resolution_ = 1; double grid_length_x_ = 21; @@ -60,6 +81,38 @@ grid_map::GridMap ObjectsToCostMapTest::construct_gridmap() return gm; } +PredictedObject ObjectsToCostMapTest::get_object( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 dimension, + const bool box_type) const +{ + PredictedObject object; + object.classification.push_back(LABEL{}); + object.classification.at(0).label = LABEL::CAR; + object.classification.at(0).probability = 0.8; + object.kinematics.initial_pose_with_covariance.pose = pose; + object.shape.dimensions = dimension; + if (box_type) { + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + return object; + } + + object.shape.type = autoware_perception_msgs::msg::Shape::POLYGON; + + object.shape.footprint.points.emplace_back(toPoint32( + autoware::universe_utils::calcOffsetPose(pose, -0.5 * dimension.x, -0.5 * dimension.y, 0.0))); + + object.shape.footprint.points.emplace_back(toPoint32( + autoware::universe_utils::calcOffsetPose(pose, -0.5 * dimension.x, 0.5 * dimension.y, 0.0))); + + object.shape.footprint.points.emplace_back(toPoint32( + autoware::universe_utils::calcOffsetPose(pose, 0.5 * dimension.x, 0.5 * dimension.y, 0.0))); + + object.shape.footprint.points.emplace_back(toPoint32( + autoware::universe_utils::calcOffsetPose(pose, 0.5 * dimension.x, -0.5 * dimension.y, 0.0))); + + return object; +} + /* grid_y | @@ -69,29 +122,89 @@ grid_y | map_y |--------grid_x */ -TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) +TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects_BoxType) { - auto objs = std::make_shared(); - autoware_perception_msgs::msg::PredictedObject object; + auto objs = std::make_shared(); + + geometry_msgs::msg::Pose obj_pose; + obj_pose.position.x = 1; + obj_pose.position.y = 2; + obj_pose.position.z = 1; + obj_pose.orientation.x = 0; + obj_pose.orientation.y = 0; + obj_pose.orientation.z = 0; + obj_pose.orientation.w = 1; + + geometry_msgs::msg::Vector3 dimension; + dimension.x = 5; + dimension.y = 3; + dimension.z = 2; + + const auto object = get_object(obj_pose, dimension); + objs->objects.push_back(object); - object.classification.push_back(autoware_perception_msgs::msg::ObjectClassification{}); - object.classification.at(0).label = LABEL::CAR; - object.classification.at(0).probability = 0.8; + grid_map::GridMap gridmap = construct_gridmap(); + ObjectsToCostmap objectsToCostmap; - object.kinematics.initial_pose_with_covariance.pose.position.x = 1; - object.kinematics.initial_pose_with_covariance.pose.position.y = 2; - object.kinematics.initial_pose_with_covariance.pose.position.z = 1; - // yaw=0 for easy test - object.kinematics.initial_pose_with_covariance.pose.orientation.x = 0; - object.kinematics.initial_pose_with_covariance.pose.orientation.y = 0; - object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0; - object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1; + const double expand_polygon_size = 0.0; + const double size_of_expansion_kernel = 1; // do not expand for easy test check + grid_map::Matrix objects_costmap = objectsToCostmap.makeCostmapFromObjects( + gridmap, expand_polygon_size, size_of_expansion_kernel, objs); - object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - object.shape.dimensions.x = 5; - object.shape.dimensions.y = 3; - object.shape.dimensions.z = 2; + // yaw = 0,so we can just calculate like this easily + int expected_non_empty_cost_grid_num = + (object.shape.dimensions.x * object.shape.dimensions.y) / grid_resolution_; + // check if cost is correct + int non_empty_cost_grid_num = 0; + for (int i = 0; i < objects_costmap.rows(); i++) { + for (int j = 0; j < objects_costmap.cols(); j++) { + if (objects_costmap(i, j) == object.classification.at(0).probability) { + non_empty_cost_grid_num += 1; + } + } + } + + EXPECT_EQ(non_empty_cost_grid_num, expected_non_empty_cost_grid_num); + + float obj_center_x = + grid_length_x_ / 2 - object.kinematics.initial_pose_with_covariance.pose.position.x; + float obj_center_y = + grid_length_y_ / 2 - object.kinematics.initial_pose_with_covariance.pose.position.y; + float obj_left_x = obj_center_x - object.shape.dimensions.x / 2.; + float obj_right_x = obj_center_x + object.shape.dimensions.x / 2.; + float obj_bottom_y = obj_center_y - object.shape.dimensions.y / 2.; + float obj_top_y = obj_center_y + object.shape.dimensions.y / 2.; + int index_x_min = static_cast(obj_left_x / grid_resolution_); + int index_x_max = static_cast(obj_right_x / grid_resolution_); + int index_y_min = static_cast(obj_bottom_y / grid_resolution_); + int index_y_max = static_cast(obj_top_y / grid_resolution_); + for (int i = index_x_min; i < index_x_max; i++) { + for (int j = index_y_min; j < index_y_max; j++) { + EXPECT_DOUBLE_EQ(objects_costmap(i, j), object.classification.at(0).probability); + } + } +} + +TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects_PolygonType) +{ + auto objs = std::make_shared(); + + geometry_msgs::msg::Pose obj_pose; + obj_pose.position.x = 1; + obj_pose.position.y = 2; + obj_pose.position.z = 1; + obj_pose.orientation.x = 0; + obj_pose.orientation.y = 0; + obj_pose.orientation.z = 0; + obj_pose.orientation.w = 1; + + geometry_msgs::msg::Vector3 dimension; + dimension.x = 5; + dimension.y = 3; + dimension.z = 2; + + const auto object = get_object(obj_pose, dimension, false); objs->objects.push_back(object); grid_map::GridMap gridmap = construct_gridmap(); diff --git a/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp index 6c52e2dc6ce9d..b98479f43c2aa 100644 --- a/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include