add humble-navigation2

navigation2/nav2_costmap_2d/test/CMakeLists.txt

@@ -0,0 +1,6 @@
+set(TEST_MAP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/map)
+set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files)
+
+add_subdirectory(unit)
+add_subdirectory(integration)
+add_subdirectory(regression)

navigation2/nav2_costmap_2d/test/costmap_params.yaml

@@ -0,0 +1,31 @@
+global_frame: map
+robot_base_frame: base_link
+update_frequency: 5.0
+publish_frequency: 0.0
+static_map: true
+rolling_window: false
+
+#START VOXEL STUFF
+map_type: voxel
+origin_z: 0.0
+z_resolution: 0.2
+z_voxels: 10
+unknown_threshold: 10
+mark_threshold: 0
+#END VOXEL STUFF
+
+transform_tolerance: 0.3
+obstacle_max_range: 2.5
+obstacle_min_range: 0.0
+max_obstacle_height: 2.0
+raytrace_max_range: 3.0
+raytrace_min_range: 0.0
+footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
+#robot_radius: 0.46
+footprint_padding: 0.01
+inflation_radius: 0.55
+cost_scaling_factor: 10.0
+lethal_cost_threshold: 100
+observation_sources: base_scan
+base_scan: {data_type: LaserScan, expected_update_rate: 0.4,
+  observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

navigation2/nav2_costmap_2d/test/integration/CMakeLists.txt

@@ -0,0 +1,110 @@
+ament_add_gtest_executable(footprint_tests_exec
+  footprint_tests.cpp
+)
+target_link_libraries(footprint_tests_exec
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::layers
+)
+
+ament_add_gtest_executable(test_collision_checker_exec
+  test_costmap_topic_collision_checker.cpp
+)
+target_link_libraries(test_collision_checker_exec
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::nav2_costmap_2d_client
+  ${PROJECT_NAME}::layers
+)
+
+ament_add_gtest_executable(inflation_tests_exec
+  inflation_tests.cpp
+)
+target_link_libraries(inflation_tests_exec
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::layers
+)
+
+ament_add_gtest_executable(obstacle_tests_exec
+  obstacle_tests.cpp
+)
+target_link_libraries(obstacle_tests_exec
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::layers
+)
+
+ament_add_gtest_executable(range_tests_exec
+  range_tests.cpp
+)
+target_link_libraries(range_tests_exec
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::layers
+)
+
+ament_add_gtest(dyn_params_tests
+  dyn_params_tests.cpp
+)
+target_link_libraries(dyn_params_tests
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_test(test_collision_checker
+  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:test_collision_checker_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(obstacle_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:obstacle_tests_exec>
+)
+
+ament_add_test(range_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:range_tests_exec>
+)
+
+## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml,
+## which has a dependency on rosbag playback
+# ament_add_gtest_executable(costmap_tester
+#   costmap_tester.cpp
+# )
+# ament_target_dependencies(costmap_tester
+#   ${dependencies}
+# )
+# target_link_libraries(costmap_tester
+#   ${PROJECT_NAME}::nav2_costmap_2d_core
+#   layers
+# )

navigation2/nav2_costmap_2d/test/integration/costmap_tester.cpp

@@ -0,0 +1,175 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+
+namespace nav2_costmap_2d
+{
+
+std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+
+class CostmapTester : public testing::Test
+{
+public:
+  explicit CostmapTester(tf2_ros::Buffer & tf);
+  void checkConsistentCosts();
+  void compareCellToNeighbors(
+    nav2_costmap_2d::Costmap2D & costmap,
+    unsigned int x, unsigned int y);
+  void compareCells(
+    nav2_costmap_2d::Costmap2D & costmap,
+    unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
+  virtual void TestBody() {}
+};
+
+CostmapTester::CostmapTester(tf2_ros::Buffer & tf)
+{
+  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap", tf);
+}
+
+void CostmapTester::checkConsistentCosts()
+{
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+
+  // get a copy of the costmap contained by our ros wrapper
+  costmap->saveMap("costmap_test.pgm");
+
+  // loop through the costmap and check for any unexpected drop-offs in costs
+  for (unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i) {
+    for (unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j) {
+      compareCellToNeighbors(*costmap, i, j);
+    }
+  }
+}
+
+void CostmapTester::compareCellToNeighbors(
+  nav2_costmap_2d::Costmap2D & costmap,
+  unsigned int x, unsigned int y)
+{
+  // we'll compare the cost of this cell with that of
+  // its eight neighbors to see if they're reasonable
+  for (int offset_x = -1; offset_x <= 1; ++offset_x) {
+    for (int offset_y = -1; offset_y <= 1; ++offset_y) {
+      int nx = x + offset_x;
+      int ny = y + offset_y;
+
+      // check to make sure that the neighbor cell is a legal one
+      if (nx >= 0 && nx < static_cast<int>(costmap.getSizeInCellsX()) && ny >= 0 &&
+        ny < static_cast<int>(costmap.getSizeInCellsY()))
+      {
+        compareCells(costmap, x, y, nx, ny);
+      }
+    }
+  }
+}
+
+// for all lethal and inscribed costs,
+// we'll make sure that their neighbors have the cost values we'd expect
+void CostmapTester::compareCells(
+  nav2_costmap_2d::Costmap2D & costmap,
+  unsigned int x, unsigned int y, unsigned int nx, unsigned int ny)
+{
+  double cell_distance = hypot(static_cast<int>(x - nx), static_cast<int>(y - ny));
+
+  unsigned char cell_cost = costmap.getCost(x, y);
+  unsigned char neighbor_cost = costmap.getCost(nx, ny);
+
+  if (cell_cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
+    // if the cell is a lethal obstacle,
+    // then we know that all its neighbors should have equal or slighlty less cost
+    unsigned char expected_lowest_cost = 0;
+    EXPECT_TRUE(
+      neighbor_cost >= expected_lowest_cost ||
+      (cell_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE));
+  } else if (cell_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+    // the furthest valid distance from an obstacle
+    // is the inscribed radius plus the cell distance away
+    double furthest_valid_distance = 0;
+    unsigned char expected_lowest_cost = 0;
+    if (neighbor_cost < expected_lowest_cost) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(
+          "costmap_tester"),
+        "Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", // NOLINT
+        x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost,
+        cell_distance, furthest_valid_distance);
+      RCLCPP_ERROR(
+        rclcpp::get_logger("costmap_tester"), "Cell: (%d, %d), Neighbor: (%d, %d)",
+        x, y, nx, ny);
+      costmap.saveMap("failing_costmap.pgm");
+    }
+    EXPECT_TRUE(
+      neighbor_cost >= expected_lowest_cost ||
+      (furthest_valid_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE));
+  }
+}
+}   // namespace nav2_costmap_2d
+
+nav2_costmap_2d::CostmapTester * map_tester = NULL;
+tf2_ros::TransformListener * tfl_;
+tf2_ros::Buffer * tf_;
+
+TEST(CostmapTester, checkConsistentCosts) {
+  map_tester->checkConsistentCosts();
+}
+
+void testCallback()
+{
+  int test_result = RUN_ALL_TESTS();
+  RCLCPP_INFO(rclcpp::get_logger("costmap_tester"), "gtest return value: %d", test_result);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
+  testing::InitGoogleTest(&argc, argv);
+
+  tf_ = new tf2_ros::Buffer(node->get_clock());
+  tfl_ = new tf2_ros::TransformListener(*tf_);
+  map_tester = new nav2_costmap_2d::CostmapTester(*tf_);
+  rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer(30000ms, testCallback);
+  rclcpp::spin(costmap_ros_);
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_costmap_2d/test/integration/costmap_tests_launch.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+#
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import launch_ros.actions
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py')
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    lifecycle_manager = launch_ros.actions.Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager',
+        output='screen',
+        parameters=[{'node_names': ['map_server']}, {'autostart': True}])
+
+    ld = LaunchDescription([
+        IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
+        lifecycle_manager
+    ])
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='costmap_tests',
+        output='screen'
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_costmap_2d/test/integration/dyn_params_tests.cpp

@@ -0,0 +1,105 @@
+// Copyright (c) 2021 Wyca Robotics
+//
+// 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 <memory>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class DynParamTestNode
+{
+public:
+  DynParamTestNode() {}
+  ~DynParamTestNode() {}
+};
+
+TEST(DynParamTestNode, testDynParamsSet)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("dyn_param_tester");
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+  costmap->on_configure(rclcpp_lifecycle::State());
+
+  // Set tf between default global_frame and robot_base_frame in order not to block in on_activate
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
+    std::make_unique<tf2_ros::TransformBroadcaster>(node);
+  geometry_msgs::msg::TransformStamped t;
+  t.header.stamp = node->get_clock()->now();
+  t.header.frame_id = "map";
+  t.child_frame_id = "base_link";
+  tf_broadcaster_->sendTransform(t);
+  t.header.frame_id = "map";
+  t.child_frame_id = "test_frame";
+  tf_broadcaster_->sendTransform(t);
+
+  costmap->on_activate(rclcpp_lifecycle::State());
+
+  auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->shared_from_this(),
+    "/test_costmap/test_costmap",
+    rmw_qos_profile_parameters);
+  auto results1 = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("robot_radius", 1.234),
+    rclcpp::Parameter("footprint_padding", 2.345),
+    rclcpp::Parameter("transform_tolerance", 3.456),
+    rclcpp::Parameter("publish_frequency", 4.567),
+    rclcpp::Parameter("resolution", 5.678),
+    rclcpp::Parameter("origin_x", 6.789),
+    rclcpp::Parameter("origin_y", 7.891),
+    rclcpp::Parameter("width", 2),
+    rclcpp::Parameter("height", 3),
+    rclcpp::Parameter(
+      "footprint",
+      "[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]"),
+    rclcpp::Parameter("robot_base_frame", "test_frame"),
+  });
+
+  // Try setting robot_base_frame to an invalid frame, should be rejected
+  auto results2 = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("robot_base_frame", "wrong_test_frame"),
+  });
+
+  rclcpp::spin_some(costmap->get_node_base_interface());
+
+  EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234);
+  EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345);
+  EXPECT_EQ(costmap->get_parameter("transform_tolerance").as_double(), 3.456);
+  EXPECT_EQ(costmap->get_parameter("publish_frequency").as_double(), 4.567);
+  EXPECT_EQ(costmap->get_parameter("resolution").as_double(), 5.678);
+  EXPECT_EQ(costmap->get_parameter("origin_x").as_double(), 6.789);
+  EXPECT_EQ(costmap->get_parameter("origin_y").as_double(), 7.891);
+  EXPECT_EQ(costmap->get_parameter("width").as_int(), 2);
+  EXPECT_EQ(costmap->get_parameter("height").as_int(), 3);
+  EXPECT_EQ(
+    costmap->get_parameter("footprint").as_string(),
+    "[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]");
+  EXPECT_EQ(costmap->get_parameter("robot_base_frame").as_string(), "test_frame");
+
+  costmap->on_deactivate(rclcpp_lifecycle::State());
+  costmap->on_cleanup(rclcpp_lifecycle::State());
+  costmap->on_shutdown(rclcpp_lifecycle::State());
+}

navigation2/nav2_costmap_2d/test/integration/footprint_tests.cpp

@@ -0,0 +1,204 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Dave Hershberger
+*********************************************************************/
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "nav2_costmap_2d/footprint.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class FootprintTestNode
+{
+public:
+  FootprintTestNode()
+  {
+    // Default footprint padding and footprint radius from Costmap2DROS
+    testFootprint(0.01f, 0.1);
+  }
+
+  ~FootprintTestNode() {}
+
+  void testFootprint(double footprint_padding, std::string footprint)
+  {
+    footprint_padding_ = footprint_padding;
+    if (footprint != "" && footprint != "[]") {
+      std::vector<geometry_msgs::msg::Point> new_footprint;
+      if (nav2_costmap_2d::makeFootprintFromString(footprint, new_footprint)) {
+        setRobotFootprint(new_footprint);
+      } else {
+        RCLCPP_ERROR(rclcpp::get_logger("footprint_tester"), "Invalid footprint string");
+      }
+    }
+  }
+
+  void testFootprint(double footprint_padding, double robot_radius)
+  {
+    footprint_padding_ = footprint_padding;
+    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()
+  {
+    footprint_tester_ = std::make_shared<FootprintTestNode>();
+  }
+
+  ~TestNode() {}
+
+protected:
+  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 = footprint_tester_->getRobotFootprint();
+  // With no specification of footprint or radius,
+  // defaults to 0.1 meter radius plus 0.01 meter padding.
+  EXPECT_EQ(16u, footprint.size());
+
+  EXPECT_NEAR(0.11f, footprint[0].x, 0.0001);
+  EXPECT_NEAR(0.0f, footprint[0].y, 0.0001);
+  EXPECT_EQ(0.0f, footprint[0].z);
+}
+
+TEST_F(TestNode, unpadded_footprint_from_string_param)
+{
+  footprint_tester_->testFootprint(0.0, "[[1, 1], [-1, 1], [-1, -1]]");
+
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
+  EXPECT_EQ(3u, footprint.size());
+
+  EXPECT_EQ(1.0f, footprint[0].x);
+  EXPECT_EQ(1.0f, footprint[0].y);
+  EXPECT_EQ(0.0f, footprint[0].z);
+
+  EXPECT_EQ(-1.0f, footprint[1].x);
+  EXPECT_EQ(1.0f, footprint[1].y);
+  EXPECT_EQ(0.0f, footprint[1].z);
+
+  EXPECT_EQ(-1.0f, footprint[2].x);
+  EXPECT_EQ(-1.0f, footprint[2].y);
+  EXPECT_EQ(0.0f, footprint[2].z);
+}
+
+TEST_F(TestNode, padded_footprint_from_string_param)
+{
+  footprint_tester_->testFootprint(0.5, "[[1, 1], [-1, 1], [-1, -1]]");
+
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
+  EXPECT_EQ(3u, footprint.size());
+
+  EXPECT_EQ(1.5f, footprint[0].x);
+  EXPECT_EQ(1.5f, footprint[0].y);
+  EXPECT_EQ(0.0f, footprint[0].z);
+
+  EXPECT_EQ(-1.5f, footprint[1].x);
+  EXPECT_EQ(1.5f, footprint[1].y);
+  EXPECT_EQ(0.0f, footprint[1].z);
+
+  EXPECT_EQ(-1.5f, footprint[2].x);
+  EXPECT_EQ(-1.5f, footprint[2].y);
+  EXPECT_EQ(0.0f, footprint[2].z);
+}
+
+TEST_F(TestNode, radius_param)
+{
+  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(16u, footprint.size());
+
+  // Check the first point
+  EXPECT_EQ(10.0f, footprint[0].x);
+  EXPECT_EQ(0.0f, footprint[0].y);
+  EXPECT_EQ(0.0f, footprint[0].z);
+
+  // Check the 4th point, which should be 90 degrees around the circle from the first.
+  EXPECT_NEAR(0.0f, footprint[4].x, 0.0001);
+  EXPECT_NEAR(10.0f, footprint[4].y, 0.0001);
+  EXPECT_EQ(0.0f, footprint[4].z);
+}
+
+TEST_F(TestNode, footprint_from_same_level_param)
+{
+  footprint_tester_->testFootprint(0.0, "[[1, 2], [3, 4], [5, 6]]");
+  std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
+  EXPECT_EQ(3u, footprint.size());
+
+  EXPECT_EQ(1.0f, footprint[0].x);
+  EXPECT_EQ(2.0f, footprint[0].y);
+  EXPECT_EQ(0.0f, footprint[0].z);
+
+  EXPECT_EQ(3.0f, footprint[1].x);
+  EXPECT_EQ(4.0f, footprint[1].y);
+  EXPECT_EQ(0.0f, footprint[1].z);
+
+  EXPECT_EQ(5.0f, footprint[2].x);
+  EXPECT_EQ(6.0f, footprint[2].y);
+  EXPECT_EQ(0.0f, footprint[2].z);
+}

navigation2/nav2_costmap_2d/test/integration/inflation_tests.cpp

@@ -0,0 +1,644 @@
+/*
+ * Copyright (c) 2013, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * @author David Lu!!
+ * Test harness for InflationLayer for Costmap2D
+ */
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/obstacle_layer.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_costmap_2d/observation_buffer.hpp"
+#include "../testing_helper.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+using geometry_msgs::msg::Point;
+using nav2_costmap_2d::CellData;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode() {}
+
+  ~TestNode() {}
+
+  std::vector<Point> setRadii(
+    nav2_costmap_2d::LayeredCostmap & layers,
+    double length, double width);
+
+  void validatePointInflation(
+    unsigned int mx, unsigned int my,
+    nav2_costmap_2d::Costmap2D * costmap,
+    std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
+    double inflation_radius);
+
+  void initNode(std::vector<rclcpp::Parameter> parameters);
+  void initNode(double inflation_radius);
+
+  void waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer);
+
+protected:
+  nav2_util::LifecycleNode::SharedPtr node_;
+};
+
+std::vector<Point> TestNode::setRadii(
+  nav2_costmap_2d::LayeredCostmap & layers,
+  double length, double width)
+{
+  std::vector<Point> polygon;
+  Point p;
+  p.x = width;
+  p.y = length;
+  polygon.push_back(p);
+  p.x = width;
+  p.y = -length;
+  polygon.push_back(p);
+  p.x = -width;
+  p.y = -length;
+  polygon.push_back(p);
+  p.x = -width;
+  p.y = length;
+  polygon.push_back(p);
+  layers.setFootprint(polygon);
+
+  return polygon;
+}
+
+void TestNode::waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer)
+{
+  while (!slayer->isCurrent()) {
+    rclcpp::spin_some(node_->get_node_base_interface());
+  }
+}
+
+// Test that a single point gets inflated properly
+void TestNode::validatePointInflation(
+  unsigned int mx, unsigned int my,
+  nav2_costmap_2d::Costmap2D * costmap,
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
+  double inflation_radius)
+{
+  bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
+  memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
+  std::map<double, std::vector<CellData>> m;
+  CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
+  m[0].push_back(initial);
+  for (std::map<double, std::vector<CellData>>::iterator bin = m.begin();
+    bin != m.end(); ++bin)
+  {
+    for (unsigned int i = 0; i < bin->second.size(); ++i) {
+      const CellData cell = bin->second[i];
+      if (!seen[cell.index_]) {
+        seen[cell.index_] = true;
+        unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
+        unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
+        double dist = std::hypot(dx, dy);
+
+        unsigned char expected_cost = ilayer->computeCost(dist);
+        ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
+
+        if (dist > inflation_radius) {
+          continue;
+        }
+
+        if (dist == bin->first) {
+          // Adding to our current bin could cause a reallocation
+          // Which appears to cause the iterator to get messed up
+          dist += 0.001;
+        }
+
+        if (cell.x_ > 0) {
+          CellData data(costmap->getIndex(cell.x_ - 1, cell.y_),
+            cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
+          m[dist].push_back(data);
+        }
+        if (cell.y_ > 0) {
+          CellData data(costmap->getIndex(cell.x_, cell.y_ - 1),
+            cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
+          m[dist].push_back(data);
+        }
+        if (cell.x_ < costmap->getSizeInCellsX() - 1) {
+          CellData data(costmap->getIndex(cell.x_ + 1, cell.y_),
+            cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
+          m[dist].push_back(data);
+        }
+        if (cell.y_ < costmap->getSizeInCellsY() - 1) {
+          CellData data(costmap->getIndex(cell.x_, cell.y_ + 1),
+            cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
+          m[dist].push_back(data);
+        }
+      }
+    }
+  }
+  delete[] seen;
+}
+
+void TestNode::initNode(std::vector<rclcpp::Parameter> parameters)
+{
+  auto options = rclcpp::NodeOptions();
+  options.parameter_overrides(parameters);
+
+  node_ = std::make_shared<nav2_util::LifecycleNode>(
+    "inflation_test_node", "", options);
+
+  // Declare non-plugin specific costmap parameters
+  node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
+  node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+  node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+  node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+  node_->declare_parameter(
+    "unknown_cost_value",
+    rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+  node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+  node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+  node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+}
+
+void TestNode::initNode(double inflation_radius)
+{
+  std::vector<rclcpp::Parameter> parameters;
+  // Set cost_scaling_factor parameter to 1.0 for inflation layer
+  parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
+  parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius));
+
+  initNode(parameters);
+}
+
+TEST_F(TestNode, testAdjacentToObstacleCanStillMove)
+{
+  initNode(4.1);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  // Footprint with inscribed radius = 2.1
+  //               circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
+  layers.setFootprint(polygon);
+
+  addObservation(olayer, 0, 0, MAX_Z);
+
+  layers.updateMap(0, 0, 0);
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  // printMap(*costmap);
+  EXPECT_EQ(nav2_costmap_2d::LETHAL_OBSTACLE, costmap->getCost(0, 0));
+  EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(1, 0));
+  EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(2, 0));
+  EXPECT_TRUE(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost(3, 0));
+  EXPECT_TRUE(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost(2, 1));
+  EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(1, 1));
+}
+
+TEST_F(TestNode, testInflationShouldNotCreateUnknowns)
+{
+  initNode(4.1);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  // Footprint with inscribed radius = 2.1
+  // circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
+  layers.setFootprint(polygon);
+
+  addObservation(olayer, 0, 0, MAX_Z);
+
+  layers.updateMap(0, 0, 0);
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+
+  EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0u);
+}
+
+TEST_F(TestNode, testInflationInUnkown)
+{
+  std::vector<rclcpp::Parameter> parameters;
+  // Set cost_scaling_factor parameter to 1.0 for inflation layer
+  parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
+  parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", 4.1));
+  parameters.push_back(rclcpp::Parameter("inflation.inflate_unknown", true));
+
+  initNode(parameters);
+
+  node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
+
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
+  layers.resizeMap(9, 9, 1, 0, 0);
+
+  // Footprint with inscribed radius = 2.1
+  // circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+  layers.setFootprint(polygon);
+
+  addObservation(olayer, 4, 4, MAX_Z, 0.0, 0.0, MAX_Z, true, false);
+
+  layers.updateMap(0, 0, 0);
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+
+  // Only the 4 corners of the map should remain unknown
+  EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 4u);
+}
+
+TEST_F(TestNode, testInflationAroundUnkown)
+{
+  auto inflation_radius = 4.1;
+  std::vector<rclcpp::Parameter> parameters;
+  // Set cost_scaling_factor parameter to 1.0 for inflation layer
+  parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0));
+  parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius));
+  parameters.push_back(rclcpp::Parameter("inflation.inflate_around_unknown", true));
+
+  initNode(parameters);
+
+  node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
+
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  // Footprint with inscribed radius = 2.1
+  // circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+  layers.setFootprint(polygon);
+  layers.updateMap(0, 0, 0);
+
+  layers.getCostmap()->setCost(4, 4, nav2_costmap_2d::NO_INFORMATION);
+  ilayer->updateCosts(*layers.getCostmap(), 0, 0, 10, 10);
+
+  validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
+}
+
+/**
+ * Test for the cost function correctness with a larger range and different values
+ */
+TEST_F(TestNode, testCostFunctionCorrectness)
+{
+  initNode(10.5);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  layers.resizeMap(100, 100, 1, 0, 0);
+  // Footprint with inscribed radius = 5.0
+  //               circumscribed radius = 8.0
+  std::vector<Point> polygon = setRadii(layers, 5.0, 6.25);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
+  layers.setFootprint(polygon);
+
+  addObservation(olayer, 50, 50, MAX_Z);
+
+  layers.updateMap(0, 0, 0);
+  nav2_costmap_2d::Costmap2D * map = layers.getCostmap();
+
+  // Verify that the circumscribed cost lower bound is as expected: based on the cost function.
+  // unsigned char c = ilayer->computeCost(8.0);
+  // ASSERT_EQ(ilayer->getCircumscribedCost(), c);
+
+  for (unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++) {
+    // To the right
+    ASSERT_EQ(map->getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    ASSERT_EQ(map->getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    // To the left
+    ASSERT_EQ(map->getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    ASSERT_EQ(map->getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    // Down
+    ASSERT_EQ(map->getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    ASSERT_EQ(map->getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    // Up
+    ASSERT_EQ(map->getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+    ASSERT_EQ(map->getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+  }
+
+  // Verify the normalized cost attenuates as expected
+  for (unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++) {
+    unsigned char expectedValue = ilayer->computeCost(i / 1.0);
+    ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
+  }
+
+  // Update with no hits. Should clear (revert to the static map
+  /*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
+  cloud.points.resize(0);
+
+  p.x = 0.0;
+  p.y = 0.0;
+  p.z = MAX_Z;
+
+  Observation obs2(p, cloud, 100.0, 100.0);
+  std::vector<Observation> obsBuf2;
+  obsBuf2.push_back(obs2);
+
+  map->updateWorld(0, 0, obsBuf2, obsBuf2);
+
+  for(unsigned int i = 0; i < 100; i++)
+    for(unsigned int j = 0; j < 100; j++)
+      ASSERT_EQ(map->getCost(i, j), nav2_costmap_2d::FREE_SPACE);*/
+}
+
+/**
+ * Test that there is no regression and that costs do not get
+ * underestimated with the distance-as-key map used to replace
+ * the previously used priority queue. This is a more thorough
+ * test of the cost function being correctly applied.
+ */
+TEST_F(TestNode, testInflationOrderCorrectness)
+{
+  const double inflation_radius = 4.1;
+  initNode(inflation_radius);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  // Footprint with inscribed radius = 2.1
+  //               circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
+  layers.setFootprint(polygon);
+
+  // Add two diagonal cells, they would induce problems under the
+  // previous implementations
+  addObservation(olayer, 4, 4, MAX_Z);
+  addObservation(olayer, 5, 5, MAX_Z);
+
+  layers.updateMap(0, 0, 0);
+
+  validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
+  validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius);
+}
+
+/**
+ * Test inflation for both static and dynamic obstacles
+ */
+TEST_F(TestNode, testInflation)
+{
+  initNode(1);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  // Footprint with inscribed radius = 2.1
+  // circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 1, 1);
+
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+  layers.setFootprint(polygon);
+
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  waitForMap(slayer);
+
+  layers.updateMap(0, 0, 0);
+  // printMap(*costmap);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20u);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28u);
+
+  /*/ Iterate over all id's and verify they are obstacles
+  for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
+    unsigned int ind = *it;
+    unsigned int x, y;
+    map.indexToCells(ind, x, y);
+    ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
+    ASSERT_EQ(map.getCost(x, y) == nav2_costmap_2d::LETHAL_OBSTACLE ||
+      map.getCost(x, y) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
+  }*/
+
+  addObservation(olayer, 0, 0, 0.4);
+  layers.updateMap(0, 0, 0);
+
+  // It and its 2 neighbors makes 3 obstacles
+  ASSERT_EQ(
+    countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) +
+    countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 51u);
+
+  // @todo Rewrite
+  // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
+  addObservation(olayer, 2, 0);
+  layers.updateMap(0, 0, 0);
+
+  // Now we expect insertions for it, and 2 more neighbors, but not all 5.
+  // Free space will propagate from
+  // the origin to the target, clearing the point at <0, 0>,
+  // but not over-writing the inflation of the obstacle
+  // at <0, 1>
+  ASSERT_EQ(
+    countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) +
+    countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 54u);
+
+  // Add an obstacle at <1, 9>. This will inflate obstacles around it
+  addObservation(olayer, 1, 9);
+  layers.updateMap(0, 0, 0);
+
+  ASSERT_EQ(costmap->getCost(1, 9), nav2_costmap_2d::LETHAL_OBSTACLE);
+  ASSERT_EQ(costmap->getCost(0, 9), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+  ASSERT_EQ(costmap->getCost(2, 9), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+
+  // Add an obstacle and verify that it over-writes its inflated status
+  addObservation(olayer, 0, 9);
+  layers.updateMap(0, 0, 0);
+
+  ASSERT_EQ(costmap->getCost(0, 9), nav2_costmap_2d::LETHAL_OBSTACLE);
+}
+
+/**
+ * Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
+ */
+TEST_F(TestNode, testInflation2)
+{
+  initNode(1);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  // Footprint with inscribed radius = 2.1
+  // circumscribed radius = 3.1
+  std::vector<Point> polygon = setRadii(layers, 1, 1);
+
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
+  layers.setFootprint(polygon);
+
+  waitForMap(slayer);
+
+  // Creat a small L-Shape all at once
+  addObservation(olayer, 1, 1, MAX_Z);
+  addObservation(olayer, 2, 1, MAX_Z);
+  addObservation(olayer, 2, 2, MAX_Z);
+  layers.updateMap(0, 0, 0);
+
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  // printMap(*costmap);
+  ASSERT_EQ(costmap->getCost(2, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+  ASSERT_EQ(costmap->getCost(3, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+}
+
+/**
+ * Test inflation behavior, starting with an empty map
+ */
+TEST_F(TestNode, testInflation3)
+{
+  initNode(3);
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  // 1 2 3
+  std::vector<Point> polygon = setRadii(layers, 1, 1.75);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
+  layers.setFootprint(polygon);
+
+  // There should be no occupied cells
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 0u);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 0u);
+  printMap(*costmap);
+  // Add an obstacle at 5,5
+  addObservation(olayer, 5, 5, MAX_Z);
+  layers.updateMap(0, 0, 0);
+  printMap(*costmap);
+
+  // Test fails because updated cell value is 0
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::FREE_SPACE, false), 29u);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1u);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4u);
+
+  // Update again - should see no change
+  layers.updateMap(0, 0, 0);
+
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::FREE_SPACE, false), 29u);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1u);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4u);
+}
+
+/**
+ * Test dynamic parameter setting of inflation layer
+ */
+TEST_F(TestNode, testDynParamsSet)
+{
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
+  costmap->on_configure(rclcpp_lifecycle::State());
+
+  costmap->on_activate(rclcpp_lifecycle::State());
+
+  auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
+    costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
+    costmap->get_node_graph_interface(),
+    costmap->get_node_services_interface());
+
+  auto results = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("inflation_layer.inflation_radius", 0.0),
+    rclcpp::Parameter("inflation_layer.cost_scaling_factor", 0.0),
+    rclcpp::Parameter("inflation_layer.inflate_unknown", true),
+    rclcpp::Parameter("inflation_layer.inflate_around_unknown", true),
+    rclcpp::Parameter("inflation_layer.enabled", false)
+  });
+
+  rclcpp::spin_until_future_complete(
+    costmap->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(costmap->get_parameter("inflation_layer.inflation_radius").as_double(), 0.0);
+  EXPECT_EQ(costmap->get_parameter("inflation_layer.cost_scaling_factor").as_double(), 0.0);
+  EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_unknown").as_bool(), true);
+  EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_around_unknown").as_bool(), true);
+  EXPECT_EQ(costmap->get_parameter("inflation_layer.enabled").as_bool(), false);
+
+  costmap->on_deactivate(rclcpp_lifecycle::State());
+  costmap->on_cleanup(rclcpp_lifecycle::State());
+  costmap->on_shutdown(rclcpp_lifecycle::State());
+}

