add humble-navigation2

navigation2/nav2_util/test/CMakeLists.txt

@@ -0,0 +1,47 @@
+ament_add_gtest(test_execution_timer test_execution_timer.cpp)
+
+ament_add_gtest(test_node_utils test_node_utils.cpp)
+target_link_libraries(test_node_utils ${library_name})
+
+find_package(std_srvs REQUIRED)
+find_package(test_msgs REQUIRED)
+
+ament_add_gtest(test_service_client test_service_client.cpp)
+ament_target_dependencies(test_service_client std_srvs)
+target_link_libraries(test_service_client ${library_name})
+
+ament_add_gtest(test_string_utils test_string_utils.cpp)
+target_link_libraries(test_string_utils ${library_name})
+
+find_package(rclcpp_lifecycle REQUIRED)
+ament_add_gtest(test_lifecycle_utils test_lifecycle_utils.cpp)
+ament_target_dependencies(test_lifecycle_utils rclcpp_lifecycle)
+target_link_libraries(test_lifecycle_utils ${library_name})
+
+ament_add_gtest(test_actions test_actions.cpp)
+ament_target_dependencies(test_actions rclcpp_action test_msgs)
+target_link_libraries(test_actions ${library_name})
+
+ament_add_gtest(test_lifecycle_node test_lifecycle_node.cpp)
+ament_target_dependencies(test_lifecycle_node rclcpp_lifecycle)
+target_link_libraries(test_lifecycle_node ${library_name})
+
+ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp)
+ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle)
+target_link_libraries(test_lifecycle_cli_node ${library_name})
+
+ament_add_gtest(test_geometry_utils test_geometry_utils.cpp)
+ament_target_dependencies(test_geometry_utils geometry_msgs)
+target_link_libraries(test_geometry_utils ${library_name})
+
+ament_add_gtest(test_odometry_utils test_odometry_utils.cpp)
+ament_target_dependencies(test_odometry_utils nav_msgs geometry_msgs)
+target_link_libraries(test_odometry_utils ${library_name})
+
+ament_add_gtest(test_robot_utils test_robot_utils.cpp)
+ament_target_dependencies(test_robot_utils geometry_msgs)
+target_link_libraries(test_robot_utils ${library_name})
+
+ament_add_gtest(test_validation_messages test_validation_messages.cpp)
+ament_target_dependencies(test_validation_messages rclcpp_lifecycle)
+target_link_libraries(test_validation_messages ${library_name})

navigation2/nav2_util/test/test_actions.cpp

