add humble-navigation2

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;
+}