navigation2/nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -0,0 +1,556 @@
+/*
+ * Copyright (c) 2013, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * @author David Lu!!
+ * Test harness for ObstacleLayer for Costmap2D
+ */
+
+#include <memory>
+#include <string>
+#include <algorithm>
+#include <utility>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/observation_buffer.hpp"
+#include "../testing_helper.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+using std::begin;
+using std::end;
+using std::for_each;
+using std::all_of;
+using std::none_of;
+using std::pair;
+using std::string;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestLifecycleNode(const string & name)
+  : nav2_util::LifecycleNode(name)
+  {
+  }
+
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode()
+  {
+    node_ = std::make_shared<TestLifecycleNode>("obstacle_test_node");
+    node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
+    node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+    node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+    node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter(
+      "unknown_cost_value",
+      rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+    node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+    node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+  }
+
+  ~TestNode() {}
+
+protected:
+  std::shared_ptr<TestLifecycleNode> node_;
+};
+
+/*
+ * For reference, the static map looks like this:
+ *
+ *   0   0   0   0   0   0   0 254 254 254
+ *
+ *   0   0   0   0   0   0   0 254 254 254
+ *
+ *   0   0   0 254 254 254   0   0   0   0
+ *
+ *   0   0   0   0   0   0   0   0   0   0
+ *
+ *   0   0   0   0   0   0   0   0   0   0
+ *
+ *   0   0   0   0 254   0   0 254 254 254
+ *
+ *   0   0   0   0 254   0   0 254 254 254
+ *
+ *   0   0   0   0   0   0   0 254 254 254
+ *
+ *   0   0   0   0   0   0   0   0   0   0
+ *
+ *   0   0   0   0   0   0   0   0   0   0
+ *
+ *   upper left is 0,0, lower right is 9,9
+ */
+
+#if (0)
+/**
+ * Test for ray tracing free space
+ */
+TEST_F(TestNode, testRaytracing) {
+  tf2_ros::Buffer tf(node_->get_clock());
+
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  addStaticLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
+
+  // Add a point at 0, 0, 0
+  addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
+
+  // This actually puts the LETHAL (254) point in the costmap at (0,0)
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+  // printMap(*(layers.getCostmap()));
+
+  int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // We expect just one obstacle to be added (20 in static map)
+  ASSERT_EQ(lethal_count, 21);
+}
+
+/**
+ * Test for ray tracing free space
+ */
+TEST_F(TestNode, testRaytracing2) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  addStaticLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
+
+  // If we print map now, it is 10x10 all value 0
+  // printMap(*(layers.getCostmap()));
+
+  // Update will fill in the costmap with the static map
+  layers.updateMap(0, 0, 0);
+
+  // If we print the map now, we get the static map
+  // printMap(*(layers.getCostmap()));
+
+  // Static map has 20 LETHAL cells (see diagram above)
+  int obs_before = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
+  ASSERT_EQ(obs_before, 20);
+
+  // The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
+  // we would expect cells <0, 0> thru <8, 8> to be traced through
+  // however the static map is not cleared by obstacle layer
+  addObservation(olayer, 9.5, 9.5, MAX_Z / 2, 0.5, 0.5, MAX_Z / 2);
+  layers.updateMap(0, 0, 0);
+
+  // If we print map now, we have static map + <9,9> is LETHAL
+  // printMap(*(layers.getCostmap()));
+  int obs_after = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Change from previous test:
+  // No obstacles from the static map will be cleared, so the
+  // net change is +1.
+  ASSERT_EQ(obs_after, obs_before + 1);
+
+  // Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
+  for (int i = 0; i < olayer->getSizeInCellsY(); ++i) {
+    olayer->setCost(i, i, nav2_costmap_2d::LETHAL_OBSTACLE);
+  }
+  // This will updateBounds, which will raytrace the static observation added
+  // above, thus clearing out the diagonal again!
+  layers.updateMap(0, 0, 0);
+
+  // Map now has diagonal except <0,0> filled with LETHAL (254)
+  // printMap(*(layers.getCostmap()));
+  int with_static = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Should thus be the same
+  ASSERT_EQ(with_static, obs_after);
+  // If 21 are filled, 79 should be free
+  ASSERT_EQ(79, countValues(*(layers.getCostmap()), nav2_costmap_2d::FREE_SPACE));
+}
+
+/**
+ * Test for wave interference
+ */
+TEST_F(TestNode, testWaveInterference) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
+  // Start with an empty map, no rolling window, tracking unknown
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
+  layers.resizeMap(10, 10, 1, 0, 0);
+  auto olayer = addObstacleLayer(layers, tf, node_);
+
+  // If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
+  // printMap(*(layers.getCostmap()));
+
+  // Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
+  addObservation(olayer, 3.0, 3.0, MAX_Z);
+  addObservation(olayer, 5.0, 5.0, MAX_Z);
+  addObservation(olayer, 7.0, 7.0, MAX_Z);
+  layers.updateMap(0, 0, 0);
+
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  // 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free
+  // <0,0> is footprint and is free
+  // printMap(*costmap);
+  ASSERT_EQ(3, countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE));
+  ASSERT_EQ(92, countValues(*costmap, nav2_costmap_2d::NO_INFORMATION));
+  ASSERT_EQ(5, countValues(*costmap, nav2_costmap_2d::FREE_SPACE));
+}
+
+/**
+ * Make sure we ignore points outside of our z threshold
+ */
+TEST_F(TestNode, testZThreshold) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  // Start with an empty map
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  auto olayer = addObstacleLayer(layers, tf, node_);
+
+  // A point cloud with 2 points falling in a cell with a non-lethal cost
+  addObservation(olayer, 0.0, 5.0, 0.4);
+  addObservation(olayer, 1.0, 5.0, 2.2);
+
+  layers.updateMap(0, 0, 0);
+
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1);
+}
+
+/**
+ * Verify that dynamic obstacles are added
+ */
+TEST_F(TestNode, testDynamicObstacles) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  addStaticLayer(layers, tf, node_);
+
+  auto olayer = addObstacleLayer(layers, tf, node_);
+
+  // Add a point cloud and verify its insertion. There should be only one new one
+  addObservation(olayer, 0.0, 0.0);
+  addObservation(olayer, 0.0, 0.0);
+  addObservation(olayer, 0.0, 0.0);
+
+  layers.updateMap(0, 0, 0);
+
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  // Should now have 1 insertion and no deletions
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21);
+
+  // Repeating the call - we should see no insertions or deletions
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21);
+}
+
+/**
+ * Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
+ */
+TEST_F(TestNode, testMultipleAdditions) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  addStaticLayer(layers, tf, node_);
+
+  auto olayer = addObstacleLayer(layers, tf, node_);
+
+  // A point cloud with one point that falls within an existing obstacle
+  addObservation(olayer, 9.5, 0.0);
+  layers.updateMap(0, 0, 0);
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  // printMap(*costmap);
+
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20);
+}
+#endif
+/**
+ * Verify correct init/reset cycling of layer
+ */
+TEST_F(TestNode, testRepeatedResets) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
+
+  // TODO(orduno) Add obstacle layer
+
+  // Define a node-level parameter
+  pair<string, string> node_dummy = {"node_dummy_param", "node_dummy_val"};
+  node_->declare_parameter(node_dummy.first, rclcpp::ParameterValue(node_dummy.second));
+
+  // Define a layer-level parameter
+  pair<string, string> layer_dummy = {"dummy_param", "dummy_val"};
+
+  // Set parameters
+  auto plugins = layers.getPlugins();
+  for_each(
+    begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
+      string layer_param = layer_dummy.first + "_" + plugin->getName();
+
+      // Notice we are using Layer::declareParameter
+      plugin->declareParameter(layer_param, rclcpp::ParameterValue(layer_dummy.second));
+    });
+
+  // Check that all parameters have been set
+  // node-level param
+  ASSERT_TRUE(node_->has_parameter(node_dummy.first));
+
+  // layer-level param
+  ASSERT_TRUE(
+    all_of(
+      begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
+        string layer_param = layer_dummy.first + "_" + plugin->getName();
+        return plugin->hasParameter(layer_param);
+      }));
+
+  // Reset all layers. Parameters should be declared if not declared, otherwise skipped.
+  ASSERT_NO_THROW(
+    for_each(
+      begin(*plugins), end(*plugins), [](const auto & plugin) {
+        plugin->reset();
+      }));
+}
+
+
+/**
+ * Test for ray tracing free space
+ */
+TEST_F(TestNode, testRaytracing) {
+  tf2_ros::Buffer tf(node_->get_clock());
+
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
+
+  // This actually puts the LETHAL (254) point in the costmap at (0,0)
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+  // printMap(*(layers.getCostmap()));
+
+  int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  ASSERT_EQ(lethal_count, 1);
+
+  addObservation(olayer, 1.0, 1.0, MAX_Z / 2, 0, 0, MAX_Z / 2, true, true, 100.0, 5.0, 100.0, 5.0);
+
+  // This actually puts the LETHAL (254) point in the costmap at (0,0)
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+  // printMap(*(layers.getCostmap()));
+
+  // New observation should not be recorded as min_range is higher than obstacle range
+  lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  ASSERT_EQ(lethal_count, 1);
+}
+
+/**
+ * Test dynamic parameter setting of obstacle layer
+ */
+TEST_F(TestNode, testDynParamsSetObstacle)
+{
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  // Add obstacle layer
+  std::vector<std::string> plugins_str;
+  plugins_str.push_back("obstacle_layer");
+  costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str));
+  costmap->declare_parameter(
+    "obstacle_layer.plugin",
+    rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer")));
+
+  costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
+  costmap->on_configure(rclcpp_lifecycle::State());
+
+  costmap->on_activate(rclcpp_lifecycle::State());
+
+  auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
+    costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
+    costmap->get_node_graph_interface(),
+    costmap->get_node_services_interface());
+
+  auto results = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("obstacle_layer.combination_method", 5),
+    rclcpp::Parameter("obstacle_layer.max_obstacle_height", 4.0),
+    rclcpp::Parameter("obstacle_layer.enabled", false),
+    rclcpp::Parameter("obstacle_layer.footprint_clearing_enabled", false)
+  });
+
+  rclcpp::spin_until_future_complete(
+    costmap->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.combination_method").as_int(), 5);
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.max_obstacle_height").as_double(), 4.0);
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.enabled").as_bool(), false);
+  EXPECT_EQ(costmap->get_parameter("obstacle_layer.footprint_clearing_enabled").as_bool(), false);
+
+  costmap->on_deactivate(rclcpp_lifecycle::State());
+  costmap->on_cleanup(rclcpp_lifecycle::State());
+  costmap->on_shutdown(rclcpp_lifecycle::State());
+}
+
+/**
+ * Test dynamic parameter setting of voxel layer
+ */
+TEST_F(TestNode, testDynParamsSetVoxel)
+{
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  // Add voxel layer
+  std::vector<std::string> plugins_str;
+  plugins_str.push_back("voxel_layer");
+  costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str));
+  costmap->declare_parameter(
+    "voxel_layer.plugin",
+    rclcpp::ParameterValue(std::string("nav2_costmap_2d::VoxelLayer")));
+
+  costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
+  costmap->on_configure(rclcpp_lifecycle::State());
+
+  costmap->on_activate(rclcpp_lifecycle::State());
+
+  auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
+    costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
+    costmap->get_node_graph_interface(),
+    costmap->get_node_services_interface());
+
+  auto results = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("voxel_layer.combination_method", 0),
+    rclcpp::Parameter("voxel_layer.mark_threshold", 1),
+    rclcpp::Parameter("voxel_layer.unknown_threshold", 10),
+    rclcpp::Parameter("voxel_layer.z_resolution", 0.4),
+    rclcpp::Parameter("voxel_layer.origin_z", 1.0),
+    rclcpp::Parameter("voxel_layer.z_voxels", 14),
+    rclcpp::Parameter("voxel_layer.max_obstacle_height", 4.0),
+    rclcpp::Parameter("voxel_layer.footprint_clearing_enabled", false),
+    rclcpp::Parameter("voxel_layer.enabled", false),
+    rclcpp::Parameter("voxel_layer.publish_voxel_map", true)
+  });
+
+  rclcpp::spin_until_future_complete(
+    costmap->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.combination_method").as_int(), 0);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.mark_threshold").as_int(), 1);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.unknown_threshold").as_int(), 10);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.z_resolution").as_double(), 0.4);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.origin_z").as_double(), 1.0);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.z_voxels").as_int(), 14);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.max_obstacle_height").as_double(), 4.0);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.footprint_clearing_enabled").as_bool(), false);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.enabled").as_bool(), false);
+  EXPECT_EQ(costmap->get_parameter("voxel_layer.publish_voxel_map").as_bool(), true);
+
+  costmap->on_deactivate(rclcpp_lifecycle::State());
+  costmap->on_cleanup(rclcpp_lifecycle::State());
+  costmap->on_shutdown(rclcpp_lifecycle::State());
+}
+
+/**
+ * Test dynamic parameter setting of static layer
+ */
+TEST_F(TestNode, testDynParamsSetStatic)
+{
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
+  costmap->on_configure(rclcpp_lifecycle::State());
+
+  costmap->on_activate(rclcpp_lifecycle::State());
+
+  auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
+    costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
+    costmap->get_node_graph_interface(),
+    costmap->get_node_services_interface());
+
+  auto results = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("static_layer.transform_tolerance", 1.0),
+    rclcpp::Parameter("static_layer.enabled", false),
+    rclcpp::Parameter("static_layer.map_subscribe_transient_local", false),
+    rclcpp::Parameter("static_layer.map_topic", "dynamic_topic"),
+    rclcpp::Parameter("static_layer.subscribe_to_updates", true)
+  });
+
+  rclcpp::spin_until_future_complete(
+    costmap->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(costmap->get_parameter("static_layer.transform_tolerance").as_double(), 1.0);
+  EXPECT_EQ(costmap->get_parameter("static_layer.enabled").as_bool(), false);
+  EXPECT_EQ(costmap->get_parameter("static_layer.map_subscribe_transient_local").as_bool(), false);
+  EXPECT_EQ(costmap->get_parameter("static_layer.map_topic").as_string(), "dynamic_topic");
+  EXPECT_EQ(costmap->get_parameter("static_layer.subscribe_to_updates").as_bool(), true);
+
+  costmap->on_deactivate(rclcpp_lifecycle::State());
+  costmap->on_cleanup(rclcpp_lifecycle::State());
+  costmap->on_shutdown(rclcpp_lifecycle::State());
+}

navigation2/nav2_costmap_2d/test/integration/range_tests.cpp

