diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 19c2edb56f..b9480fdffa 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -55,3 +55,8 @@ 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(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 0000000000..8a657d81ad --- /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 + +#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("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(); + } +}