@@ -0,0 +1,553 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <memory>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "test_msgs/action/fibonacci.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+using Fibonacci = test_msgs::action::Fibonacci;
+using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+class FibonacciServerNode : public rclcpp::Node
+{
+public:
+  FibonacciServerNode()
+  : rclcpp::Node("fibonacci_server_node")
+  {
+  }
+
+  ~FibonacciServerNode()
+  {
+  }
+
+  void on_init()
+  {
+    action_server_ = std::make_shared<nav2_util::SimpleActionServer<Fibonacci>>(
+      shared_from_this(),
+      "fibonacci",
+      std::bind(&FibonacciServerNode::execute, this));
+
+    deactivate_subs_ = create_subscription<std_msgs::msg::Empty>(
+      "deactivate_server",
+      1,
+      [this](std_msgs::msg::Empty::UniquePtr /*msg*/) {
+        RCLCPP_INFO(this->get_logger(), "Deactivating");
+        action_server_->deactivate();
+      });
+
+    activate_subs_ = create_subscription<std_msgs::msg::Empty>(
+      "activate_server",
+      1,
+      [this](std_msgs::msg::Empty::UniquePtr /*msg*/) {
+        RCLCPP_INFO(this->get_logger(), "Activating");
+        action_server_->activate();
+      });
+
+    omit_preempt_subs_ = create_subscription<std_msgs::msg::Empty>(
+      "omit_preemption",
+      1,
+      [this](std_msgs::msg::Empty::UniquePtr /*msg*/) {
+        RCLCPP_INFO(this->get_logger(), "Ignoring preemptions");
+        do_premptions_ = false;
+      });
+  }
+
+  void on_term()
+  {
+    // when nothing's running make sure everything's dead.
+    // const std::shared_ptr<const Fibonacci::Goal> a = action_server_->accept_pending_goal();
+    // const std::shared_ptr<const Fibonacci::Goal> b = action_server_->get_current_goal();
+    // assert(a == b);
+    // assert(action_server_->is_cancel_requested() == false);
+    // auto feedback = std::make_shared<Fibonacci::Feedback>();
+    // action_server_->publish_feedback(feedback);
+    action_server_.reset();
+  }
+
+  void execute()
+  {
+    rclcpp::Rate loop_rate(10);
+
+preempted:
+    // Initialize the goal, feedback, and result
+    auto goal = action_server_->get_current_goal();
+    auto feedback = std::make_shared<Fibonacci::Feedback>();
+    auto result = std::make_shared<Fibonacci::Result>();
+
+    // Fibonacci-specific initialization
+    auto & sequence = feedback->sequence;
+    sequence.push_back(0);
+    sequence.push_back(1);
+
+    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+      // Should be check periodically if this action has been canceled
+      // or if the server has been deactivated.
+      if (action_server_->is_cancel_requested() || !action_server_->is_server_active()) {
+        result->sequence = sequence;
+        return;
+      }
+
+      // Check if we've gotten an new goal, pre-empting the current one
+      if (do_premptions_ && action_server_->is_preempt_requested()) {
+        action_server_->accept_pending_goal();
+        goto preempted;
+      }
+
+      // Update the sequence
+      sequence.push_back(sequence[i] + sequence[i - 1]);
+
+      // Publish feedback
+      action_server_->publish_feedback(feedback);
+      loop_rate.sleep();
+    }
+
+    // Check if goal is done
+    if (rclcpp::ok()) {
+      result->sequence = sequence;
+      action_server_->succeeded_current(result);
+    }
+  }
+
+private:
+  std::shared_ptr<nav2_util::SimpleActionServer<Fibonacci>> action_server_;
+  rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr deactivate_subs_;
+  rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr activate_subs_;
+  rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr omit_preempt_subs_;
+
+  bool do_premptions_{true};
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture()
+  {
+  }
+
+  void Setup()
+  {
+    server_thread_ =
+      std::make_shared<std::thread>(std::bind(&RclCppFixture::server_thread_func, this));
+  }
+
+  ~RclCppFixture()
+  {
+    server_thread_->join();
+  }
+
+  void server_thread_func()
+  {
+    auto node = std::make_shared<FibonacciServerNode>();
+    node->on_init();
+    rclcpp::spin(node->get_node_base_interface());
+    node->on_term();
+    node.reset();
+  }
+
+  std::shared_ptr<std::thread> server_thread_;
+};
+
+RclCppFixture g_rclcppfixture;
+
+class ActionTestNode : public rclcpp::Node
+{
+public:
+  ActionTestNode()
+  : rclcpp::Node(nav2_util::generate_internal_node_name("action_test_node"))
+  {
+  }
+
+  void on_init()
+  {
+    action_client_ = rclcpp_action::create_client<Fibonacci>(shared_from_this(), "fibonacci");
+    action_client_->wait_for_action_server();
+
+    deactivate_pub_ = this->create_publisher<std_msgs::msg::Empty>("deactivate_server", 1);
+    activate_pub_ = this->create_publisher<std_msgs::msg::Empty>("activate_server", 1);
+    omit_prempt_pub_ = this->create_publisher<std_msgs::msg::Empty>("omit_preemption", 1);
+  }
+
+  void on_term()
+  {
+    action_client_.reset();
+  }
+
+  void deactivate_server()
+  {
+    deactivate_pub_->publish(std_msgs::msg::Empty());
+  }
+
+  void activate_server()
+  {
+    activate_pub_->publish(std_msgs::msg::Empty());
+  }
+
+  void omit_server_preemptions()
+  {
+    omit_prempt_pub_->publish(std_msgs::msg::Empty());
+  }
+
+  rclcpp_action::Client<Fibonacci>::SharedPtr action_client_;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr deactivate_pub_;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr activate_pub_;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr omit_prempt_pub_;
+};
+
+class ActionTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    node_ = std::make_shared<ActionTestNode>();
+    node_->on_init();
+  }
+
+  void TearDown() override
+  {
+    std::cout << " Teardown" << std::endl;
+    node_->on_term();
+    std::cout << " Teardown..." << std::endl;
+    node_.reset();
+    std::cout << " Teardown complete" << std::endl;
+  }
+
+  std::shared_ptr<ActionTestNode> node_;
+};
+
+TEST_F(ActionTest, test_simple_action)
+{
+  node_->activate_server();
+
+  // The goal for this invocation
+  auto goal = Fibonacci::Goal();
+  goal.order = 12;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  // The final result
+  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+  EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  int sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  EXPECT_EQ(sum, 376);
+  SUCCEED();
+}
+
+TEST_F(ActionTest, test_simple_action_with_feedback)
+{
+  int feedback_sum = 0;
+
+  // A callback to accumulate the intermediate values
+  auto feedback_callback = [&feedback_sum](
+    rclcpp_action::ClientGoalHandle<Fibonacci>::SharedPtr /*goal_handle*/,
+    const std::shared_ptr<const Fibonacci::Feedback> feedback)
+    {
+      feedback_sum += feedback->sequence.back();
+    };
+
+  // The goal for this invocation
+  auto goal = Fibonacci::Goal();
+  goal.order = 10;
+
+  auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
+  send_goal_options.feedback_callback = feedback_callback;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal, send_goal_options);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_result), rclcpp::FutureReturnCode::SUCCESS);
+
+  // The final result
+  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+  EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  int sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  EXPECT_EQ(sum, 143);
+  EXPECT_GE(feedback_sum, 0);  // We should have received *some* feedback
+  SUCCEED();
+}
+
+TEST_F(ActionTest, test_simple_action_activation_cycling)
+{
+  // The goal for this invocation
+  auto goal = Fibonacci::Goal();
+
+  // Sending a goal that will take a long time to calculate
+  goal.order = 12'000'000;
+
+  // Start by sending goal on an active server
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  // Deactivate while running
+  node_->deactivate_server();
+
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  // The action should be reported as aborted.
+  EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::ABORTED);
+
+  // Cycle back to active
+  node_->activate_server();
+
+  goal.order = 12;
+
+  // Send the goal
+  future_goal_handle = node_->action_client_->async_send_goal(goal);
+  std::cout << "Sent goal, spinning til complete..." << std::endl;
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  future_result = node_->action_client_->async_get_result(goal_handle);
+  std::cout << "Getting result, spinning til complete..." << std::endl;
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  // Now the action should have been successfully executed.
+  EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::SUCCEEDED);
+  SUCCEED();
+}
+
+TEST_F(ActionTest, test_simple_action_preemption)
+{
+  // The goal for this invocation
+  auto goal = Fibonacci::Goal();
+
+  // Sending a goal that will take a long time to calculate
+  goal.order = 12'000'000;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  std::cout << "Sent goal, spinning til complete..." << std::endl;
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  // Preempt the goal
+  auto preemption_goal = Fibonacci::Goal();
+  preemption_goal.order = 1;
+
+  // Send the goal
+  future_goal_handle = node_->action_client_->async_send_goal(preemption_goal);
+  std::cout << "Sent goal, spinning til complete..." << std::endl;
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  std::cout << "Getting result, spinning til complete..." << std::endl;
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  // The final result
+  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+  EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  int sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  EXPECT_EQ(sum, 1);
+  SUCCEED();
+}
+
+TEST_F(ActionTest, test_simple_action_preemption_after_succeeded)
+{
+  // Test race condition between successfully completing an action and receiving a preemption.
+  auto goal = Fibonacci::Goal();
+  goal.order = 20;
+
+  auto preemption = Fibonacci::Goal();
+  preemption.order = 1;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  node_->omit_server_preemptions();
+
+  auto future_preempt_handle = node_->action_client_->async_send_goal(preemption);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  // Get the results
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result of initial goal
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  // The final result
+  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+  EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  int sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  EXPECT_EQ(sum, 17710);
+
+  // Now get the preemption result
+  goal_handle = future_preempt_handle.get();
+
+  // Wait for the result of initial goal
+  future_result = node_->action_client_->async_get_result(goal_handle);
+  ASSERT_EQ(
+    rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  // The final result
+  result = future_result.get();
+  EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  EXPECT_EQ(sum, 1);
+  SUCCEED();
+}
+
+TEST_F(ActionTest, test_handle_goal_deactivated)
+{
+  node_->deactivate_server();
+  auto goal = Fibonacci::Goal();
+  goal.order = 12;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  node_->activate_server();
+
+  SUCCEED();
+}
+
+TEST_F(ActionTest, test_handle_cancel)
+{
+  auto goal = Fibonacci::Goal();
+  goal.order = 14000000;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      future_goal_handle), rclcpp::FutureReturnCode::SUCCESS);
+
+  // Cancel the goal
+  auto cancel_response = node_->action_client_->async_cancel_goal(future_goal_handle.get());
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(
+      node_,
+      cancel_response), rclcpp::FutureReturnCode::SUCCESS);
+
+  // Check cancelled
+  EXPECT_EQ(future_goal_handle.get()->get_status(), rclcpp_action::GoalStatus::STATUS_CANCELING);
+
+  SUCCEED();
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  g_rclcppfixture.Setup();
+  ::testing::InitGoogleTest(&argc, argv);
+  auto result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  rclcpp::Rate(1).sleep();
+  return result;
+}

