test: add test for objects_to_costmap (tier4#1137)

* test: add test for objects_to_costmap

Signed-off-by: suchang <chang.su@autocore.ai>

* ci(pre-commit): autofix

* test: change variable name for spell-check

Signed-off-by: suchang <chang.su@autocore.ai>

* test: add tf2 related header

Signed-off-by: suchang <chang.su@autocore.ai>

* ci(pre-commit): autofix

* test: add tf2 related header file

Signed-off-by: suchang <chang.su@autocore.ai>

* ci(pre-commit): autofix

* test: change linked lib

Signed-off-by: suchang <chang.su@autocore.ai>

* Include tf2_geometry_msgs.hpp in objects_to_costmap.hpp: fix build error

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* ci(pre-commit): autofix

* fix pre-commit.ci error

Signed-off-by: suchang <chang.su@autocore.ai>

Co-authored-by: suchang <chang.su@autocore.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>

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)

planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp

@@ -46,7 +46,11 @@
 #define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_
 
 #include <grid_map_ros/grid_map_ros.hpp>
-#include <rclcpp/rclcpp.hpp>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 

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 <costmap_generator/objects_to_costmap.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+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::PredictedObjects>();
+  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<int>(obj_left_x / grid_resolution_);
+  int index_x_max = static_cast<int>(obj_right_x / grid_resolution_);
+  int index_y_min = static_cast<int>(obj_bottom_y / grid_resolution_);
+  int index_y_max = static_cast<int>(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);
+    }
+  }
+}