@@ -0,0 +1,294 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Bytes Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <memory>
+#include <string>
+#include <algorithm>
+#include <utility>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/observation_buffer.hpp"
+#include "../testing_helper.hpp"
+#include "sensor_msgs/msg/range.hpp"
+
+using std::begin;
+using std::end;
+using std::for_each;
+using std::all_of;
+using std::none_of;
+using std::pair;
+using std::string;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  ~RclCppFixture()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+RclCppFixture g_rclcppfixture;
+
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestLifecycleNode(const string & name)
+  : nav2_util::LifecycleNode(name)
+  {
+  }
+
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode()
+  : node_(std::make_shared<TestLifecycleNode>("range_test_node")),
+    tf_(node_->get_clock())
+  {
+    tf_.setUsingDedicatedThread(true);
+    // Standard non-plugin specific parameters
+    node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
+    node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+    node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+    node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter(
+      "unknown_cost_value",
+      rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+    node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+    node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("range")));
+    node_->declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
+
+
+    // Range sensor specific parameters
+    node_->declare_parameter(
+      "range.topics",
+      rclcpp::ParameterValue(
+        std::vector<std::string>{"/range/topic"}));
+    node_->declare_parameter("range.phi", rclcpp::ParameterValue(1.2));
+    node_->declare_parameter("range.clear_on_max_reading", rclcpp::ParameterValue(true));
+  }
+
+  ~TestNode() {}
+
+protected:
+  std::shared_ptr<TestLifecycleNode> node_;
+  tf2_ros::Buffer tf_;
+};
+
+// Test clearing at max range
+TEST_F(TestNode, testClearingAtMaxRange) {
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp = node_->now();
+  transform.header.frame_id = "frame";
+  transform.child_frame_id = "base_link";
+  transform.transform.translation.y = 5;
+  transform.transform.translation.x = 2;
+  tf_.setTransform(transform, "default_authority", true);
+
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
+  addRangeLayer(layers, tf_, node_, rlayer);
+
+  sensor_msgs::msg::Range msg;
+  msg.min_range = 1.0;
+  msg.max_range = 7.0;
+  msg.range = 2.0;
+  msg.header.stamp = node_->now();
+  msg.header.frame_id = "base_link";
+  msg.radiation_type = msg.ULTRASOUND;
+  msg.field_of_view = 0.174533;  // 10 deg
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 254);
+
+  msg.range = 7.0;
+  msg.header.stamp = node_->now();
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 0);
+}
+
+// Testing fixed scan with robot forward motion
+TEST_F(TestNode, testProbabalisticModelForward) {
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp = node_->now();
+  transform.header.frame_id = "frame";
+  transform.child_frame_id = "base_link";
+  transform.transform.translation.y = 5;
+  transform.transform.translation.x = 2;
+  tf_.setTransform(transform, "default_authority", true);
+
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
+  addRangeLayer(layers, tf_, node_, rlayer);
+
+  sensor_msgs::msg::Range msg;
+  msg.min_range = 1.0;
+  msg.max_range = 10.0;
+  msg.range = 3.0;
+  msg.header.stamp = node_->now();
+  msg.header.frame_id = "base_link";
+  msg.radiation_type = msg.ULTRASOUND;
+  msg.field_of_view = 0.174533;  // 10 deg
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+  transform.transform.translation.y = 5;
+  transform.transform.translation.x = 4;
+  tf_.setTransform(transform, "default_authority", true);
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+  transform.transform.translation.y = 5;
+  transform.transform.translation.x = 6;
+  tf_.setTransform(transform, "default_authority", true);
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  ASSERT_EQ(layers.getCostmap()->getCost(5, 5), 254);
+  ASSERT_EQ(layers.getCostmap()->getCost(6, 5), 0);
+  ASSERT_EQ(layers.getCostmap()->getCost(7, 5), 254);
+  ASSERT_EQ(layers.getCostmap()->getCost(8, 5), 0);
+  ASSERT_EQ(layers.getCostmap()->getCost(9, 5), 254);
+}
+
+// Testing fixed motion with downward movement
+TEST_F(TestNode, testProbabalisticModelDownward) {
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp = node_->now();
+  transform.header.frame_id = "frame";
+  transform.child_frame_id = "base_link";
+  transform.transform.translation.y = 3;
+  transform.transform.translation.x = 2;
+  tf_.setTransform(transform, "default_authority", true);
+
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
+  addRangeLayer(layers, tf_, node_, rlayer);
+
+  sensor_msgs::msg::Range msg;
+  msg.min_range = 1.0;
+  msg.max_range = 10.0;
+  msg.range = 1.0;
+  msg.header.stamp = node_->now();
+  msg.header.frame_id = "base_link";
+  msg.radiation_type = msg.ULTRASOUND;
+  msg.field_of_view = 0.174533;  // 10 deg
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+  transform.transform.translation.y = 5;
+  transform.transform.translation.x = 2;
+  tf_.setTransform(transform, "default_authority", true);
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
+
+  transform.transform.translation.y = 7;
+  transform.transform.translation.x = 2;
+  tf_.setTransform(transform, "default_authority", true);
+
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+//  printMap(*(layers.getCostmap()));
+
+  ASSERT_EQ(layers.getCostmap()->getCost(3, 3), 254);
+  ASSERT_EQ(layers.getCostmap()->getCost(3, 4), 0);
+  ASSERT_EQ(layers.getCostmap()->getCost(3, 5), 254);
+  ASSERT_EQ(layers.getCostmap()->getCost(3, 6), 0);
+  ASSERT_EQ(layers.getCostmap()->getCost(3, 7), 254);
+}

navigation2/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp

@@ -0,0 +1,359 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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 <string>
+#include <vector>
+#include <memory>
+#include <chrono>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/static_layer.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
+#include "../testing_helper.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_broadcaster.h"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
+#include "nav2_util/geometry_utils.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+using nav2_util::geometry_utils::orientationAroundZAxis;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
+{
+public:
+  DummyCostmapSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name)
+  : CostmapSubscriber(node, topic_name)
+  {}
+
+  void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
+  {
+    costmap_msg_ = msg;
+    costmap_received_ = true;
+  }
+};
+
+class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
+{
+public:
+  DummyFootprintSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name,
+    tf2_ros::Buffer & tf_)
+  : FootprintSubscriber(node, topic_name, tf_)
+  {}
+
+  void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
+  {
+    footprint_ = msg;
+    footprint_received_ = true;
+  }
+};
+
+class TestCollisionChecker : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestCollisionChecker(std::string name)
+  : LifecycleNode(name),
+    global_frame_("map")
+  {
+    // Declare non-plugin specific costmap parameters
+    declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
+    declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
+    declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+    declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    declare_parameter(
+      "unknown_cost_value",
+      rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
+    declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+  }
+
+  nav2_util::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(get_logger(), "Configuring");
+    callback_group_ = create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      get_node_base_interface(),
+      get_node_timers_interface(),
+      callback_group_);
+    tf_buffer_->setCreateTimerInterface(timer_interface);
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this());
+
+    std::string costmap_topic = "costmap_raw";
+    std::string footprint_topic = "published_footprint";
+
+    costmap_sub_ = std::make_shared<DummyCostmapSubscriber>(
+      shared_from_this(),
+      costmap_topic);
+
+    footprint_sub_ = std::make_shared<DummyFootprintSubscriber>(
+      shared_from_this(),
+      footprint_topic,
+      *tf_buffer_);
+
+    collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
+      *costmap_sub_, *footprint_sub_, get_name());
+
+    layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
+    // Add Static Layer
+    std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+    addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_);
+
+    while (!slayer->isCurrent()) {
+      rclcpp::spin_some(this->get_node_base_interface());
+    }
+    // Add Inflation Layer
+    std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+    addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer, callback_group_);
+
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_callback_group(callback_group_, get_node_base_interface());
+    executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn
+  on_activate(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(get_logger(), "Activating");
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(get_logger(), "Deactivating");
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn
+  on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_INFO(get_logger(), "Cleaning Up");
+    delete layers_;
+    layers_ = nullptr;
+
+    executor_thread_.reset();
+    tf_buffer_.reset();
+
+    footprint_sub_.reset();
+    costmap_sub_.reset();
+
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  ~TestCollisionChecker() {}
+
+  bool testPose(double x, double y, double theta)
+  {
+    rclcpp::Time stamp = now();
+    publishPose(x, y, theta, stamp);
+    geometry_msgs::msg::Pose2D pose;
+    pose.x = x;
+    pose.y = y;
+    pose.theta = theta;
+
+    setPose(x, y, theta, stamp);
+    publishFootprint();
+    publishCostmap();
+    rclcpp::sleep_for(std::chrono::milliseconds(1000));
+    return collision_checker_->isCollisionFree(pose);
+  }
+
+  void setFootprint(double footprint_padding, double robot_radius)
+  {
+    std::vector<geometry_msgs::msg::Point> new_footprint;
+    new_footprint = nav2_costmap_2d::makeFootprintFromRadius(robot_radius);
+    nav2_costmap_2d::padFootprint(new_footprint, footprint_padding);
+    footprint_ = new_footprint;
+    layers_->setFootprint(footprint_);
+  }
+
+protected:
+  void setPose(double x, double y, double theta, const rclcpp::Time & stamp)
+  {
+    x_ = x;
+    y_ = y;
+    yaw_ = theta;
+    stamp_ = stamp;
+
+    current_pose_.pose.position.x = x_;
+    current_pose_.pose.position.y = y_;
+    current_pose_.pose.position.z = 0;
+    current_pose_.pose.orientation = orientationAroundZAxis(yaw_);
+    current_pose_.header.stamp = stamp;
+  }
+
+  void publishFootprint()
+  {
+    geometry_msgs::msg::PolygonStamped oriented_footprint;
+    oriented_footprint.header.frame_id = global_frame_;
+    oriented_footprint.header.stamp = stamp_;
+    nav2_costmap_2d::transformFootprint(x_, y_, yaw_, footprint_, oriented_footprint);
+    footprint_sub_->setFootprint(
+      std::make_shared<geometry_msgs::msg::PolygonStamped>(oriented_footprint));
+  }
+
+  void publishCostmap()
+  {
+    layers_->updateMap(x_, y_, yaw_);
+    costmap_sub_->setCostmap(
+      std::make_shared<nav2_msgs::msg::Costmap>(toCostmapMsg(layers_->getCostmap())));
+  }
+
+  void publishPose(double x, double y, double /*theta*/, const rclcpp::Time & stamp)
+  {
+    geometry_msgs::msg::TransformStamped tf_stamped;
+    tf_stamped.header.frame_id = "map";
+    tf_stamped.header.stamp = stamp;
+    tf_stamped.child_frame_id = "base_link";
+    tf_stamped.transform.translation.x = x;
+    tf_stamped.transform.translation.y = y;
+    tf_stamped.transform.rotation.w = 1.0;
+    tf_broadcaster_->sendTransform(tf_stamped);
+  }
+
+  nav2_msgs::msg::Costmap
+  toCostmapMsg(nav2_costmap_2d::Costmap2D * costmap)
+  {
+    double resolution = costmap->getResolution();
+
+    double wx, wy;
+    costmap->mapToWorld(0, 0, wx, wy);
+
+    unsigned char * data = costmap->getCharMap();
+
+    nav2_msgs::msg::Costmap costmap_msg;
+    costmap_msg.header.frame_id = global_frame_;
+    costmap_msg.header.stamp = stamp_;
+    costmap_msg.metadata.layer = "master";
+    costmap_msg.metadata.resolution = resolution;
+    costmap_msg.metadata.size_x = costmap->getSizeInCellsX();
+    costmap_msg.metadata.size_y = costmap->getSizeInCellsY();
+    costmap_msg.metadata.origin.position.x = wx - resolution / 2;
+    costmap_msg.metadata.origin.position.y = wy - resolution / 2;
+    costmap_msg.metadata.origin.position.z = 0.0;
+    costmap_msg.metadata.origin.orientation.w = 1.0;
+    costmap_msg.data.resize(costmap_msg.metadata.size_x * costmap_msg.metadata.size_y);
+
+    for (unsigned int i = 0; i < costmap_msg.data.size(); i++) {
+      costmap_msg.data[i] = data[i];
+    }
+
+    return costmap_msg;
+  }
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+
+  std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
+  std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
+  std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+
+  nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
+  std::string global_frame_;
+  double x_, y_, yaw_;
+  rclcpp::Time stamp_;
+  geometry_msgs::msg::PoseStamped current_pose_;
+  std::vector<geometry_msgs::msg::Point> footprint_;
+};
+
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode()
+  {
+    collision_checker_ = std::make_shared<TestCollisionChecker>("test_collision_checker");
+    collision_checker_->on_configure(collision_checker_->get_current_state());
+    collision_checker_->on_activate(collision_checker_->get_current_state());
+  }
+
+  ~TestNode()
+  {
+    collision_checker_->on_deactivate(collision_checker_->get_current_state());
+    collision_checker_->on_cleanup(collision_checker_->get_current_state());
+  }
+
+protected:
+  std::shared_ptr<TestCollisionChecker> collision_checker_;
+};
+
+TEST_F(TestNode, unknownSpace)
+{
+  collision_checker_->setFootprint(0, 1);
+
+  // Completely off map
+  ASSERT_EQ(collision_checker_->testPose(5, 13, 0), false);
+
+  // Partially off map
+  ASSERT_EQ(collision_checker_->testPose(5, 9.5, 0), false);
+
+  // In unknown region inside map
+  ASSERT_EQ(collision_checker_->testPose(2, 4, 0), false);
+}
+
+TEST_F(TestNode, FreeSpace)
+{
+  collision_checker_->setFootprint(0, 1);
+
+  // In complete free space
+  ASSERT_EQ(collision_checker_->testPose(2, 8.5, 0), true);
+
+  // Partially in inscribed space
+  ASSERT_EQ(collision_checker_->testPose(2.5, 7, 0), true);
+}
+
+TEST_F(TestNode, CollisionSpace)
+{
+  collision_checker_->setFootprint(0, 1);
+
+  // Completely in obstacle
+  ASSERT_EQ(collision_checker_->testPose(8.5, 6.5, 0), false);
+
+  // Partially in obstacle
+  ASSERT_EQ(collision_checker_->testPose(4.5, 4.5, 0), false);
+}

navigation2/nav2_costmap_2d/test/map/TenByTen.yaml

@@ -0,0 +1,6 @@
+image: TenByTen.pgm
+resolution: 1.0
+origin: [0,0,0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

navigation2/nav2_costmap_2d/test/regression/CMakeLists.txt

@@ -0,0 +1,27 @@
+# Bresenham2D corner cases test
+ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp)
+target_link_libraries(costmap_bresenham_2d
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+# OrderLayer for checking Costmap2D plugins API calling order
+add_library(order_layer SHARED
+            order_layer.cpp)
+ament_target_dependencies(order_layer
+  ${dependencies}
+)
+target_link_libraries(order_layer
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+install(TARGETS
+  order_layer
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Costmap2D plugins API calling order test
+ament_add_gtest(plugin_api_order plugin_api_order.cpp)
+target_link_libraries(plugin_api_order
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)

navigation2/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp

@@ -0,0 +1,159 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2022 Samsung Research Russia
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Alexey Merzlyakov
+*********************************************************************/
+#include <nav2_costmap_2d/costmap_2d.hpp>
+#include <gtest/gtest.h>
+
+class CostmapAction
+{
+public:
+  explicit CostmapAction(
+    unsigned char * costmap, unsigned int size, unsigned char mark_val = 128)
+  : costmap_(costmap), size_(size), mark_val_(mark_val)
+  {
+  }
+
+  inline void operator()(unsigned int off)
+  {
+    ASSERT_TRUE(off < size_);
+    costmap_[off] = mark_val_;
+  }
+
+  inline unsigned int get(unsigned int off)
+  {
+    return costmap_[off];
+  }
+
+private:
+  unsigned char * costmap_;
+  unsigned int size_;
+  unsigned char mark_val_;
+};
+
+class CostmapTest : public nav2_costmap_2d::Costmap2D
+{
+public:
+  CostmapTest(
+    unsigned int size_x, unsigned int size_y, double resolution,
+    double origin_x, double origin_y, unsigned char default_val = 0)
+  : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val)
+  {
+  }
+
+  unsigned char * getCostmap()
+  {
+    return costmap_;
+  }
+
+  unsigned int getSize()
+  {
+    return size_x_ * size_y_;
+  }
+
+  void raytraceLine(
+    CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1,
+    unsigned int y1,
+    unsigned int max_length = UINT_MAX, unsigned int min_length = 0)
+  {
+    nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+};
+
+TEST(costmap_2d, bresenham2DBoundariesCheck)
+{
+  const unsigned int sz_x = 60;
+  const unsigned int sz_y = 60;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 6;
+  CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
+  CostmapAction ca(ct.getCostmap(), ct.getSize());
+
+  // Initial point - some assymetrically standing point in order to cover most corner cases
+  const unsigned int x0 = 2;
+  const unsigned int y0 = 4;
+  // (x1, y1) point will move
+  unsigned int x1, y1;
+
+  // Running on (x, 0) edge
+  y1 = 0;
+  for (x1 = 0; x1 < sz_x; x1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (x, sz_y) edge
+  y1 = sz_y - 1;
+  for (x1 = 0; x1 < sz_x; x1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (0, y) edge
+  x1 = 0;
+  for (y1 = 0; y1 < sz_y; y1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (sz_x, y) edge
+  x1 = sz_x - 1;
+  for (y1 = 0; y1 < sz_y; y1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+}
+
+TEST(costmap_2d, bresenham2DSamePoint)
+{
+  const unsigned int sz_x = 60;
+  const unsigned int sz_y = 60;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 0;
+  CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
+  CostmapAction ca(ct.getCostmap(), ct.getSize());
+
+  // Initial point
+  const double x0 = 2;
+  const double y0 = 4;
+
+  unsigned int offset = y0 * sz_x + x0;
+  unsigned char val_before = ca.get(offset);
+  // Same point to check
+  ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length);
+  unsigned char val_after = ca.get(offset);
+  ASSERT_FALSE(val_before == val_after);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

navigation2/nav2_costmap_2d/test/regression/order_layer.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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. Reserved.
+
+#include "order_layer.hpp"
+
+#include <chrono>
+#include <stdexcept>
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+OrderLayer::OrderLayer()
+: activated_(false)
+{
+}
+
+void OrderLayer::activate()
+{
+  std::this_thread::sleep_for(100ms);
+  activated_ = true;
+}
+
+void OrderLayer::deactivate()
+{
+  activated_ = false;
+}
+
+void OrderLayer::updateBounds(
+  double, double, double, double *, double *, double *, double *)
+{
+  if (!activated_) {
+    throw std::runtime_error("update before activated");
+  }
+}
+
+void OrderLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D &, int, int, int, int)
+{
+  if (!activated_) {
+    throw std::runtime_error("update before activated");
+  }
+}
+
+}  // namespace nav2_costmap_2d
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::OrderLayer, nav2_costmap_2d::Layer)

navigation2/nav2_costmap_2d/test/regression/order_layer.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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. Reserved.
+
+#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+
+#include "nav2_costmap_2d/layer.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class OrderLayer : public nav2_costmap_2d::Layer
+{
+public:
+  OrderLayer();
+
+  virtual void activate();
+  virtual void deactivate();
+
+  virtual void reset() {}
+  virtual bool isClearable() {return false;}
+
+  virtual void updateBounds(
+    double, double, double, double *, double *, double *, double *);
+
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D &, int, int, int, int);
+
+private:
+  bool activated_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_

navigation2/nav2_costmap_2d/test/regression/order_layer.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="order_layer">
+    <class type="nav2_costmap_2d::OrderLayer" base_class_type="nav2_costmap_2d::Layer">
+      <description>Plugin checking order of activate() and updateBounds()/updateCosts() calls</description>
+    </class>
+  </library>
+</class_libraries>

navigation2/nav2_costmap_2d/test/regression/plugin_api_order.cpp

@@ -0,0 +1,53 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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. Reserved.
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <nav2_costmap_2d/costmap_2d_ros.hpp>
+#include <gtest/gtest.h>
+
+TEST(CostmapPluginsTester, checkPluginAPIOrder)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap_ros");
+
+  // Workaround to avoid setting base_link->map transform
+  costmap_ros->set_parameter(rclcpp::Parameter("robot_base_frame", "map"));
+  // Specifying order verification plugin in the parameters
+  std::vector<std::string> plugins_str;
+  plugins_str.push_back("order_layer");
+  costmap_ros->set_parameter(rclcpp::Parameter("plugins", plugins_str));
+  costmap_ros->declare_parameter(
+    "order_layer.plugin",
+    rclcpp::ParameterValue(std::string("nav2_costmap_2d::OrderLayer")));
+
+  // Do actual test: ensure that plugin->updateBounds()/updateCosts()
+  // will be called after plugin->activate()
+  costmap_ros->on_configure(costmap_ros->get_current_state());
+  costmap_ros->on_activate(costmap_ros->get_current_state());
+
+  // Do cleanup
+  costmap_ros->on_deactivate(costmap_ros->get_current_state());
+  costmap_ros->on_cleanup(costmap_ros->get_current_state());
+  costmap_ros->on_shutdown(costmap_ros->get_current_state());
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

navigation2/nav2_costmap_2d/test/simple_driving_test.xml

@@ -0,0 +1,14 @@
+<launch>
+  <param name="/use_sim_time" value="true"/>
+
+  <node name="rosplay" pkg="rosbag" type="play"
+        args="-s 5 -r 1 --clock --hz=10 $(find nav2_costmap_2d)/test/simple_driving_test_indexed.bag" />
+
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find nav2_costmap_2d)/test/willow-full-0.025.pgm 0.025" />
+
+  <rosparam file="$(find nav2_costmap_2d)/test/costmap_params.yaml" command="load" ns="simple_driving_test/test_costmap" />
+  <test time-limit="600" test-name="simple_driving_test" pkg="nav2_costmap_2d" type="costmap_tester">
+    <param name="wait_time" value="40.0" />
+  </test>
+
+</launch>

navigation2/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+#
+# 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.
+
+import os
+
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    mapFile = os.getenv('TEST_MAP')
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            package='nav2_map_server',
+            executable='map_server',
+            name='map_server',
+            output='screen',
+            parameters=[{'yaml_filename': mapFile}])
+    ])

navigation2/nav2_costmap_2d/test/testing_helper.hpp

@@ -0,0 +1,146 @@
+// Copyright (c) Willow Garage
+// Copyright (c) 2019 Intel Corporation
+//
+// 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.
+
+#ifndef TESTING_HELPER_HPP_
+#define TESTING_HELPER_HPP_
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/static_layer.hpp"
+#include "nav2_costmap_2d/range_sensor_layer.hpp"
+#include "nav2_costmap_2d/obstacle_layer.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+const double MAX_Z(1.0);
+
+char printableCost(unsigned char cost)
+{
+  switch (cost) {
+    case nav2_costmap_2d::NO_INFORMATION: return '?';
+    case nav2_costmap_2d::LETHAL_OBSTACLE: return 'L';
+    case nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
+    case nav2_costmap_2d::FREE_SPACE: return '.';
+    default: return '0' + (unsigned char) (10 * cost / 255);
+  }
+}
+
+void printMap(nav2_costmap_2d::Costmap2D & costmap)
+{
+  printf("map:\n");
+  for (unsigned int i = 0; i < costmap.getSizeInCellsY(); i++) {
+    for (unsigned int j = 0; j < costmap.getSizeInCellsX(); j++) {
+      printf("%4d", static_cast<int>(costmap.getCost(j, i)));
+    }
+    printf("\n\n");
+  }
+}
+
+unsigned int countValues(
+  nav2_costmap_2d::Costmap2D & costmap,
+  unsigned char value, bool equal = true)
+{
+  unsigned int count = 0;
+  for (unsigned int i = 0; i < costmap.getSizeInCellsY(); i++) {
+    for (unsigned int j = 0; j < costmap.getSizeInCellsX(); j++) {
+      unsigned char c = costmap.getCost(j, i);
+      if ((equal && c == value) || (!equal && c != value)) {
+        count += 1;
+      }
+    }
+  }
+  return count;
+}
+
+void addStaticLayer(
+  nav2_costmap_2d::LayeredCostmap & layers,
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer,
+  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
+{
+  slayer = std::make_shared<nav2_costmap_2d::StaticLayer>();
+  layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(slayer));
+  slayer->initialize(&layers, "static", &tf, node, callback_group);
+}
+
+void addObstacleLayer(
+  nav2_costmap_2d::LayeredCostmap & layers,
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer,
+  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
+{
+  olayer = std::make_shared<nav2_costmap_2d::ObstacleLayer>();
+  olayer->initialize(&layers, "obstacles", &tf, node, callback_group);
+  layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(olayer));
+}
+
+void addRangeLayer(
+  nav2_costmap_2d::LayeredCostmap & layers,
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> & rlayer,
+  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
+{
+  rlayer = std::make_shared<nav2_costmap_2d::RangeSensorLayer>();
+  rlayer->initialize(&layers, "range", &tf, node, callback_group);
+  layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(rlayer));
+}
+
+void addObservation(
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer, double x, double y, double z = 0.0,
+  double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true,
+  double raytrace_max_range = 100.0,
+  double raytrace_min_range = 0.0,
+  double obstacle_max_range = 100.0,
+  double obstacle_min_range = 0.0)
+{
+  sensor_msgs::msg::PointCloud2 cloud;
+  sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2FieldsByString(1, "xyz");
+  modifier.resize(1);
+  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
+  *iter_x = x;
+  *iter_y = y;
+  *iter_z = z;
+
+  geometry_msgs::msg::Point p;
+  p.x = ox;
+  p.y = oy;
+  p.z = oz;
+
+  nav2_costmap_2d::Observation obs(p, cloud, obstacle_max_range, obstacle_min_range,
+    raytrace_max_range, raytrace_min_range);
+  olayer->addStaticObservation(obs, marking, clearing);
+}
+
+void addInflationLayer(
+  nav2_costmap_2d::LayeredCostmap & layers,
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
+  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr)
+{
+  ilayer = std::make_shared<nav2_costmap_2d::InflationLayer>();
+  ilayer->initialize(&layers, "inflation", &tf, node, callback_group);
+  std::shared_ptr<nav2_costmap_2d::Layer> ipointer(ilayer);
+  layers.addPlugin(ipointer);
+}
+
+
+#endif  // TESTING_HELPER_HPP_