navigation2/nav2_util/test/test_execution_timer.cpp

@@ -0,0 +1,33 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <thread>
+
+#include "nav2_util/execution_timer.hpp"
+#include "gtest/gtest.h"
+
+using nav2_util::ExecutionTimer;
+using std::this_thread::sleep_for;
+using namespace std::chrono_literals;
+
+TEST(ExecutionTimer, BasicDelay)
+{
+  ExecutionTimer t;
+  t.start();
+  sleep_for(10ns);
+  t.end();
+  ASSERT_GE(t.elapsed_time(), 10ns);
+  ASSERT_GE(t.elapsed_time_in_seconds(), 1e-8);
+}

navigation2/nav2_util/test/test_geometry_utils.cpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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 "nav2_util/geometry_utils.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "gtest/gtest.h"
+
+using nav2_util::geometry_utils::euclidean_distance;
+using nav2_util::geometry_utils::calculate_path_length;
+
+TEST(GeometryUtils, euclidean_distance_point_3d)
+{
+  geometry_msgs::msg::Point point1;
+  point1.x = 3.0;
+  point1.y = 2.0;
+  point1.z = 1.0;
+
+  geometry_msgs::msg::Point point2;
+  point2.x = 1.0;
+  point2.y = 2.0;
+  point2.z = 3.0;
+
+  ASSERT_NEAR(euclidean_distance(point1, point2, true), 2.82843, 1e-5);
+}
+
+TEST(GeometryUtils, euclidean_distance_point_2d)
+{
+  geometry_msgs::msg::Point point1;
+  point1.x = 3.0;
+  point1.y = 2.0;
+  point1.z = 1.0;
+
+  geometry_msgs::msg::Point point2;
+  point2.x = 1.0;
+  point2.y = 2.0;
+  point2.z = 3.0;
+
+  ASSERT_NEAR(euclidean_distance(point1, point2), 2.0, 1e-5);
+}
+
+TEST(GeometryUtils, euclidean_distance_pose_3d)
+{
+  geometry_msgs::msg::Pose pose1;
+  pose1.position.x = 7.0;
+  pose1.position.y = 4.0;
+  pose1.position.z = 3.0;
+
+  geometry_msgs::msg::Pose pose2;
+  pose2.position.x = 17.0;
+  pose2.position.y = 6.0;
+  pose2.position.z = 2.0;
+
+  ASSERT_NEAR(euclidean_distance(pose1, pose2, true), 10.24695, 1e-5);
+}
+
+TEST(GeometryUtils, euclidean_distance_pose_2d)
+{
+  geometry_msgs::msg::Pose pose1;
+  pose1.position.x = 7.0;
+  pose1.position.y = 4.0;
+  pose1.position.z = 3.0;
+
+  geometry_msgs::msg::Pose pose2;
+  pose2.position.x = 17.0;
+  pose2.position.y = 6.0;
+  pose2.position.z = 2.0;
+
+  ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.19804, 1e-5);
+}
+
+TEST(GeometryUtils, calculate_path_length)
+{
+  nav_msgs::msg::Path straight_line_path;
+  size_t nb_path_points = 10;
+  float distance_between_poses = 2.0;
+  float current_x_loc = 0.0;
+
+  for (size_t i = 0; i < nb_path_points; ++i) {
+    geometry_msgs::msg::PoseStamped pose_stamped_msg;
+    pose_stamped_msg.pose.position.x = current_x_loc;
+
+    straight_line_path.poses.push_back(pose_stamped_msg);
+
+    current_x_loc += distance_between_poses;
+  }
+
+  ASSERT_NEAR(
+    calculate_path_length(straight_line_path),
+    (nb_path_points - 1) * distance_between_poses, 1e-5);
+
+  ASSERT_NEAR(
+    calculate_path_length(straight_line_path, straight_line_path.poses.size()),
+    0.0, 1e-5);
+
+  nav_msgs::msg::Path circle_path;
+  float polar_distance = 2.0;
+  uint32_t current_polar_angle_deg = 0;
+  constexpr float pi = 3.14159265358979;
+
+  while (current_polar_angle_deg != 360) {
+    float x_loc = polar_distance * std::cos(current_polar_angle_deg * (pi / 180.0));
+    float y_loc = polar_distance * std::sin(current_polar_angle_deg * (pi / 180.0));
+
+    geometry_msgs::msg::PoseStamped pose_stamped_msg;
+    pose_stamped_msg.pose.position.x = x_loc;
+    pose_stamped_msg.pose.position.y = y_loc;
+
+    circle_path.poses.push_back(pose_stamped_msg);
+
+    current_polar_angle_deg += 1;
+  }
+
+  ASSERT_NEAR(
+    calculate_path_length(circle_path),
+    2 * pi * polar_distance, 1e-1);
+}

navigation2/nav2_util/test/test_lifecycle_cli_node.cpp

