From f50d6c193aeeecb8f3bfef8f43a14aa041a98304 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Fabian=20K=C3=B6nig?= <fabiankoenig@gmail.com>
Date: Thu, 12 Oct 2023 19:31:46 +0000
Subject: [PATCH] Add unittest for costmap activation
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Fabian König <fabiankoenig@gmail.com>
---
 nav2_costmap_2d/test/unit/CMakeLists.txt     |  7 +++
 nav2_costmap_2d/test/unit/lifecycle_test.cpp | 54 ++++++++++++++++++++
 2 files changed, 61 insertions(+)
 create mode 100644 nav2_costmap_2d/test/unit/lifecycle_test.cpp

diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt
index 19c2edb56fe..7c804a82265 100644
--- a/nav2_costmap_2d/test/unit/CMakeLists.txt
+++ b/nav2_costmap_2d/test/unit/CMakeLists.txt
@@ -55,3 +55,10 @@ ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_p
 target_link_libraries(denoise_layer_test
   nav2_costmap_2d_core layers
 )
+
+ament_add_gtest_executable(lifecycle_test
+lifecycle_test.cpp
+)
+target_link_libraries(lifecycle_test
+  nav2_costmap_2d_core
+)
\ No newline at end of file
diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp
new file mode 100644
index 00000000000..8a657d81ad1
--- /dev/null
+++ b/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>("test_costmap");
+  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();
+  }
+}