test(costmap_generator): unit test implementation for costmap generat…

…or (#9149) * modify costmap generator directory structure Signed-off-by: mohammad alqudah <[email protected]> * rename class CostmapGenerator to CostmapGeneratorNode Signed-off-by: mohammad alqudah <[email protected]> * unit test for object_map_utils Signed-off-by: mohammad alqudah <[email protected]> * catch error from lookupTransform Signed-off-by: mohammad alqudah <[email protected]> * use polling subscriber in costmap generator node Signed-off-by: mohammad alqudah <[email protected]> * add test for costmap generator node Signed-off-by: mohammad alqudah <[email protected]> * add test for isActive() Signed-off-by: mohammad alqudah <[email protected]> * revert unnecessary changes Signed-off-by: mohammad alqudah <[email protected]> * remove commented out line Signed-off-by: mohammad alqudah <[email protected]> * minor fix Signed-off-by: mohammad alqudah <[email protected]> * Update planning/autoware_costmap_generator/src/costmap_generator.cpp Co-authored-by: Kosuke Takeuchi <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Kosuke Takeuchi <[email protected]>
autowarefoundation · Nov 12, 2024 · d187722 · d187722
1 parent a3b2723
commit d187722
Show file tree

Hide file tree

Showing 15 changed files with 844 additions and 365 deletions.
diff --git a/common/autoware_test_utils/test_map/overlap_map.osm b/common/autoware_test_utils/test_map/overlap_map.osm
diff --git 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/...e_costmap_generator/costmap_generator.hpp → ...e/costmap_generator/costmap_generator.hpp b/...e_costmap_generator/costmap_generator.hpp → ...e/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 <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/ros/processing_time_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/system/time_keeper.hpp>
@@ -72,8 +73,12 @@
 #include <memory>
 #include <vector>
 
+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<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr pub_processing_time_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_ms_;
 
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_points_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::PredictedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr sub_lanelet_bin_map_;
-  rclcpp::Subscription<tier4_planning_msgs::msg::Scenario>::SharedPtr sub_scenario_;
+  autoware::universe_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
+    sub_points_{this, "~/input/points_no_ground", autoware::universe_utils::SingleDepthSensorQoS()};
+  autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> sub_objects_{
+    this, "~/input/objects"};
+  autoware::universe_utils::InterProcessPollingSubscriber<tier4_planning_msgs::msg::Scenario>
+    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<std::chrono::milliseconds> 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/...re_costmap_generator/object_map_utils.hpp → ...tmap_generator/utils/object_map_utils.hpp b/...re_costmap_generator/object_map_utils.hpp → ...tmap_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 <grid_map_ros/grid_map_ros.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -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<geometry_msgs::msg::Polygon> & 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/..._costmap_generator/objects_to_costmap.hpp → ...ap_generator/utils/objects_to_costmap.hpp b/..._costmap_generator/objects_to_costmap.hpp → ...ap_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 <grid_map_ros/grid_map_ros.hpp>
 
@@ -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/...e_costmap_generator/points_to_costmap.hpp → ...map_generator/utils/points_to_costmap.hpp b/...e_costmap_generator/points_to_costmap.hpp → ...map_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 <grid_map_ros/grid_map_ros.hpp>
 
@@ -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
@@ -19,6 +19,7 @@
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_perception_msgs</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>generate_parameter_library</depend>
   <depend>grid_map_ros</depend>