@@ -0,0 +1,112 @@
+// Copyright (c) 2020 Samsung Research
+//
+// 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_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
+#define NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
+
+#include <cstdlib>
+#include <memory>
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#ifdef _WIN32
+#include <windows.h>
+#endif
+
+class DummyNode : public nav2_util::LifecycleNode
+{
+public:
+  DummyNode()
+  : nav2_util::LifecycleNode("nav2_test_cli", "")
+  {
+    activated = false;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/)
+  {
+    activated = true;
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  bool activated;
+};
+
+class Handle
+{
+public:
+  Handle()
+  {
+    node = std::make_shared<DummyNode>();
+    thread = std::make_shared<nav2_util::NodeThread>(node->get_node_base_interface());
+  }
+  ~Handle()
+  {
+    thread.reset();
+    node.reset();
+  }
+
+  std::shared_ptr<nav2_util::NodeThread> thread;
+  std::shared_ptr<DummyNode> node;
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  ~RclCppFixture()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+RclCppFixture g_rclcppfixture;
+
+TEST(LifeycleCLI, fails_no_node_name)
+{
+  Handle handle;
+  auto rc = system("ros2 run nav2_util lifecycle_bringup");
+  (void)rc;
+#ifdef _WIN32
+  Sleep(1000);
+#else
+  sleep(1);
+#endif
+  // check node didn't mode
+  EXPECT_EQ(handle.node->activated, false);
+  SUCCEED();
+}
+
+TEST(LifeycleCLI, succeeds_node_name)
+{
+  Handle handle;
+  auto rc = system("ros2 run nav2_util lifecycle_bringup nav2_test_cli");
+#ifdef _WIN32
+  Sleep(3000);
+#else
+  sleep(3);
+#endif
+  // check node moved
+  (void)rc;
+  EXPECT_EQ(handle.node->activated, true);
+  SUCCEED();
+}
+
+#endif  // NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_

navigation2/nav2_util/test/test_lifecycle_node.cpp

@@ -0,0 +1,85 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+// For the following two tests, if the LifecycleNode doesn't shut down properly,
+// the overall test will hang since the rclcpp thread will still be running,
+// preventing the executable from exiting (the test will hang)
+
+TEST(LifecycleNode, RclcppNodeExitsCleanly)
+{
+  // Make sure the node exits cleanly when using an rclcpp_node and associated thread
+  auto node1 = std::make_shared<nav2_util::LifecycleNode>("test_node", "");
+  std::this_thread::sleep_for(std::chrono::seconds(1));
+  SUCCEED();
+}
+
+TEST(LifecycleNode, MultipleRclcppNodesExitCleanly)
+{
+  // Try a couple nodes w/ rclcpp_node and threads
+  auto node1 = std::make_shared<nav2_util::LifecycleNode>("test_node1", "");
+  auto node2 = std::make_shared<nav2_util::LifecycleNode>("test_node2", "");
+
+  std::this_thread::sleep_for(std::chrono::seconds(1));
+  SUCCEED();
+}
+
+TEST(LifecycleNode, OnPreshutdownCbFires)
+{
+  // Ensure the on_rcl_preshutdown_cb fires
+
+  class MyNodeType : public nav2_util::LifecycleNode
+  {
+public:
+    MyNodeType(
+      const std::string & node_name)
+    : nav2_util::LifecycleNode(node_name) {}
+
+    bool fired = false;
+
+protected:
+    void on_rcl_preshutdown() override
+    {
+      fired = true;
+
+      nav2_util::LifecycleNode::on_rcl_preshutdown();
+    }
+  };
+
+  auto node = std::make_shared<MyNodeType>("test_node");
+
+  ASSERT_EQ(node->fired, false);
+
+  rclcpp::shutdown();
+
+  ASSERT_EQ(node->fired, true);
+
+  // Fire dtor to ensure nothing insane happens, e.g. exceptions.
+  node.reset();
+
+  SUCCEED();
+}

navigation2/nav2_util/test/test_lifecycle_utils.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <thread>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_utils.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using nav2_util::startup_lifecycle_nodes;
+using nav2_util::reset_lifecycle_nodes;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+void SpinNodesUntilDone(
+  std::vector<rclcpp_lifecycle::LifecycleNode::SharedPtr> nodes,
+  std::atomic<bool> * test_done)
+{
+  rclcpp::executors::SingleThreadedExecutor exec;
+  for (const auto & node : nodes) {
+    exec.add_node(node->get_node_base_interface());
+  }
+  while (rclcpp::ok() && !(*test_done)) {
+    exec.spin_some();
+  }
+}
+
+TEST(Lifecycle, interface)
+{
+  std::vector<rclcpp_lifecycle::LifecycleNode::SharedPtr> nodes;
+  nodes.push_back(rclcpp_lifecycle::LifecycleNode::make_shared("foo"));
+  nodes.push_back(rclcpp_lifecycle::LifecycleNode::make_shared("bar"));
+
+  std::atomic<bool> done(false);
+  std::thread node_thread(SpinNodesUntilDone, nodes, &done);
+  startup_lifecycle_nodes("/foo:/bar");
+  reset_lifecycle_nodes("/foo:/bar");
+  done = true;
+  node_thread.join();
+  SUCCEED();
+}

navigation2/nav2_util/test/test_node_utils.cpp

@@ -0,0 +1,128 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "nav2_util/node_utils.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+
+using nav2_util::sanitize_node_name;
+using nav2_util::generate_internal_node_name;
+using nav2_util::generate_internal_node;
+using nav2_util::add_namespaces;
+using nav2_util::time_to_string;
+using nav2_util::declare_parameter_if_not_declared;
+using nav2_util::get_plugin_type_param;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(SanitizeNodeName, SanitizeNodeName)
+{
+  ASSERT_EQ(sanitize_node_name("bar"), "bar");
+  ASSERT_EQ(sanitize_node_name("/foo/bar"), "_foo_bar");
+}
+
+TEST(TimeToString, IsLengthCorrect)
+{
+  ASSERT_EQ(time_to_string(0).length(), 0u);
+  ASSERT_EQ(time_to_string(1).length(), 1u);
+  ASSERT_EQ(time_to_string(10).length(), 10u);
+  ASSERT_EQ(time_to_string(20)[0], '0');
+}
+
+TEST(TimeToString, TimeToStringDifferent)
+{
+  auto time1 = time_to_string(8);
+  auto time2 = time_to_string(8);
+  ASSERT_NE(time1, time2);
+}
+
+TEST(GenerateInternalNodeName, GenerateNodeName)
+{
+  auto defaultName = generate_internal_node_name();
+  ASSERT_EQ(defaultName[0], '_');
+  ASSERT_EQ(defaultName.length(), 9u);
+}
+
+TEST(AddNamespaces, AddNamespaceSlash)
+{
+  ASSERT_EQ(add_namespaces("hi", "bye"), "hi/bye");
+  ASSERT_EQ(add_namespaces("hi/", "bye"), "/hi/bye");
+}
+
+TEST(DeclareParameterIfNotDeclared, DeclareParameterIfNotDeclared)
+{
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  std::string param;
+
+  // test declared parameter
+  node->declare_parameter("foobar", "foo");
+  declare_parameter_if_not_declared(node, "foobar", rclcpp::ParameterValue{"bar"});
+  node->get_parameter("foobar", param);
+  ASSERT_EQ(param, "foo");
+
+  // test undeclared parameter
+  declare_parameter_if_not_declared(node, "waldo", rclcpp::ParameterValue{"fred"});
+  node->get_parameter("waldo", param);
+  ASSERT_EQ(param, "fred");
+}
+
+TEST(GetPluginTypeParam, GetPluginTypeParam)
+{
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  node->declare_parameter("Foo.plugin", "bar");
+  ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar");
+  ASSERT_EXIT(get_plugin_type_param(node, "Waldo"), ::testing::ExitedWithCode(255), ".*");
+}
+
+TEST(TestParamCopying, TestParamCopying)
+{
+  auto node1 = std::make_shared<rclcpp::Node>("test_node1");
+  auto node2 = std::make_shared<rclcpp::Node>("test_node2");
+
+  // Tests for (1) multiple types, (2) recursion, (3) overriding values
+  node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1"))));
+  node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123));
+  node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar"))));
+  node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve"))));
+  node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2"))));
+
+  // Show Node2 is empty of Node1's parameters, but contains its own
+  EXPECT_FALSE(node2->has_parameter("Foo1"));
+  EXPECT_FALSE(node2->has_parameter("Foo2"));
+  EXPECT_FALSE(node2->has_parameter("Foo.bar"));
+  EXPECT_TRUE(node2->has_parameter("Foo"));
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2"));
+
+  nav2_util::copy_all_parameters(node1, node2);
+
+  // Test new parameters exist, of expected value, and original param is not overridden
+  EXPECT_TRUE(node2->has_parameter("Foo1"));
+  EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1"));
+  EXPECT_TRUE(node2->has_parameter("Foo2"));
+  EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123);
+  EXPECT_TRUE(node2->has_parameter("Foo.bar"));
+  EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve"));
+  EXPECT_TRUE(node2->has_parameter("Foo"));
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2"));
+}