navigation2/nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -0,0 +1,58 @@
+ament_add_gtest(array_parser_test array_parser_test.cpp)
+target_link_libraries(array_parser_test
+  nav2_costmap_2d_core
+)
+
+ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp)
+target_link_libraries(collision_footprint_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp)
+target_link_libraries(costmap_convesion_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_gtest(declare_parameter_test declare_parameter_test.cpp)
+target_link_libraries(declare_parameter_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_gtest(costmap_filter_test costmap_filter_test.cpp)
+target_link_libraries(costmap_filter_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_gtest(keepout_filter_test keepout_filter_test.cpp)
+target_link_libraries(keepout_filter_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::filters
+)
+
+ament_add_gtest(speed_filter_test speed_filter_test.cpp)
+target_link_libraries(speed_filter_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::filters
+)
+
+ament_add_gtest(binary_filter_test binary_filter_test.cpp)
+target_link_libraries(binary_filter_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::filters
+)
+
+ament_add_gtest(copy_window_test copy_window_test.cpp)
+target_link_libraries(copy_window_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp)
+target_link_libraries(costmap_filter_service_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_test.cpp)
+target_link_libraries(denoise_layer_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  ${PROJECT_NAME}::layers
+)

navigation2/nav2_costmap_2d/test/unit/array_parser_test.cpp

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/array_parser.hpp"
+
+TEST(array_parser, basic_operation)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf;
+  vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]]", error);
+  EXPECT_EQ(2u, vvf.size() );
+  EXPECT_EQ(2u, vvf[0].size() );
+  EXPECT_EQ(2u, vvf[1].size() );
+  EXPECT_EQ(1.0f, vvf[0][0]);
+  EXPECT_EQ(2.2f, vvf[0][1]);
+  EXPECT_EQ(0.3f, vvf[1][0]);
+  EXPECT_EQ(-40000.0f, vvf[1][1]);
+  EXPECT_EQ("", error);
+}
+
+TEST(array_parser, missing_open)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf;
+  vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]]", error);
+  EXPECT_NE(error, "");
+}
+
+TEST(array_parser, missing_close)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf;
+  vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]", error);
+  EXPECT_NE(error, "");
+}
+
+TEST(array_parser, wrong_depth)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf;
+  vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]", error);
+  EXPECT_NE(error, "");
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

navigation2/nav2_costmap_2d/test/unit/binary_filter_test.cpp

@@ -0,0 +1,877 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <chrono>
+#include <vector>
+#include <tuple>
+#include <functional>
+#include <stdexcept>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/msg/costmap_filter_info.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_costmap_2d/costmap_filters/binary_filter.hpp"
+
+using namespace std::chrono_literals;
+
+static const char FILTER_NAME[]{"binary_filter"};
+static const char INFO_TOPIC[]{"costmap_filter_info"};
+static const char MASK_TOPIC[]{"mask"};
+static const char BINARY_STATE_TOPIC[]{"binary_state"};
+
+static const double NO_TRANSLATION = 0.0;
+static const double TRANSLATION_X = 1.0;
+static const double TRANSLATION_Y = 1.0;
+
+static const uint8_t INCORRECT_TYPE = 200;
+
+class InfoPublisher : public rclcpp::Node
+{
+public:
+  InfoPublisher(uint8_t type, const char * mask_topic, double base, double multiplier)
+  : Node("costmap_filter_info_pub")
+  {
+    publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
+      INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+    std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> msg =
+      std::make_unique<nav2_msgs::msg::CostmapFilterInfo>();
+    msg->type = type;
+    msg->filter_mask_topic = mask_topic;
+    msg->base = static_cast<float>(base);
+    msg->multiplier = static_cast<float>(multiplier);
+
+    publisher_->publish(std::move(msg));
+  }
+
+  ~InfoPublisher()
+  {
+    publisher_.reset();
+  }
+
+private:
+  rclcpp::Publisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
+};  // InfoPublisher
+
+class MaskPublisher : public rclcpp::Node
+{
+public:
+  explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask)
+  : Node("mask_pub")
+  {
+    publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+      MASK_TOPIC,
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+    publisher_->publish(mask);
+  }
+
+  ~MaskPublisher()
+  {
+    publisher_.reset();
+  }
+
+private:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr publisher_;
+};  // MaskPublisher
+
+class BinaryStateSubscriber : public rclcpp::Node
+{
+public:
+  explicit BinaryStateSubscriber(const std::string & binary_state_topic, bool default_state)
+  : Node("binary_state_sub"), binary_state_updated_(false)
+  {
+    subscriber_ = this->create_subscription<std_msgs::msg::Bool>(
+      binary_state_topic, rclcpp::QoS(10),
+      std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1));
+
+    // Initialize with default state
+    msg_ = std::make_shared<std_msgs::msg::Bool>();
+    msg_->data = default_state;
+  }
+
+  void binaryStateCallback(
+    const std_msgs::msg::Bool::SharedPtr msg)
+  {
+    msg_ = msg;
+    binary_state_updated_ = true;
+  }
+
+  std_msgs::msg::Bool::SharedPtr getBinaryState()
+  {
+    return msg_;
+  }
+
+  inline bool binaryStateUpdated()
+  {
+    return binary_state_updated_;
+  }
+
+  inline void resetBinaryStateIndicator()
+  {
+    binary_state_updated_ = false;
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr subscriber_;
+  std_msgs::msg::Bool::SharedPtr msg_;
+  bool binary_state_updated_;
+};  // BinaryStateSubscriber
+
+class TestMask : public nav_msgs::msg::OccupancyGrid
+{
+public:
+  TestMask(
+    unsigned int width, unsigned int height, double resolution,
+    const std::string & mask_frame)
+  : width_(width), height_(height)
+  {
+    // Fill filter mask info
+    header.frame_id = mask_frame;
+    info.resolution = resolution;
+    info.width = width_;
+    info.height = height_;
+    info.origin.position.x = 0.0;
+    info.origin.position.y = 0.0;
+    info.origin.position.z = 0.0;
+    info.origin.orientation.x = 0.0;
+    info.origin.orientation.y = 0.0;
+    info.origin.orientation.z = 0.0;
+    info.origin.orientation.w = 1.0;
+
+    // Fill test mask as follows:
+    //
+    //  mask           (10,11)
+    //   *----------------*
+    //   |91|92|...|99|100|
+    //   |...             |
+    //   |...             |
+    //   |11|12|13|...| 20|
+    //   | 1| 2| 3|...| 10|
+    //   |-1| 0| 0|...|  0|
+    //   *----------------*
+    // (0,0)
+    data.resize(width_ * height_, nav2_util::OCC_GRID_UNKNOWN);
+
+    unsigned int mx, my;
+    data[0] = nav2_util::OCC_GRID_UNKNOWN;
+    for (mx = 1; mx < width_; mx++) {
+      data[mx] = nav2_util::OCC_GRID_FREE;
+    }
+    unsigned int it;
+    for (my = 1; my < height_; my++) {
+      for (mx = 0; mx < width_; mx++) {
+        it = mx + my * width_;
+        data[it] = makeData(mx, my);
+      }
+    }
+  }
+
+  inline int8_t makeData(unsigned int mx, unsigned int my)
+  {
+    return mx + (my - 1) * width_ + 1;
+  }
+
+private:
+  const unsigned int width_;
+  const unsigned int height_;
+};  // TestMask
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode()
+  : default_state_(false) {}
+
+  ~TestNode() {}
+
+protected:
+  void createMaps(const std::string & mask_frame);
+  void publishMaps(uint8_t type, const char * mask_topic, double base, double multiplier);
+  void rePublishInfo(uint8_t type, const char * mask_topic, double base, double multiplier);
+  void rePublishMask();
+  void setDefaultState(bool default_state);  // NOTE: must be called before createBinaryFilter()
+  bool createBinaryFilter(const std::string & global_frame, double flip_threshold);
+  void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame);
+  void publishTransform();
+
+  // Test methods
+  void testFullMask(
+    double base, double multiplier, double flip_threshold, double tr_x, double tr_y);
+  void testSimpleMask(
+    double base, double multiplier, double flip_threshold, double tr_x, double tr_y);
+  void testOutOfMask();
+  void testIncorrectTF();
+  void testResetFilter();
+
+  void resetMaps();
+  void reset();
+
+  std::shared_ptr<nav2_costmap_2d::BinaryFilter> binary_filter_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> master_grid_;
+
+  bool default_state_;
+
+private:
+  void waitSome(const std::chrono::nanoseconds & duration);
+  std_msgs::msg::Bool::SharedPtr getBinaryState();
+  std_msgs::msg::Bool::SharedPtr waitBinaryState();
+  bool getSign(
+    unsigned int x, unsigned int y, double base, double multiplier, double flip_threshold);
+  void verifyBinaryState(bool sign, std_msgs::msg::Bool::SharedPtr state);
+
+  const unsigned int width_ = 10;
+  const unsigned int height_ = 11;
+  const double resolution_ = 1.0;
+
+  nav2_util::LifecycleNode::SharedPtr node_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::unique_ptr<geometry_msgs::msg::TransformStamped> transform_;
+
+  std::shared_ptr<TestMask> mask_;
+
+  std::shared_ptr<InfoPublisher> info_publisher_;
+  std::shared_ptr<MaskPublisher> mask_publisher_;
+  std::shared_ptr<BinaryStateSubscriber> binary_state_subscriber_;
+};
+
+void TestNode::createMaps(const std::string & mask_frame)
+{
+  // Make map and mask put as follows:
+  //  master_grid     (12,13)
+  //    *----------------*
+  //    |                |
+  //    |  mask  (10,11) |
+  //    |   *-------*    |
+  //    |   |///////|    |
+  //    |   |///////|    |
+  //    |   |///////|    |
+  //    |   *-------*    |
+  //    | (0,0)          |
+  //    |                |
+  //    *----------------*
+  // (-2,-2)
+
+  // Create master_grid_
+  master_grid_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+    width_ + 4, height_ + 4, resolution_, -2.0, -2.0, nav2_costmap_2d::FREE_SPACE);
+
+  // Create mask_
+  mask_ = std::make_shared<TestMask>(width_, height_, resolution_, mask_frame);
+}
+
+void TestNode::publishMaps(
+  uint8_t type, const char * mask_topic, double base, double multiplier)
+{
+  info_publisher_ = std::make_shared<InfoPublisher>(type, mask_topic, base, multiplier);
+  mask_publisher_ = std::make_shared<MaskPublisher>(*mask_);
+}
+
+void TestNode::rePublishInfo(
+  uint8_t type, const char * mask_topic, double base, double multiplier)
+{
+  info_publisher_.reset();
+  info_publisher_ = std::make_shared<InfoPublisher>(type, mask_topic, base, multiplier);
+  // Allow both CostmapFilterInfo and filter mask subscribers
+  // to receive a new message
+  waitSome(100ms);
+}
+
+void TestNode::rePublishMask()
+{
+  mask_publisher_.reset();
+  mask_publisher_ = std::make_shared<MaskPublisher>(*mask_);
+  // Allow filter mask subscriber to receive a new message
+  waitSome(100ms);
+}
+
+void TestNode::waitSome(const std::chrono::nanoseconds & duration)
+{
+  rclcpp::Time start_time = node_->now();
+  while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) {
+    rclcpp::spin_some(node_->get_node_base_interface());
+    rclcpp::spin_some(binary_state_subscriber_);
+    std::this_thread::sleep_for(10ms);
+  }
+}
+
+std_msgs::msg::Bool::SharedPtr TestNode::getBinaryState()
+{
+  std::this_thread::sleep_for(100ms);
+  rclcpp::spin_some(binary_state_subscriber_);
+  return binary_state_subscriber_->getBinaryState();
+}
+
+std_msgs::msg::Bool::SharedPtr TestNode::waitBinaryState()
+{
+  const std::chrono::nanoseconds timeout = 500ms;
+
+  rclcpp::Time start_time = node_->now();
+  binary_state_subscriber_->resetBinaryStateIndicator();
+  while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(timeout)) {
+    if (binary_state_subscriber_->binaryStateUpdated()) {
+      binary_state_subscriber_->resetBinaryStateIndicator();
+      return binary_state_subscriber_->getBinaryState();
+    }
+    rclcpp::spin_some(binary_state_subscriber_);
+    std::this_thread::sleep_for(10ms);
+  }
+  return nullptr;
+}
+
+void TestNode::setDefaultState(bool default_state)
+{
+  default_state_ = default_state;
+}
+
+bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_threshold)
+{
+  node_ = std::make_shared<nav2_util::LifecycleNode>("test_node");
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false);
+
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".default_state", rclcpp::ParameterValue(default_state_));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".default_state", default_state_));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".binary_state_topic", rclcpp::ParameterValue(BINARY_STATE_TOPIC));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".binary_state_topic", BINARY_STATE_TOPIC));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".flip_threshold", rclcpp::ParameterValue(flip_threshold));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".flip_threshold", flip_threshold));
+
+  binary_filter_ = std::make_shared<nav2_costmap_2d::BinaryFilter>();
+  binary_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr);
+  binary_filter_->initializeFilter(INFO_TOPIC);
+
+  binary_state_subscriber_ =
+    std::make_shared<BinaryStateSubscriber>(BINARY_STATE_TOPIC, default_state_);
+
+  // Wait until mask will be received by BinaryFilter
+  const std::chrono::nanoseconds timeout = 500ms;
+  rclcpp::Time start_time = node_->now();
+  while (!binary_filter_->isActive()) {
+    if (node_->now() - start_time > rclcpp::Duration(timeout)) {
+      return false;
+    }
+    rclcpp::spin_some(node_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return true;
+}
+
+void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame)
+{
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+
+  transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+  transform_->header.frame_id = mask_frame;
+  transform_->child_frame_id = global_frame;
+
+  transform_->header.stamp = node_->now() + rclcpp::Duration(100ms);
+  transform_->transform.translation.x = TRANSLATION_X;
+  transform_->transform.translation.y = TRANSLATION_Y;
+  transform_->transform.translation.z = 0.0;
+  transform_->transform.rotation.x = 0.0;
+  transform_->transform.rotation.y = 0.0;
+  transform_->transform.rotation.z = 0.0;
+  transform_->transform.rotation.w = 1.0;
+
+  tf_broadcaster_->sendTransform(*transform_);
+
+  // Allow tf_buffer_ to be filled by listener
+  waitSome(100ms);
+}
+
+void TestNode::publishTransform()
+{
+  if (tf_broadcaster_) {
+    transform_->header.stamp = node_->now() + rclcpp::Duration(100ms);
+    tf_broadcaster_->sendTransform(*transform_);
+  }
+}
+
+bool TestNode::getSign(
+  unsigned int x, unsigned int y, double base, double multiplier, double flip_threshold)
+{
+  const int8_t cost = mask_->makeData(x, y);
+  return base + cost * multiplier > flip_threshold;
+}
+
+void TestNode::verifyBinaryState(bool sign, std_msgs::msg::Bool::SharedPtr state)
+{
+  ASSERT_TRUE(state != nullptr);
+  if (sign) {
+    EXPECT_FALSE(state->data == default_state_);
+  } else {
+    EXPECT_TRUE(state->data == default_state_);
+  }
+}
+
+void TestNode::testFullMask(
+  double base, double multiplier, double flip_threshold, double tr_x, double tr_y)
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  std_msgs::msg::Bool::SharedPtr binary_state;
+
+  unsigned int x, y;
+  bool prev_sign = false;
+  bool sign;
+
+  // data = 0
+  x = 1;
+  y = 0;
+  pose.x = x - tr_x;
+  pose.y = y - tr_y;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  sign = getSign(x, y, base, multiplier, flip_threshold);
+  if (sign != prev_sign) {
+    // Binary filter just flipped
+    binary_state = waitBinaryState();
+    prev_sign = sign;
+  } else {
+    // Binary filter state should not be changed
+    binary_state = getBinaryState();
+  }
+  verifyBinaryState(sign, binary_state);
+
+  // data in range [1..100] (sparsed for testing speed)
+  for (y = 1; y < height_; y += 2) {
+    for (x = 0; x < width_; x += 2) {
+      pose.x = x - tr_x;
+      pose.y = y - tr_y;
+      publishTransform();
+      binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+
+      sign = getSign(x, y, base, multiplier, flip_threshold);
+      if (prev_sign != sign) {
+        // Binary filter just flipped
+        binary_state = waitBinaryState();
+        prev_sign = sign;
+      } else {
+        // Binary filter state should not be changed
+        binary_state = getBinaryState();
+      }
+      verifyBinaryState(sign, binary_state);
+    }
+  }
+
+  // data = -1 (unknown)
+  bool prev_state = binary_state->data;
+  pose.x = -tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = getBinaryState();
+  ASSERT_TRUE(binary_state != nullptr);
+  ASSERT_EQ(binary_state->data, prev_state);  // Binary state won't be updated
+}
+
+void TestNode::testSimpleMask(
+  double base, double multiplier, double flip_threshold, double tr_x, double tr_y)
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  std_msgs::msg::Bool::SharedPtr binary_state;
+
+  unsigned int x, y;
+  bool prev_sign = false;
+  bool sign;
+
+  // data = 0
+  x = 1;
+  y = 0;
+  pose.x = x - tr_x;
+  pose.y = y - tr_y;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  sign = getSign(x, y, base, multiplier, flip_threshold);
+  if (sign != prev_sign) {
+    // Binary filter just flipped
+    binary_state = waitBinaryState();
+    prev_sign = sign;
+  } else {
+    // Binary filter state should not be changed
+    binary_state = getBinaryState();
+  }
+  verifyBinaryState(sign, binary_state);
+
+  // data = <some_middle_value>
+  x = width_ / 2 - 1;
+  y = height_ / 2 - 1;
+  pose.x = x - tr_x;
+  pose.y = y - tr_y;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+
+  sign = getSign(x, y, base, multiplier, flip_threshold);
+  if (prev_sign != sign) {
+    // Binary filter just flipped
+    binary_state = waitBinaryState();
+    prev_sign = sign;
+  } else {
+    // Binary filter state should not be changed
+    binary_state = getBinaryState();
+  }
+  verifyBinaryState(sign, binary_state);
+
+  // data = 100
+  x = width_ - 1;
+  y = height_ - 1;
+  pose.x = x - tr_x;
+  pose.y = y - tr_y;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+
+  sign = getSign(x, y, base, multiplier, flip_threshold);
+  if (prev_sign != sign) {
+    // Binary filter just flipped
+    binary_state = waitBinaryState();
+    prev_sign = sign;
+  } else {
+    // Binary filter state should not be changed
+    binary_state = getBinaryState();
+  }
+  verifyBinaryState(sign, binary_state);
+
+  // data = -1 (unknown)
+  bool prev_state = binary_state->data;
+  pose.x = -tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = getBinaryState();
+  ASSERT_TRUE(binary_state != nullptr);
+  ASSERT_EQ(binary_state->data, prev_state);  // Binary state won't be updated
+}
+
+void TestNode::testOutOfMask()
+{
+  // base, multiplier and flip_threshold should have values as below for this test
+  const double base = 0.0;
+  const double multiplier = 1.0;
+  const double flip_threshold = 10.0;
+
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  std_msgs::msg::Bool::SharedPtr binary_state;
+
+  // data = <some_middle_value>
+  pose.x = width_ / 2 - 1;
+  pose.y = height_ / 2 - 1;
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = waitBinaryState();
+  verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state);
+
+  // Then go to out of mask bounds and ensure that binary state is set back to default
+  pose.x = -2.0;
+  pose.y = -2.0;
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = getBinaryState();
+  ASSERT_TRUE(binary_state != nullptr);
+  ASSERT_EQ(binary_state->data, default_state_);
+
+  pose.x = width_ + 1.0;
+  pose.y = height_ + 1.0;
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = getBinaryState();
+  ASSERT_TRUE(binary_state != nullptr);
+  ASSERT_EQ(binary_state->data, default_state_);
+}
+
+void TestNode::testIncorrectTF()
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  std_msgs::msg::Bool::SharedPtr binary_state;
+
+  // data = <some_middle_value>
+  pose.x = width_ / 2 - 1;
+  pose.y = height_ / 2 - 1;
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = waitBinaryState();
+  ASSERT_TRUE(binary_state == nullptr);
+}
+
+void TestNode::testResetFilter()
+{
+  // base, multiplier and flip_threshold should have values as below for this test
+  const double base = 0.0;
+  const double multiplier = 1.0;
+  const double flip_threshold = 10.0;
+
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  std_msgs::msg::Bool::SharedPtr binary_state;
+
+  // Switch-on binary filter
+  pose.x = width_ / 2 - 1;
+  pose.y = height_ / 2 - 1;
+  publishTransform();
+  binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  binary_state = waitBinaryState();
+  verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state);
+
+  // Reset binary filter and check its state was resetted to default
+  binary_filter_->resetFilter();
+  binary_state = waitBinaryState();
+  ASSERT_TRUE(binary_state != nullptr);
+  ASSERT_EQ(binary_state->data, default_state_);
+}
+
+void TestNode::resetMaps()
+{
+  mask_.reset();
+  master_grid_.reset();
+}
+
+void TestNode::reset()
+{
+  resetMaps();
+  info_publisher_.reset();
+  mask_publisher_.reset();
+  binary_state_subscriber_.reset();
+  binary_filter_.reset();
+  node_.reset();
+  tf_listener_.reset();
+  tf_broadcaster_.reset();
+  tf_buffer_.reset();
+}
+
+TEST_F(TestNode, testBinaryState)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  ASSERT_TRUE(createBinaryFilter("map", 10.0));
+
+  // Test BinaryFilter
+  testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testBinaryStateScaled)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 100.0, -1.0);
+  ASSERT_TRUE(createBinaryFilter("map", 35.0));
+
+  // Test BinaryFilter
+  testFullMask(100.0, -1.0, 35.0, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testInvertedBinaryState)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  setDefaultState(true);
+  ASSERT_TRUE(createBinaryFilter("map", 10.0));
+
+  // Test BinaryFilter
+  testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testOutOfBounds)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  ASSERT_TRUE(createBinaryFilter("map", 10.0));
+
+  // Test BinaryFilter
+  testOutOfMask();
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testInfoRePublish)
+{
+  // Initialize test system
+  createMaps("map");
+  // Publish Info with incorrect dummy mask topic
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, "dummy_topic", 0.0, 1.0);
+  ASSERT_FALSE(createBinaryFilter("map", 10.0));
+
+  // Re-publish filter info with correct mask topic
+  // and ensure that everything works fine
+  rePublishInfo(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+
+  // Test BinaryFilter
+  testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testMaskRePublish)
+{
+  // Create mask in incorrect frame
+  createMaps("dummy");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  EXPECT_TRUE(createBinaryFilter("map", 10.0));
+
+  // Create mask in correct frame
+  resetMaps();
+  createMaps("map");
+  // Re-publish correct filter mask and ensure that everything works fine
+  rePublishMask();
+
+  // Test BinaryFilter
+  testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testIncorrectFilterType)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(INCORRECT_TYPE, MASK_TOPIC, 0.0, 1.0);
+  ASSERT_FALSE(createBinaryFilter("map", 10.0));
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testDifferentFrame)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  ASSERT_TRUE(createBinaryFilter("odom", 10.0));
+  createTFBroadcaster("map", "odom");
+
+  // Test BinaryFilter
+  testSimpleMask(0.0, 1.0, 10.0, TRANSLATION_X, TRANSLATION_Y);
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testIncorrectFrame)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  ASSERT_TRUE(createBinaryFilter("odom", 10.0));
+  // map->odom TF does not exit
+
+  // Test BinaryFilter with incorrect TF chain
+  testIncorrectTF();
+
+  // Clean-up
+  binary_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testResetState)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0);
+  ASSERT_TRUE(createBinaryFilter("map", 10.0));
+
+  testResetFilter();
+
+  // Clean-up
+  // do not need to resetFilter(): this was already done in testResetFilter()
+  reset();
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}

