From a95cb67a9b8cc740122053bb5d1bc42bf1e65cf6 Mon Sep 17 00:00:00 2001 From: storrrrrrrrm <103425473+storrrrrrrrm@users.noreply.github.com> Date: Mon, 27 Jun 2022 17:36:55 +0800 Subject: [PATCH] test: add test for objects_to_costmap (#1137) * test: add test for objects_to_costmap Signed-off-by: suchang * ci(pre-commit): autofix * test: change variable name for spell-check Signed-off-by: suchang * test: add tf2 related header Signed-off-by: suchang * ci(pre-commit): autofix * test: add tf2 related header file Signed-off-by: suchang * ci(pre-commit): autofix * test: change linked lib Signed-off-by: suchang * Include tf2_geometry_msgs.hpp in objects_to_costmap.hpp: fix build error Signed-off-by: Maxime CLEMENT * ci(pre-commit): autofix * fix pre-commit.ci error Signed-off-by: suchang Co-authored-by: suchang Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Maxime CLEMENT --- planning/costmap_generator/CMakeLists.txt | 7 + .../costmap_generator/objects_to_costmap.hpp | 6 +- .../test/test_objects_to_costmap.cpp | 136 ++++++++++++++++++ 3 files changed, 148 insertions(+), 1 deletion(-) create mode 100644 planning/costmap_generator/test/test_objects_to_costmap.cpp diff --git a/planning/costmap_generator/CMakeLists.txt b/planning/costmap_generator/CMakeLists.txt index 648d01ee4c4e4..b0a64648b5e44 100644 --- a/planning/costmap_generator/CMakeLists.txt +++ b/planning/costmap_generator/CMakeLists.txt @@ -47,6 +47,13 @@ rclcpp_components_register_node(costmap_generator_node if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) + ament_add_ros_isolated_gtest(test_objects_to_costmap + test/test_objects_to_costmap.cpp + ) + + target_link_libraries(test_objects_to_costmap + costmap_generator_lib + ) endif() ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp index 92915ab392f3d..d683052ced28d 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp @@ -46,7 +46,11 @@ #define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ #include -#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif #include diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/costmap_generator/test/test_objects_to_costmap.cpp new file mode 100644 index 0000000000000..667ee70ef53ff --- /dev/null +++ b/planning/costmap_generator/test/test_objects_to_costmap.cpp @@ -0,0 +1,136 @@ +// Copyright 2022 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 + +using LABEL = autoware_auto_perception_msgs::msg::ObjectClassification; + +class ObjectsToCostMapTest : public ::testing::Test +{ +protected: + void SetUp() override { rclcpp::init(0, nullptr); } + + ~ObjectsToCostMapTest() override { rclcpp::shutdown(); } + + grid_map::GridMap construct_gridmap(); + +public: + double grid_resolution_ = 1; + double grid_length_x_ = 21; + double grid_length_y_ = 21; + double grid_position_x_ = 0; + double grid_position_y_ = 0; +}; + +grid_map::GridMap ObjectsToCostMapTest::construct_gridmap() +{ + grid_map::GridMap gm; + + // set gridmap size + gm.setFrameId("map"); + gm.setGeometry( + grid_map::Length(grid_length_x_, grid_length_y_), grid_resolution_, + grid_map::Position(grid_position_x_, grid_position_y_)); + + // set initial value + gm.add("objects", 0.); + + // set car postion in map frame to center of grid + grid_map::Position p; + p.x() = 0; + p.y() = 0; + gm.setPosition(p); + + return gm; +} + +/* +grid_y +| +| map_x--- +| | +| | +| map_y +|--------grid_x +*/ +TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) +{ + auto objs = std::make_shared(); + autoware_auto_perception_msgs::msg::PredictedObject object; + + object.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + object.classification.at(0).label = LABEL::CAR; + object.classification.at(0).probability = 0.8; + + 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; + + object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.dimensions.x = 5; + object.shape.dimensions.y = 3; + object.shape.dimensions.z = 2; + + objs->objects.push_back(object); + + grid_map::GridMap gridmap = construct_gridmap(); + ObjectsToCostmap objectsToCostmap; + + 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); + + // 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); + } + } +}