navigation2/nav2_util/test/test_odometry_utils.cpp

@@ -0,0 +1,115 @@
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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 <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/odometry_utils.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "gtest/gtest.h"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(OdometryUtils, test_smoothed_velocity)
+{
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  auto odom_pub = node->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
+
+  nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom");
+
+  nav_msgs::msg::Odometry odom_msg;
+  geometry_msgs::msg::Twist twist_msg;
+
+  auto time = node->now();
+
+  odom_msg.header.stamp = time;
+  odom_msg.twist.twist.linear.x = 1.0;
+  odom_msg.twist.twist.linear.y = 1.0;
+  odom_msg.twist.twist.angular.z = 1.0;
+
+  odom_pub->publish(odom_msg);
+  rclcpp::spin_some(node);
+
+  twist_msg = odom_smoother.getTwist();
+  EXPECT_EQ(twist_msg.linear.x, 1.0);
+  EXPECT_EQ(twist_msg.linear.y, 1.0);
+  EXPECT_EQ(twist_msg.angular.z, 1.0);
+
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.1);
+  odom_msg.twist.twist.linear.x = 2.0;
+  odom_msg.twist.twist.linear.y = 2.0;
+  odom_msg.twist.twist.angular.z = 2.0;
+  odom_pub->publish(odom_msg);
+
+  std::this_thread::sleep_for(100ms);
+  rclcpp::spin_some(node);
+
+  twist_msg = odom_smoother.getTwist();
+  EXPECT_EQ(twist_msg.linear.x, 1.5);
+  EXPECT_EQ(twist_msg.linear.y, 1.5);
+  EXPECT_EQ(twist_msg.angular.z, 1.5);
+
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2);
+  odom_msg.twist.twist.linear.x = 3.0;
+  odom_msg.twist.twist.linear.y = 3.0;
+  odom_msg.twist.twist.angular.z = 3.0;
+  odom_pub->publish(odom_msg);
+
+  std::this_thread::sleep_for(100ms);
+  rclcpp::spin_some(node);
+
+  twist_msg = odom_smoother.getTwist();
+  EXPECT_EQ(twist_msg.linear.x, 2.0);
+  EXPECT_EQ(twist_msg.linear.y, 2.0);
+  EXPECT_EQ(twist_msg.angular.z, 2.0);
+
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.45);
+  odom_msg.twist.twist.linear.x = 4.0;
+  odom_msg.twist.twist.linear.y = 4.0;
+  odom_msg.twist.twist.angular.z = 4.0;
+  odom_pub->publish(odom_msg);
+
+  std::this_thread::sleep_for(100ms);
+  rclcpp::spin_some(node);
+
+  twist_msg = odom_smoother.getTwist();
+  EXPECT_EQ(twist_msg.linear.x, 3.5);
+  EXPECT_EQ(twist_msg.linear.y, 3.5);
+  EXPECT_EQ(twist_msg.angular.z, 3.5);
+
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(1.0);
+  odom_msg.twist.twist.linear.x = 5.0;
+  odom_msg.twist.twist.linear.y = 5.0;
+  odom_msg.twist.twist.angular.z = 5.0;
+  odom_pub->publish(odom_msg);
+
+  std::this_thread::sleep_for(100ms);
+  rclcpp::spin_some(node);
+
+  twist_msg = odom_smoother.getTwist();
+  EXPECT_EQ(twist_msg.linear.x, 5.0);
+  EXPECT_EQ(twist_msg.linear.y, 5.0);
+  EXPECT_EQ(twist_msg.angular.z, 5.0);
+}

navigation2/nav2_util/test/test_robot_utils.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2020 Samsung Research
+//
+// 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 <cmath>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "gtest/gtest.h"
+#include "nav2_util/node_thread.hpp"
+#include "tf2_ros/create_timer_ros.h"
+
+TEST(RobotUtils, LookupExceptionError)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>("name", rclcpp::NodeOptions());
+  geometry_msgs::msg::PoseStamped global_pose;
+  tf2_ros::Buffer tf(node->get_clock());
+  ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1));
+  global_pose.header.frame_id = "base_link";
+  ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1));
+}
+
+TEST(RobotUtils, validateTwist)
+{
+  geometry_msgs::msg::Twist msg;
+  EXPECT_TRUE(nav2_util::validateTwist(msg));
+
+  msg.linear.x = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.linear.x = 1;
+  msg.linear.y = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.linear.y = 1;
+  msg.linear.z = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+
+  msg.linear.z = 1;
+  msg.angular.x = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.angular.x = 1;
+  msg.angular.y = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+  msg.angular.y = 1;
+  msg.angular.z = NAN;
+  EXPECT_FALSE(nav2_util::validateTwist(msg));
+}