navigation2/nav2_costmap_2d/test/unit/copy_window_test.cpp

@@ -0,0 +1,52 @@
+// Copyright (c) 2021 Samsung Research Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(CopyWindow, copyValidWindow)
+{
+  nav2_costmap_2d::Costmap2D src(10, 10, 0.1, 0.0, 0.0);
+  nav2_costmap_2d::Costmap2D dst(5, 5, 0.2, 100.0, 100.0);
+  // Adding 2 marked cells to source costmap
+  src.setCost(2, 2, 100);
+  src.setCost(5, 5, 200);
+
+  ASSERT_TRUE(dst.copyWindow(src, 2, 2, 6, 6, 0, 0));
+  // Check that both marked cells were copied to destination costmap
+  ASSERT_EQ(dst.getCost(0, 0), 100);
+  ASSERT_EQ(dst.getCost(3, 3), 200);
+}
+
+TEST(CopyWindow, copyInvalidWindow)
+{
+  nav2_costmap_2d::Costmap2D src(10, 10, 0.1, 0.0, 0.0);
+  nav2_costmap_2d::Costmap2D dst(5, 5, 0.2, 100.0, 100.0);
+
+  // Case1: incorrect source bounds
+  ASSERT_FALSE(dst.copyWindow(src, 9, 9, 11, 11, 0, 0));
+  // Case2: incorrect destination bounds
+  ASSERT_FALSE(dst.copyWindow(src, 0, 0, 1, 1, 5, 5));
+  ASSERT_FALSE(dst.copyWindow(src, 0, 0, 6, 6, 0, 0));
+}

navigation2/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp

@@ -0,0 +1,122 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <vector>
+#include <cmath>
+#include <limits>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
+static constexpr double RESOLUTION = 0.05;
+static constexpr double ORIGIN_X = 0.1;
+static constexpr double ORIGIN_Y = 0.2;
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode() {}
+
+  ~TestNode()
+  {
+    occ_grid_.reset();
+    costmap_.reset();
+  }
+
+protected:
+  void createMaps();
+  void verifyCostmap();
+
+private:
+  std::shared_ptr<nav_msgs::msg::OccupancyGrid> occ_grid_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+};
+
+void TestNode::createMaps()
+{
+  // Create occ_grid_ map
+  occ_grid_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+
+  const unsigned int width = 4;
+  const unsigned int height = 3;
+
+  occ_grid_->info.resolution = RESOLUTION;
+  occ_grid_->info.width = width;
+  occ_grid_->info.height = height;
+  occ_grid_->info.origin.position.x = ORIGIN_X;
+  occ_grid_->info.origin.position.y = ORIGIN_Y;
+  occ_grid_->info.origin.position.z = 0.0;
+  occ_grid_->info.origin.orientation.x = 0.0;
+  occ_grid_->info.origin.orientation.y = 0.0;
+  occ_grid_->info.origin.orientation.z = 0.0;
+  occ_grid_->info.origin.orientation.w = 1.0;
+  occ_grid_->data.resize(width * height);
+
+  int8_t data;
+  for (unsigned int i = 0; i < width * height; i++) {
+    data = i * 10;
+    if (data <= nav2_util::OCC_GRID_OCCUPIED) {
+      occ_grid_->data[i] = data;
+    } else {
+      occ_grid_->data[i] = nav2_util::OCC_GRID_UNKNOWN;
+    }
+  }
+
+  // Create costmap_ (convert OccupancyGrid -> to Costmap2D)
+  costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(*occ_grid_);
+}
+
+void TestNode::verifyCostmap()
+{
+  // Verify Costmap2D info
+  EXPECT_NEAR(costmap_->getResolution(), RESOLUTION, EPSILON);
+  EXPECT_NEAR(costmap_->getOriginX(), ORIGIN_X, EPSILON);
+  EXPECT_NEAR(costmap_->getOriginY(), ORIGIN_Y, EPSILON);
+
+  // Verify Costmap2D data
+  unsigned int it;
+  unsigned char data, data_ref;
+  for (it = 0; it < (costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1); it++) {
+    data = costmap_->getCharMap()[it];
+    if (it != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1) {
+      data_ref = std::round(
+        static_cast<double>(nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) * it /
+        10);
+    } else {
+      data_ref = nav2_costmap_2d::NO_INFORMATION;
+    }
+    EXPECT_EQ(data, data_ref);
+  }
+}
+
+TEST_F(TestNode, convertOccGridToCostmap)
+{
+  createMaps();
+  verifyCostmap();
+}

navigation2/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp

@@ -0,0 +1,150 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+
+static const char FILTER_NAME[]{"costmap_filter"};
+
+class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
+{
+public:
+  // Dummy implementations of virtual methods
+  void initializeFilter(
+    const std::string &) {}
+
+  void process(
+    nav2_costmap_2d::Costmap2D &,
+    int, int, int, int,
+    const geometry_msgs::msg::Pose2D &) {}
+
+  void resetFilter() {}
+
+  // Actual testing methods
+  void setName(const std::string & name)
+  {
+    name_ = name;
+  }
+
+  void setNode(const nav2_util::LifecycleNode::WeakPtr & node)
+  {
+    node_ = node;
+  }
+
+  bool getEnabled()
+  {
+    return enabled_;
+  }
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode()
+  {
+    // Create new LifecycleNode
+    node_ = std::make_shared<nav2_util::LifecycleNode>("test_node");
+
+    // Create new CostmapFilter
+    costmap_filter_ = std::make_shared<CostmapFilterWrapper>();
+    costmap_filter_->setNode(node_);
+    costmap_filter_->setName(FILTER_NAME);
+
+    // Set CostmapFilter ROS-parameters
+    node_->declare_parameter(
+      std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue("filter_info"));
+    node_->set_parameter(
+      rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", "filter_info"));
+  }
+
+  ~TestNode()
+  {
+    costmap_filter_.reset();
+    node_.reset();
+  }
+
+  template<class T>
+  typename T::Response::SharedPtr send_request(
+    nav2_util::LifecycleNode::SharedPtr node,
+    typename rclcpp::Client<T>::SharedPtr client,
+    typename T::Request::SharedPtr request)
+  {
+    auto result = client->async_send_request(request);
+
+    // Wait for the result
+    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+      return result.get();
+    } else {
+      return nullptr;
+    }
+  }
+
+protected:
+  nav2_util::LifecycleNode::SharedPtr node_;
+  std::shared_ptr<CostmapFilterWrapper> costmap_filter_;
+};
+
+TEST_F(TestNode, testEnableService)
+{
+  costmap_filter_->onInitialize();
+
+  RCLCPP_INFO(node_->get_logger(), "Testing enabling service");
+  auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
+  auto client = node_->create_client<std_srvs::srv::SetBool>(
+    std::string(FILTER_NAME) + "/toggle_filter");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for enabling service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // Set costmap filter enabled
+  req->data = true;
+  auto resp = send_request<std_srvs::srv::SetBool>(node_, client, req);
+
+  ASSERT_NE(resp, nullptr);
+  ASSERT_TRUE(resp->success);
+  ASSERT_EQ(resp->message, "Enabled");
+  ASSERT_TRUE(costmap_filter_->getEnabled());
+
+  // Set costmap filter disabled
+  req->data = false;
+  resp = send_request<std_srvs::srv::SetBool>(node_, client, req);
+
+  ASSERT_NE(resp, nullptr);
+  ASSERT_TRUE(resp->success);
+  ASSERT_EQ(resp->message, "Disabled");
+  ASSERT_FALSE(costmap_filter_->getEnabled());
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}

navigation2/nav2_costmap_2d/test/unit/costmap_filter_test.cpp

@@ -0,0 +1,105 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
+
+class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
+{
+public:
+  CostmapFilterWrapper() {}
+
+  bool worldToMask(
+    nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
+    double wx, double wy, unsigned int & mx, unsigned int & my) const
+  {
+    return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my);
+  }
+
+  // API coverage
+  void initializeFilter(const std::string &) {}
+  void process(
+    nav2_costmap_2d::Costmap2D &, int, int, int, int, const geometry_msgs::msg::Pose2D &)
+  {}
+  void resetFilter() {}
+};
+
+TEST(CostmapFilter, testWorldToMask)
+{
+  // Create occupancy grid for test as follows:
+  //
+  //   ^
+  //   |        (6,6)
+  //   |    *-----*
+  //   |    |/////| <- mask
+  //   |    |/////|
+  //   |    *-----*
+  //   |  (3,3)
+  //   *---------------->
+  // (0,0)
+
+  const unsigned int width = 3;
+  const unsigned int height = 3;
+
+  auto mask = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+  mask->header.frame_id = "map";
+  mask->info.resolution = 1.0;
+  mask->info.width = width;
+  mask->info.height = height;
+  mask->info.origin.position.x = 3.0;
+  mask->info.origin.position.y = 3.0;
+
+  mask->data.resize(width * height, nav2_util::OCC_GRID_OCCUPIED);
+
+  CostmapFilterWrapper cf;
+  unsigned int mx, my;
+  // Point inside mask
+  ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my));
+  ASSERT_EQ(mx, 1u);
+  ASSERT_EQ(my, 2u);
+  // Corner cases
+  ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my));
+  ASSERT_EQ(mx, 0u);
+  ASSERT_EQ(my, 0u);
+  ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my));
+  ASSERT_EQ(mx, 2u);
+  ASSERT_EQ(my, 2u);
+  // Point outside mask
+  ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my));
+  ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my));
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}

navigation2/nav2_costmap_2d/test/unit/declare_parameter_test.cpp

@@ -0,0 +1,75 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class LayerWrapper : public nav2_costmap_2d::Layer
+{
+  void reset() {}
+  void updateBounds(double, double, double, double *, double *, double *, double *) {}
+  void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {}
+  bool isClearable() {return false;}
+};
+
+TEST(DeclareParameter, useValidParameter)
+{
+  LayerWrapper layer;
+  nav2_util::LifecycleNode::SharedPtr node =
+    std::make_shared<nav2_util::LifecycleNode>("test_node");
+  tf2_ros::Buffer tf(node->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  layer.initialize(&layers, "test_layer", &tf, node, nullptr);
+
+  layer.declareParameter("test1", rclcpp::ParameterValue("test_val1"));
+  try {
+    std::string val = node->get_parameter("test_layer.test1").as_string();
+    EXPECT_EQ(val, "test_val1");
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    FAIL() << "test_layer.test1 parameter is not set";
+  }
+}
+
+TEST(DeclareParameter, useInvalidParameter)
+{
+  LayerWrapper layer;
+  nav2_util::LifecycleNode::SharedPtr node =
+    std::make_shared<nav2_util::LifecycleNode>("test_node");
+  tf2_ros::Buffer tf(node->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  layer.initialize(&layers, "test_layer", &tf, node, nullptr);
+
+  layer.declareParameter("test2", rclcpp::PARAMETER_STRING);
+  try {
+    std::string val = node->get_parameter("test_layer.test2").as_string();
+    FAIL() << "Incorrectly handling test_layer.test2 parameter which was not set";
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    SUCCEED();
+  }
+}

navigation2/nav2_costmap_2d/test/unit/denoise_layer_test.cpp

@@ -0,0 +1,520 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// 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 <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+#include <tuple>
+#include <stdexcept>
+#include <algorithm>
+
+#include "nav2_costmap_2d/denoise_layer.hpp"
+#include "image_tests_helper.hpp"
+
+namespace nav2_costmap_2d
+{
+/**
+ * @brief nav2_costmap_2d::DenoiseLayer class wrapper
+ *
+ * Provides access to DenoiseLayer private methods for testing them in isolation
+ */
+class DenoiseLayerTester : public ::testing::Test
+{
+public:
+  void removeSinglePixels(
+    Image<uint8_t> & image, ConnectivityType connectivity,
+    bool no_information_is_obstacle = true)
+  {
+    denoise_.group_connectivity_type_ = connectivity;
+    denoise_.no_information_is_obstacle_ = no_information_is_obstacle;
+    denoise_.removeSinglePixels(image);
+  }
+
+  void removeGroups(
+    Image<uint8_t> & image, ConnectivityType connectivity,
+    size_t minimal_group_size, bool no_information_is_obstacle = true)
+  {
+    denoise_.group_connectivity_type_ = connectivity;
+    denoise_.minimal_group_size_ = minimal_group_size;
+    denoise_.no_information_is_obstacle_ = no_information_is_obstacle;
+    denoise_.removeGroups(image);
+  }
+
+  void denoise(
+    Image<uint8_t> & image, ConnectivityType connectivity,
+    size_t minimal_group_size, bool no_information_is_obstacle = true)
+  {
+    denoise_.group_connectivity_type_ = connectivity;
+    denoise_.minimal_group_size_ = minimal_group_size;
+    denoise_.no_information_is_obstacle_ = no_information_is_obstacle;
+    denoise_.denoise(image);
+  }
+
+  bool reset()
+  {
+    denoise_.current_ = true;
+    denoise_.reset();
+    return denoise_.current_;
+  }
+
+  static void initialize(nav2_costmap_2d::DenoiseLayer & d)
+  {
+    d.onInitialize();
+  }
+
+  static bool & touchCurrent(nav2_costmap_2d::DenoiseLayer & d)
+  {
+    return d.current_;
+  }
+
+  static void configure(
+    nav2_costmap_2d::DenoiseLayer & d, ConnectivityType connectivity, size_t minimal_group_size)
+  {
+    d.enabled_ = true;
+    d.group_connectivity_type_ = connectivity;
+    d.minimal_group_size_ = minimal_group_size;
+  }
+
+  static std::tuple<bool, ConnectivityType, size_t> getParameters(
+    const nav2_costmap_2d::DenoiseLayer & d)
+  {
+    return std::make_tuple(d.enabled_, d.group_connectivity_type_, d.minimal_group_size_);
+  }
+
+protected:
+  std::vector<uint8_t> image_buffer_bytes;
+  std::vector<uint8_t> image_buffer_bytes2;
+  std::vector<uint8_t> image_buffer_bytes3;
+
+private:
+  nav2_costmap_2d::DenoiseLayer denoise_;
+};
+
+}
+
+using namespace nav2_costmap_2d;
+
+TEST_F(DenoiseLayerTester, removeSinglePixels4way) {
+  const auto in = imageFromString<uint8_t>(
+    "x.x."
+    "x..x"
+    ".x.."
+    "xx.x", image_buffer_bytes);
+  const auto exp = imageFromString<uint8_t>(
+    "x..."
+    "x..."
+    ".x.."
+    "xx..", image_buffer_bytes2);
+  auto out = clone(in, image_buffer_bytes3);
+  removeSinglePixels(out, ConnectivityType::Way4);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removeSinglePixels4wayNoInformationIsEmpty) {
+  const std::map<char, uint8_t> legend = {{'.', 0}, {'n', NO_INFORMATION}, {'x', LETHAL_OBSTACLE}};
+  const auto in = imageFromString<uint8_t>(
+    "x.x."
+    "xnnx"
+    "nxnn"
+    "xx.x", image_buffer_bytes, legend);
+  const auto exp = imageFromString<uint8_t>(
+    "x..."
+    "xnn."
+    "nxnn"
+    "xx..", image_buffer_bytes2, legend);
+  auto out = clone(in, image_buffer_bytes3);
+  removeSinglePixels(out, ConnectivityType::Way4, false);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removeSinglePixels8way) {
+  const auto in = imageFromString<uint8_t>(
+    "x.x."
+    "x..x"
+    ".x.."
+    "xx.x", image_buffer_bytes);
+  const auto exp = imageFromString<uint8_t>(
+    "x.x."
+    "x..x"
+    ".x.."
+    "xx..", image_buffer_bytes2);
+
+  auto out = clone(in, image_buffer_bytes3);
+  removeSinglePixels(out, ConnectivityType::Way8);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removeSinglePixelsFromExtremelySmallImage) {
+  {
+    const auto in = imageFromString<uint8_t>(
+      "x", image_buffer_bytes);
+    const auto exp = imageFromString<uint8_t>(
+      ".", image_buffer_bytes2);
+
+    auto out = clone(in, image_buffer_bytes3);
+    removeSinglePixels(out, ConnectivityType::Way8);
+
+    ASSERT_TRUE(isEqual(out, exp));
+  }
+
+  {
+    const auto in = imageFromString<uint8_t>(
+      "x."
+      ".x", image_buffer_bytes);
+    const auto exp = imageFromString<uint8_t>(
+      "x."
+      ".x", image_buffer_bytes2);
+
+    auto out = clone(in, image_buffer_bytes3);
+    removeSinglePixels(out, ConnectivityType::Way8);
+
+    ASSERT_TRUE(isEqual(out, exp));
+  }
+
+  {
+    const auto in = imageFromString<uint8_t>(
+      "x."
+      ".x", image_buffer_bytes);
+    const auto exp = imageFromString<uint8_t>(
+      ".."
+      "..", image_buffer_bytes2);
+
+    auto out = clone(in, image_buffer_bytes3);
+    removeSinglePixels(out, ConnectivityType::Way4);
+
+    ASSERT_TRUE(isEqual(out, exp));
+  }
+}
+
+TEST_F(DenoiseLayerTester, removeSinglePixelsFromNonBinary) {
+  // buffer for 9 pixels with neutral (between FREE_SPACE and INSCRIBED_INFLATED_OBSTACLE) value
+  image_buffer_bytes.assign(9, 250);
+  Image<uint8_t> in = makeImage<uint8_t>(3, 3, image_buffer_bytes);
+  in.row(1)[1] = NO_INFORMATION;
+  Image<uint8_t> exp = clone(in, image_buffer_bytes2);
+  exp.row(1)[1] = FREE_SPACE;
+
+  auto out = clone(in, image_buffer_bytes3);
+  removeSinglePixels(out, ConnectivityType::Way4);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removePixelsGroup4way) {
+  const auto in = imageFromString<uint8_t>(
+    ".xx..xx"
+    "..x.x.."
+    "x..x..x"
+    "x......"
+    "...x.xx"
+    "xxx..xx"
+    "....xx.", image_buffer_bytes);
+  const auto exp = imageFromString<uint8_t>(
+    ".xx...."
+    "..x...."
+    "......."
+    "......."
+    ".....xx"
+    "xxx..xx"
+    "....xx.", image_buffer_bytes2);
+
+  auto out = clone(in, image_buffer_bytes3);
+  removeGroups(out, ConnectivityType::Way4, 3);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removePixelsGroup4wayNoInformationIsEmpty) {
+  const std::map<char, uint8_t> legend = {{'.', 0}, {'n', NO_INFORMATION}, {'x', LETHAL_OBSTACLE}};
+  const auto in = imageFromString<uint8_t>(
+    ".xxnnxx"
+    "..xnx.."
+    "x..x..x"
+    "x......"
+    "nnnxnxx"
+    "xxx..xx"
+    "....xx.", image_buffer_bytes, legend);
+  const auto exp = imageFromString<uint8_t>(
+    ".xxnn.."
+    "..xn..."
+    "......."
+    "......."
+    "nnn.nxx"
+    "xxx..xx"
+    "....xx.", image_buffer_bytes2, legend);
+
+  auto out = clone(in, image_buffer_bytes3);
+  removeGroups(out, ConnectivityType::Way4, 3, false);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removePixelsGroup8way) {
+  const auto in = imageFromString<uint8_t>(
+    ".xx..xx"
+    "..x.x.."
+    "x..x..x"
+    "x......"
+    "...x.xx"
+    "xxx..xx"
+    "....xx.", image_buffer_bytes);
+  const auto exp = imageFromString<uint8_t>(
+    ".xx..xx"
+    "..x.x.."
+    "...x..."
+    "......."
+    "...x.xx"
+    "xxx..xx"
+    "....xx.", image_buffer_bytes2);
+
+  auto out = clone(in, image_buffer_bytes3);
+  removeGroups(out, ConnectivityType::Way8, 3);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, removePixelsGroupFromExtremelySmallImage) {
+  {
+    const auto in = imageFromString<uint8_t>(
+      "x", image_buffer_bytes);
+    const auto exp = imageFromString<uint8_t>(
+      ".", image_buffer_bytes2);
+
+    auto out = clone(in, image_buffer_bytes3);
+    removeGroups(out, ConnectivityType::Way8, 3);
+
+    ASSERT_TRUE(isEqual(out, exp));
+  }
+
+  {
+    const auto in = imageFromString<uint8_t>(
+      "x."
+      ".x", image_buffer_bytes);
+    const auto exp = imageFromString<uint8_t>(
+      ".."
+      "..", image_buffer_bytes2);
+
+    auto out = clone(in, image_buffer_bytes3);
+    removeGroups(out, ConnectivityType::Way8, 3);
+
+    ASSERT_TRUE(isEqual(out, exp));
+  }
+}
+
+TEST_F(DenoiseLayerTester, removePixelsGroupFromNonBinary) {
+  // buffer for 9 pixels with neutral (between FREE_SPACE and INSCRIBED_INFLATED_OBSTACLE) value
+  image_buffer_bytes.assign(9, 250);
+  Image<uint8_t> in = makeImage<uint8_t>(3, 3, image_buffer_bytes);
+  in.row(1)[1] = 255;
+
+  Image<uint8_t> exp = clone(in, image_buffer_bytes2);
+  exp.row(1)[1] = 0;
+
+  auto out = clone(in, image_buffer_bytes3);
+  removeGroups(out, ConnectivityType::Way4, 2);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, denoiseSingles) {
+  const auto in = imageFromString<uint8_t>(
+    "xx."
+    "..."
+    "..x", image_buffer_bytes);
+  const auto exp = imageFromString<uint8_t>(
+    "xx."
+    "..."
+    "...", image_buffer_bytes2);
+
+  auto out = clone(in, image_buffer_bytes3);
+  denoise(out, ConnectivityType::Way4, 2);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, denoiseGroups) {
+  const auto in = imageFromString<uint8_t>(
+    "xx."
+    "x.x"
+    "..x", image_buffer_bytes);
+  const auto exp = imageFromString<uint8_t>(
+    "xx."
+    "x.."
+    "...", image_buffer_bytes2);
+
+  auto out = clone(in, image_buffer_bytes3);
+  denoise(out, ConnectivityType::Way4, 3);
+
+  ASSERT_TRUE(isEqual(out, exp)) <<
+    "input:" << std::endl << in << std::endl <<
+    "output:" << std::endl << out << std::endl <<
+    "expected:" << std::endl << exp;
+}
+
+TEST_F(DenoiseLayerTester, denoiseEmpty) {
+  Image<uint8_t> in;
+
+  ASSERT_NO_THROW(denoise(in, ConnectivityType::Way4, 2));
+}
+
+TEST_F(DenoiseLayerTester, denoiseNothing) {
+  Image<uint8_t> in = makeImage<uint8_t>(1, 1, image_buffer_bytes);
+
+  ASSERT_NO_THROW(denoise(in, ConnectivityType::Way4, 1));
+}
+
+TEST_F(DenoiseLayerTester, constructorAndDestructor) {
+  ASSERT_NO_THROW(
+    // []()
+    {
+      nav2_costmap_2d::DenoiseLayer layer;
+    });
+}
+
+TEST_F(DenoiseLayerTester, reset) {
+  ASSERT_FALSE(reset());
+}
+
+TEST_F(DenoiseLayerTester, isClearable) {
+  nav2_costmap_2d::DenoiseLayer layer;
+
+  ASSERT_FALSE(layer.isClearable());
+}
+
+TEST_F(DenoiseLayerTester, updateBounds) {
+  nav2_costmap_2d::DenoiseLayer layer;
+
+  const std::array<double, 4> region = {1., 2., 3., 4.};
+  auto r = region;
+
+  ASSERT_NO_THROW(layer.updateBounds(0., 0., 0., &r[0], &r[1], &r[2], &r[3]));
+  ASSERT_EQ(r, region);
+}
+
+TEST_F(DenoiseLayerTester, updateCostsIfDisabled) {
+  nav2_costmap_2d::DenoiseLayer layer;
+  nav2_costmap_2d::Costmap2D costmap(1, 1, 1., 0., 0., 255);
+
+  layer.updateCosts(costmap, 0, 0, 1, 1);
+
+  ASSERT_EQ(costmap.getCost(0), 255);
+}
+
+TEST_F(DenoiseLayerTester, updateCosts) {
+  nav2_costmap_2d::DenoiseLayer layer;
+  nav2_costmap_2d::Costmap2D costmap(1, 1, 1., 0., 0.);
+  costmap.setCost(0, 0, NO_INFORMATION);
+  DenoiseLayerTester::configure(layer, ConnectivityType::Way4, 2);
+
+  layer.updateCosts(costmap, 0, 0, 1, 1);
+
+  ASSERT_EQ(costmap.getCost(0), FREE_SPACE);
+}
+
+// Copy paste from declare_parameter_test.cpp
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture rcl_cpp_fixture;
+
+std::shared_ptr<nav2_costmap_2d::DenoiseLayer> constructLayer(
+  std::shared_ptr<nav2_util::LifecycleNode> node =
+  std::make_shared<nav2_util::LifecycleNode>("test_node"))
+{
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto layers = std::make_shared<nav2_costmap_2d::LayeredCostmap>("frame", false, false);
+
+  auto deleter = [node, tf, layers](nav2_costmap_2d::DenoiseLayer * ptr)
+    {
+      delete ptr;
+    };
+  auto layer = std::shared_ptr<nav2_costmap_2d::DenoiseLayer>(
+    new nav2_costmap_2d::DenoiseLayer, deleter);
+  layer->initialize(layers.get(), "test_layer", tf.get(), node, nullptr);
+  return layer;
+}
+
+TEST_F(DenoiseLayerTester, initializeDefault) {
+  auto layer = constructLayer();
+
+  DenoiseLayerTester::initialize(*layer);
+
+  ASSERT_EQ(
+    DenoiseLayerTester::getParameters(*layer),
+    std::make_tuple(true, ConnectivityType::Way8, 2));
+}
+
+TEST_F(DenoiseLayerTester, initializeCustom) {
+  auto node = std::make_shared<nav2_util::LifecycleNode>("test_node");
+  auto layer = constructLayer(node);
+  node->set_parameter(
+    rclcpp::Parameter(layer->getFullName("minimal_group_size"), rclcpp::ParameterValue(5)));
+  node->set_parameter(
+    rclcpp::Parameter(layer->getFullName("group_connectivity_type"), rclcpp::ParameterValue(4)));
+
+  DenoiseLayerTester::initialize(*layer);
+
+  ASSERT_EQ(
+    DenoiseLayerTester::getParameters(*layer),
+    std::make_tuple(true, ConnectivityType::Way4, 5));
+}
+
+TEST_F(DenoiseLayerTester, initializeInvalid) {
+  auto node = std::make_shared<nav2_util::LifecycleNode>("test_node");
+  auto layer = constructLayer(node);
+  node->set_parameter(
+    rclcpp::Parameter(layer->getFullName("minimal_group_size"), rclcpp::ParameterValue(-1)));
+  node->set_parameter(
+    rclcpp::Parameter(layer->getFullName("group_connectivity_type"), rclcpp::ParameterValue(3)));
+
+  DenoiseLayerTester::initialize(*layer);
+
+  ASSERT_EQ(
+    DenoiseLayerTester::getParameters(*layer),
+    std::make_tuple(true, ConnectivityType::Way8, 1));
+}

navigation2/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp

@@ -0,0 +1,252 @@
+// Copyright (c) 2020 Shivang Patel
+//
+// 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 <string>
+#include <vector>
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_costmap_2d/footprint.hpp"
+
+TEST(collision_footprint, test_basic)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -0.5;
+  p1.y = 0.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 0.0;
+  p2.y = 0.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 0.5;
+  p3.y = 0.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = 0.0;
+  p4.y = -0.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+}
+
+TEST(collision_footprint, test_point_cost)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  auto value = collision_checker.pointCost(50, 50);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+}
+
+TEST(collision_footprint, test_world_to_map)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  unsigned int x, y;
+
+  collision_checker.worldToMap(1.0, 1.0, x, y);
+
+  auto value = collision_checker.pointCost(x, y);
+
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  costmap_->setCost(50, 50, 200);
+  collision_checker.worldToMap(5.0, 5.0, x, y);
+
+  EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001);
+}
+
+TEST(collision_footprint, test_footprint_at_pose_with_movement)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 254);
+
+  for (unsigned int i = 40; i <= 60; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmap_->setCost(i, j, 0);
+    }
+  }
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint);
+  EXPECT_NEAR(up_value, 254.0, 0.001);
+
+  auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint);
+  EXPECT_NEAR(down_value, 254.0, 0.001);
+}
+
+TEST(collision_footprint, test_point_and_line_cost)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.10000, 0, 0.0, 0.0);
+
+  costmap_->setCost(62, 50, 254);
+  costmap_->setCost(39, 60, 254);
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
+  collision_checker(costmap_);
+
+  auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
+  EXPECT_NEAR(value, 0.0, 0.001);
+
+  auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint);
+  EXPECT_NEAR(left_value, 254.0, 0.001);
+
+  auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint);
+  EXPECT_NEAR(right_value, 254.0, 0.001);
+}
+
+TEST(collision_footprint, not_enough_points)
+{
+  geometry_msgs::msg::Point p1;
+  p1.x = 2.0;
+  p1.y = 2.0;
+
+  geometry_msgs::msg::Point p2;
+  p2.x = -2.0;
+  p2.y = -2.0;
+
+  std::vector<geometry_msgs::msg::Point> footprint = {p1, p2};
+  double min_dist = 0.0;
+  double max_dist = 0.0;
+
+  nav2_costmap_2d::calculateMinAndMaxDistances(footprint, min_dist, max_dist);
+  EXPECT_EQ(min_dist, std::numeric_limits<double>::max());
+  EXPECT_EQ(max_dist, 0.0f);
+}
+
+TEST(collision_footprint, to_point_32) {
+  geometry_msgs::msg::Point p;
+  p.x = 123.0;
+  p.y = 456.0;
+  p.z = 789.0;
+
+  geometry_msgs::msg::Point32 p32;
+  p32 = nav2_costmap_2d::toPoint32(p);
+  EXPECT_NEAR(p.x, p32.x, 1e-5);
+  EXPECT_NEAR(p.y, p32.y, 1e-5);
+  EXPECT_NEAR(p.z, p32.z, 1e-5);
+}
+
+TEST(collision_footprint, to_polygon) {
+  geometry_msgs::msg::Point p1;
+  p1.x = 1.2;
+  p1.y = 3.4;
+  p1.z = 5.1;
+
+  geometry_msgs::msg::Point p2;
+  p2.x = -5.6;
+  p2.y = -7.8;
+  p2.z = -9.1;
+  std::vector<geometry_msgs::msg::Point> pts = {p1, p2};
+
+  geometry_msgs::msg::Polygon poly;
+  poly = nav2_costmap_2d::toPolygon(pts);
+
+  EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0]));
+  EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5);
+  EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5);
+  EXPECT_NEAR(poly.points[0].z, p1.z, 1e-5);
+  EXPECT_NEAR(poly.points[1].x, p2.x, 1e-5);
+  EXPECT_NEAR(poly.points[1].y, p2.y, 1e-5);
+  EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5);
+}
+
+TEST(collision_footprint, make_footprint_from_string) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint);
+  EXPECT_EQ(result, true);
+  EXPECT_EQ(4u, footprint.size());
+  EXPECT_NEAR(footprint[0].x, 1.0, 1e-5);
+  EXPECT_NEAR(footprint[0].y, 2.2, 1e-5);
+  EXPECT_NEAR(footprint[1].x, 0.3, 1e-5);
+  EXPECT_NEAR(footprint[1].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[2].x, -0.3, 1e-5);
+  EXPECT_NEAR(footprint[2].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[3].x, -1.0, 1e-5);
+  EXPECT_NEAR(footprint[3].y, 2.2, 1e-5);
+}
+
+TEST(collision_footprint, make_footprint_from_string_parse_error) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[bad_string", footprint);
+  EXPECT_EQ(result, false);
+}
+
+TEST(collision_footprint, make_footprint_from_string_two_points_error) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4]", footprint);
+  EXPECT_EQ(result, false);
+}
+
+TEST(collision_footprint, make_footprint_from_string_not_pairs) {
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = nav2_costmap_2d::makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint);
+  EXPECT_EQ(result, false);
+}

