Re-enable costmap tests (footprint and inflation tests) (#805)

* re-enabling obstacle, inflation, and footprint tests

* remove static tests, redesign footprint_tests without Costmap2DROS

* passing linters and uncrustify

* replace for_each with for loop

nav2_costmap_2d/package.xml

@@ -46,6 +46,7 @@
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_testing</test_depend>
+  <test_depend>nav2_lifecycle_manager</test_depend>
 
   <export>
 	  <build_type>ament_cmake</build_type>

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -348,7 +348,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
 
   RCLCPP_DEBUG(get_logger(), "Entering loop");
 
-  rclcpp::Rate r(frequency);	// 200ms by default
+  rclcpp::Rate r(frequency);    // 200ms by default
 
   while (rclcpp::ok() && !map_update_thread_shutdown_) {
     nav2_util::ExecutionTimer timer;

nav2_costmap_2d/src/layer.cpp

@@ -83,9 +83,9 @@ Layer::hasParameter(const std::string & param_name)
 void
 Layer::undeclareAllParameters()
 {
-  std::for_each(begin(local_params_), end(local_params_), [this](const std::string & param_name) {
-     node_->undeclare_parameter(getFullName(param_name));
-  });
+  for (auto & param_name : local_params_) {
+    node_->undeclare_parameter(getFullName(param_name));
+  }
   local_params_.clear();
 }
 

nav2_costmap_2d/test/integration/CMakeLists.txt

@@ -1,35 +1,24 @@
-# ament_add_gtest_executable(footprint_tests_exec
-#   footprint_tests.cpp
-# )
-# ament_target_dependencies(footprint_tests_exec
-#   ${dependencies}
-# )
-# target_link_libraries(footprint_tests_exec
-#   nav2_costmap_2d_core
-#   layers
-# )
-
-# ament_add_gtest_executable(static_tests_exec
-#   static_tests.cpp
-# )
-# ament_target_dependencies(static_tests_exec
-#   ${dependencies}
-# )
-# target_link_libraries(static_tests_exec
-#   nav2_costmap_2d_core
-#   layers
-# )
+ament_add_gtest_executable(footprint_tests_exec
+  footprint_tests.cpp
+)
+ament_target_dependencies(footprint_tests_exec
+  ${dependencies}
+)
+target_link_libraries(footprint_tests_exec
+  nav2_costmap_2d_core
+  layers
+)
 
-# ament_add_gtest_executable(inflation_tests_exec
-#   inflation_tests.cpp
-# )
-# ament_target_dependencies(inflation_tests_exec
-#   ${dependencies}
-# )
-# target_link_libraries(inflation_tests_exec
-#   nav2_costmap_2d_core
-#   layers
-# )
+ament_add_gtest_executable(inflation_tests_exec
+  inflation_tests.cpp
+)
+ament_target_dependencies(inflation_tests_exec
+  ${dependencies}
+)
+target_link_libraries(inflation_tests_exec
+  nav2_costmap_2d_core
+  layers
+)
 
 ament_add_gtest_executable(obstacle_tests_exec
   obstacle_tests.cpp
@@ -42,35 +31,25 @@ target_link_libraries(obstacle_tests_exec
   layers
 )
 
-# ament_add_test(footprint_tests
-#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
-#   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-#   ENV
-#     TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
-#     TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
-#     TEST_EXECUTABLE=$<TARGET_FILE:footprint_tests_exec>
-# )
-
-# ament_add_test(static_tests
-#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
-#   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-#   ENV
-#     TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
-#     TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
-#     TEST_EXECUTABLE=$<TARGET_FILE:static_tests_exec>
-# )
+ament_add_test(footprint_tests
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:footprint_tests_exec>
+)
 
-# ament_add_test(inflation_tests
-#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
-#   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-#   ENV
-#     TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
-#     TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
-#     TEST_EXECUTABLE=$<TARGET_FILE:inflation_tests_exec>
-# )
+ament_add_test(inflation_tests
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:inflation_tests_exec>
+)
 
 ament_add_test(obstacle_tests
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO

nav2_costmap_2d/test/integration/costmap_tests_launch.py

@@ -20,9 +20,9 @@
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import ExecuteProcess
-import launch_ros.actions
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
 

nav2_costmap_2d/test/integration/footprint_tests.cpp

@@ -41,7 +41,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #include "tf2_ros/transform_listener.h"
-#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
 
 class RclCppFixture
 {
@@ -51,12 +51,14 @@ class RclCppFixture
 };
 RclCppFixture g_rclcppfixture;
 
-class FootprintTestNode : public nav2_costmap_2d::Costmap2DROS
+class FootprintTestNode
 {
 public:
-  FootprintTestNode(std::string name, tf2_ros::Buffer & buffer)
-  : nav2_costmap_2d::Costmap2DROS(name, buffer)
-  {}
+  FootprintTestNode()
+  {
+    // Default footprint padding and footprint radius from Costmap2DROS
+    testFootprint(0.01f, 0.1);
+  }
 
   ~FootprintTestNode() {}
 
@@ -66,56 +68,54 @@ class FootprintTestNode : public nav2_costmap_2d::Costmap2DROS
     if (footprint != "" && footprint != "[]") {
       std::vector<geometry_msgs::msg::Point> new_footprint;
       if (nav2_costmap_2d::makeFootprintFromString(footprint, new_footprint)) {
-        nav2_costmap_2d::Costmap2DROS::setUnpaddedRobotFootprint(new_footprint);
+        setRobotFootprint(new_footprint);
       } else {
-        RCLCPP_ERROR(get_logger(), "Invalid footprint string");
+        RCLCPP_ERROR(rclcpp::get_logger("footprint_tester"), "Invalid footprint string");
       }
     }
   }
+
   void testFootprint(double footprint_padding, double robot_radius)
   {
     footprint_padding_ = footprint_padding;
-    nav2_costmap_2d::Costmap2DROS::setUnpaddedRobotFootprint(
-      nav2_costmap_2d::makeFootprintFromRadius(robot_radius));
+    setRobotFootprint(nav2_costmap_2d::makeFootprintFromRadius(robot_radius));
+  }
+
+  std::vector<geometry_msgs::msg::Point> getRobotFootprint()
+  {
+    return footprint_;
   }
+
+protected:
+  void setRobotFootprint(const std::vector<geometry_msgs::msg::Point> & points)
+  {
+    footprint_ = points;
+    nav2_costmap_2d::padFootprint(footprint_, footprint_padding_);
+  }
+
+  double footprint_padding_;
+  std::vector<geometry_msgs::msg::Point> footprint_;
 };
 
 class TestNode : public ::testing::Test
 {
 public:
   TestNode()
   {
-    auto node = nav2_util::LifecycleNode::make_shared("footprint_tests");
-
-    tf_ = new tf2_ros::Buffer(node->get_clock());
-    tfl_ = new tf2_ros::TransformListener(*tf_);
-
-    // This empty transform is added to satisfy the constructor of
-    // Costmap2DROS, which waits for the transform from map to base_link
-    // to become available.
-    geometry_msgs::msg::TransformStamped base_rel_map;
-    base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
-    base_rel_map.child_frame_id = "base_link";
-    base_rel_map.header.frame_id = "map";
-    base_rel_map.header.stamp = node->now();
-    tf_->setTransform(base_rel_map, "footprint_tests");
-
-    costmap_ = new FootprintTestNode("costmap_footprint_tests", *tf_);
+    footprint_tester_ = std::make_shared<FootprintTestNode>();
   }
 
   ~TestNode() {}
 
 protected:
-  FootprintTestNode * costmap_;
-  tf2_ros::TransformListener * tfl_;
-  tf2_ros::Buffer * tf_;
+  std::shared_ptr<FootprintTestNode> footprint_tester_;
 };
 
 // Start with empty test before updating test footprints
 TEST_F(TestNode, footprint_empty)
 {
   // FootprintTestNode cm("costmap_footprint_empty", *tf_);
-  std::vector<geometry_msgs::msg::Point> footprint = costmap_->getRobotFootprint();
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
   // With no specification of footprint or radius,
   // defaults to 0.1 meter radius plus 0.01 meter padding.
   EXPECT_EQ(16, footprint.size());
@@ -127,9 +127,9 @@ TEST_F(TestNode, footprint_empty)
 
 TEST_F(TestNode, unpadded_footprint_from_string_param)
 {
-  costmap_->testFootprint(0.0, "[[1, 1], [-1, 1], [-1, -1]]");
+  footprint_tester_->testFootprint(0.0, "[[1, 1], [-1, 1], [-1, -1]]");
 
-  std::vector<geometry_msgs::msg::Point> footprint = costmap_->getRobotFootprint();
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
   EXPECT_EQ(3, footprint.size());
 
   EXPECT_EQ(1.0f, footprint[0].x);
@@ -147,9 +147,9 @@ TEST_F(TestNode, unpadded_footprint_from_string_param)
 
 TEST_F(TestNode, padded_footprint_from_string_param)
 {
-  costmap_->testFootprint(0.5, "[[1, 1], [-1, 1], [-1, -1]]");
+  footprint_tester_->testFootprint(0.5, "[[1, 1], [-1, 1], [-1, -1]]");
 
-  std::vector<geometry_msgs::msg::Point> footprint = costmap_->getRobotFootprint();
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
   EXPECT_EQ(3, footprint.size());
 
   EXPECT_EQ(1.5f, footprint[0].x);
@@ -167,8 +167,8 @@ TEST_F(TestNode, padded_footprint_from_string_param)
 
 TEST_F(TestNode, radius_param)
 {
-  costmap_->testFootprint(0, 10.0);
-  std::vector<geometry_msgs::msg::Point> footprint = costmap_->getRobotFootprint();
+  footprint_tester_->testFootprint(0, 10.0);
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
   // Circular robot has 16-point footprint auto-generated.
   EXPECT_EQ(16, footprint.size());
 
@@ -185,8 +185,8 @@ TEST_F(TestNode, radius_param)
 
 TEST_F(TestNode, footprint_from_same_level_param)
 {
-  costmap_->testFootprint(0.0, "[[1, 2], [3, 4], [5, 6]]");
-  std::vector<geometry_msgs::msg::Point> footprint = costmap_->getRobotFootprint();
+  footprint_tester_->testFootprint(0.0, "[[1, 2], [3, 4], [5, 6]]");
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
   EXPECT_EQ(3, footprint.size());
 
   EXPECT_EQ(1.0f, footprint[0].x);