navigation2/nav2_util/test/test_service_client.cpp

@@ -0,0 +1,99 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+#include "nav2_util/service_client.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "gtest/gtest.h"
+
+using nav2_util::ServiceClient;
+using std::string;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
+{
+public:
+  TestServiceClient(
+    const std::string & name,
+    const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr())
+  : ServiceClient(name, provided_node) {}
+
+  string name() {return node_->get_name();}
+  const rclcpp::Node::SharedPtr & getNode() {return node_;}
+};
+
+TEST(ServiceClient, can_ServiceClient_use_passed_in_node)
+{
+  auto node = rclcpp::Node::make_shared("test_node");
+  TestServiceClient t("bar", node);
+  ASSERT_EQ(t.getNode(), node);
+  ASSERT_EQ(t.name(), "test_node");
+}
+
+TEST(ServiceClient, can_ServiceClient_invoke_in_callback)
+{
+  int a = 0;
+  auto service_node = rclcpp::Node::make_shared("service_node");
+  auto service = service_node->create_service<std_srvs::srv::Empty>(
+    "empty_srv",
+    [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) {
+      a = 1;
+    });
+  auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);});
+
+  auto pub_node = rclcpp::Node::make_shared("pub_node");
+  auto pub = pub_node->create_publisher<std_msgs::msg::Empty>(
+    "empty_topic",
+    rclcpp::QoS(1).transient_local());
+  auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);});
+
+  auto sub_node = rclcpp::Node::make_shared("sub_node");
+  ServiceClient<std_srvs::srv::Empty> client("empty_srv", sub_node);
+  auto sub = sub_node->create_subscription<std_msgs::msg::Empty>(
+    "empty_topic",
+    rclcpp::QoS(1),
+    [&client](std_msgs::msg::Empty::SharedPtr) {
+      auto req = std::make_shared<std_srvs::srv::Empty::Request>();
+      auto res = client.invoke(req);
+    });
+
+  pub->publish(std_msgs::msg::Empty());
+  rclcpp::spin_some(sub_node);
+
+  rclcpp::shutdown();
+  srv_thread.join();
+  pub_thread.join();
+  ASSERT_EQ(a, 1);
+}
+
+TEST(ServiceClient, can_ServiceClient_timeout)
+{
+  rclcpp::init(0, nullptr);
+  auto node = rclcpp::Node::make_shared("test_node");
+  TestServiceClient t("bar", node);
+  rclcpp::spin_some(node);
+  bool ready = t.wait_for_service(std::chrono::milliseconds(10));
+  rclcpp::shutdown();
+  ASSERT_EQ(ready, false);
+}

navigation2/nav2_util/test/test_string_utils.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include "nav2_util/string_utils.hpp"
+#include "gtest/gtest.h"
+
+using nav2_util::split;
+using nav2_util::Tokens;
+
+TEST(Split, SplitFunction)
+{
+  ASSERT_EQ(split("", ':'), Tokens({""}));
+  ASSERT_EQ(split("foo", ':'), Tokens{"foo"});
+  ASSERT_EQ(split("foo:bar", ':'), Tokens({"foo", "bar"}));
+  ASSERT_EQ(split("foo:bar:", ':'), Tokens({"foo", "bar", ""}));
+  ASSERT_EQ(split(":", ':'), Tokens({"", ""}));
+  ASSERT_EQ(split("foo::bar", ':'), Tokens({"foo", "", "bar"}));
+  ASSERT_TRUE(nav2_util::strip_leading_slash(std::string("/hi")) == std::string("hi"));
+}

navigation2/nav2_util/test/test_validation_messages.cpp