navigation2/nav2_costmap_2d/test/unit/image_processing_test.cpp

@@ -0,0 +1,527 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// 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 <gtest/gtest.h>
+#include <cmath>
+
+#include "nav2_costmap_2d/denoise/image_processing.hpp"
+#include "image_tests_helper.hpp"
+
+using namespace nav2_costmap_2d;
+using namespace imgproc_impl;
+
+struct ImageProcTester : public ::testing::Test
+{
+protected:
+  std::vector<uint8_t> image_buffer_bytes_;
+  std::vector<uint8_t> image_buffer_bytes2_;
+  std::vector<uint8_t> image_buffer_bytes3_;
+  std::vector<uint16_t> image_buffer_words_;
+};
+
+TEST(OutOfBounds, outOfBoundsAccess) {
+  // check access to nullptr row (up)
+  {
+    out_of_bounds_policy::ReplaceToZero<uint8_t> c(nullptr, nullptr, 2);
+    uint8_t * any_non_null = reinterpret_cast<uint8_t *>(&c);
+    ASSERT_EQ(c.up(any_non_null), uint8_t(0));
+  }
+  // check out of bounds access
+  {
+    std::array<uint8_t, 3> data = {1, 2, 3};
+    out_of_bounds_policy::ReplaceToZero<uint8_t> c(data.data(), data.data(), 2);
+    auto left_out_of_bounds = std::prev(data.data());
+    auto right_out_of_bounds = data.data() + data.size();
+
+    ASSERT_EQ(c.up(left_out_of_bounds), uint8_t(0));
+    ASSERT_EQ(c.up(right_out_of_bounds), uint8_t(0));
+
+    ASSERT_EQ(c.down(left_out_of_bounds), uint8_t(0));
+    ASSERT_EQ(c.down(right_out_of_bounds), uint8_t(0));
+  }
+}
+
+TEST_F(ImageProcTester, calculateHistogramWithoutTruncation) {
+  image_buffer_words_ = {0, 2, 1, 0, 3, 4, 1, 2, 0};
+  Image<uint16_t> image = makeImage(3, 3, image_buffer_words_);
+  const uint16_t max_bin_size = 3;  // three zeros
+  const uint16_t max_value = 4;
+  const auto hist = histogram(image, max_value, max_bin_size);
+
+  const std::array<uint8_t, 5> expected = {3, 2, 2, 1, 1};
+  ASSERT_EQ(hist.size(), expected.size());
+  ASSERT_TRUE(std::equal(expected.begin(), expected.end(), hist.begin()));
+}
+
+TEST_F(ImageProcTester, calculateHistogramWithTruncation) {
+  image_buffer_words_ = {0, 2, 1, 0, 3, 4, 1, 2, 0};
+  Image<uint16_t> image = makeImage(3, 3, image_buffer_words_);
+  const uint16_t max_bin_size = 2;
+  const uint16_t max_value = 4;
+  const auto hist = histogram(image, max_value, max_bin_size);
+
+  const std::array<uint8_t, 5> expected = {2, 2, 2, 1, 1};
+  ASSERT_EQ(hist.size(), expected.size());
+  ASSERT_TRUE(std::equal(expected.begin(), expected.end(), hist.begin()));
+}
+
+TEST_F(ImageProcTester, calculateHistogramOfEmpty) {
+  const uint16_t max_bin_size = 1;
+  const uint16_t max_value = 0;
+  Image<uint16_t> empty;
+
+  const auto hist = histogram(empty, max_value, max_bin_size);
+  ASSERT_TRUE(hist.empty());
+}
+
+
+TEST(EquivalenceLabelTrees, newLabelsTest) {
+  EquivalenceLabelTrees<uint8_t> eq;
+  eq.reset(10, 10, ConnectivityType::Way4);
+  ASSERT_EQ(eq.makeLabel(), 1);
+  ASSERT_EQ(eq.makeLabel(), 2);
+  ASSERT_EQ(eq.makeLabel(), 3);
+}
+
+TEST(EquivalenceLabelTrees, unionTest) {
+  EquivalenceLabelTrees<uint8_t> eq;
+  eq.reset(10, 10, ConnectivityType::Way4);
+
+  // create 5 single nodes
+  for (size_t i = 1; i < 6; ++i) {
+    eq.makeLabel();
+  }
+  ASSERT_EQ(eq.unionTrees(4, 3), 3);
+  ASSERT_EQ(eq.unionTrees(5, 3), 3);
+  ASSERT_EQ(eq.unionTrees(4, 1), 1);
+  ASSERT_EQ(eq.unionTrees(2, 5), 1);
+}
+
+struct ConnectedComponentsTester : public ImageProcTester
+{
+protected:
+  template<ConnectivityType connectivity>
+  bool fingerTest();
+
+  template<ConnectivityType connectivity>
+  bool spiralTest();
+
+  inline static bool isBackground(uint8_t pixel)
+  {
+    return pixel == BACKGROUND_CODE;
+  }
+
+  inline Image<uint8_t> makeChessboardLikeImage(
+    size_t rows, size_t cols,
+    std::vector<uint8_t> & buffer) const;
+
+protected:
+  MemoryBuffer buffer_;
+  imgproc_impl::EquivalenceLabelTrees<uint8_t> label_trees_;
+  static const uint8_t BACKGROUND_CODE = 0;
+  static const uint8_t FOREGROUND_CODE = 255;
+};
+
+Image<uint8_t> ConnectedComponentsTester::makeChessboardLikeImage(
+  size_t rows, size_t cols,
+  std::vector<uint8_t> & buffer) const
+{
+  Image<uint8_t> image = makeImage<uint8_t>(rows, cols, buffer, cols * 3);
+
+  auto inverse = [](uint8_t v) {
+      return (v == BACKGROUND_CODE) ? FOREGROUND_CODE : BACKGROUND_CODE;
+    };
+
+  uint8_t current_value = FOREGROUND_CODE;
+  for (size_t j = 0; j < cols; ++j) {
+    *(image.row(0) + j) = current_value;
+    current_value = inverse(current_value);
+  }
+
+  for (size_t i = 1; i < rows; ++i) {
+    auto up = image.row(i - 1);
+    auto current = image.row(i);
+    for (size_t j = 0; j < cols; ++j, ++up, ++current) {
+      *current = inverse(*up);
+    }
+  }
+  return image;
+}
+
+TEST_F(ConnectedComponentsTester, way4EmptyTest) {
+  Image<uint8_t> empty;
+  uint8_t total_labels;
+  connectedComponents<ConnectivityType::Way4>(
+    empty, buffer_, label_trees_,
+    isBackground, total_labels);
+  ASSERT_EQ(total_labels, uint8_t(0));
+}
+
+TEST_F(ConnectedComponentsTester, way4SinglePixelTest) {
+  Image<uint8_t> input = makeImage(1, 1, image_buffer_bytes_);
+  uint8_t total_labels;
+  {
+    input.row(0)[0] = BACKGROUND_CODE;
+
+    const auto result = connectedComponents<ConnectivityType::Way4>(
+      input, buffer_, label_trees_,
+      isBackground, total_labels);
+
+    ASSERT_EQ(result.row(0)[0], 0);
+    ASSERT_EQ(total_labels, 1);
+  }
+  {
+    input.row(0)[0] = FOREGROUND_CODE;
+
+    const auto result = connectedComponents<ConnectivityType::Way4>(
+      input, buffer_, label_trees_,
+      isBackground, total_labels);
+
+    ASSERT_EQ(result.row(0)[0], 1);
+    ASSERT_EQ(total_labels, 2);
+  }
+}
+
+TEST_F(ConnectedComponentsTester, way4ImageSmallTest) {
+  {
+    Image<uint8_t> input = makeImage(1, 2, image_buffer_bytes_);
+    uint8_t total_labels;
+    input.row(0)[0] = BACKGROUND_CODE;
+    input.row(0)[1] = FOREGROUND_CODE;
+
+    const auto result = connectedComponents<ConnectivityType::Way4>(
+      input, buffer_, label_trees_,
+      isBackground, total_labels);
+
+    ASSERT_EQ(total_labels, uint8_t(2));
+    ASSERT_EQ(result.row(0)[0], 0);
+    ASSERT_EQ(result.row(0)[1], 1);
+  }
+  {
+    Image<uint8_t> input = makeImage(2, 1, image_buffer_bytes_);
+    uint8_t total_labels;
+    input.row(0)[0] = BACKGROUND_CODE;
+    input.row(1)[0] = FOREGROUND_CODE;
+
+    const auto result = connectedComponents<ConnectivityType::Way4>(
+      input, buffer_, label_trees_,
+      isBackground, total_labels);
+
+    ASSERT_EQ(total_labels, uint8_t(2));
+    ASSERT_EQ(result.row(0)[0], 0);
+    ASSERT_EQ(result.row(1)[0], 1);
+  }
+}
+
+TEST_F(ConnectedComponentsTester, way4LabelsOverflowTest) {
+  // big chessboard image
+  Image<uint8_t> input = makeChessboardLikeImage(32, 17, image_buffer_bytes_);
+  uint8_t total_labels;
+
+  ASSERT_THROW(
+    (connectedComponents<ConnectivityType::Way4>(input, buffer_, label_trees_,
+        isBackground, total_labels)),
+    LabelOverflow);
+}
+
+template<class T>
+struct UniformLabel
+{
+  const Image<T> & labels;
+  std::map<T, T> labels_map = {{0, 0}};
+  size_t next_label = 1;
+  T at(size_t i, size_t j)
+  {
+    const T label = labels.row(i)[j];
+
+    if (labels_map.find(label) == labels_map.end()) {
+      labels_map[label] = next_label++;
+    }
+    return labels_map[label];
+  }
+};
+
+template<class T>
+bool isEqualLabels(const Image<T> & lhs, const Image<T> & rhs)
+{
+  UniformLabel<T> l = {lhs};
+  UniformLabel<T> r = {rhs};
+
+  for (size_t i = 0; i < lhs.rows(); ++i) {
+    for (size_t j = 0; j < lhs.rows(); ++j) {
+      if (l.at(i, j) != r.at(i, j)) {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+TEST_F(ConnectedComponentsTester, way4ImageStepsTest) {
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "..xx"
+    ".xx."
+    "xx.."
+    "....", image_buffer_bytes_);
+  const Image<uint8_t> expected_labels = imageFromString<uint8_t>(
+    "..xx"
+    ".xx."
+    "xx.."
+    "....", image_buffer_bytes2_);
+  uint8_t total_labels;
+  const auto result = connectedComponents<ConnectivityType::Way4>(
+    input, buffer_, label_trees_,
+    isBackground, total_labels);
+
+  ASSERT_EQ(total_labels, uint8_t(2));
+  ASSERT_TRUE(isEqualLabels(result, expected_labels));
+}
+
+/// @brief create mapping '.'->0, 'a'->1, 'b'->2, ... max_symbol->n
+std::map<char, uint8_t> makeLabelsMap(char max_symbol)
+{
+  std::map<char, uint8_t> labels_map = {{'.', 0}};
+
+  for (char s = 'a'; s <= max_symbol; ++s) {
+    labels_map.emplace(s, uint8_t(s - 'a' + 1));
+  }
+  return labels_map;
+}
+
+TEST_F(ConnectedComponentsTester, way8ImageStepsTest) {
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "....xx"
+    "..xx.."
+    "xx...."
+    "...xx."
+    ".....x"
+    "....x.", image_buffer_bytes_);
+  const Image<uint8_t> expected_labels = imageFromString<uint8_t>(
+    "....aa"
+    "..aa.."
+    "aa...."
+    "...bb."
+    ".....b"
+    "....b.", image_buffer_bytes2_, makeLabelsMap('b'));
+  uint8_t total_labels;
+
+  const auto result = connectedComponents<ConnectivityType::Way8>(
+    input, buffer_, label_trees_,
+    isBackground, total_labels);
+
+  ASSERT_EQ(total_labels, uint8_t(3));
+  ASSERT_TRUE(isEqualLabels(result, expected_labels));
+}
+
+TEST_F(ConnectedComponentsTester, way4ImageSieveTest) {
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "x.x.x"
+    ".x.x."
+    "x.x.x"
+    ".x.x."
+    "x.x.x", image_buffer_bytes_);
+  const Image<uint8_t> expected_labels = imageFromString<uint8_t>(
+    "a.b.c"
+    ".d.e."
+    "f.g.h"
+    ".i.j."
+    "k.l.m", image_buffer_bytes2_, makeLabelsMap('m'));
+  uint8_t total_labels;
+
+  const auto result = connectedComponents<ConnectivityType::Way4>(
+    input, buffer_, label_trees_,
+    isBackground, total_labels);
+
+  ASSERT_EQ(total_labels, uint8_t(14));
+  ASSERT_TRUE(isEqualLabels(result, expected_labels));
+}
+
+template<ConnectivityType connectivity>
+bool ConnectedComponentsTester::fingerTest()
+{
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "....."
+    "....x"
+    "..x.x"
+    "x.x.x"
+    "x.x.x", image_buffer_bytes_);
+  const Image<uint8_t> expected_labels = imageFromString<uint8_t>(
+    "....."
+    "....c"
+    "..b.c"
+    "a.b.c"
+    "a.b.c", image_buffer_bytes2_, makeLabelsMap('c'));
+  uint8_t total_labels;
+
+  const auto result = connectedComponents<connectivity>(input, buffer_,
+      label_trees_, isBackground, total_labels);
+
+  return total_labels == 4 && isEqualLabels(result, expected_labels);
+}
+
+TEST_F(ConnectedComponentsTester, way4ImageFingerTest) {
+  ASSERT_TRUE(fingerTest<ConnectivityType::Way4>());
+}
+
+TEST_F(ConnectedComponentsTester, way8ImageFingerTest) {
+  ASSERT_TRUE(fingerTest<ConnectivityType::Way8>());
+}
+
+template<ConnectivityType connectivity>
+bool ConnectedComponentsTester::spiralTest()
+{
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    ".xxxxxx"
+    "......x"
+    ".xxxx.x"
+    ".x..x.x"
+    ".x.xx.x"
+    ".x....x"
+    ".xxxxxx", image_buffer_bytes_);
+  const Image<uint8_t> expected_labels = imageFromString<uint8_t>(
+    ".xxxxxx"
+    "......x"
+    ".xxxx.x"
+    ".x..x.x"
+    ".x.xx.x"
+    ".x....x"
+    ".xxxxxx", image_buffer_bytes2_);
+  uint8_t total_labels;
+
+  const auto result = connectedComponents<connectivity>(input, buffer_,
+      label_trees_, isBackground, total_labels);
+  return total_labels == 2 && isEqualLabels(result, expected_labels);
+}
+
+TEST_F(ConnectedComponentsTester, way4ImageSpiralTest) {
+  ASSERT_TRUE(spiralTest<ConnectivityType::Way4>());
+}
+
+TEST_F(ConnectedComponentsTester, way8ImageSpiralTest) {
+  ASSERT_TRUE(spiralTest<ConnectivityType::Way8>());
+}
+
+TEST_F(ConnectedComponentsTester, groupsRemoverUint16LabelOverflow) {
+  Image<uint8_t> image = makeChessboardLikeImage(512, 512, image_buffer_bytes_);
+  GroupsRemover remover;
+  MemoryBuffer buffer;
+  remover.removeGroups(image, buffer, ConnectivityType::Way4, 2, isBackground);
+  const auto bg = BACKGROUND_CODE;
+  image.forEach([bg](uint8_t v) {ASSERT_EQ(v, bg);});
+}
+
+ShapeBuffer3x3 shape_buffer{};
+const Image<uint8_t> cross_shape = createShape(shape_buffer, ConnectivityType::Way4);
+
+uint8_t max_list(std::initializer_list<uint8_t> lst)
+{
+  return std::max(lst);
+}
+
+TEST_F(ImageProcTester, emptyImage) {
+  Image<uint8_t> input;
+  Image<uint8_t> output;
+
+  ASSERT_NO_THROW(morphologyOperation(input, output, cross_shape, max_list));
+}
+
+TEST_F(ImageProcTester, wrongShapeSize) {
+  Image<uint8_t> input = makeImage(1, 1, image_buffer_bytes_);
+  Image<uint8_t> output = makeImage(1, 1, image_buffer_bytes2_);
+  ASSERT_THROW(
+    morphologyOperation(input, output, makeImage(2, 2, image_buffer_bytes3_), max_list),
+    std::logic_error);
+}
+
+TEST_F(ImageProcTester, wrongSize) {
+  Image<uint8_t> input = makeImage(3, 2, image_buffer_bytes_);
+
+  {
+    Image<uint8_t> output = makeImage(2, 2, image_buffer_bytes2_);
+    ASSERT_THROW(
+      morphologyOperation(input, output, cross_shape, max_list),
+      std::logic_error);
+  }
+
+  {
+    Image<uint8_t> output = makeImage(3, 3, image_buffer_bytes2_);
+    ASSERT_THROW(
+      morphologyOperation(input, output, cross_shape, max_list),
+      std::logic_error);
+  }
+}
+
+TEST_F(ImageProcTester, singlePixelImage) {
+  image_buffer_bytes_ = {255};
+  Image<uint8_t> input = makeImage(1, 1, image_buffer_bytes_);
+  Image<uint8_t> output = makeImage(1, 1, image_buffer_bytes2_);
+
+  morphologyOperation(input, output, cross_shape, max_list);
+
+  ASSERT_EQ(output.row(0)[0], 0);
+}
+
+TEST_F(ImageProcTester, cornersImage) {
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "x..x"
+    "...."
+    "...."
+    "x..x", image_buffer_bytes_);
+  Image<uint8_t> expected = imageFromString<uint8_t>(
+    ".xx."
+    "x..x"
+    "x..x"
+    ".xx.", image_buffer_bytes2_);
+  Image<uint8_t> output = makeImage(input.rows(), input.columns(), image_buffer_bytes3_);
+
+  morphologyOperation(input, output, cross_shape, max_list);
+
+  ASSERT_TRUE(isEqual(output, expected));
+}
+
+TEST_F(ImageProcTester, horizontalBordersImage) {
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "x..x"
+    "x..x"
+    "x..x"
+    "x..x", image_buffer_bytes_);
+  Image<uint8_t> expected = imageFromString<uint8_t>(
+    "xxxx"
+    "xxxx"
+    "xxxx"
+    "xxxx", image_buffer_bytes2_);
+  Image<uint8_t> output = makeImage(input.rows(), input.columns(), image_buffer_bytes3_);
+
+  morphologyOperation(input, output, cross_shape, max_list);
+
+  ASSERT_TRUE(isEqual(output, expected));
+}
+
+TEST_F(ImageProcTester, verticalBordersImage) {
+  const Image<uint8_t> input = imageFromString<uint8_t>(
+    "xxxx"
+    "...."
+    "...."
+    "xxxx", image_buffer_bytes_);
+  Image<uint8_t> expected = imageFromString<uint8_t>(
+    "xxxx"
+    "xxxx"
+    "xxxx"
+    "xxxx", image_buffer_bytes2_);
+  Image<uint8_t> output = makeImage(input.rows(), input.columns(), image_buffer_bytes3_);
+
+  morphologyOperation(input, output, cross_shape, max_list);
+
+  ASSERT_TRUE(isEqual(output, expected));
+}