@@ -0,0 +1,368 @@
+// Copyright (c) 2024 GoesM
+//
+// 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_util/validate_messages.hpp"
+
+TEST(ValidateMessagesTest, DoubleValueCheck) {
+  // Test valid double value
+  EXPECT_TRUE(nav2_util::validateMsg(3.14));
+  // Test invalid double value (infinity)
+  EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits<double>::infinity()));
+  // Test invalid double value (NaN)
+  EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits<double>::quiet_NaN()));
+}
+
+TEST(ValidateMessagesTest, TimeStampCheck)
+{
+  // Test valid time stamp
+  builtin_interfaces::msg::Time valid_time_stamp;
+  valid_time_stamp.sec = 123;
+  valid_time_stamp.nanosec = 456789;
+  EXPECT_TRUE(nav2_util::validateMsg(valid_time_stamp));
+  // Test invalid time stamp (nanosec out of range)
+  builtin_interfaces::msg::Time invalid_time_stamp;
+  invalid_time_stamp.sec = 123;
+  invalid_time_stamp.nanosec = 1e9;   // 1 second = 1e9 nanoseconds
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_time_stamp));
+}
+
+TEST(ValidateMessagesTest, HeaderCheck)
+{
+  // Test valid header with non-empty frame_id
+  std_msgs::msg::Header valid_header;
+  valid_header.stamp.sec = 123;
+  valid_header.stamp.nanosec = 456789;
+  valid_header.frame_id = "map";
+  EXPECT_TRUE(nav2_util::validateMsg(valid_header));
+  // Test invalid header with empty frame_id
+  std_msgs::msg::Header invalid_header;
+  invalid_header.stamp.sec = 123;
+  invalid_header.stamp.nanosec = 456789;
+  invalid_header.frame_id = "";
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_header));
+  invalid_header.stamp.sec = 123;
+  invalid_header.stamp.nanosec = 1e9;
+  invalid_header.frame_id = "map";
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_header));
+}
+
+TEST(ValidateMessagesTest, PointCheck)
+{
+  // Test valid Point message
+  geometry_msgs::msg::Point valid_point;
+  valid_point.x = 1.0;
+  valid_point.y = 2.0;
+  valid_point.z = 3.0;
+  EXPECT_TRUE(nav2_util::validateMsg(valid_point));
+  // Test invalid Point message with NaN value
+  geometry_msgs::msg::Point invalid_point;
+  invalid_point.x = 1.0;
+  invalid_point.y = std::numeric_limits<double>::quiet_NaN();
+  invalid_point.z = 3.0;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_point));
+  // Test invalid Point message with NaN value
+  invalid_point.x = std::numeric_limits<double>::quiet_NaN();
+  invalid_point.y = 2.0;
+  invalid_point.z = 3.0;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_point));
+  // Test invalid Point message with NaN value
+  invalid_point.x = 1.0;
+  invalid_point.y = 2.0;
+  invalid_point.z = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_point));
+}
+
+TEST(ValidateMessagesTest, QuaternionCheck)
+{
+  // Test valid Quaternion message
+  geometry_msgs::msg::Quaternion valid_quaternion;
+  valid_quaternion.x = 0.0;
+  valid_quaternion.y = 0.0;
+  valid_quaternion.z = 0.0;
+  valid_quaternion.w = 1.0;
+  EXPECT_TRUE(nav2_util::validateMsg(valid_quaternion));
+  // Test invalid Quaternion message with invalid magnitude
+  geometry_msgs::msg::Quaternion invalid_quaternion;
+  invalid_quaternion.x = 0.1;
+  invalid_quaternion.y = 0.2;
+  invalid_quaternion.z = 0.3;
+  invalid_quaternion.w = 0.5;   // Invalid magnitude (should be 1.0)
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion));
+
+  // One NaN value
+  invalid_quaternion.x = 0.0;
+  invalid_quaternion.y = std::numeric_limits<double>::quiet_NaN();
+  invalid_quaternion.z = 0.0;
+  invalid_quaternion.w = 1.0;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion));
+  invalid_quaternion.x = std::numeric_limits<double>::quiet_NaN();
+  invalid_quaternion.y = 0.0;
+  invalid_quaternion.z = 0.0;
+  invalid_quaternion.w = 1.0;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion));
+  invalid_quaternion.x = 0.0;
+  invalid_quaternion.y = 0.0;
+  invalid_quaternion.z = std::numeric_limits<double>::quiet_NaN();
+  invalid_quaternion.w = 1.0;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion));
+  invalid_quaternion.x = 0.0;
+  invalid_quaternion.y = 0.0;
+  invalid_quaternion.z = 1.0;
+  invalid_quaternion.w = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion));
+}
+
+TEST(ValidateMessagesTest, PoseCheck)
+{
+  // Test valid Pose message
+  geometry_msgs::msg::Pose valid_pose;
+  valid_pose.position.x = 1.0;
+  valid_pose.position.y = 2.0;
+  valid_pose.position.z = 3.0;
+  valid_pose.orientation.x = 1.0;
+  valid_pose.orientation.y = 0.0;
+  valid_pose.orientation.z = 0.0;
+  valid_pose.orientation.w = 0.0;
+  EXPECT_TRUE(nav2_util::validateMsg(valid_pose));
+  // Test invalid Pose message with invalid position
+  geometry_msgs::msg::Pose invalid_pose;
+  invalid_pose.position.x = 1.0;
+  invalid_pose.position.y = std::numeric_limits<double>::quiet_NaN();
+  invalid_pose.position.z = 3.0;
+  invalid_pose.orientation.x = 1.0;
+  invalid_pose.orientation.y = 0.0;
+  invalid_pose.orientation.z = 0.0;
+  invalid_pose.orientation.w = 0.0;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_pose));
+  // Test invalid Pose message with invalid orientation
+  invalid_pose.position.x = 1.0;
+  invalid_pose.position.y = 2.0;
+  invalid_pose.position.z = 3.0;
+  invalid_pose.orientation.x = 0.1;
+  invalid_pose.orientation.y = 0.2;
+  invalid_pose.orientation.z = 0.3;
+  invalid_pose.orientation.w = 0.4;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_pose));
+}
+
+
+TEST(ValidateMessagesTest, MapMetaDataCheck) {
+  // Test valid MapMetaData message
+  nav_msgs::msg::MapMetaData valid_map_meta_data;
+  valid_map_meta_data.resolution = 0.05;
+  valid_map_meta_data.width = 100;
+  valid_map_meta_data.height = 100;
+  geometry_msgs::msg::Pose valid_origin;
+  valid_origin.position.x = 0.0;
+  valid_origin.position.y = 0.0;
+  valid_origin.position.z = 0.0;
+  valid_origin.orientation.x = 0.0;
+  valid_origin.orientation.y = 0.0;
+  valid_origin.orientation.z = 0.0;
+  valid_origin.orientation.w = 1.0;
+  valid_map_meta_data.origin = valid_origin;
+  EXPECT_TRUE(nav2_util::validateMsg(valid_map_meta_data));
+
+  // Test invalid origin message
+  nav_msgs::msg::MapMetaData invalid_map_meta_data;
+  invalid_map_meta_data.resolution = 100.0;
+  invalid_map_meta_data.width = 100;
+  invalid_map_meta_data.height = 100;
+  geometry_msgs::msg::Pose invalid_origin;
+  invalid_origin.position.x = 0.0;
+  invalid_origin.position.y = 0.0;
+  invalid_origin.position.z = 0.0;
+  invalid_origin.orientation.x = 0.0;
+  invalid_origin.orientation.y = 0.0;
+  invalid_origin.orientation.z = 1.0;
+  invalid_origin.orientation.w = 1.0;
+  invalid_map_meta_data.origin = invalid_origin;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data));
+
+  // Test invalid resolution message
+  invalid_map_meta_data.resolution = std::numeric_limits<double>::quiet_NaN();
+  invalid_map_meta_data.width = 100;
+  invalid_map_meta_data.height = 100;
+  invalid_map_meta_data.origin = valid_origin;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data));
+
+  // Test invalid MapMetaData message with zero width
+  invalid_map_meta_data.resolution = 0.05;
+  invalid_map_meta_data.width = 0;
+  invalid_map_meta_data.height = 100;
+  invalid_map_meta_data.origin = valid_origin;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data));
+}
+
+TEST(ValidateMessagesTest, OccupancyGridCheck) {
+  // Test valid OccupancyGrid message
+  nav_msgs::msg::OccupancyGrid valid_occupancy_grid;
+  valid_occupancy_grid.header.frame_id = "map";
+  valid_occupancy_grid.info.resolution = 0.05;
+  valid_occupancy_grid.info.width = 100;
+  valid_occupancy_grid.info.height = 100;
+  std::vector<int8_t> data(100 * 100, 0);   // Initialize with zeros
+  valid_occupancy_grid.data = data;
+  EXPECT_TRUE(nav2_util::validateMsg(valid_occupancy_grid));
+
+  // Test invalid header message with wrong data size
+  nav_msgs::msg::OccupancyGrid invalid_occupancy_grid;
+  invalid_occupancy_grid.header.frame_id = "";    // Incorrect id
+  invalid_occupancy_grid.info.resolution = 0.05;
+  invalid_occupancy_grid.info.width = 100;
+  invalid_occupancy_grid.info.height = 100;
+  invalid_occupancy_grid.data = data;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid));
+
+  // Test invalid info message with wrong data size
+  invalid_occupancy_grid.header.frame_id = "map";
+  invalid_occupancy_grid.info.resolution = 0.05;
+  invalid_occupancy_grid.info.width = 0;    // Incorrect width
+  invalid_occupancy_grid.info.height = 100;
+  invalid_occupancy_grid.data = data;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid));
+
+  // Test invalid OccupancyGrid message with wrong data size
+  invalid_occupancy_grid.header.frame_id = "map";
+  invalid_occupancy_grid.info.resolution = 0.05;
+  invalid_occupancy_grid.info.width = 100;
+  invalid_occupancy_grid.info.height = 100;
+  std::vector<int8_t> invalid_data(100 * 99, 0);   // Incorrect data size
+  invalid_occupancy_grid.data = invalid_data;
+  EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid));
+}
+
+TEST(ValidateMessagesTest, PoseWithCovarianceCheck) {
+  // Valid message
+  geometry_msgs::msg::PoseWithCovariance validate_msg;
+  validate_msg.covariance[0] = 0.25;
+  // assign other covariance values...
+  validate_msg.covariance[35] = 0.06853891909122467;
+
+  validate_msg.pose.position.x = 0.50010401010515571;
+  validate_msg.pose.position.y = 1.7468730211257935;
+  validate_msg.pose.position.z = 0.0;
+
+  validate_msg.pose.orientation.x = 0.9440542194053062;
+  validate_msg.pose.orientation.y = 0.0;
+  validate_msg.pose.orientation.z = 0.0;
+  validate_msg.pose.orientation.w = -0.32979028309372299;
+
+  EXPECT_TRUE(nav2_util::validateMsg(validate_msg));
+
+  // Invalid messages
+  geometry_msgs::msg::PoseWithCovariance invalidate_msg1;
+  invalidate_msg1.covariance[0] = 0.25;
+  // assign other covariance values...
+  invalidate_msg1.covariance[7] = NAN;
+  invalidate_msg1.covariance[9] = NAN;
+  invalidate_msg1.covariance[35] = 0.06853891909122467;
+
+  invalidate_msg1.pose.position.x = 0.50010401010515571;
+  invalidate_msg1.pose.position.y = 1.7468730211257935;
+  invalidate_msg1.pose.position.z = 0.0;
+
+  invalidate_msg1.pose.orientation.x = 0.9440542194053062;
+  invalidate_msg1.pose.orientation.y = 0.0;
+  invalidate_msg1.pose.orientation.z = 0.0;
+  invalidate_msg1.pose.orientation.w = -0.32979028309372299;
+
+  EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1));
+
+  geometry_msgs::msg::PoseWithCovariance invalidate_msg2;
+  invalidate_msg2.covariance[0] = 0.25;
+  // assign other covariance values...
+  invalidate_msg2.covariance[35] = 0.06853891909122467;
+
+  invalidate_msg2.pose.position.x = NAN;
+  invalidate_msg2.pose.position.y = 1.7468730211257935;
+  invalidate_msg2.pose.position.z = 0.0;
+
+  invalidate_msg2.pose.orientation.x = 0.9440542194053062;
+  invalidate_msg2.pose.orientation.y = 0.0;
+  invalidate_msg2.pose.orientation.z = 0.0;
+  invalidate_msg2.pose.orientation.w = -0.32979028309372299;
+
+  EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2));
+}
+
+TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) {
+  // Valid message
+  geometry_msgs::msg::PoseWithCovarianceStamped validate_msg;
+  validate_msg.header.frame_id = "map";
+  validate_msg.header.stamp.sec = 1711029956;
+  validate_msg.header.stamp.nanosec = 146734875;
+
+  validate_msg.pose.covariance[0] = 0.25;
+  // assign other covariance values...
+  validate_msg.pose.covariance[35] = 0.06853891909122467;
+
+  validate_msg.pose.pose.position.x = 0.50010401010515571;
+  validate_msg.pose.pose.position.y = 1.7468730211257935;
+  validate_msg.pose.pose.position.z = 0.0;
+
+  validate_msg.pose.pose.orientation.x = 0.9440542194053062;
+  validate_msg.pose.pose.orientation.y = 0.0;
+  validate_msg.pose.pose.orientation.z = 0.0;
+  validate_msg.pose.pose.orientation.w = -0.32979028309372299;
+
+  EXPECT_TRUE(nav2_util::validateMsg(validate_msg));
+
+  // Invalid messages
+  geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1;
+  invalidate_msg1.header.frame_id = "map";
+  invalidate_msg1.header.stamp.sec = 1711029956;
+  invalidate_msg1.header.stamp.nanosec = 146734875;
+
+  invalidate_msg1.pose.covariance[0] = 0.25;
+  // assign other covariance values...
+  invalidate_msg1.pose.covariance[7] = NAN;
+  invalidate_msg1.pose.covariance[9] = NAN;
+  invalidate_msg1.pose.covariance[35] = 0.06853891909122467;
+
+  invalidate_msg1.pose.pose.position.x = 0.50010401010515571;
+  invalidate_msg1.pose.pose.position.y = 1.7468730211257935;
+  invalidate_msg1.pose.pose.position.z = 0.0;
+
+  invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062;
+  invalidate_msg1.pose.pose.orientation.y = 0.0;
+  invalidate_msg1.pose.pose.orientation.z = 0.0;
+  invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299;
+
+  EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1));
+
+  geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2;
+  invalidate_msg2.header.frame_id = "";
+  invalidate_msg2.header.stamp.sec = 1711029956;
+  invalidate_msg2.header.stamp.nanosec = 146734875;
+
+  invalidate_msg2.pose.covariance[0] = 0.25;
+  // assign other covariance values...
+  invalidate_msg2.pose.covariance[35] = 0.06853891909122467;
+
+  invalidate_msg2.pose.pose.position.x = 0.50010401010515571;
+  invalidate_msg2.pose.pose.position.y = 1.7468730211257935;
+  invalidate_msg2.pose.pose.position.z = 0.0;
+
+  invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062;
+  invalidate_msg2.pose.pose.orientation.y = 0.0;
+  invalidate_msg2.pose.pose.orientation.z = 0.0;
+  invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299;
+
+  EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2));
+}
+
+
+// Add more test cases for other validateMsg functions if needed