navigation2/nav2_costmap_2d/test/unit/image_test.cpp

@@ -0,0 +1,110 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// 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 <gtest/gtest.h>
+
+#include "nav2_costmap_2d/denoise/image.hpp"
+#include "image_tests_helper.hpp"
+
+using namespace nav2_costmap_2d;
+
+struct ImageTester : public ::testing::Test
+{
+protected:
+  std::vector<uint8_t> image_buffer_bytes;
+  std::vector<uint16_t> image_buffer_words;
+};
+
+TEST_F(ImageTester, emptyProps) {
+  Image<uint8_t> empty;
+  ASSERT_EQ(empty.rows(), 0ul);
+  ASSERT_EQ(empty.columns(), 0ul);
+  ASSERT_EQ(empty.step(), 0ul);
+}
+
+TEST_F(ImageTester, memoryAccess) {
+  std::array<uint8_t, 7> buffer{};
+  for (uint8_t i = 0; i < buffer.size(); ++i) {
+    buffer[i] = i;
+  }
+  // buffer[3] is unused
+  Image<uint8_t> wrapper(2, 3, buffer.data(), 4);
+
+  ASSERT_EQ(wrapper.row(0), buffer.data());
+  ASSERT_EQ(wrapper.row(1), buffer.data() + 4);
+}
+
+TEST_F(ImageTester, forEach) {
+  Image<uint8_t> image = makeImage(3, 2, image_buffer_bytes);
+  const uint8_t non_zero_initial_value = 42;
+  uint8_t current(non_zero_initial_value);
+
+  image.forEach(
+    [&](uint8_t & pixel) {
+      pixel = current++;
+    });
+
+  for (size_t i = 0; i < image.rows(); ++i) {
+    for (size_t j = 0; j < image.columns(); ++j) {
+      ASSERT_EQ(image.row(i)[j], non_zero_initial_value + i * image.columns() + j);
+    }
+  }
+}
+
+TEST_F(ImageTester, convert) {
+  image_buffer_words = {1, 2, 3, 4, 5, 6};
+  Image<uint16_t> source = makeImage(2, 3, image_buffer_words);
+  Image<uint8_t> target = makeImage(2, 3, image_buffer_bytes);
+
+  source.convert(
+    target, [](uint16_t s, uint8_t & t) {
+      t = s * 2;
+    });
+
+  const std::array<uint8_t, 6> expected = {2, 4, 6, 8, 10, 12};
+  ASSERT_TRUE(std::equal(expected.begin(), expected.end(), image_buffer_bytes.begin()));
+}
+
+TEST_F(ImageTester, convertDifferentSizes) {
+  Image<uint16_t> source = makeImage(2, 3, image_buffer_words);
+  Image<uint8_t> target = makeImage(3, 2, image_buffer_bytes);
+  auto do_nothing = [](uint16_t /*src*/, uint8_t & /*trg*/) {};
+
+  // Extra parentheses need to protect commas in template arguments
+  ASSERT_THROW((source.convert(target, do_nothing)), std::logic_error);
+}
+
+TEST_F(ImageTester, convertEmptyImages) {
+  const Image<uint16_t> source;
+  Image<uint8_t> target;
+  auto shouldn_t_be_called = [](uint16_t /*src*/, uint8_t & /*trg*/) {
+      throw std::logic_error("");
+    };
+
+  ASSERT_NO_THROW((source.convert(target, shouldn_t_be_called)));
+}
+
+TEST_F(ImageTester, convertWrongSize) {
+  Image<uint16_t> source = makeImage(2, 3, image_buffer_words);
+
+  auto do_nothing = [](uint16_t /*src*/, uint8_t & /*trg*/) {};
+  {
+    Image<uint8_t> target = makeImage(3, 3, image_buffer_bytes);
+    ASSERT_THROW((source.convert(target, do_nothing)), std::logic_error);
+  }
+  {
+    Image<uint8_t> target = makeImage(2, 2, image_buffer_bytes);
+    ASSERT_THROW((source.convert(target, do_nothing)), std::logic_error);
+  }
+}

navigation2/nav2_costmap_2d/test/unit/image_tests_helper.hpp

@@ -0,0 +1,124 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// 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.
+
+#ifndef NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_
+#define NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_
+
+#include "nav2_costmap_2d/denoise/image.hpp"
+#include <cmath>
+#include <string>
+#include <vector>
+#include <map>
+#include <stdexcept>
+#include <algorithm>
+
+namespace nav2_costmap_2d
+{
+template<class T>
+Image<T> makeImage(size_t rows, size_t columns, std::vector<T> & buffer, size_t step = 0)
+{
+  step = std::max(step, columns);
+  buffer.resize(rows * step);
+  return Image<T>(rows, columns, buffer.data(), step);
+}
+
+template<class T>
+Image<T> clone(const Image<T> & source, std::vector<T> & buffer)
+{
+  buffer.resize(source.rows() * source.columns());
+  Image<T> result(source.rows(), source.columns(), buffer.data(), source.columns());
+
+  for (size_t row = 0; row < source.rows(); ++row) {
+    for (size_t column = 0; column < source.columns(); ++column) {
+      result.row(row)[column] = source.row(row)[column];
+    }
+  }
+  return result;
+}
+
+/**
+ * @brief Decodes image from a string
+ *
+ * Used only for tests.
+ * Each character of the string will be replaced with a code from the codes
+ * and written to the corresponding pixel of the image
+ * The image is always square, i.e. the number of rows is equal to the number of columns
+ * For example, string
+ * "x.x"
+ * ".x."
+ * "..."
+ * describes a 3x3 image in which a v-shape is drawn with code 255 (with default codes map)
+ * @throw std::logic_error if the format of the string is incorrect
+ */
+template<class T>
+Image<T> imageFromString(
+  const std::string & s, std::vector<T> & buffer,
+  const std::map<char, T> & codes = {{'.', 0}, {'x', 255}})
+{
+  const size_t side_size = static_cast<size_t>(std::sqrt(s.size()));
+
+  if (size_t(side_size) * side_size != s.size()) {
+    throw std::logic_error("Test data error: parseBinaryMatrix: Unexpected input string size");
+  }
+
+  const size_t step = static_cast<size_t>(side_size * 3);
+
+  Image<T> image = makeImage(side_size, side_size, buffer, step);
+  auto iter = s.begin();
+  image.forEach(
+    [&](T & pixel) {
+      try {
+        pixel = codes.at(*iter);
+        ++iter;
+      } catch (...) {
+        throw std::logic_error(
+          "Test data error: parseBinaryMatrix: Unexpected symbol: " +
+          std::string(1, *iter));
+      }
+    });
+  return image;
+}
+
+/**
+ * @brief Checks exact match of images
+ *
+ * @return true if images a and b have the same type, size, and data. Otherwise false
+ */
+inline bool isEqual(const Image<uint8_t> & a, const Image<uint8_t> & b)
+{
+  bool equal = a.rows() == b.rows() && a.columns() == b.columns();
+
+  for (size_t row = 0; row < a.rows() && equal; ++row) {
+    for (size_t column = 0; column < a.columns() && equal; ++column) {
+      equal = a.row(row)[column] == b.row(row)[column];
+    }
+  }
+  return equal;
+}
+
+template<class T>
+std::ostream & operator<<(std::ostream & out, const Image<T> & image)
+{
+  for (size_t i = 0; i < image.rows(); ++i) {
+    for (size_t j = 0; j < image.columns(); ++j) {
+      out << int64_t(image.row(i)[j]) << " ";
+    }
+    out << std::endl;
+  }
+  return out;
+}
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_

navigation2/nav2_costmap_2d/test/unit/keepout_filter_test.cpp

@@ -0,0 +1,481 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <chrono>
+#include <vector>
+#include <functional>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/msg/costmap_filter_info.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
+
+using namespace std::chrono_literals;
+
+static const char FILTER_NAME[]{"keepout_filter"};
+static const char INFO_TOPIC[]{"costmap_filter_info"};
+static const char MASK_TOPIC[]{"mask"};
+
+class InfoPublisher : public rclcpp::Node
+{
+public:
+  InfoPublisher(double base, double multiplier)
+  : Node("costmap_filter_info_pub")
+  {
+    publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
+      INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+    std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> msg =
+      std::make_unique<nav2_msgs::msg::CostmapFilterInfo>();
+    msg->type = 0;
+    msg->filter_mask_topic = MASK_TOPIC;
+    msg->base = static_cast<float>(base);
+    msg->multiplier = static_cast<float>(multiplier);
+
+    publisher_->publish(std::move(msg));
+  }
+
+  ~InfoPublisher()
+  {
+    publisher_.reset();
+  }
+
+private:
+  rclcpp::Publisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
+};  // InfoPublisher
+
+class MaskPublisher : public rclcpp::Node
+{
+public:
+  explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask)
+  : Node("mask_pub")
+  {
+    publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+      MASK_TOPIC,
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+    publisher_->publish(mask);
+  }
+
+  ~MaskPublisher()
+  {
+    publisher_.reset();
+  }
+
+private:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr publisher_;
+};  // MaskPublisher
+
+struct Point
+{
+  unsigned int x, y;
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode() {}
+
+  ~TestNode() {}
+
+protected:
+  void createMaps(unsigned char master_value, int8_t mask_value, const std::string & mask_frame);
+  void publishMaps();
+  void rePublishInfo(double base, double multiplier);
+  void rePublishMask();
+  void waitSome(const std::chrono::nanoseconds & duration);
+  void createKeepoutFilter(const std::string & global_frame);
+  void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame);
+  void verifyMasterGrid(unsigned char free_value, unsigned char keepout_value);
+  void testStandardScenario(unsigned char free_value, unsigned char keepout_value);
+  void testFramesScenario(unsigned char free_value, unsigned char keepout_value);
+  void reset();
+
+  std::shared_ptr<nav2_costmap_2d::KeepoutFilter> keepout_filter_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> master_grid_;
+
+  std::vector<Point> keepout_points_;
+
+private:
+  nav2_util::LifecycleNode::SharedPtr node_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::unique_ptr<geometry_msgs::msg::TransformStamped> transform_;
+
+  std::shared_ptr<nav_msgs::msg::OccupancyGrid> mask_;
+
+  std::shared_ptr<InfoPublisher> info_publisher_;
+  std::shared_ptr<MaskPublisher> mask_publisher_;
+};
+
+void TestNode::createMaps(
+  unsigned char master_value, int8_t mask_value, const std::string & mask_frame)
+{
+  // Make map and mask put as follows:
+  //
+  //  map             (10,10)
+  //   *----------------*
+  //   |   mask (6,6)   |
+  //   |    *-----*     |
+  //   |    |/////|     |
+  //   |    |/////|     |
+  //   |    *-----*     |
+  //   |  (3,3)         |
+  //   *----------------*
+  // (0,0)
+
+  const double resolution = 1.0;
+
+  // Create master_grid_
+  unsigned int width = 10;
+  unsigned int height = 10;
+  master_grid_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+    width, height, resolution, 0.0, 0.0, master_value);
+
+  // Create mask_
+  width = 3;
+  height = 3;
+  mask_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+  mask_->info.resolution = resolution;
+  mask_->header.frame_id = mask_frame;
+  mask_->info.width = width;
+  mask_->info.height = height;
+  mask_->info.origin.position.x = 3.0;
+  mask_->info.origin.position.y = 3.0;
+  mask_->info.origin.position.z = 0.0;
+  mask_->info.origin.orientation.x = 0.0;
+  mask_->info.origin.orientation.y = 0.0;
+  mask_->info.origin.orientation.z = 0.0;
+  mask_->info.origin.orientation.w = 1.0;
+  mask_->data.resize(width * height, mask_value);
+}
+
+void TestNode::publishMaps()
+{
+  info_publisher_ = std::make_shared<InfoPublisher>(0.0, 1.0);
+  mask_publisher_ = std::make_shared<MaskPublisher>(*mask_);
+}
+
+void TestNode::rePublishInfo(double base, double multiplier)
+{
+  info_publisher_.reset();
+  info_publisher_ = std::make_shared<InfoPublisher>(base, multiplier);
+  // Allow both CostmapFilterInfo and filter mask subscribers
+  // to receive a new message
+  waitSome(100ms);
+}
+
+void TestNode::rePublishMask()
+{
+  mask_publisher_.reset();
+  mask_publisher_ = std::make_shared<MaskPublisher>(*mask_);
+  // Allow filter mask subscriber to receive a new message
+  waitSome(100ms);
+}
+
+void TestNode::waitSome(const std::chrono::nanoseconds & duration)
+{
+  rclcpp::Time start_time = node_->now();
+  while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) {
+    rclcpp::spin_some(node_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+}
+
+void TestNode::createKeepoutFilter(const std::string & global_frame)
+{
+  node_ = std::make_shared<nav2_util::LifecycleNode>("test_node");
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false);
+
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC));
+
+  keepout_filter_ = std::make_shared<nav2_costmap_2d::KeepoutFilter>();
+  keepout_filter_->initialize(&layers, std::string(FILTER_NAME), tf_buffer_.get(), node_, nullptr);
+  keepout_filter_->initializeFilter(INFO_TOPIC);
+
+  // Wait until mask will be received by KeepoutFilter
+  while (!keepout_filter_->isActive()) {
+    rclcpp::spin_some(node_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+}
+
+void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame)
+{
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+
+  transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+  transform_->header.frame_id = mask_frame;
+  transform_->child_frame_id = global_frame;
+
+  transform_->header.stamp = node_->now();
+  transform_->transform.translation.x = 1.0;
+  transform_->transform.translation.y = 1.0;
+  transform_->transform.translation.z = 0.0;
+  transform_->transform.rotation.x = 0.0;
+  transform_->transform.rotation.y = 0.0;
+  transform_->transform.rotation.z = 0.0;
+  transform_->transform.rotation.w = 1.0;
+
+  tf_broadcaster_->sendTransform(*transform_);
+
+  // Allow tf_buffer_ to be filled by listener
+  waitSome(100ms);
+}
+
+void TestNode::verifyMasterGrid(unsigned char free_value, unsigned char keepout_value)
+{
+  unsigned int x, y;
+  bool is_checked;
+
+  for (y = 0; y < master_grid_->getSizeInCellsY(); y++) {
+    for (x = 0; x < master_grid_->getSizeInCellsX(); x++) {
+      is_checked = false;
+      for (std::vector<Point>::iterator it = keepout_points_.begin();
+        it != keepout_points_.end(); it++)
+      {
+        if (x == it->x && y == it->y) {
+          EXPECT_EQ(master_grid_->getCost(x, y), keepout_value);
+          is_checked = true;
+          break;
+        }
+      }
+      if (!is_checked) {
+        EXPECT_EQ(master_grid_->getCost(x, y), free_value);
+      }
+    }
+  }
+}
+
+void TestNode::testStandardScenario(unsigned char free_value, unsigned char keepout_value)
+{
+  geometry_msgs::msg::Pose2D pose;
+  // Intersection window: added 4 points
+  keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose);
+  keepout_points_.push_back(Point{3, 3});
+  keepout_points_.push_back(Point{3, 4});
+  keepout_points_.push_back(Point{4, 3});
+  keepout_points_.push_back(Point{4, 4});
+  verifyMasterGrid(free_value, keepout_value);
+  // Two windows outside on the horisontal/vertical edge: no new points added
+  keepout_filter_->process(*master_grid_, 3, 6, 5, 7, pose);
+  keepout_filter_->process(*master_grid_, 6, 3, 7, 5, pose);
+  verifyMasterGrid(free_value, keepout_value);
+  // Corner window: added 1 point
+  keepout_filter_->process(*master_grid_, 5, 5, 6, 6, pose);
+  keepout_points_.push_back(Point{5, 5});
+  verifyMasterGrid(free_value, keepout_value);
+  // Outside windows: no new points added
+  keepout_filter_->process(*master_grid_, 0, 0, 2, 2, pose);
+  keepout_filter_->process(*master_grid_, 0, 7, 2, 9, pose);
+  keepout_filter_->process(*master_grid_, 7, 0, 9, 2, pose);
+  keepout_filter_->process(*master_grid_, 7, 7, 9, 9, pose);
+  verifyMasterGrid(free_value, keepout_value);
+}
+
+void TestNode::testFramesScenario(unsigned char free_value, unsigned char keepout_value)
+{
+  geometry_msgs::msg::Pose2D pose;
+  // Intersection window: added all 9 points because of map->odom frame shift
+  keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose);
+  keepout_points_.push_back(Point{2, 2});
+  keepout_points_.push_back(Point{2, 3});
+  keepout_points_.push_back(Point{2, 4});
+  keepout_points_.push_back(Point{3, 2});
+  keepout_points_.push_back(Point{3, 3});
+  keepout_points_.push_back(Point{3, 4});
+  keepout_points_.push_back(Point{4, 2});
+  keepout_points_.push_back(Point{4, 3});
+  keepout_points_.push_back(Point{4, 4});
+  verifyMasterGrid(free_value, keepout_value);
+}
+
+void TestNode::reset()
+{
+  mask_.reset();
+  master_grid_.reset();
+  info_publisher_.reset();
+  mask_publisher_.reset();
+  keepout_filter_.reset();
+  node_.reset();
+  tf_listener_.reset();
+  tf_broadcaster_.reset();
+  tf_buffer_.reset();
+  keepout_points_.clear();
+}
+
+TEST_F(TestNode, testFreeMasterLethalKeepout)
+{
+  // Initialize test system
+  createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
+  publishMaps();
+  createKeepoutFilter("map");
+
+  // Test KeepoutFilter
+  testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testUnknownMasterNonLethalKeepout)
+{
+  // Initialize test system
+  createMaps(
+    nav2_costmap_2d::NO_INFORMATION,
+    (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2,
+    "map");
+  publishMaps();
+  createKeepoutFilter("map");
+
+  // Test KeepoutFilter
+  testStandardScenario(
+    nav2_costmap_2d::NO_INFORMATION,
+    (nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) / 2);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testFreeKeepout)
+{
+  // Initialize test system
+  createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE, "map");
+  publishMaps();
+  createKeepoutFilter("map");
+
+  // Test KeepoutFilter
+  geometry_msgs::msg::Pose2D pose;
+  // Check whole area window
+  keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose);
+  // There should be no one point appeared on master_grid_ after process()
+  verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testUnknownKeepout)
+{
+  // Initialize test system
+  createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN, "map");
+  publishMaps();
+  createKeepoutFilter("map");
+
+  // Test KeepoutFilter
+  geometry_msgs::msg::Pose2D pose;
+  // Check whole area window
+  keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose);
+  // There should be no one point appeared on master_grid_ after process()
+  verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testInfoRePublish)
+{
+  // Initialize test system
+  createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
+  publishMaps();
+  createKeepoutFilter("map");
+
+  // Re-publish filter info (with incorrect base and multiplier)
+  // and test that everything is working after
+  rePublishInfo(0.1, 0.2);
+
+  // Test KeepoutFilter
+  testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testMaskRePublish)
+{
+  // Initialize test system
+  createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
+  publishMaps();
+  createKeepoutFilter("map");
+
+  // Re-publish filter mask and test that everything is working after
+  rePublishMask();
+
+  // Test KeepoutFilter
+  testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testDifferentFrames)
+{
+  // Initialize test system
+  createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
+  publishMaps();
+  createKeepoutFilter("odom");
+  createTFBroadcaster("map", "odom");
+
+  // Test KeepoutFilter
+  testFramesScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  // Clean-up
+  keepout_filter_->resetFilter();
+  reset();
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}

navigation2/nav2_costmap_2d/test/unit/lifecycle_test.cpp

@@ -0,0 +1,54 @@
+// 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 <memory>
+
+#include "gtest/gtest.h"
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+
+
+TEST(LifecylceTest, CheckInitialTfTimeout) {
+  rclcpp::init(0, nullptr);
+
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(rclcpp::NodeOptions());
+  costmap->set_parameter({"initial_transform_timeout", 0.0});
+
+  std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}};
+
+  {
+    const auto state_after_configure = costmap->configure();
+    ASSERT_EQ(state_after_configure.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+    // Without providing the transform from global to robot base the activation should fail
+    // and the costmap should transition into the inactive state.
+    const auto state_after_activate = costmap->activate();
+    ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+  }
+
+  // Set a dummy transform from global to robot base
+  geometry_msgs::msg::TransformStamped transform_global_to_robot{};
+  transform_global_to_robot.header.frame_id = costmap->getGlobalFrameID();
+  transform_global_to_robot.child_frame_id = costmap->getBaseFrameID();
+  costmap->getTfBuffer()->setTransform(transform_global_to_robot, "test", true);
+  // Now the costmap should successful transition into the active state
+  {
+    const auto state_after_activate = costmap->activate();
+    ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+  }
+
+  rclcpp::shutdown();
+  if (spin_thread.joinable()) {
+    spin_thread.join();
+  }
+}

navigation2/nav2_costmap_2d/test/unit/speed_filter_test.cpp

@@ -0,0 +1,789 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <chrono>
+#include <vector>
+#include <tuple>
+#include <functional>
+#include <stdexcept>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/msg/costmap_filter_info.hpp"
+#include "nav2_msgs/msg/speed_limit.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_costmap_2d/costmap_filters/speed_filter.hpp"
+
+using namespace std::chrono_literals;
+
+static const char FILTER_NAME[]{"speed_filter"};
+static const char INFO_TOPIC[]{"costmap_filter_info"};
+static const char MASK_TOPIC[]{"mask"};
+static const char SPEED_LIMIT_TOPIC[]{"speed_limit"};
+
+static const double NO_TRANSLATION = 0.0;
+static const double TRANSLATION_X = 1.0;
+static const double TRANSLATION_Y = 1.0;
+
+static const uint8_t INCORRECT_TYPE = 200;
+
+static constexpr double EPSILON = 1e-5;
+
+class InfoPublisher : public rclcpp::Node
+{
+public:
+  InfoPublisher(uint8_t type, double base, double multiplier)
+  : Node("costmap_filter_info_pub")
+  {
+    publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
+      INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+    std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> msg =
+      std::make_unique<nav2_msgs::msg::CostmapFilterInfo>();
+    msg->type = type;
+    msg->filter_mask_topic = MASK_TOPIC;
+    msg->base = static_cast<float>(base);
+    msg->multiplier = static_cast<float>(multiplier);
+
+    publisher_->publish(std::move(msg));
+  }
+
+  ~InfoPublisher()
+  {
+    publisher_.reset();
+  }
+
+private:
+  rclcpp::Publisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
+};  // InfoPublisher
+
+class MaskPublisher : public rclcpp::Node
+{
+public:
+  explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask)
+  : Node("mask_pub")
+  {
+    publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+      MASK_TOPIC,
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+    publisher_->publish(mask);
+  }
+
+  ~MaskPublisher()
+  {
+    publisher_.reset();
+  }
+
+private:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr publisher_;
+};  // MaskPublisher
+
+class SpeedLimitSubscriber : public rclcpp::Node
+{
+public:
+  explicit SpeedLimitSubscriber(const std::string & speed_limit_topic)
+  : Node("speed_limit_sub"), speed_limit_updated_(false)
+  {
+    subscriber_ = this->create_subscription<nav2_msgs::msg::SpeedLimit>(
+      speed_limit_topic, rclcpp::QoS(10),
+      std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1));
+  }
+
+  void speedLimitCallback(
+    const nav2_msgs::msg::SpeedLimit::SharedPtr msg)
+  {
+    msg_ = msg;
+    speed_limit_updated_ = true;
+  }
+
+  nav2_msgs::msg::SpeedLimit::SharedPtr getSpeedLimit()
+  {
+    return msg_;
+  }
+
+  inline bool speedLimitUpdated()
+  {
+    return speed_limit_updated_;
+  }
+
+  inline void resetSpeedLimitIndicator()
+  {
+    speed_limit_updated_ = false;
+  }
+
+private:
+  rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr subscriber_;
+  nav2_msgs::msg::SpeedLimit::SharedPtr msg_;
+  bool speed_limit_updated_;
+};  // SpeedLimitSubscriber
+
+class TestMask : public nav_msgs::msg::OccupancyGrid
+{
+public:
+  TestMask(
+    unsigned int width, unsigned int height, double resolution,
+    const std::string & mask_frame)
+  : width_(width), height_(height)
+  {
+    // Fill filter mask info
+    header.frame_id = mask_frame;
+    info.resolution = resolution;
+    info.width = width_;
+    info.height = height_;
+    info.origin.position.x = 0.0;
+    info.origin.position.y = 0.0;
+    info.origin.position.z = 0.0;
+    info.origin.orientation.x = 0.0;
+    info.origin.orientation.y = 0.0;
+    info.origin.orientation.z = 0.0;
+    info.origin.orientation.w = 1.0;
+
+    // Fill test mask as follows:
+    //
+    //  mask           (10,11)
+    //   *----------------*
+    //   |91|92|...|99|100|
+    //   |...             |
+    //   |...             |
+    //   |11|12|13|...| 20|
+    //   | 1| 2| 3|...| 10|
+    //   |-1| 0| 0|...|  0|
+    //   *----------------*
+    // (0,0)
+    data.resize(width_ * height_, nav2_util::OCC_GRID_UNKNOWN);
+
+    unsigned int mx, my;
+    data[0] = -1;
+    for (mx = 1; mx < width_; mx++) {
+      data[mx] = 0;
+    }
+    unsigned int it;
+    for (my = 1; my < height_; my++) {
+      for (mx = 0; mx < width_; mx++) {
+        it = mx + my * width_;
+        data[it] = makeData(mx, my);
+      }
+    }
+  }
+
+  inline int8_t makeData(unsigned int mx, unsigned int my)
+  {
+    return mx + (my - 1) * width_ + 1;
+  }
+
+private:
+  const unsigned int width_;
+  const unsigned int height_;
+};  // TestMask
+
+class TestNode : public ::testing::Test
+{
+public:
+  TestNode() {}
+
+  ~TestNode() {}
+
+protected:
+  void createMaps(const std::string & mask_frame);
+  void publishMaps(uint8_t type, double base, double multiplier);
+  void rePublishInfo(uint8_t type, double base, double multiplier);
+  void rePublishMask();
+  bool createSpeedFilter(const std::string & global_frame);
+  void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame);
+  void publishTransform();
+
+  // Test methods
+  void testFullMask(
+    uint8_t type, double base, double multiplier,
+    double tr_x, double tr_y);
+  void testSimpleMask(
+    uint8_t type, double base, double multiplier,
+    double tr_x, double tr_y);
+  void testOutOfMask(uint8_t type, double base, double multiplier);
+  void testIncorrectLimits(uint8_t type, double base, double multiplier);
+
+  void reset();
+
+  std::shared_ptr<nav2_costmap_2d::SpeedFilter> speed_filter_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> master_grid_;
+
+private:
+  void waitSome(const std::chrono::nanoseconds & duration);
+  void verifySpeedLimit(
+    uint8_t type, double base, double multiplier,
+    unsigned int x, unsigned int y,
+    nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit);
+  nav2_msgs::msg::SpeedLimit::SharedPtr getSpeedLimit();
+  nav2_msgs::msg::SpeedLimit::SharedPtr waitSpeedLimit();
+
+  const unsigned int width_ = 10;
+  const unsigned int height_ = 11;
+  const double resolution_ = 1.0;
+
+  nav2_util::LifecycleNode::SharedPtr node_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::unique_ptr<geometry_msgs::msg::TransformStamped> transform_;
+
+  std::shared_ptr<TestMask> mask_;
+
+  std::shared_ptr<InfoPublisher> info_publisher_;
+  std::shared_ptr<MaskPublisher> mask_publisher_;
+  std::shared_ptr<SpeedLimitSubscriber> speed_limit_subscriber_;
+};
+
+void TestNode::createMaps(const std::string & mask_frame)
+{
+  // Make map and mask put as follows:
+  //  master_grid     (12,13)
+  //    *----------------*
+  //    |                |
+  //    |  mask  (10,11) |
+  //    |   *-------*    |
+  //    |   |///////|    |
+  //    |   |///////|    |
+  //    |   |///////|    |
+  //    |   *-------*    |
+  //    | (0,0)          |
+  //    |                |
+  //    *----------------*
+  // (-2,-2)
+
+  // Create master_grid_
+  master_grid_ = std::make_shared<nav2_costmap_2d::Costmap2D>(
+    width_ + 4, height_ + 4, resolution_, -2.0, -2.0, nav2_costmap_2d::FREE_SPACE);
+
+  // Create mask_
+  mask_ = std::make_shared<TestMask>(width_, height_, resolution_, mask_frame);
+}
+
+void TestNode::publishMaps(uint8_t type, double base, double multiplier)
+{
+  info_publisher_ = std::make_shared<InfoPublisher>(type, base, multiplier);
+  mask_publisher_ = std::make_shared<MaskPublisher>(*mask_);
+}
+
+void TestNode::rePublishInfo(uint8_t type, double base, double multiplier)
+{
+  info_publisher_.reset();
+  info_publisher_ = std::make_shared<InfoPublisher>(type, base, multiplier);
+  // Allow both CostmapFilterInfo and filter mask subscribers
+  // to receive a new message
+  waitSome(100ms);
+}
+
+void TestNode::rePublishMask()
+{
+  mask_publisher_.reset();
+  mask_publisher_ = std::make_shared<MaskPublisher>(*mask_);
+  // Allow filter mask subscriber to receive a new message
+  waitSome(100ms);
+}
+
+nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::getSpeedLimit()
+{
+  std::this_thread::sleep_for(100ms);
+  rclcpp::spin_some(speed_limit_subscriber_);
+  return speed_limit_subscriber_->getSpeedLimit();
+}
+
+nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::waitSpeedLimit()
+{
+  const std::chrono::nanoseconds timeout = 500ms;
+
+  rclcpp::Time start_time = node_->now();
+  speed_limit_subscriber_->resetSpeedLimitIndicator();
+  while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(timeout)) {
+    if (speed_limit_subscriber_->speedLimitUpdated()) {
+      speed_limit_subscriber_->resetSpeedLimitIndicator();
+      return speed_limit_subscriber_->getSpeedLimit();
+    }
+    rclcpp::spin_some(speed_limit_subscriber_);
+    std::this_thread::sleep_for(10ms);
+  }
+  return nullptr;
+}
+
+void TestNode::waitSome(const std::chrono::nanoseconds & duration)
+{
+  rclcpp::Time start_time = node_->now();
+  while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) {
+    rclcpp::spin_some(node_->get_node_base_interface());
+    rclcpp::spin_some(speed_limit_subscriber_);
+    std::this_thread::sleep_for(10ms);
+  }
+}
+
+bool TestNode::createSpeedFilter(const std::string & global_frame)
+{
+  node_ = std::make_shared<nav2_util::LifecycleNode>("test_node");
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);  // One-thread broadcasting-listening model
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false);
+
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC));
+  node_->declare_parameter(
+    std::string(FILTER_NAME) + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC));
+  node_->set_parameter(
+    rclcpp::Parameter(std::string(FILTER_NAME) + ".speed_limit_topic", SPEED_LIMIT_TOPIC));
+
+  speed_filter_ = std::make_shared<nav2_costmap_2d::SpeedFilter>();
+  speed_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr);
+  speed_filter_->initializeFilter(INFO_TOPIC);
+
+  speed_limit_subscriber_ = std::make_shared<SpeedLimitSubscriber>(SPEED_LIMIT_TOPIC);
+
+  // Wait until mask will be received by SpeedFilter
+  const std::chrono::nanoseconds timeout = 500ms;
+  rclcpp::Time start_time = node_->now();
+  while (!speed_filter_->isActive()) {
+    if (node_->now() - start_time > rclcpp::Duration(timeout)) {
+      return false;
+    }
+    rclcpp::spin_some(node_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return true;
+}
+
+void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame)
+{
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+
+  transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+  transform_->header.frame_id = mask_frame;
+  transform_->child_frame_id = global_frame;
+
+  transform_->header.stamp = node_->now() + rclcpp::Duration(100ms);
+  transform_->transform.translation.x = TRANSLATION_X;
+  transform_->transform.translation.y = TRANSLATION_Y;
+  transform_->transform.translation.z = 0.0;
+  transform_->transform.rotation.x = 0.0;
+  transform_->transform.rotation.y = 0.0;
+  transform_->transform.rotation.z = 0.0;
+  transform_->transform.rotation.w = 1.0;
+
+  tf_broadcaster_->sendTransform(*transform_);
+
+  // Allow tf_buffer_ to be filled by listener
+  waitSome(100ms);
+}
+
+void TestNode::publishTransform()
+{
+  if (tf_broadcaster_) {
+    transform_->header.stamp = node_->now() + rclcpp::Duration(100ms);
+    tf_broadcaster_->sendTransform(*transform_);
+  }
+}
+
+void TestNode::verifySpeedLimit(
+  uint8_t type, double base, double multiplier,
+  unsigned int x, unsigned int y,
+  nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit)
+{
+  int8_t cost = mask_->makeData(x, y);
+  // expected_limit is being calculated by using float32 base and multiplier
+  double expected_limit = cost * multiplier + base;
+  if (type == nav2_costmap_2d::SPEED_FILTER_PERCENT) {
+    if (expected_limit < 0.0 || expected_limit > 100.0) {
+      expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT;
+    }
+    EXPECT_TRUE(speed_limit->percentage);
+    EXPECT_TRUE(speed_limit->speed_limit >= 0.0);
+    EXPECT_TRUE(speed_limit->speed_limit <= 100.0);
+    EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON);
+  } else if (type == nav2_costmap_2d::SPEED_FILTER_ABSOLUTE) {
+    if (expected_limit < 0.0) {
+      expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT;
+    }
+    EXPECT_FALSE(speed_limit->percentage);
+    EXPECT_TRUE(speed_limit->speed_limit >= 0.0);
+    EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON);
+  } else {
+    FAIL() << "The type of costmap filter is unknown";
+  }
+}
+
+void TestNode::testFullMask(
+  uint8_t type, double base, double multiplier,
+  double tr_x, double tr_y)
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit;
+
+  // data = 0
+  pose.x = 1 - tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = getSpeedLimit();
+  ASSERT_TRUE(speed_limit == nullptr);
+
+  // data in range [1..100]
+  unsigned int x, y;
+  for (y = 1; y < height_; y++) {
+    for (x = 0; x < width_; x++) {
+      pose.x = x - tr_x;
+      pose.y = y - tr_y;
+      publishTransform();
+      speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+      speed_limit = waitSpeedLimit();
+      ASSERT_TRUE(speed_limit != nullptr);
+      verifySpeedLimit(type, base, multiplier, x, y, speed_limit);
+    }
+  }
+
+  // data = 0
+  pose.x = 1 - tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = waitSpeedLimit();
+  ASSERT_TRUE(speed_limit != nullptr);
+  EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
+
+  // data = -1
+  pose.x = -tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = getSpeedLimit();
+  ASSERT_TRUE(speed_limit != nullptr);
+  EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
+}
+
+void TestNode::testSimpleMask(
+  uint8_t type, double base, double multiplier,
+  double tr_x, double tr_y)
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit;
+
+  // data = 0
+  pose.x = 1 - tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = getSpeedLimit();
+  ASSERT_TRUE(speed_limit == nullptr);
+
+  // data = <some_middle_value>
+  unsigned int x = width_ / 2 - 1;
+  unsigned int y = height_ / 2 - 1;
+  pose.x = x - tr_x;
+  pose.y = y - tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = waitSpeedLimit();
+  ASSERT_TRUE(speed_limit != nullptr);
+  verifySpeedLimit(type, base, multiplier, x, y, speed_limit);
+
+  // data = 100
+  x = width_ - 1;
+  y = height_ - 1;
+  pose.x = x - tr_x;
+  pose.y = y - tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = waitSpeedLimit();
+  ASSERT_TRUE(speed_limit != nullptr);
+  verifySpeedLimit(type, base, multiplier, x, y, speed_limit);
+
+  // data = 0
+  pose.x = 1 - tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = waitSpeedLimit();
+  ASSERT_TRUE(speed_limit != nullptr);
+  EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
+
+  // data = -1
+  pose.x = -tr_x;
+  pose.y = -tr_y;
+  publishTransform();
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = getSpeedLimit();
+  ASSERT_TRUE(speed_limit != nullptr);
+  EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT);
+}
+
+void TestNode::testOutOfMask(uint8_t type, double base, double multiplier)
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  nav2_msgs::msg::SpeedLimit::SharedPtr old_speed_limit, speed_limit;
+
+  // data = <some_middle_value>
+  pose.x = width_ / 2 - 1;
+  pose.y = height_ / 2 - 1;
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  old_speed_limit = waitSpeedLimit();
+  ASSERT_TRUE(old_speed_limit != nullptr);
+  verifySpeedLimit(type, base, multiplier, pose.x, pose.y, old_speed_limit);
+
+  // Then go to out of mask bounds and ensure that speed limit was not updated
+  pose.x = -2.0;
+  pose.y = -2.0;
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = getSpeedLimit();
+  ASSERT_TRUE(speed_limit == old_speed_limit);
+
+  pose.x = width_ + 1.0;
+  pose.y = height_ + 1.0;
+  speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+  speed_limit = getSpeedLimit();
+  ASSERT_TRUE(speed_limit == old_speed_limit);
+}
+
+void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier)
+{
+  const int min_i = 0;
+  const int min_j = 0;
+  const int max_i = width_ + 4;
+  const int max_j = height_ + 4;
+
+  geometry_msgs::msg::Pose2D pose;
+  nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit;
+
+  std::vector<std::tuple<unsigned int, unsigned int>> points;
+
+  // Some middle point corresponding to correct speed limit value
+  points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1));
+  // (0, 1) point corresponding to incorrect limit value: data = 1, value < 0
+  points.push_back(std::make_tuple(0, 1));
+  // Some middle point corresponding to correct speed limit value
+  points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1));
+  // (width_ - 1, height_ - 1) point corresponding to incorrect limit value:
+  // data = 100, value > 100
+  points.push_back(std::make_tuple(width_ - 1, height_ - 1));
+
+  for (auto it = points.begin(); it != points.end(); ++it) {
+    pose.x = static_cast<double>(std::get<0>(*it));
+    pose.y = static_cast<double>(std::get<1>(*it));
+    speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
+    speed_limit = waitSpeedLimit();
+    ASSERT_TRUE(speed_limit != nullptr);
+    verifySpeedLimit(type, base, multiplier, pose.x, pose.y, speed_limit);
+  }
+}
+
+void TestNode::reset()
+{
+  mask_.reset();
+  master_grid_.reset();
+  info_publisher_.reset();
+  mask_publisher_.reset();
+  speed_limit_subscriber_.reset();
+  speed_filter_.reset();
+  node_.reset();
+  tf_listener_.reset();
+  tf_broadcaster_.reset();
+  tf_buffer_.reset();
+}
+
+TEST_F(TestNode, testPercentSpeedLimit)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testFullMask(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testIncorrectPercentSpeedLimit)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testAbsoluteSpeedLimit)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testFullMask(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testOutOfBounds)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testOutOfMask(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testInfoRePublish)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Re-publish filter info (with incorrect base and multiplier)
+  // and test that everything is working after
+  rePublishInfo(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2);
+
+  // Test SpeedFilter
+  testSimpleMask(
+    nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testMaskRePublish)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Re-publish filter mask and test that everything is working after
+  rePublishMask();
+
+  // Test SpeedFilter
+  testSimpleMask(
+    nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testIncorrectFilterType)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(INCORRECT_TYPE, 1.23, 4.5);
+  EXPECT_FALSE(createSpeedFilter("map"));
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testDifferentFrame)
+{
+  // Initialize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0);
+  EXPECT_TRUE(createSpeedFilter("odom"));
+  createTFBroadcaster("map", "odom");
+
+  // Test SpeedFilter
+  testFullMask(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0, TRANSLATION_X, TRANSLATION_Y);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}