add humble-navigation2

navigation2/nav2_util/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_util)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(bondcpp REQUIRED)
+find_package(bond REQUIRED)
+find_package(action_msgs REQUIRED)
+
+set(dependencies
+    nav2_msgs
+    tf2_ros
+    tf2
+    tf2_geometry_msgs
+    geometry_msgs
+    nav_msgs
+    rclcpp
+    lifecycle_msgs
+    rclcpp_action
+    rclcpp_lifecycle
+    bondcpp
+    bond
+    action_msgs
+    rcl_interfaces
+)
+
+nav2_package()
+
+include_directories(include)
+
+set(library_name ${PROJECT_NAME}_core)
+add_subdirectory(src)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+ament_package()

navigation2/nav2_util/README.md

@@ -0,0 +1,12 @@
+# Nav2 Util
+
+The `nav2_util` package contains utilities abstracted from individual packages which may find use in other uses. Some examples of things you'll find here:
+
+- Geometry utilities for computing distances and values in paths
+- A Nav2 specific lifecycle node wrapper for boilerplate code and useful common utilities like `declare_parameter_if_not_declared()`
+- Simplified service clients
+- Simplified action servers
+- Transformation and robot pose helpers
+
+The long-term aim is for these utilities to find more permanent homes in other packages (within and outside of Nav2) or migrate to the raw tools made available in ROS 2.
+ 

navigation2/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp

@@ -0,0 +1,49 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
+#define NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
+
+#include <string>
+#include "nav2_util/service_client.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "nav2_msgs/srv/clear_entire_costmap.hpp"
+
+namespace nav2_util
+{
+/**
+ * @class nav2_util::ClearEntirelyCostmapServiceClient
+ * @brief A service client to clear costmaps entirely
+ */
+class ClearEntirelyCostmapServiceClient
+  : public nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  /**
+* @brief A constructor for nav2_util::ClearEntirelyCostmapServiceClient
+   */
+  explicit ClearEntirelyCostmapServiceClient(const std::string & service_name)
+  : nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>(service_name)
+  {
+  }
+
+  using clearEntirelyCostmapServiceRequest =
+    nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>::RequestType;
+  using clearEntirelyCostmapServiceResponse =
+    nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>::ResponseType;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_

navigation2/nav2_util/include/nav2_util/costmap.hpp

@@ -0,0 +1,140 @@
+// 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.
+
+#ifndef NAV2_UTIL__COSTMAP_HPP_
+#define NAV2_UTIL__COSTMAP_HPP_
+
+#include <vector>
+#include <cstdint>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/msg/costmap_meta_data.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+namespace nav2_util
+{
+
+enum class TestCostmap
+{
+  open_space,
+  bounded,
+  bottom_left_obstacle,
+  top_left_obstacle,
+  maze1,
+  maze2
+};
+
+/**
+ * @class nav2_util::Costmap
+ * @brief Class for a single layered costmap initialized from an
+ * occupancy grid representing the map.
+ */
+class Costmap
+{
+public:
+  typedef uint8_t CostValue;
+
+  /**
+   * @brief A constructor for nav2_util::Costmap
+   * @param node Ptr to a node
+   * @param trinary_costmap Whether the costmap should be trinary
+   * @param track_unknown_space Whether to track unknown space in costmap
+   * @param lethal_threshold The lethal space cost threshold to use
+   * @param unknown_cost_value Internal costmap cell value for unknown space
+   */
+  Costmap(
+    rclcpp::Node * node, bool trinary_costmap = true, bool track_unknown_space = true,
+    int lethal_threshold = 100, int unknown_cost_value = -1);
+  Costmap() = delete;
+  ~Costmap();
+
+  /**
+   * @brief Set the static map of this costmap
+   * @param occupancy_grid Occupancy grid to populate this costmap with
+   */
+  void set_static_map(const nav_msgs::msg::OccupancyGrid & occupancy_grid);
+
+  /**
+   * @brief Set the test costmap type of this costmap
+   * @param testCostmapType Type of stored costmap to use
+   */
+  void set_test_costmap(const TestCostmap & testCostmapType);
+
+  /**
+   * @brief Get a costmap message from this object
+   * @param specifications Parameters of costmap
+   * @return Costmap msg of this costmap
+   */
+  nav2_msgs::msg::Costmap get_costmap(const nav2_msgs::msg::CostmapMetaData & specifications);
+
+  /**
+   * @brief Get a metadata message from this object
+   * @return Costmap metadata of this costmap
+   */
+  nav2_msgs::msg::CostmapMetaData get_properties() {return costmap_properties_;}
+
+  /**
+   * @brief Get whether some coordinates are free
+   * @return bool if free
+   */
+  bool is_free(const unsigned int x_coordinate, const unsigned int y_coordinate) const;
+
+  /**
+   * @brief Get whether some index in the costmap is free
+   * @return bool if free
+   */
+  bool is_free(const unsigned int index) const;
+
+  // Mapping for often used cost values
+  static const CostValue no_information;
+  static const CostValue lethal_obstacle;
+  static const CostValue inscribed_inflated_obstacle;
+  static const CostValue medium_cost;
+  static const CostValue free_space;
+
+private:
+  /**
+   * @brief Get data from the test
+   * @return data
+   */
+  std::vector<uint8_t> get_test_data(const TestCostmap configuration);
+
+  /**
+   * @brief Get the interpreted value in the costmap
+   * @return uint value
+   */
+  uint8_t interpret_value(const int8_t value) const;
+
+  // Costmap isn't itself a node
+  rclcpp::Node * node_;
+
+  // TODO(orduno): For now, only holding costs from static map
+  nav2_msgs::msg::CostmapMetaData costmap_properties_;
+  std::vector<uint8_t> costs_;
+
+  // Static layer parameters
+  bool trinary_costmap_;
+  bool track_unknown_space_;
+  int lethal_threshold_;
+  int unknown_cost_value_;
+
+  // Flags to determine the origin of the costmap
+  bool map_provided_;
+  bool using_test_map_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__COSTMAP_HPP_

navigation2/nav2_util/include/nav2_util/execution_timer.hpp

@@ -0,0 +1,52 @@
+// 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.
+
+#ifndef NAV2_UTIL__EXECUTION_TIMER_HPP_
+#define NAV2_UTIL__EXECUTION_TIMER_HPP_
+
+#include <chrono>
+
+namespace nav2_util
+{
+
+/// @brief Measures execution time of code between calls to start and end
+class ExecutionTimer
+{
+public:
+  using Clock = std::chrono::high_resolution_clock;
+  using nanoseconds = std::chrono::nanoseconds;
+
+  /// @brief Call just prior to code you want to measure
+  void start() {start_ = Clock::now();}
+
+  /// @brief Call just after the code you want to measure
+  void end() {end_ = Clock::now();}
+
+  /// @brief Extract the measured time as an integral std::chrono::duration object
+  nanoseconds elapsed_time() {return end_ - start_;}
+
+  /// @brief Extract the measured time as a floating point number of seconds.
+  double elapsed_time_in_seconds()
+  {
+    return std::chrono::duration<double>(end_ - start_).count();
+  }
+
+protected:
+  Clock::time_point start_;
+  Clock::time_point end_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__EXECUTION_TIMER_HPP_

navigation2/nav2_util/include/nav2_util/geometry_utils.hpp

@@ -0,0 +1,185 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__GEOMETRY_UTILS_HPP_
+#define NAV2_UTIL__GEOMETRY_UTILS_HPP_
+
+#include <cmath>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace nav2_util
+{
+namespace geometry_utils
+{
+
+/**
+ * @brief Get a geometry_msgs Quaternion from a yaw angle
+ * @param angle Yaw angle to generate a quaternion from
+ * @return geometry_msgs Quaternion
+ */
+inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
+{
+  tf2::Quaternion q;
+  q.setRPY(0, 0, angle);  // void returning function
+  return tf2::toMsg(q);
+}
+
+/**
+ * @brief Get the euclidean distance between 2 geometry_msgs::Points
+ * @param pos1 First point
+ * @param pos1 Second point
+ * @param is_3d True if a true L2 distance is desired (default false)
+ * @return double L2 distance
+ */
+inline double euclidean_distance(
+  const geometry_msgs::msg::Point & pos1,
+  const geometry_msgs::msg::Point & pos2,
+  const bool is_3d = false)
+{
+  double dx = pos1.x - pos2.x;
+  double dy = pos1.y - pos2.y;
+
+  if (is_3d) {
+    double dz = pos1.z - pos2.z;
+    return std::hypot(dx, dy, dz);
+  }
+
+  return std::hypot(dx, dy);
+}
+
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::Poses
+ * @param pos1 First pose
+ * @param pos1 Second pose
+ * @param is_3d True if a true L2 distance is desired (default false)
+ * @return double euclidean distance
+ */
+inline double euclidean_distance(
+  const geometry_msgs::msg::Pose & pos1,
+  const geometry_msgs::msg::Pose & pos2,
+  const bool is_3d = false)
+{
+  double dx = pos1.position.x - pos2.position.x;
+  double dy = pos1.position.y - pos2.position.y;
+
+  if (is_3d) {
+    double dz = pos1.position.z - pos2.position.z;
+    return std::hypot(dx, dy, dz);
+  }
+
+  return std::hypot(dx, dy);
+}
+
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
+ * @param pos1 First pose
+ * @param pos1 Second pose
+ * @param is_3d True if a true L2 distance is desired (default false)
+ * @return double L2 distance
+ */
+inline double euclidean_distance(
+  const geometry_msgs::msg::PoseStamped & pos1,
+  const geometry_msgs::msg::PoseStamped & pos2,
+  const bool is_3d = false)
+{
+  return euclidean_distance(pos1.pose, pos2.pose, is_3d);
+}
+
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::Pose2D
+ * @param pos1 First pose
+ * @param pos1 Second pose
+ * @return double L2 distance
+ */
+inline double euclidean_distance(
+  const geometry_msgs::msg::Pose2D & pos1,
+  const geometry_msgs::msg::Pose2D & pos2)
+{
+  double dx = pos1.x - pos2.x;
+  double dy = pos1.y - pos2.y;
+
+  return std::hypot(dx, dy);
+}
+
+/**
+ * Find element in iterator with the minimum calculated value
+ */
+template<typename Iter, typename Getter>
+inline Iter min_by(Iter begin, Iter end, Getter getCompareVal)
+{
+  if (begin == end) {
+    return end;
+  }
+  auto lowest = getCompareVal(*begin);
+  Iter lowest_it = begin;
+  for (Iter it = ++begin; it != end; ++it) {
+    auto comp = getCompareVal(*it);
+    if (comp < lowest) {
+      lowest = comp;
+      lowest_it = it;
+    }
+  }
+  return lowest_it;
+}
+
+/**
+ * Find first element in iterator that is greater integrated distance than comparevalue
+ */
+template<typename Iter, typename Getter>
+inline Iter first_after_integrated_distance(Iter begin, Iter end, Getter getCompareVal)
+{
+  if (begin == end) {
+    return end;
+  }
+  Getter dist = 0.0;
+  for (Iter it = begin; it != end - 1; it++) {
+    dist += euclidean_distance(*it, *(it + 1));
+    if (dist > getCompareVal) {
+      return it + 1;
+    }
+  }
+  return end;
+}
+
+/**
+ * @brief Calculate the length of the provided path, starting at the provided index
+ * @param path Path containing the poses that are planned
+ * @param start_index Optional argument specifying the starting index for
+ * the calculation of path length. Provide this if you want to calculate length of a
+ * subset of the path.
+ * @return double Path length
+ */
+inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0)
+{
+  if (start_index + 1 >= path.poses.size()) {
+    return 0.0;
+  }
+  double path_length = 0.0;
+  for (size_t idx = start_index; idx < path.poses.size() - 1; ++idx) {
+    path_length += euclidean_distance(path.poses[idx].pose, path.poses[idx + 1].pose);
+  }
+  return path_length;
+}
+
+}  // namespace geometry_utils
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__GEOMETRY_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/lifecycle_node.hpp

@@ -0,0 +1,212 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__LIFECYCLE_NODE_HPP_
+#define NAV2_UTIL__LIFECYCLE_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "nav2_util/node_thread.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "bondcpp/bond.hpp"
+#include "bond/msg/constants.hpp"
+
+namespace nav2_util
+{
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+/**
+ * @class nav2_util::LifecycleNode
+ * @brief A lifecycle node wrapper to enable common Nav2 needs such as manipulating parameters
+ */
+class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  /**
+   * @brief A lifecycle node constructor
+   * @param node_name Name for the node
+   * @param namespace Namespace for the node, if any
+   * @param options Node options
+   */
+  LifecycleNode(
+    const std::string & node_name,
+    const std::string & ns = "",
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  virtual ~LifecycleNode();
+
+  typedef struct
+  {
+    double from_value;
+    double to_value;
+    double step;
+  } floating_point_range;
+
+  typedef struct
+  {
+    int from_value;
+    int to_value;
+    int step;
+  } integer_range;
+
+  /**
+   * @brief Declare a parameter that has no integer or floating point range constraints
+   * @param node_name Name of parameter
+   * @param default_value Default node value to add
+   * @param description Node description
+   * @param additional_constraints Any additional constraints on the parameters to list
+   * @param read_only Whether this param should be considered read only
+   */
+  void add_parameter(
+    const std::string & name, const rclcpp::ParameterValue & default_value,
+    const std::string & description = "", const std::string & additional_constraints = "",
+    bool read_only = false)
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+
+    descriptor.name = name;
+    descriptor.description = description;
+    descriptor.additional_constraints = additional_constraints;
+    descriptor.read_only = read_only;
+
+    declare_parameter(descriptor.name, default_value, descriptor);
+  }
+
+  /**
+   * @brief Declare a parameter that has a floating point range constraint
+   * @param node_name Name of parameter
+   * @param default_value Default node value to add
+   * @param fp_range floating point range
+   * @param description Node description
+   * @param additional_constraints Any additional constraints on the parameters to list
+   * @param read_only Whether this param should be considered read only
+   */
+  void add_parameter(
+    const std::string & name, const rclcpp::ParameterValue & default_value,
+    const floating_point_range fp_range,
+    const std::string & description = "", const std::string & additional_constraints = "",
+    bool read_only = false)
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+
+    descriptor.name = name;
+    descriptor.description = description;
+    descriptor.additional_constraints = additional_constraints;
+    descriptor.read_only = read_only;
+    descriptor.floating_point_range.resize(1);
+    descriptor.floating_point_range[0].from_value = fp_range.from_value;
+    descriptor.floating_point_range[0].to_value = fp_range.to_value;
+    descriptor.floating_point_range[0].step = fp_range.step;
+
+    declare_parameter(descriptor.name, default_value, descriptor);
+  }
+
+  /**
+   * @brief Declare a parameter that has an integer range constraint
+   * @param node_name Name of parameter
+   * @param default_value Default node value to add
+   * @param integer_range Integer range
+   * @param description Node description
+   * @param additional_constraints Any additional constraints on the parameters to list
+   * @param read_only Whether this param should be considered read only
+   */
+  void add_parameter(
+    const std::string & name, const rclcpp::ParameterValue & default_value,
+    const integer_range int_range,
+    const std::string & description = "", const std::string & additional_constraints = "",
+    bool read_only = false)
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+
+    descriptor.name = name;
+    descriptor.description = description;
+    descriptor.additional_constraints = additional_constraints;
+    descriptor.read_only = read_only;
+    descriptor.integer_range.resize(1);
+    descriptor.integer_range[0].from_value = int_range.from_value;
+    descriptor.integer_range[0].to_value = int_range.to_value;
+    descriptor.integer_range[0].step = int_range.step;
+
+    declare_parameter(descriptor.name, default_value, descriptor);
+  }
+
+  /**
+   * @brief Get a shared pointer of this
+   */
+  std::shared_ptr<nav2_util::LifecycleNode> shared_from_this()
+  {
+    return std::static_pointer_cast<nav2_util::LifecycleNode>(
+      rclcpp_lifecycle::LifecycleNode::shared_from_this());
+  }
+
+  /**
+   * @brief Abstracted on_error state transition callback, since unimplemented as of 2020
+   * in the managed ROS2 node state machine
+   * @param state State prior to error transition
+   * @return Return type for success or failed transition to error state
+   */
+  nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/)
+  {
+    RCLCPP_FATAL(
+      get_logger(),
+      "Lifecycle node %s does not have error state implemented", get_name());
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  /**
+   * @brief Perform preshutdown activities before our Context is shutdown.
+   * Note that this is related to our Context's shutdown sequence, not the
+   * lifecycle node state machine.
+   */
+  virtual void on_rcl_preshutdown();
+
+  /**
+   * @brief Create bond connection to lifecycle manager
+   */
+  void createBond();
+
+  /**
+   * @brief Destroy bond connection to lifecycle manager
+   */
+  void destroyBond();
+
+protected:
+  /**
+   * @brief Print notifications for lifecycle node
+   */
+  void printLifecycleNodeNotification();
+
+  /**
+   * Register our preshutdown callback for this Node's rcl Context.
+   * The callback fires before this Node's Context is shutdown.
+   * Note this is not directly related to the lifecycle state machine.
+   */
+  void register_rcl_preshutdown_callback();
+  std::unique_ptr<rclcpp::PreShutdownCallbackHandle> rcl_preshutdown_cb_handle_{nullptr};
+
+  /**
+   * Run some common cleanup steps shared between rcl preshutdown and destruction.
+   */
+  void runCleanups();
+
+  // Connection to tell that server is still up
+  std::unique_ptr<bond::Bond> bond_{nullptr};
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__LIFECYCLE_NODE_HPP_

navigation2/nav2_util/include/nav2_util/lifecycle_service_client.hpp

@@ -0,0 +1,64 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
+#define NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "lifecycle_msgs/srv/change_state.hpp"
+#include "lifecycle_msgs/srv/get_state.hpp"
+#include "nav2_util/service_client.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_util
+{
+
+/// Helper functions to interact with a lifecycle node.
+class LifecycleServiceClient
+{
+public:
+  explicit LifecycleServiceClient(const std::string & lifecycle_node_name);
+  LifecycleServiceClient(
+    const std::string & lifecycle_node_name,
+    rclcpp::Node::SharedPtr parent_node);
+
+  /// Trigger a state change
+  /**
+   * Throws std::runtime_error on failure
+   */
+  bool change_state(
+    const uint8_t transition,  // takes a lifecycle_msgs::msg::Transition id
+    const std::chrono::seconds timeout);
+
+  /// Trigger a state change, returning result
+  bool change_state(std::uint8_t transition);
+
+  /// Get the current state as a lifecycle_msgs::msg::State id value
+  /**
+   * Throws std::runtime_error on failure
+   */
+  uint8_t get_state(const std::chrono::seconds timeout = std::chrono::seconds(2));
+
+protected:
+  rclcpp::Node::SharedPtr node_;
+  ServiceClient<lifecycle_msgs::srv::ChangeState> change_state_;
+  ServiceClient<lifecycle_msgs::srv::GetState> get_state_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_

navigation2/nav2_util/include/nav2_util/lifecycle_utils.hpp

@@ -0,0 +1,82 @@
+// 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.
+
+#ifndef NAV2_UTIL__LIFECYCLE_UTILS_HPP_
+#define NAV2_UTIL__LIFECYCLE_UTILS_HPP_
+
+#include <vector>
+#include <string>
+#include <chrono>
+#include "nav2_util/string_utils.hpp"
+
+namespace nav2_util
+{
+
+/// Transition the given lifecycle nodes to the ACTIVATED state in order
+/** At this time, service calls frequently hang for unknown reasons. The only
+ *  way to combat that is to timeout the service call and retry it. To use this
+ *  function, estimate how long your nodes should take to at each transition and
+ *  set your timeout accordingly.
+ * \param[in] node_names A vector of the fully qualified node names to startup.
+ * \param[in] service_call_timeout The maximum amount of time to wait for a
+ *            service call.
+ * \param[in] retries The number of times to try a state transition service call
+ */
+void startup_lifecycle_nodes(
+  const std::vector<std::string> & node_names,
+  const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(),
+  const int retries = 3);
+
+/// Transition the given lifecycle nodes to the ACTIVATED state in order.
+/**
+ * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
+ */
+void startup_lifecycle_nodes(
+  const std::string & nodes,
+  const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(),
+  const int retries = 3)
+{
+  startup_lifecycle_nodes(split(nodes, ':'), service_call_timeout, retries);
+}
+
+/// Transition the given lifecycle nodes to the UNCONFIGURED state in order
+/** At this time, service calls frequently hang for unknown reasons. The only
+ *  way to combat that is to timeout the service call and retry it. To use this
+ *  function, estimate how long your nodes should take to at each transition and
+ *  set your timeout accordingly.
+ * \param[in] node_names A vector of the fully qualified node names to reset.
+ * \param[in] service_call_timeout The maximum amount of time to wait for a
+ *            service call.
+ * \param[in] retries The number of times to try a state transition service call
+ */
+void reset_lifecycle_nodes(
+  const std::vector<std::string> & node_names,
+  const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(),
+  const int retries = 3);
+
+/// Transition the given lifecycle nodes to the UNCONFIGURED state in order.
+/**
+ * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
+ */
+void reset_lifecycle_nodes(
+  const std::string & nodes,
+  const std::chrono::seconds service_call_timeout = std::chrono::seconds::max(),
+  const int retries = 3)
+{
+  reset_lifecycle_nodes(split(nodes, ':'), service_call_timeout, retries);
+}
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__LIFECYCLE_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/line_iterator.hpp

@@ -0,0 +1,200 @@
+// Copyright (c) 2012, Willow Garage, Inc.
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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.
+
+#ifndef NAV2_UTIL__LINE_ITERATOR_HPP_
+#define NAV2_UTIL__LINE_ITERATOR_HPP_
+
+#include <stdlib.h>
+
+namespace nav2_util
+{
+
+
+/**
+ * @class nav2_util::LineIterator
+ * @brief An iterator implementing Bresenham Ray-Tracing.
+ */
+class LineIterator
+{
+public:
+  /**
+   * @brief A constructor for LineIterator
+   * @param x0 Starting x
+   * @param y0 Starting y
+   * @param x1 Ending x
+   * @param y1 Ending y
+   */
+  LineIterator(int x0, int y0, int x1, int y1)
+  : x0_(x0),
+    y0_(y0),
+    x1_(x1),
+    y1_(y1),
+    x_(x0),    // X and Y start of at first endpoint.
+    y_(y0),
+    deltax_(abs(x1 - x0)),
+    deltay_(abs(y1 - y0)),
+    curpixel_(0)
+  {
+    if (x1_ >= x0_) {                // The x-values are increasing
+      xinc1_ = 1;
+      xinc2_ = 1;
+    } else {                      // The x-values are decreasing
+      xinc1_ = -1;
+      xinc2_ = -1;
+    }
+
+    if (y1_ >= y0_) {               // The y-values are increasing
+      yinc1_ = 1;
+      yinc2_ = 1;
+    } else {                      // The y-values are decreasing
+      yinc1_ = -1;
+      yinc2_ = -1;
+    }
+
+    if (deltax_ >= deltay_) {        // There is at least one x-value for every y-value
+      xinc1_ = 0;                  // Don't change the x when numerator >= denominator
+      yinc2_ = 0;                  // Don't change the y for every iteration
+      den_ = deltax_;
+      num_ = deltax_ / 2;
+      numadd_ = deltay_;
+      numpixels_ = deltax_;         // There are more x-values than y-values
+    } else {                      // There is at least one y-value for every x-value
+      xinc2_ = 0;                  // Don't change the x for every iteration
+      yinc1_ = 0;                  // Don't change the y when numerator >= denominator
+      den_ = deltay_;
+      num_ = deltay_ / 2;
+      numadd_ = deltax_;
+      numpixels_ = deltay_;         // There are more y-values than x-values
+    }
+  }
+
+  /**
+   * @brief If the iterator is valid
+   * @return bool If valid
+   */
+  bool isValid() const
+  {
+    return curpixel_ <= numpixels_;
+  }
+
+  /**
+   * @brief Advance iteration along the line
+   */
+  void advance()
+  {
+    num_ += numadd_;              // Increase the numerator by the top of the fraction
+    if (num_ >= den_) {           // Check if numerator >= denominator
+      num_ -= den_;               // Calculate the new numerator value
+      x_ += xinc1_;               // Change the x as appropriate
+      y_ += yinc1_;               // Change the y as appropriate
+    }
+    x_ += xinc2_;                 // Change the x as appropriate
+    y_ += yinc2_;                 // Change the y as appropriate
+
+    curpixel_++;
+  }
+
+  /**
+   * @brief Get current X value
+   * @return X
+   */
+  int getX() const
+  {
+    return x_;
+  }
+
+  /**
+   * @brief Get current Y value
+   * @return Y
+   */
+  int getY() const
+  {
+    return y_;
+  }
+
+  /**
+   * @brief Get initial X value
+   * @return X
+   */
+  int getX0() const
+  {
+    return x0_;
+  }
+
+  /**
+   * @brief Get initial Y value
+   * @return Y
+   */
+  int getY0() const
+  {
+    return y0_;
+  }
+
+  /**
+   * @brief Get terminal X value
+   * @return X
+   */
+  int getX1() const
+  {
+    return x1_;
+  }
+
+  /**
+   * @brief Get terminal Y value
+   * @return Y
+   */
+  int getY1() const
+  {
+    return y1_;
+  }
+
+private:
+  int x0_;  ///< X coordinate of first end point.
+  int y0_;  ///< Y coordinate of first end point.
+  int x1_;  ///< X coordinate of second end point.
+  int y1_;  ///< Y coordinate of second end point.
+
+  int x_;  ///< X coordinate of current point.
+  int y_;  ///< Y coordinate of current point.
+
+  int deltax_;  ///< Difference between Xs of endpoints.
+  int deltay_;  ///< Difference between Ys of endpoints.
+
+  int curpixel_;  ///< index of current point in line loop.
+
+  int xinc1_, xinc2_, yinc1_, yinc2_;
+  int den_, num_, numadd_, numpixels_;
+};
+
+}  // end namespace nav2_util
+
+#endif  // NAV2_UTIL__LINE_ITERATOR_HPP_

navigation2/nav2_util/include/nav2_util/node_thread.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__NODE_THREAD_HPP_
+#define NAV2_UTIL__NODE_THREAD_HPP_
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_util
+{
+/**
+ * @class nav2_util::NodeThread
+ * @brief A background thread to process node/executor callbacks
+ */
+class NodeThread
+{
+public:
+  /**
+   * @brief A background thread to process node callbacks constructor
+   * @param node_base Interface to Node to spin in thread
+   */
+  explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
+
+  /**
+   * @brief A background thread to process executor's callbacks constructor
+   * @param executor Interface to executor to spin in thread
+   */
+  explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor);
+
+  /**
+   * @brief A background thread to process node callbacks constructor
+   * @param node Node pointer to spin in thread
+   */
+  template<typename NodeT>
+  explicit NodeThread(NodeT node)
+  : NodeThread(node->get_node_base_interface())
+  {}
+
+  /**
+   * @brief A destructor
+   */
+  ~NodeThread();
+
+protected:
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_;
+  std::unique_ptr<std::thread> thread_;
+  rclcpp::Executor::SharedPtr executor_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__NODE_THREAD_HPP_

navigation2/nav2_util/include/nav2_util/node_utils.hpp

@@ -0,0 +1,176 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__NODE_UTILS_HPP_
+#define NAV2_UTIL__NODE_UTILS_HPP_
+
+#include <vector>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+
+namespace nav2_util
+{
+
+/// Replace invalid characters in a potential node name
+/**
+ * There is frequently a need to create internal nodes. They must have a name,
+ * and commonly the name is based on some parameter related to the node's
+ * purpose. However, only alphanumeric characters and '_' are allowed in node
+ * names. This function replaces any invalid character with a '_'
+ *
+ * \param[in] potential_node_name Potential name but possibly with invalid charaters.
+ * \return A copy of the input string but with non-alphanumeric characters replaced with '_'
+ */
+std::string sanitize_node_name(const std::string & potential_node_name);
+
+/// Concatenate two namespaces to produce an absolute namespace
+/**
+ * \param[in] top_ns The namespace to place first
+ * \param[in] sub_ns The namespace to place after top_ns
+ * \return An absolute namespace starting with "/"
+*/
+std::string add_namespaces(const std::string & top_ns, const std::string & sub_ns = "");
+
+/// Add some random characters to a node name to ensure it is unique in the system
+/**
+ * There are utility classes that create an internal private node to interact
+ * with the system. These private nodes are given a generated name. If multiple
+ * clients end up using the same service, there is the potential for node name
+ * conflicts. To ensure node names are globally unique, this appends some random
+ * numbers to the end of the prefix.
+ *
+ * \param[in] prefix A string to help understand the purpose of the node.
+ * \return A copy of the prefix + '_' + 8 random digits. eg. prefix_12345678
+ */
+std::string generate_internal_node_name(const std::string & prefix = "");
+
+/// Creates a node with a name as generated by generate_internal_node_name
+/**
+ *  Creates a node with the following settings:
+ *  - name generated by generate_internal_node_name
+ *  - no parameter services
+ *  - no parameter event publisher
+ *
+ * \param[in] prefix A string to help understand the purpose of the node.
+ * \return A shared_ptr to the node.
+ */
+rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix = "");
+
+/// Generates a pseudo random string of digits.
+/**
+ * Generates pseudo random digits by converting the current system time to a
+ * string. This means that any length more than 8 or so digits will just get
+ * padded with zeros and doesn't add any additional randomness.
+ *
+ * \param[in] len Length of the output string
+ * \return A string containing random digits
+ */
+std::string time_to_string(size_t len);
+
+/// Declares static ROS2 parameter and sets it to a given value if it was not already declared
+/* Declares static ROS2 parameter and sets it to a given value
+ * if it was not already declared.
+ *
+ * \param[in] node A node in which given parameter to be declared
+ * \param[in] param_name The name of parameter
+ * \param[in] default_value Parameter value to initialize with
+ * \param[in] parameter_descriptor Parameter descriptor (optional)
+ */
+template<typename NodeT>
+void declare_parameter_if_not_declared(
+  NodeT node,
+  const std::string & param_name,
+  const rclcpp::ParameterValue & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+  rcl_interfaces::msg::ParameterDescriptor())
+{
+  if (!node->has_parameter(param_name)) {
+    node->declare_parameter(param_name, default_value, parameter_descriptor);
+  }
+}
+
+/// Declares static ROS2 parameter with given type if it was not already declared
+/* Declares static ROS2 parameter with given type if it was not already declared.
+ *
+ * \param[in] node A node in which given parameter to be declared
+ * \param[in] param_type The type of parameter
+ * \param[in] default_value Parameter value to initialize with
+ * \param[in] parameter_descriptor Parameter descriptor (optional)
+ */
+template<typename NodeT>
+void declare_parameter_if_not_declared(
+  NodeT node,
+  const std::string & param_name,
+  const rclcpp::ParameterType & param_type,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+  rcl_interfaces::msg::ParameterDescriptor())
+{
+  if (!node->has_parameter(param_name)) {
+    node->declare_parameter(param_name, param_type, parameter_descriptor);
+  }
+}
+
+/// Gets the type of plugin for the selected node and its plugin
+/**
+ * Gets the type of plugin for the selected node and its plugin.
+ * Actually seeks for the value of "<plugin_name>.plugin" parameter.
+ *
+ * \param[in] node Selected node
+ * \param[in] plugin_name The name of plugin the type of which is being searched for
+ * \return A string containing the type of plugin (the value of "<plugin_name>.plugin" parameter)
+ */
+template<typename NodeT>
+std::string get_plugin_type_param(
+  NodeT node,
+  const std::string & plugin_name)
+{
+  declare_parameter_if_not_declared(node, plugin_name + ".plugin", rclcpp::PARAMETER_STRING);
+  std::string plugin_type;
+  try {
+    if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) {
+      RCLCPP_FATAL(
+        node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str());
+      exit(-1);
+    }
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str());
+    exit(-1);
+  }
+
+  return plugin_type;
+}
+
+/**
+ * @brief A method to copy all parameters from one node (parent) to another (child).
+ * May throw parameter exceptions in error conditions
+ * @param parent Node to copy parameters from
+ * @param child Node to copy parameters to
+ */
+template<typename NodeT1, typename NodeT2>
+void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child)
+{
+  using Parameters = std::vector<rclcpp::Parameter>;
+  std::vector<std::string> param_names = parent->list_parameters({}, 0).names;
+  Parameters params = parent->get_parameters(param_names);
+  for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) {
+    if (!child->has_parameter(iter->get_name())) {
+      child->declare_parameter(iter->get_name(), iter->get_parameter_value());
+    }
+  }
+}
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__NODE_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/occ_grid_values.hpp

@@ -0,0 +1,50 @@
+// Copyright (c) 2020 Samsung Research Russia
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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 <ORGANIZATION> nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Alexey Merzlyakov
+
+#ifndef NAV2_UTIL__OCC_GRID_VALUES_HPP_
+#define NAV2_UTIL__OCC_GRID_VALUES_HPP_
+
+namespace nav2_util
+{
+
+/**
+  * @brief OccupancyGrid data constants
+  */
+static constexpr int8_t OCC_GRID_UNKNOWN = -1;
+static constexpr int8_t OCC_GRID_FREE = 0;
+static constexpr int8_t OCC_GRID_OCCUPIED = 100;
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__OCC_GRID_VALUES_HPP_

navigation2/nav2_util/include/nav2_util/odometry_utils.hpp

@@ -0,0 +1,102 @@
+// 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.
+
+#ifndef NAV2_UTIL__ODOMETRY_UTILS_HPP_
+#define NAV2_UTIL__ODOMETRY_UTILS_HPP_
+
+#include <cmath>
+#include <chrono>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <deque>
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @class OdomSmoother
+ * Wrapper for getting smooth odometry readings using a simple moving avergae.
+ * Subscribes to the topic with a mutex.
+ */
+class OdomSmoother
+{
+public:
+  /**
+   * @brief Constructor that subscribes to an Odometry topic
+   * @param parent NodeHandle for creating subscriber
+   * @param filter_duration Duration for odom history (seconds)
+   * @param odom_topic Topic on which odometry should be received
+   */
+  explicit OdomSmoother(
+    const rclcpp::Node::WeakPtr & parent,
+    double filter_duration = 0.3,
+    const std::string & odom_topic = "odom");
+
+  /**
+   * @brief Overloadded Constructor for nav_util::LifecycleNode parent
+   * that subscribes to an Odometry topic
+   * @param parent NodeHandle for creating subscriber
+   * @param filter_duration Duration for odom history (seconds)
+   * @param odom_topic Topic on which odometry should be received
+   */
+  explicit OdomSmoother(
+    const nav2_util::LifecycleNode::WeakPtr & parent,
+    double filter_duration = 0.3,
+    const std::string & odom_topic = "odom");
+
+  /**
+   * @brief Get twist msg from smoother
+   * @return twist Twist msg
+   */
+  inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;}
+
+  /**
+   * @brief Get twist stamped msg from smoother
+   * @return twist TwistStamped msg
+   */
+  inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;}
+
+protected:
+  /**
+   * @brief Callback of odometry subscriber to process
+   * @param msg Odometry msg to smooth
+   */
+  void odomCallback(nav_msgs::msg::Odometry::SharedPtr msg);
+
+  /**
+   * @brief Update internal state of the smoother after getting new data
+   */
+  void updateState();
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  nav_msgs::msg::Odometry odom_cumulate_;
+  geometry_msgs::msg::TwistStamped vel_smooth_;
+  std::mutex odom_mutex_;
+
+  rclcpp::Duration odom_history_duration_;
+  std::deque<nav_msgs::msg::Odometry> odom_history_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__ODOMETRY_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/robot_utils.hpp

@@ -0,0 +1,121 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2019 Steven Macenski
+// Copyright (c) 2019 Samsung Research America
+//
+// 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__ROBOT_UTILS_HPP_
+#define NAV2_UTIL__ROBOT_UTILS_HPP_
+
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "tf2/time.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_util
+{
+/**
+* @brief get the current pose of the robot
+* @param global_pose Pose to transform
+* @param tf_buffer TF buffer to use for the transformation
+* @param global_frame Frame to transform into
+* @param robot_frame Frame to transform from
+* @param transform_timeout TF Timeout to use for transformation
+* @return bool Whether it could be transformed successfully
+*/
+bool getCurrentPose(
+  geometry_msgs::msg::PoseStamped & global_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
+  const std::string robot_frame = "base_link", const double transform_timeout = 0.1,
+  const rclcpp::Time stamp = rclcpp::Time());
+
+/**
+* @brief get an arbitrary pose in a target frame
+* @param input_pose Pose to transform
+* @param transformed_pose Output transformation
+* @param tf_buffer TF buffer to use for the transformation
+* @param target_frame Frame to transform into
+* @param transform_timeout TF Timeout to use for transformation
+* @return bool Whether it could be transformed successfully
+*/
+bool transformPoseInTargetFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
+  const double transform_timeout = 0.1);
+
+/**
+ * @brief Obtains a transform from source_frame_id at source_time ->
+ * to target_frame_id at target_time time
+ * @param source_frame_id Source frame ID to convert from
+ * @param source_time Source timestamp to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param target_time Target time to interpolate to
+ * @param transform_tolerance Transform tolerance
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+
+/**
+ * @brief Obtains a transform from source_frame_id -> to target_frame_id
+ * @param source_frame_id Source frame ID to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param transform_tolerance Transform tolerance
+ * @param tf_buffer TF buffer to use for the transformation
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+bool getTransform(
+  const std::string & source_frame_id,
+  const std::string & target_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform);
+
+/**
+ * @brief Obtains a transform from source_frame_id at source_time ->
+ * to target_frame_id at target_time time
+ * @param source_frame_id Source frame ID to convert from
+ * @param source_time Source timestamp to convert from
+ * @param target_frame_id Target frame ID to convert to
+ * @param target_time Current node time to interpolate to
+ * @param fixed_frame_id The frame in which to assume the transform is constant in time
+ * @param transform_tolerance Transform tolerance
+ * @param tf_buffer TF buffer to use for the transformation
+ * @param tf_transform Output source->target transform
+ * @return True if got correct transform, otherwise false
+ */
+bool getTransform(
+  const std::string & source_frame_id,
+  const rclcpp::Time & source_time,
+  const std::string & target_frame_id,
+  const rclcpp::Time & target_time,
+  const std::string & fixed_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform);
+
+/**
+ * @brief Validates a twist message contains no nans or infs
+ * @param msg Twist message to validate
+ * @return True if valid, false if contains unactionable values
+ */
+bool validateTwist(const geometry_msgs::msg::Twist & msg);
+
+}  // end namespace nav2_util
+
+#endif  // NAV2_UTIL__ROBOT_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/service_client.hpp

@@ -0,0 +1,158 @@
+// 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.
+
+#ifndef NAV2_UTIL__SERVICE_CLIENT_HPP_
+#define NAV2_UTIL__SERVICE_CLIENT_HPP_
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @class nav2_util::ServiceClient
+ * @brief A simple wrapper on ROS2 services for invoke() and block-style calling
+ */
+template<class ServiceT>
+class ServiceClient
+{
+public:
+  /**
+  * @brief A constructor
+  * @param service_name name of the service to call
+  * @param provided_node Node to create the service client off of
+  */
+  explicit ServiceClient(
+    const std::string & service_name,
+    const rclcpp::Node::SharedPtr & provided_node)
+  : service_name_(service_name), node_(provided_node)
+  {
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+    client_ = node_->create_client<ServiceT>(
+      service_name,
+      rclcpp::ServicesQoS().get_rmw_qos_profile(),
+      callback_group_);
+  }
+
+  using RequestType = typename ServiceT::Request;
+  using ResponseType = typename ServiceT::Response;
+
+  /**
+  * @brief Invoke the service and block until completed or timed out
+  * @param request The request object to call the service using
+  * @param timeout Maximum timeout to wait for, default infinite
+  * @return Response A pointer to the service response from the request
+  */
+  typename ResponseType::SharedPtr invoke(
+    typename RequestType::SharedPtr & request,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1))
+  {
+    while (!client_->wait_for_service(std::chrono::seconds(1))) {
+      if (!rclcpp::ok()) {
+        throw std::runtime_error(
+                service_name_ + " service client: interrupted while waiting for service");
+      }
+      RCLCPP_INFO(
+        node_->get_logger(), "%s service client: waiting for service to appear...",
+        service_name_.c_str());
+    }
+
+    RCLCPP_DEBUG(
+      node_->get_logger(), "%s service client: send async request",
+      service_name_.c_str());
+    auto future_result = client_->async_send_request(request);
+
+    if (callback_group_executor_.spin_until_future_complete(future_result, timeout) !=
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      // Pending request must be manually cleaned up if execution is interrupted or timed out
+      client_->remove_pending_request(future_result);
+      throw std::runtime_error(service_name_ + " service client: async_send_request failed");
+    }
+
+    return future_result.get();
+  }
+
+  /**
+  * @brief Invoke the service and block until completed
+  * @param request The request object to call the service using
+  * @param Response A pointer to the service response from the request
+  * @return bool Whether it was successfully called
+  */
+  bool invoke(
+    typename RequestType::SharedPtr & request,
+    typename ResponseType::SharedPtr & response)
+  {
+    while (!client_->wait_for_service(std::chrono::seconds(1))) {
+      if (!rclcpp::ok()) {
+        throw std::runtime_error(
+                service_name_ + " service client: interrupted while waiting for service");
+      }
+      RCLCPP_INFO(
+        node_->get_logger(), "%s service client: waiting for service to appear...",
+        service_name_.c_str());
+    }
+
+    RCLCPP_DEBUG(
+      node_->get_logger(), "%s service client: send async request",
+      service_name_.c_str());
+    auto future_result = client_->async_send_request(request);
+
+    if (callback_group_executor_.spin_until_future_complete(future_result) !=
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      // Pending request must be manually cleaned up if execution is interrupted or timed out
+      client_->remove_pending_request(future_result);
+      return false;
+    }
+
+    response = future_result.get();
+    return response.get();
+  }
+
+  /**
+  * @brief Block until a service is available or timeout
+  * @param timeout Maximum timeout to wait for, default infinite
+  * @return bool true if service is available
+  */
+  bool wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
+  {
+    return client_->wait_for_service(timeout);
+  }
+
+  /**
+  * @brief Gets the service name
+  * @return string Service name
+  */
+  std::string getServiceName()
+  {
+    return service_name_;
+  }
+
+protected:
+  std::string service_name_;
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  typename rclcpp::Client<ServiceT>::SharedPtr client_;
+};
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__SERVICE_CLIENT_HPP_

navigation2/nav2_util/include/nav2_util/simple_action_server.hpp

@@ -0,0 +1,613 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
+#define NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <future>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav2_util/node_thread.hpp"
+
+namespace nav2_util
+{
+
+/**
+ * @class nav2_util::SimpleActionServer
+ * @brief An action server wrapper to make applications simpler using Actions
+ */
+template<typename ActionT>
+class SimpleActionServer
+{
+public:
+  // Callback function to complete main work. This should itself deal with its
+  // own exceptions, but if for some reason one is thrown, it will be caught
+  // in SimpleActionServer and terminate the action itself.
+  typedef std::function<void ()> ExecuteCallback;
+
+  // Callback function to notify the user that an exception was thrown that
+  // the simple action server caught (or another failure) and the action was
+  // terminated. To avoid using, catch exceptions in your application such that
+  // the SimpleActionServer will never need to terminate based on failed action
+  // ExecuteCallback.
+  typedef std::function<void ()> CompletionCallback;
+
+  /**
+   * @brief An constructor for SimpleActionServer
+   * @param node Ptr to node to make actions
+   * @param action_name Name of the action to call
+   * @param execute_callback Execution  callback function of Action
+   * @param server_timeout Timeout to to react to stop or preemption requests
+   * @param spin_thread Whether to spin with a dedicated thread internally
+   * @param options Options to pass to the underlying rcl_action_server_t
+   */
+  template<typename NodeT>
+  explicit SimpleActionServer(
+    NodeT node,
+    const std::string & action_name,
+    ExecuteCallback execute_callback,
+    CompletionCallback completion_callback = nullptr,
+    std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
+    bool spin_thread = false,
+    const rcl_action_server_options_t & options = rcl_action_server_get_default_options())
+  : SimpleActionServer(
+      node->get_node_base_interface(),
+      node->get_node_clock_interface(),
+      node->get_node_logging_interface(),
+      node->get_node_waitables_interface(),
+      action_name, execute_callback, completion_callback, server_timeout, spin_thread, options)
+  {}
+
+  /**
+   * @brief An constructor for SimpleActionServer
+   * @param <node interfaces> Abstract node interfaces to make actions
+   * @param action_name Name of the action to call
+   * @param execute_callback Execution  callback function of Action
+   * @param server_timeout Timeout to to react to stop or preemption requests
+   * @param spin_thread Whether to spin with a dedicated thread internally
+   * @param options Options to pass to the underlying rcl_action_server_t
+   */
+  explicit SimpleActionServer(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
+    const std::string & action_name,
+    ExecuteCallback execute_callback,
+    CompletionCallback completion_callback = nullptr,
+    std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
+    bool spin_thread = false,
+    const rcl_action_server_options_t & options = rcl_action_server_get_default_options())
+  : node_base_interface_(node_base_interface),
+    node_clock_interface_(node_clock_interface),
+    node_logging_interface_(node_logging_interface),
+    node_waitables_interface_(node_waitables_interface),
+    action_name_(action_name),
+    execute_callback_(execute_callback),
+    completion_callback_(completion_callback),
+    server_timeout_(server_timeout),
+    spin_thread_(spin_thread)
+  {
+    using namespace std::placeholders;  // NOLINT
+    if (spin_thread_) {
+      callback_group_ = node_base_interface->create_callback_group(
+        rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    }
+    action_server_ = rclcpp_action::create_server<ActionT>(
+      node_base_interface_,
+      node_clock_interface_,
+      node_logging_interface_,
+      node_waitables_interface_,
+      action_name_,
+      std::bind(&SimpleActionServer::handle_goal, this, _1, _2),
+      std::bind(&SimpleActionServer::handle_cancel, this, _1),
+      std::bind(&SimpleActionServer::handle_accepted, this, _1),
+      options,
+      callback_group_);
+    if (spin_thread_) {
+      executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      executor_->add_callback_group(callback_group_, node_base_interface_);
+      executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
+    }
+  }
+
+  /**
+   * @brief handle the goal requested: accept or reject. This implementation always accepts.
+   * @param uuid Goal ID
+   * @param Goal A shared pointer to the specific goal
+   * @return GoalResponse response of the goal processed
+   */
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & /*uuid*/,
+    std::shared_ptr<const typename ActionT::Goal>/*goal*/)
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!server_active_) {
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    debug_msg("Received request for goal acceptance");
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  /**
+   * @brief Accepts cancellation requests of action server.
+   * @param uuid Goal ID
+   * @param Goal A server goal handle to cancel
+   * @return CancelResponse response of the goal cancelled
+   */
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle)
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!handle->is_active()) {
+      warn_msg(
+        "Received request for goal cancellation,"
+        "but the handle is inactive, so reject the request");
+      return rclcpp_action::CancelResponse::REJECT;
+    }
+
+    debug_msg("Received request for goal cancellation");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  /**
+   * @brief Handles accepted goals and adds to preempted queue to switch to
+   * @param Goal A server goal handle to cancel
+   */
+  void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle)
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+    debug_msg("Receiving a new goal");
+
+    if (is_active(current_handle_) || is_running()) {
+      debug_msg("An older goal is active, moving the new goal to a pending slot.");
+
+      if (is_active(pending_handle_)) {
+        debug_msg(
+          "The pending slot is occupied."
+          " The previous pending goal will be terminated and replaced.");
+        terminate(pending_handle_);
+      }
+      pending_handle_ = handle;
+      preempt_requested_ = true;
+    } else {
+      if (is_active(pending_handle_)) {
+        // Shouldn't reach a state with a pending goal but no current one.
+        error_msg("Forgot to handle a preemption. Terminating the pending goal.");
+        terminate(pending_handle_);
+        preempt_requested_ = false;
+      }
+
+      current_handle_ = handle;
+
+      // Return quickly to avoid blocking the executor, so spin up a new thread
+      debug_msg("Executing goal asynchronously.");
+      execution_future_ = std::async(std::launch::async, [this]() {work();});
+    }
+  }
+
+  /**
+   * @brief Computed background work and processes stop requests
+   */
+  void work()
+  {
+    while (rclcpp::ok() && !stop_execution_ && is_active(current_handle_)) {
+      debug_msg("Executing the goal...");
+      try {
+        execute_callback_();
+      } catch (std::exception & ex) {
+        RCLCPP_ERROR(
+          node_logging_interface_->get_logger(),
+          "Action server failed while executing action callback: \"%s\"", ex.what());
+        terminate_all();
+        completion_callback_();
+        return;
+      }
+
+      debug_msg("Blocking processing of new goal handles.");
+      std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+      if (stop_execution_) {
+        warn_msg("Stopping the thread per request.");
+        terminate_all();
+        completion_callback_();
+        break;
+      }
+
+      if (is_active(current_handle_)) {
+        warn_msg("Current goal was not completed successfully.");
+        terminate(current_handle_);
+        completion_callback_();
+      }
+
+      if (is_active(pending_handle_)) {
+        debug_msg("Executing a pending handle on the existing thread.");
+        accept_pending_goal();
+      } else {
+        debug_msg("Done processing available goals.");
+        break;
+      }
+    }
+    debug_msg("Worker thread done.");
+  }
+
+  /**
+   * @brief Active action server
+   */
+  void activate()
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+    server_active_ = true;
+    stop_execution_ = false;
+  }
+
+  /**
+   * @brief Deactive action server
+   */
+  void deactivate()
+  {
+    debug_msg("Deactivating...");
+
+    {
+      std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+      server_active_ = false;
+      stop_execution_ = true;
+    }
+
+    if (!execution_future_.valid()) {
+      return;
+    }
+
+    if (is_running()) {
+      warn_msg(
+        "Requested to deactivate server but goal is still executing."
+        " Should check if action server is running before deactivating.");
+    }
+
+    using namespace std::chrono;  //NOLINT
+    auto start_time = steady_clock::now();
+    while (execution_future_.wait_for(milliseconds(100)) != std::future_status::ready) {
+      info_msg("Waiting for async process to finish.");
+      if (steady_clock::now() - start_time >= server_timeout_) {
+        terminate_all();
+        if (completion_callback_) {completion_callback_();}
+        error_msg("Action callback is still running and missed deadline to stop");
+      }
+    }
+
+    debug_msg("Deactivation completed.");
+  }
+
+  /**
+   * @brief Whether the action server is munching on a goal
+   * @return bool If its running or not
+   */
+  bool is_running()
+  {
+    return execution_future_.valid() &&
+           (execution_future_.wait_for(std::chrono::milliseconds(0)) ==
+           std::future_status::timeout);
+  }
+
+  /**
+   * @brief Whether the action server is active or not
+   * @return bool If its active or not
+   */
+  bool is_server_active()
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+    return server_active_;
+  }
+
+  /**
+   * @brief Whether the action server has been asked to be preempted with a new goal
+   * @return bool If there's a preemption request or not
+   */
+  bool is_preempt_requested() const
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+    return preempt_requested_;
+  }
+
+  /**
+   * @brief Accept pending goals
+   * @return Goal Ptr to the  goal that's going to be accepted
+   */
+  const std::shared_ptr<const typename ActionT::Goal> accept_pending_goal()
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!pending_handle_ || !pending_handle_->is_active()) {
+      error_msg("Attempting to get pending goal when not available");
+      return std::shared_ptr<const typename ActionT::Goal>();
+    }
+
+    if (is_active(current_handle_) && current_handle_ != pending_handle_) {
+      debug_msg("Cancelling the previous goal");
+      current_handle_->abort(empty_result());
+    }
+
+    current_handle_ = pending_handle_;
+    pending_handle_.reset();
+    preempt_requested_ = false;
+
+    debug_msg("Preempted goal");
+
+    return current_handle_->get_goal();
+  }
+
+  /**
+   * @brief Terminate pending goals
+   */
+  void terminate_pending_goal()
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!pending_handle_ || !pending_handle_->is_active()) {
+      error_msg("Attempting to terminate pending goal when not available");
+      return;
+    }
+
+    terminate(pending_handle_);
+    preempt_requested_ = false;
+
+    debug_msg("Pending goal terminated");
+  }
+
+  /**
+   * @brief Get the current goal object
+   * @return Goal Ptr to the  goal that's being processed currently
+   */
+  const std::shared_ptr<const typename ActionT::Goal> get_current_goal() const
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!is_active(current_handle_)) {
+      error_msg("A goal is not available or has reached a final state");
+      return std::shared_ptr<const typename ActionT::Goal>();
+    }
+
+    return current_handle_->get_goal();
+  }
+
+  const rclcpp_action::GoalUUID get_current_goal_id() const
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!is_active(current_handle_)) {
+      error_msg("A goal is not available or has reached a final state");
+      return rclcpp_action::GoalUUID();
+    }
+
+    return current_handle_->get_goal_id();
+  }
+
+  /**
+   * @brief Get the pending goal object
+   * @return Goal Ptr to the goal that's pending
+   */
+  const std::shared_ptr<const typename ActionT::Goal> get_pending_goal() const
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!pending_handle_ || !pending_handle_->is_active()) {
+      error_msg("Attempting to get pending goal when not available");
+      return std::shared_ptr<const typename ActionT::Goal>();
+    }
+
+    return pending_handle_->get_goal();
+  }
+
+  /**
+   * @brief Whether or not a cancel command has come in
+   * @return bool Whether a cancel command has been requested or not
+   */
+  bool is_cancel_requested() const
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    // A cancel request is assumed if either handle is canceled by the client.
+
+    if (current_handle_ == nullptr) {
+      error_msg("Checking for cancel but current goal is not available");
+      return false;
+    }
+
+    if (pending_handle_ != nullptr) {
+      return pending_handle_->is_canceling();
+    }
+
+    return current_handle_->is_canceling();
+  }
+
+  /**
+   * @brief Terminate all pending and active actions
+   * @param result A result object to send to the terminated actions
+   */
+  void terminate_all(
+    typename std::shared_ptr<typename ActionT::Result> result =
+    std::make_shared<typename ActionT::Result>())
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+    terminate(current_handle_, result);
+    terminate(pending_handle_, result);
+    preempt_requested_ = false;
+  }
+
+  /**
+   * @brief Terminate the active action
+   * @param result A result object to send to the terminated action
+   */
+  void terminate_current(
+    typename std::shared_ptr<typename ActionT::Result> result =
+    std::make_shared<typename ActionT::Result>())
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+    terminate(current_handle_, result);
+  }
+
+  /**
+   * @brief Return success of the active action
+   * @param result A result object to send to the terminated actions
+   */
+  void succeeded_current(
+    typename std::shared_ptr<typename ActionT::Result> result =
+    std::make_shared<typename ActionT::Result>())
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (is_active(current_handle_)) {
+      debug_msg("Setting succeed on current goal.");
+      current_handle_->succeed(result);
+      current_handle_.reset();
+    }
+  }
+
+  /**
+   * @brief Publish feedback to the action server clients
+   * @param feedback A feedback object to send to the clients
+   */
+  void publish_feedback(typename std::shared_ptr<typename ActionT::Feedback> feedback)
+  {
+    if (!is_active(current_handle_)) {
+      error_msg("Trying to publish feedback when the current goal handle is not active");
+      return;
+    }
+
+    current_handle_->publish_feedback(feedback);
+  }
+
+protected:
+  // The SimpleActionServer isn't itself a node, so it needs interfaces to one
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_;
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface_;
+  std::string action_name_;
+
+  ExecuteCallback execute_callback_;
+  CompletionCallback completion_callback_;
+  std::future<void> execution_future_;
+  bool stop_execution_{false};
+
+  mutable std::recursive_mutex update_mutex_;
+  bool server_active_{false};
+  bool preempt_requested_{false};
+  std::chrono::milliseconds server_timeout_;
+
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> current_handle_;
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> pending_handle_;
+
+  typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
+  bool spin_thread_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+
+  /**
+   * @brief Generate an empty result object for an action type
+   */
+  constexpr auto empty_result() const
+  {
+    return std::make_shared<typename ActionT::Result>();
+  }
+
+  /**
+   * @brief Whether a given goal handle is currently active
+   * @param handle Goal handle to check
+   * @return Whether this goal handle is active
+   */
+  constexpr bool is_active(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle) const
+  {
+    return handle != nullptr && handle->is_active();
+  }
+
+  /**
+   * @brief Terminate a particular action with a result
+   * @param handle goal handle to terminate
+   * @param the Results object to terminate the action with
+   */
+  void terminate(
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> & handle,
+    typename std::shared_ptr<typename ActionT::Result> result =
+    std::make_shared<typename ActionT::Result>())
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (is_active(handle)) {
+      if (handle->is_canceling()) {
+        info_msg("Client requested to cancel the goal. Cancelling.");
+        handle->canceled(result);
+      } else {
+        warn_msg("Aborting handle.");
+        handle->abort(result);
+      }
+      handle.reset();
+    }
+  }
+
+  /**
+   * @brief Info logging
+   */
+  void info_msg(const std::string & msg) const
+  {
+    RCLCPP_INFO(
+      node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+  }
+
+  /**
+   * @brief Debug logging
+   */
+  void debug_msg(const std::string & msg) const
+  {
+    RCLCPP_DEBUG(
+      node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+  }
+
+  /**
+   * @brief Error logging
+   */
+  void error_msg(const std::string & msg) const
+  {
+    RCLCPP_ERROR(
+      node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+  }
+
+  /**
+   * @brief Warn logging
+   */
+  void warn_msg(const std::string & msg) const
+  {
+    RCLCPP_WARN(
+      node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+  }
+};
+
+}  // namespace nav2_util
+
+#endif   // NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_

navigation2/nav2_util/include/nav2_util/string_utils.hpp

@@ -0,0 +1,44 @@
+// 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.
+
+#ifndef NAV2_UTIL__STRING_UTILS_HPP_
+#define NAV2_UTIL__STRING_UTILS_HPP_
+
+#include <string>
+#include <vector>
+
+namespace nav2_util
+{
+
+typedef std::vector<std::string> Tokens;
+
+/*
+ * @brief Remove leading slash from a topic name
+ * @param in String of topic in
+ * @return String out without slash
+*/
+std::string strip_leading_slash(const std::string & in);
+
+///
+/*
+ * @brief Split a string at the delimiters
+ * @param in String to split
+ * @param Delimiter criteria
+ * @return Tokens
+*/
+Tokens split(const std::string & tokenstring, char delimiter);
+
+}  // namespace nav2_util
+
+#endif  // NAV2_UTIL__STRING_UTILS_HPP_

navigation2/nav2_util/include/nav2_util/validate_messages.hpp

@@ -0,0 +1,179 @@
+// 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.
+
+
+#ifndef  NAV2_UTIL__VALIDATE_MESSAGES_HPP_
+#define  NAV2_UTIL__VALIDATE_MESSAGES_HPP_
+
+#include <cmath>
+#include <iostream>
+
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+
+
+// @brief Validation Check
+//  Check recieved message is safe or not for the nav2-system
+//  For each msg-type known in nav2, we could check it as following:
+//  if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.")
+//
+//  Workflow of validateMsg():
+//     if here's a sub-msg-type in the recieved msg,
+//        the content of sub-msg would be checked as sub-msg-type
+//     then, check the whole recieved msg.
+//
+//  Following conditions are involved in check:
+//     1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on
+//     2> Logic Check: to avoid value with bad logic,
+//             like the size of `map` should be equal to `height*width`
+//     3> Any other needed condition could be joint here in future
+
+namespace nav2_util
+{
+
+
+bool validateMsg(const double & num)
+{
+  /*  @brief double/float value check
+   *  if here'a need to check message validation
+   *  it should be avoid to use double value like `nan`, `inf`
+   *  otherwise, we regard it as an invalid message
+   */
+  if (std::isinf(num)) {return false;}
+  if (std::isnan(num)) {return false;}
+  return true;
+}
+
+template<size_t N>
+bool validateMsg(const std::array<double, N> & msg)
+{
+  /*  @brief value check for double-array
+   *     like the field `covariance` used in the msg-type:
+   *       geometry_msgs::msg::PoseWithCovarianceStamped
+   */
+  for (const auto & element : msg) {
+    if (!validateMsg(element)) {return false;}
+  }
+
+  return true;
+}
+
+const int NSEC_PER_SEC = 1e9;  // 1 second = 1e9 nanosecond
+bool validateMsg(const builtin_interfaces::msg::Time & msg)
+{
+  if (msg.nanosec >= NSEC_PER_SEC) {
+    return false;                                      // invalid nanosec-stamp
+  }
+  return true;
+}
+
+bool validateMsg(const std_msgs::msg::Header & msg)
+{
+  //  check sub-type
+  if (!validateMsg(msg.stamp)) {return false;}
+
+  /*  @brief frame_id check
+   *  if here'a need to check message validation
+   *  it should at least have a non-empty frame_id
+   *  otherwise, we regard it as an invalid message
+   */
+  if (msg.frame_id.empty()) {return false;}
+  return true;
+}
+
+bool validateMsg(const geometry_msgs::msg::Point & msg)
+{
+  //  check sub-type
+  if (!validateMsg(msg.x)) {return false;}
+  if (!validateMsg(msg.y)) {return false;}
+  if (!validateMsg(msg.z)) {return false;}
+  return true;
+}
+
+const double epsilon = 1e-4;
+bool validateMsg(const geometry_msgs::msg::Quaternion & msg)
+{
+  //  check sub-type
+  if (!validateMsg(msg.x)) {return false;}
+  if (!validateMsg(msg.y)) {return false;}
+  if (!validateMsg(msg.z)) {return false;}
+  if (!validateMsg(msg.w)) {return false;}
+
+  if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) {
+    return false;
+  }
+
+  return true;
+}
+
+bool validateMsg(const geometry_msgs::msg::Pose & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.position)) {return false;}
+  if (!validateMsg(msg.orientation)) {return false;}
+  return true;
+}
+
+bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.pose)) {return false;}
+  if (!validateMsg(msg.covariance)) {return false;}
+
+  return true;
+}
+
+bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.header)) {return false;}
+  if (!validateMsg(msg.pose)) {return false;}
+  return true;
+}
+
+
+// Function to verify map meta information
+bool validateMsg(const nav_msgs::msg::MapMetaData & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.origin)) {return false;}
+  if (!validateMsg(msg.resolution)) {return false;}
+
+  // logic check
+  // 1> we don't need an empty map
+  if (msg.height == 0 || msg.width == 0) {return false;}
+  return true;
+}
+
+// for msg-type like map, costmap and others as `OccupancyGrid`
+bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg)
+{
+  // check sub-type
+  if (!validateMsg(msg.header)) {return false;}
+  // msg.data :  @todo any check for it ?
+  if (!validateMsg(msg.info)) {return false;}
+
+  // check logic
+  if (msg.data.size() != msg.info.width * msg.info.height) {
+    return false;                                                          // check map-size
+  }
+  return true;
+}
+
+
+}  //  namespace nav2_util
+
+
+#endif  // NAV2_UTIL__VALIDATE_MESSAGES_HPP_

navigation2/nav2_util/package.xml

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_util</name>
+  <version>1.1.18</version>
+  <description>TODO</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
+  <license>Apache-2.0</license>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>libboost-program-options-dev</build_depend>
+  <depend>nav2_common</depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>bondcpp</depend>
+  <depend>bond</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>launch</depend>
+  <depend>launch_testing_ament_cmake</depend>
+  <depend>action_msgs</depend>
+  <depend>rcl_interfaces</depend>
+
+  <exec_depend>libboost-program-options</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>std_srvs</test_depend>
+  <test_depend>test_msgs</test_depend>
+  <test_depend>action_msgs</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation2/nav2_util/src/CMakeLists.txt

@@ -0,0 +1,43 @@
+add_library(${library_name} SHARED
+  costmap.cpp
+  node_utils.cpp
+  lifecycle_service_client.cpp
+  string_utils.cpp
+  lifecycle_utils.cpp
+  lifecycle_node.cpp
+  robot_utils.cpp
+  node_thread.cpp
+  odometry_utils.cpp
+)
+
+ament_target_dependencies(${library_name}
+  rclcpp
+  nav2_msgs
+  tf2
+  tf2_ros
+  nav_msgs
+  geometry_msgs
+  lifecycle_msgs
+  rclcpp_lifecycle
+  tf2_geometry_msgs
+  bondcpp
+)
+
+add_executable(lifecycle_bringup
+  lifecycle_bringup_commandline.cpp
+)
+target_link_libraries(lifecycle_bringup ${library_name})
+
+find_package(Boost REQUIRED COMPONENTS program_options)
+
+install(TARGETS
+  ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS
+  lifecycle_bringup
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)

navigation2/nav2_util/src/costmap.cpp

@@ -0,0 +1,266 @@
+// 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 <vector>
+#include <algorithm>
+#include "nav2_util/costmap.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "nav2_util/geometry_utils.hpp"
+
+using std::vector;
+
+namespace nav2_util
+{
+using nav2_util::geometry_utils::orientationAroundZAxis;
+
+const Costmap::CostValue Costmap::no_information = 255;
+const Costmap::CostValue Costmap::lethal_obstacle = 254;
+const Costmap::CostValue Costmap::inscribed_inflated_obstacle = 253;
+const Costmap::CostValue Costmap::medium_cost = 128;
+const Costmap::CostValue Costmap::free_space = 0;
+
+// TODO(orduno): Port ROS1 Costmap package
+Costmap::Costmap(
+  rclcpp::Node * node, bool trinary_costmap, bool track_unknown_space,
+  int lethal_threshold, int unknown_cost_value)
+: node_(node), trinary_costmap_(trinary_costmap), track_unknown_space_(track_unknown_space),
+  lethal_threshold_(lethal_threshold), unknown_cost_value_(unknown_cost_value)
+{
+  if (lethal_threshold_ < 0. || lethal_threshold_ > 100.) {
+    RCLCPP_WARN(
+      node_->get_logger(), "Costmap: Lethal threshold set to %d, it should be within"
+      " bounds 0-100. This could result in potential collisions!", lethal_threshold_);
+    // lethal_threshold_ = std::max(std::min(lethal_threshold_, 100), 0);
+  }
+}
+
+Costmap::~Costmap()
+{
+}
+
+void Costmap::set_static_map(const nav_msgs::msg::OccupancyGrid & occupancy_grid)
+{
+  RCLCPP_INFO(node_->get_logger(), "Costmap: Setting static costmap");
+
+  costmap_properties_.map_load_time = node_->now();
+  costmap_properties_.update_time = node_->now();
+  costmap_properties_.layer = "Master";
+
+  // Store the properties of the occupancy grid
+  costmap_properties_.resolution = occupancy_grid.info.resolution;
+  costmap_properties_.size_x = occupancy_grid.info.width;
+  costmap_properties_.size_y = occupancy_grid.info.height;
+  costmap_properties_.origin = occupancy_grid.info.origin;
+
+  uint32_t size_x = costmap_properties_.size_x;
+  uint32_t size_y = costmap_properties_.size_y;
+
+  costs_.resize(size_x * size_y);
+
+  // TODO(orduno): for now just doing a direct mapping of values from the original static map
+  //               i.e. no cell inflation, etc.
+  std::vector<int8_t> static_map_cell_values = occupancy_grid.data;
+
+  unsigned int index = 0;
+  for (unsigned int i = 0; i < size_y; ++i) {
+    for (unsigned int j = 0; j < size_x; ++j) {
+      unsigned char value = static_map_cell_values[index];
+      costs_[index] = interpret_value(value);
+      ++index;
+    }
+  }
+
+  map_provided_ = true;
+}
+
+void Costmap::set_test_costmap(const TestCostmap & testCostmapType)
+{
+  costmap_properties_.map_load_time = node_->now();
+  costmap_properties_.update_time = node_->now();
+  costmap_properties_.layer = "master";
+  costmap_properties_.resolution = 1;
+  costmap_properties_.size_x = 10;
+  costmap_properties_.size_y = 10;
+  costmap_properties_.origin.position.x = 0.0;
+  costmap_properties_.origin.position.y = 0.0;
+  costmap_properties_.origin.position.z = 0.0;
+
+  // Define map rotation
+  // Provided as yaw with counterclockwise rotation, with yaw = 0 meaning no rotation
+  costmap_properties_.origin.orientation = orientationAroundZAxis(0.0);
+
+  costs_ = get_test_data(testCostmapType);
+
+  using_test_map_ = true;
+}
+
+nav2_msgs::msg::Costmap Costmap::get_costmap(
+  const nav2_msgs::msg::CostmapMetaData & /*specifications*/)
+{
+  if (!map_provided_ && !using_test_map_) {
+    throw std::runtime_error("Costmap has not been set.");
+  }
+
+  // TODO(orduno): build a costmap given the specifications
+  //               for now using the specs of the static map
+
+  nav2_msgs::msg::Costmap costmap;
+
+  costmap.header.stamp = node_->now();
+  costmap.header.frame_id = "map";
+  costmap.metadata = costmap_properties_;
+  costmap.data = costs_;
+
+  return costmap;
+}
+
+vector<uint8_t> Costmap::get_test_data(const TestCostmap testCostmapType)
+{
+  // TODO(orduno): alternatively use a mathematical function
+
+  const uint8_t n = no_information;
+  const uint8_t x = lethal_obstacle;
+  const uint8_t i = inscribed_inflated_obstacle;
+  const uint8_t u = medium_cost;
+  const uint8_t o = free_space;
+
+  vector<uint8_t> costmapFree =
+    // 0  1  2  3  4  5  6  7  8  9
+  {o, o, o, o, o, o, o, o, o, o,     // 0
+    o, o, o, o, o, o, o, o, o, o,    // 1
+    o, o, o, o, o, o, o, o, o, o,    // 2
+    o, o, o, o, o, o, o, o, o, o,    // 3
+    o, o, o, o, o, o, o, o, o, o,    // 4
+    o, o, o, o, o, o, o, o, o, o,    // 5
+    o, o, o, o, o, o, o, o, o, o,    // 6
+    o, o, o, o, o, o, o, o, o, o,    // 7
+    o, o, o, o, o, o, o, o, o, o,    // 8
+    o, o, o, o, o, o, o, o, o, o};   // 9
+
+  vector<uint8_t> costmapBounded =
+    // 0  1  2  3  4  5  6  7  8  9
+  {n, n, n, n, n, n, n, n, n, n,     // 0
+    n, o, o, o, o, o, o, o, o, n,    // 1
+    n, o, o, o, o, o, o, o, o, n,    // 2
+    n, o, o, o, o, o, o, o, o, n,    // 3
+    n, o, o, o, o, o, o, o, o, n,    // 4
+    n, o, o, o, o, o, o, o, o, n,    // 5
+    n, o, o, o, o, o, o, o, o, n,    // 6
+    n, o, o, o, o, o, o, o, o, n,    // 7
+    n, o, o, o, o, o, o, o, o, n,    // 8
+    n, n, n, n, n, n, n, n, n, n};   // 9
+
+  vector<uint8_t> costmapObstacleBL =
+    // 0  1  2  3  4  5  6  7  8  9
+  {n, n, n, n, n, n, n, n, n, n,     // 0
+    n, o, o, o, o, o, o, o, o, n,    // 1
+    n, o, o, o, o, o, o, o, o, n,    // 2
+    n, o, o, o, o, o, o, o, o, n,    // 3
+    n, o, o, o, o, o, o, o, o, n,    // 4
+    n, o, x, x, x, o, o, o, o, n,    // 5
+    n, o, x, x, x, o, o, o, o, n,    // 6
+    n, o, x, x, x, o, o, o, o, n,    // 7
+    n, o, o, o, o, o, o, o, o, n,    // 8
+    n, n, n, n, n, n, n, n, n, n};   // 9
+
+  vector<uint8_t> costmapObstacleTL =
+    // 0  1  2  3  4  5  6  7  8  9
+  {n, n, n, n, n, n, n, n, n, n,     // 0
+    n, o, o, o, o, o, o, o, o, n,    // 1
+    n, o, x, x, x, o, o, o, o, n,    // 2
+    n, o, x, x, x, o, o, o, o, n,    // 3
+    n, o, x, x, x, o, o, o, o, n,    // 4
+    n, o, o, o, o, o, o, o, o, n,    // 5
+    n, o, o, o, o, o, o, o, o, n,    // 6
+    n, o, o, o, o, o, o, o, o, n,    // 7
+    n, o, o, o, o, o, o, o, o, n,    // 8
+    n, n, n, n, n, n, n, n, n, n};   // 9
+
+  vector<uint8_t> costmapMaze =
+    // 0  1  2  3  4  5  6  7  8  9
+  {n, n, n, n, n, n, n, n, n, n,     // 0
+    n, o, o, o, o, o, o, o, o, n,    // 1
+    n, x, x, o, x, x, x, o, x, n,    // 2
+    n, o, o, o, o, x, o, o, o, n,    // 3
+    n, o, x, x, o, x, o, x, o, n,    // 4
+    n, o, x, x, o, x, o, x, o, n,    // 5
+    n, o, o, x, o, x, o, x, o, n,    // 6
+    n, x, o, x, o, x, o, x, o, n,    // 7
+    n, o, o, o, o, o, o, x, o, n,    // 8
+    n, n, n, n, n, n, n, n, n, n};   // 9
+
+  vector<uint8_t> costmapMaze2 =
+    // 0  1  2  3  4  5  6  7  8  9
+  {n, n, n, n, n, n, n, n, n, n,     // 0
+    n, o, o, o, o, o, o, o, o, n,    // 1
+    n, x, x, u, x, x, x, o, x, n,    // 2
+    n, o, o, o, o, o, o, o, u, n,    // 3
+    n, o, x, x, o, x, x, x, u, n,    // 4
+    n, o, x, x, o, o, o, x, u, n,    // 5
+    n, o, o, x, u, x, o, x, u, n,    // 6
+    n, x, o, x, u, x, i, x, u, n,    // 7
+    n, o, o, o, o, o, o, o, o, n,    // 8
+    n, n, n, n, n, n, n, n, n, n};   // 9
+
+  switch (testCostmapType) {
+    case TestCostmap::open_space:
+      return costmapFree;
+    case TestCostmap::bounded:
+      return costmapBounded;
+    case TestCostmap::bottom_left_obstacle:
+      return costmapObstacleBL;
+    case TestCostmap::top_left_obstacle:
+      return costmapObstacleTL;
+    case TestCostmap::maze1:
+      return costmapMaze;
+    case TestCostmap::maze2:
+      return costmapMaze2;
+    default:
+      return costmapFree;
+  }
+}
+
+uint8_t Costmap::interpret_value(const int8_t value) const
+{
+  if (track_unknown_space_ && value == unknown_cost_value_) {
+    return no_information;
+  } else if (!track_unknown_space_ && value == unknown_cost_value_) {
+    return free_space;
+  } else if (value >= lethal_threshold_) {
+    return lethal_obstacle;
+  } else if (trinary_costmap_) {
+    return free_space;
+  }
+
+  double scale = static_cast<double>(value / lethal_threshold_);
+  return static_cast<uint8_t>(scale * lethal_obstacle);
+}
+
+bool Costmap::is_free(const unsigned int x_coordinate, const unsigned int y_coordinate) const
+{
+  unsigned int index = y_coordinate * costmap_properties_.size_x + x_coordinate;
+
+  return is_free(index);
+}
+
+bool Costmap::is_free(const unsigned int index) const
+{
+  if (costs_[index] < Costmap::inscribed_inflated_obstacle) {
+    return true;
+  }
+
+  return false;
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/lifecycle_bringup_commandline.cpp

@@ -0,0 +1,47 @@
+// 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 <vector>
+#include <string>
+#include <iostream>
+#include <cstdlib>
+#include <chrono>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+
+using std::cerr;
+using namespace std::chrono_literals;
+
+void usage()
+{
+  cerr << "Invalid command line.\n\n";
+  cerr << "This command will take a set of unconfigured lifecycle nodes through the\n";
+  cerr << "CONFIGURED to the ACTIVATED state\n";
+  cerr << "The nodes are brought up in the order listed on the command line\n\n";
+  cerr << "Usage:\n";
+  cerr << " > lifecycle_startup <node name> ...\n";
+  std::exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+  if (argc == 1) {
+    usage();
+  }
+  rclcpp::init(0, nullptr);
+  nav2_util::startup_lifecycle_nodes(
+    std::vector<std::string>(argv + 1, argv + argc),
+    10s);
+  rclcpp::shutdown();
+}

navigation2/nav2_util/src/lifecycle_node.cpp

@@ -0,0 +1,129 @@
+// 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 "nav2_util/lifecycle_node.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "lifecycle_msgs/msg/state.hpp"
+
+namespace nav2_util
+{
+
+LifecycleNode::LifecycleNode(
+  const std::string & node_name,
+  const std::string & ns,
+  const rclcpp::NodeOptions & options)
+: rclcpp_lifecycle::LifecycleNode(node_name, ns, options)
+{
+  // server side never times out from lifecycle manager
+  this->declare_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true);
+  this->set_parameter(
+    rclcpp::Parameter(
+      bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true));
+
+  printLifecycleNodeNotification();
+
+  register_rcl_preshutdown_callback();
+}
+
+LifecycleNode::~LifecycleNode()
+{
+  RCLCPP_INFO(get_logger(), "Destroying");
+
+  runCleanups();
+
+  if (rcl_preshutdown_cb_handle_) {
+    rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+    context->remove_pre_shutdown_callback(*(rcl_preshutdown_cb_handle_.get()));
+    rcl_preshutdown_cb_handle_.reset();
+  }
+}
+
+void LifecycleNode::createBond()
+{
+  RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name());
+
+  bond_ = std::make_unique<bond::Bond>(
+    std::string("bond"),
+    this->get_name(),
+    shared_from_this());
+
+  bond_->setHeartbeatPeriod(0.10);
+  bond_->setHeartbeatTimeout(4.0);
+  bond_->start();
+}
+
+void LifecycleNode::runCleanups()
+{
+  /*
+   * In case this lifecycle node wasn't properly shut down, do it here.
+   * We will give the user some ability to clean up properly here, but it's
+   * best effort; i.e. we aren't trying to account for all possible states.
+   */
+  if (get_current_state().id() ==
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    this->deactivate();
+  }
+
+  if (get_current_state().id() ==
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+  {
+    this->cleanup();
+  }
+}
+
+void LifecycleNode::on_rcl_preshutdown()
+{
+  RCLCPP_INFO(
+    get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)",
+    this->get_name());
+
+  runCleanups();
+
+  destroyBond();
+}
+
+void LifecycleNode::register_rcl_preshutdown_callback()
+{
+  rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+
+  rcl_preshutdown_cb_handle_ = std::make_unique<rclcpp::PreShutdownCallbackHandle>(
+    context->add_pre_shutdown_callback(
+      std::bind(&LifecycleNode::on_rcl_preshutdown, this))
+  );
+}
+
+void LifecycleNode::destroyBond()
+{
+  RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name());
+
+  if (bond_) {
+    bond_.reset();
+  }
+}
+
+void LifecycleNode::printLifecycleNodeNotification()
+{
+  RCLCPP_INFO(
+    get_logger(),
+    "\n\t%s lifecycle node launched. \n"
+    "\tWaiting on external lifecycle transitions to activate\n"
+    "\tSee https://design.ros2.org/articles/node_lifecycle.html for more information.", get_name());
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/lifecycle_service_client.cpp

@@ -0,0 +1,102 @@
+// 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 "nav2_util/lifecycle_service_client.hpp"
+
+#include <string>
+#include <chrono>
+#include <memory>
+
+#include "lifecycle_msgs/srv/change_state.hpp"
+#include "lifecycle_msgs/srv/get_state.hpp"
+
+using nav2_util::generate_internal_node;
+using std::chrono::seconds;
+using std::make_shared;
+using std::string;
+using namespace std::chrono_literals;
+
+namespace nav2_util
+{
+
+LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_name)
+: node_(generate_internal_node(lifecycle_node_name + "_lifecycle_client")),
+  change_state_(lifecycle_node_name + "/change_state", node_),
+  get_state_(lifecycle_node_name + "/get_state", node_)
+{
+  // Block until server is up
+  rclcpp::Rate r(20);
+  while (!get_state_.wait_for_service(2s)) {
+    RCLCPP_INFO(
+      node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str());
+    r.sleep();
+  }
+}
+
+LifecycleServiceClient::LifecycleServiceClient(
+  const string & lifecycle_node_name,
+  rclcpp::Node::SharedPtr parent_node)
+: node_(parent_node),
+  change_state_(lifecycle_node_name + "/change_state", node_),
+  get_state_(lifecycle_node_name + "/get_state", node_)
+{
+  // Block until server is up
+  rclcpp::Rate r(20);
+  while (!get_state_.wait_for_service(2s)) {
+    RCLCPP_INFO(
+      node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str());
+    r.sleep();
+  }
+}
+
+bool LifecycleServiceClient::change_state(
+  const uint8_t transition,
+  const seconds timeout)
+{
+  if (!change_state_.wait_for_service(timeout)) {
+    throw std::runtime_error("change_state service is not available!");
+  }
+
+  auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
+  request->transition.id = transition;
+  auto response = change_state_.invoke(request, timeout);
+  return response.get();
+}
+
+bool LifecycleServiceClient::change_state(
+  std::uint8_t transition)
+{
+  if (!change_state_.wait_for_service(5s)) {
+    throw std::runtime_error("change_state service is not available!");
+  }
+
+  auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
+  auto response = std::make_shared<lifecycle_msgs::srv::ChangeState::Response>();
+  request->transition.id = transition;
+  return change_state_.invoke(request, response);
+}
+
+uint8_t LifecycleServiceClient::get_state(
+  const seconds timeout)
+{
+  if (!get_state_.wait_for_service(timeout)) {
+    throw std::runtime_error("get_state service is not available!");
+  }
+
+  auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();
+  auto result = get_state_.invoke(request, timeout);
+  return result->current_state.id;
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/lifecycle_utils.cpp

@@ -0,0 +1,101 @@
+// 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 <string>
+#include <thread>
+#include <vector>
+
+#include "lifecycle_msgs/srv/change_state.hpp"
+#include "lifecycle_msgs/srv/get_state.hpp"
+#include "nav2_util/lifecycle_service_client.hpp"
+
+using std::string;
+using lifecycle_msgs::msg::Transition;
+
+namespace nav2_util
+{
+
+#define RETRY(fn, retries) \
+  { \
+    int count = 0; \
+    while (true) { \
+      try { \
+        fn; \
+        break; \
+      } catch (std::runtime_error & e) { \
+        ++count; \
+        if (count > (retries)) { \
+          throw e;} \
+      } \
+    } \
+  }
+
+static void startupLifecycleNode(
+  const std::string & node_name,
+  const std::chrono::seconds service_call_timeout,
+  const int retries)
+{
+  LifecycleServiceClient sc(node_name);
+
+  // Despite waiting for the service to be available and using reliable transport
+  // service calls still frequently hang. To get reliable startup it's necessary
+  // to timeout the service call and retry it when that happens.
+  RETRY(
+    sc.change_state(Transition::TRANSITION_CONFIGURE, service_call_timeout),
+    retries);
+  RETRY(
+    sc.change_state(Transition::TRANSITION_ACTIVATE, service_call_timeout),
+    retries);
+}
+
+void startup_lifecycle_nodes(
+  const std::vector<std::string> & node_names,
+  const std::chrono::seconds service_call_timeout,
+  const int retries)
+{
+  for (const auto & node_name : node_names) {
+    startupLifecycleNode(node_name, service_call_timeout, retries);
+  }
+}
+
+static void resetLifecycleNode(
+  const std::string & node_name,
+  const std::chrono::seconds service_call_timeout,
+  const int retries)
+{
+  LifecycleServiceClient sc(node_name);
+
+  // Despite waiting for the service to be available and using reliable transport
+  // service calls still frequently hang. To get reliable reset it's necessary
+  // to timeout the service call and retry it when that happens.
+  RETRY(
+    sc.change_state(Transition::TRANSITION_DEACTIVATE, service_call_timeout),
+    retries);
+  RETRY(
+    sc.change_state(Transition::TRANSITION_CLEANUP, service_call_timeout),
+    retries);
+}
+
+void reset_lifecycle_nodes(
+  const std::vector<std::string> & node_names,
+  const std::chrono::seconds service_call_timeout,
+  const int retries)
+{
+  for (const auto & node_name : node_names) {
+    resetLifecycleNode(node_name, service_call_timeout, retries);
+  }
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/node_thread.cpp

@@ -0,0 +1,47 @@
+// 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 "nav2_util/node_thread.hpp"
+
+namespace nav2_util
+{
+
+NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base)
+: node_(node_base)
+{
+  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  thread_ = std::make_unique<std::thread>(
+    [&]()
+    {
+      executor_->add_node(node_);
+      executor_->spin();
+      executor_->remove_node(node_);
+    });
+}
+
+NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor)
+: executor_(executor)
+{
+  thread_ = std::make_unique<std::thread>([&]() {executor_->spin();});
+}
+
+NodeThread::~NodeThread()
+{
+  executor_->cancel();
+  thread_->join();
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/node_utils.cpp

@@ -0,0 +1,92 @@
+// 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 "nav2_util/node_utils.hpp"
+#include <chrono>
+#include <string>
+#include <algorithm>
+#include <cctype>
+
+using std::chrono::high_resolution_clock;
+using std::to_string;
+using std::string;
+using std::replace_if;
+using std::isalnum;
+
+namespace nav2_util
+{
+
+string sanitize_node_name(const string & potential_node_name)
+{
+  string node_name(potential_node_name);
+  // read this as `replace` characters in `node_name` `if` not alphanumeric.
+  // replace with '_'
+  replace_if(
+    begin(node_name), end(node_name),
+    [](auto c) {return !isalnum(c);},
+    '_');
+  return node_name;
+}
+
+string add_namespaces(const string & top_ns, const string & sub_ns)
+{
+  if (!top_ns.empty() && top_ns.back() == '/') {
+    if (top_ns.front() == '/') {
+      return top_ns + sub_ns;
+    } else {
+      return "/" + top_ns + sub_ns;
+    }
+  }
+
+  return top_ns + "/" + sub_ns;
+}
+
+std::string time_to_string(size_t len)
+{
+  string output(len, '0');  // prefill the string with zeros
+  auto timepoint = high_resolution_clock::now();
+  auto timecount = timepoint.time_since_epoch().count();
+  auto timestring = to_string(timecount);
+  if (timestring.length() >= len) {
+    // if `timestring` is shorter, put it at the end of `output`
+    output.replace(
+      0, len,
+      timestring,
+      timestring.length() - len, len);
+  } else {
+    // if `output` is shorter, just copy in the end of `timestring`
+    output.replace(
+      len - timestring.length(), timestring.length(),
+      timestring,
+      0, timestring.length());
+  }
+  return output;
+}
+
+std::string generate_internal_node_name(const std::string & prefix)
+{
+  return sanitize_node_name(prefix) + "_" + time_to_string(8);
+}
+
+rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix)
+{
+  auto options =
+    rclcpp::NodeOptions()
+    .start_parameter_services(false)
+    .start_parameter_event_publisher(false)
+    .arguments({"--ros-args", "-r", "__node:=" + generate_internal_node_name(prefix), "--"});
+  return rclcpp::Node::make_shared("_", options);
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/odometry_utils.cpp

@@ -0,0 +1,121 @@
+// 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 <string>
+
+#include "nav2_util/odometry_utils.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+namespace nav2_util
+{
+
+OdomSmoother::OdomSmoother(
+  const rclcpp::Node::WeakPtr & parent,
+  double filter_duration,
+  const std::string & odom_topic)
+: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration))
+{
+  auto node = parent.lock();
+  odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
+    odom_topic,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1));
+
+  odom_cumulate_.twist.twist.linear.x = 0;
+  odom_cumulate_.twist.twist.linear.y = 0;
+  odom_cumulate_.twist.twist.linear.z = 0;
+  odom_cumulate_.twist.twist.angular.x = 0;
+  odom_cumulate_.twist.twist.angular.y = 0;
+  odom_cumulate_.twist.twist.angular.z = 0;
+}
+
+OdomSmoother::OdomSmoother(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  double filter_duration,
+  const std::string & odom_topic)
+: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration))
+{
+  auto node = parent.lock();
+  odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
+    odom_topic,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1));
+
+  odom_cumulate_.twist.twist.linear.x = 0;
+  odom_cumulate_.twist.twist.linear.y = 0;
+  odom_cumulate_.twist.twist.linear.z = 0;
+  odom_cumulate_.twist.twist.angular.x = 0;
+  odom_cumulate_.twist.twist.angular.y = 0;
+  odom_cumulate_.twist.twist.angular.z = 0;
+}
+
+void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(odom_mutex_);
+
+  // update cumulated odom only if history is not empty
+  if (!odom_history_.empty()) {
+    // to store current time
+    auto current_time = rclcpp::Time(msg->header.stamp);
+
+    // to store time of the first odom in history
+    auto front_time = rclcpp::Time(odom_history_.front().header.stamp);
+
+    // update cumulated odom when duration has exceeded and pop earliest msg
+    while (current_time - front_time > odom_history_duration_) {
+      const auto & odom = odom_history_.front();
+      odom_cumulate_.twist.twist.linear.x -= odom.twist.twist.linear.x;
+      odom_cumulate_.twist.twist.linear.y -= odom.twist.twist.linear.y;
+      odom_cumulate_.twist.twist.linear.z -= odom.twist.twist.linear.z;
+      odom_cumulate_.twist.twist.angular.x -= odom.twist.twist.angular.x;
+      odom_cumulate_.twist.twist.angular.y -= odom.twist.twist.angular.y;
+      odom_cumulate_.twist.twist.angular.z -= odom.twist.twist.angular.z;
+      odom_history_.pop_front();
+
+      if (odom_history_.empty()) {
+        break;
+      }
+
+      // update with the timestamp of earliest odom message in history
+      front_time = rclcpp::Time(odom_history_.front().header.stamp);
+    }
+  }
+
+  odom_history_.push_back(*msg);
+  updateState();
+}
+
+void OdomSmoother::updateState()
+{
+  const auto & odom = odom_history_.back();
+  odom_cumulate_.twist.twist.linear.x += odom.twist.twist.linear.x;
+  odom_cumulate_.twist.twist.linear.y += odom.twist.twist.linear.y;
+  odom_cumulate_.twist.twist.linear.z += odom.twist.twist.linear.z;
+  odom_cumulate_.twist.twist.angular.x += odom.twist.twist.angular.x;
+  odom_cumulate_.twist.twist.angular.y += odom.twist.twist.angular.y;
+  odom_cumulate_.twist.twist.angular.z += odom.twist.twist.angular.z;
+
+  vel_smooth_.header = odom.header;
+  vel_smooth_.twist.linear.x = odom_cumulate_.twist.twist.linear.x / odom_history_.size();
+  vel_smooth_.twist.linear.y = odom_cumulate_.twist.twist.linear.y / odom_history_.size();
+  vel_smooth_.twist.linear.z = odom_cumulate_.twist.twist.linear.z / odom_history_.size();
+  vel_smooth_.twist.angular.x = odom_cumulate_.twist.twist.angular.x / odom_history_.size();
+  vel_smooth_.twist.angular.y = odom_cumulate_.twist.twist.angular.y / odom_history_.size();
+  vel_smooth_.twist.angular.z = odom_cumulate_.twist.twist.angular.z / odom_history_.size();
+}
+
+}  // namespace nav2_util

navigation2/nav2_util/src/robot_utils.cpp

@@ -0,0 +1,174 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2019 Steven Macenski
+// Copyright (c) 2019 Samsung Research America
+//
+// 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 <cmath>
+#include <memory>
+
+#include "nav2_util/robot_utils.hpp"
+#include "rclcpp/logger.hpp"
+
+namespace nav2_util
+{
+
+bool getCurrentPose(
+  geometry_msgs::msg::PoseStamped & global_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string global_frame,
+  const std::string robot_frame, const double transform_timeout,
+  const rclcpp::Time stamp)
+{
+  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
+  global_pose.header.frame_id = robot_frame;
+  global_pose.header.stamp = stamp;
+
+  return transformPoseInTargetFrame(
+    global_pose, global_pose, tf_buffer, global_frame, transform_timeout);
+}
+
+bool transformPoseInTargetFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
+  const double transform_timeout)
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
+
+  try {
+    transformed_pose = tf_buffer.transform(
+      input_pose, target_frame,
+      tf2::durationFromSec(transform_timeout));
+    return true;
+  } catch (tf2::LookupException & ex) {
+    RCLCPP_ERROR(
+      logger,
+      "No Transform available Error looking up target frame: %s\n", ex.what());
+  } catch (tf2::ConnectivityException & ex) {
+    RCLCPP_ERROR(
+      logger,
+      "Connectivity Error looking up target frame: %s\n", ex.what());
+  } catch (tf2::ExtrapolationException & ex) {
+    RCLCPP_ERROR(
+      logger,
+      "Extrapolation Error looking up target frame: %s\n", ex.what());
+  } catch (tf2::TimeoutException & ex) {
+    RCLCPP_ERROR(
+      logger,
+      "Transform timeout with tolerance: %.4f", transform_timeout);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      logger, "Failed to transform from %s to %s",
+      input_pose.header.frame_id.c_str(), target_frame.c_str());
+  }
+
+  return false;
+}
+
+bool getTransform(
+  const std::string & source_frame_id,
+  const std::string & target_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  tf2_transform.setIdentity();  // initialize by identical transform
+
+  if (source_frame_id == target_frame_id) {
+    // We are already in required frame
+    return true;
+  }
+
+  try {
+    // Obtaining the transform to get data from source to target frame
+    transform = tf_buffer->lookupTransform(
+      target_frame_id, source_frame_id,
+      tf2::TimePointZero, transform_tolerance);
+  } catch (tf2::TransformException & e) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("getTransform"),
+      "Failed to get \"%s\"->\"%s\" frame transform: %s",
+      source_frame_id.c_str(), target_frame_id.c_str(), e.what());
+    return false;
+  }
+
+  // Convert TransformStamped to TF2 transform
+  tf2::fromMsg(transform.transform, tf2_transform);
+  return true;
+}
+
+bool getTransform(
+  const std::string & source_frame_id,
+  const rclcpp::Time & source_time,
+  const std::string & target_frame_id,
+  const rclcpp::Time & target_time,
+  const std::string & fixed_frame_id,
+  const tf2::Duration & transform_tolerance,
+  const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+  tf2::Transform & tf2_transform)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  tf2_transform.setIdentity();  // initialize by identical transform
+
+  try {
+    // Obtaining the transform to get data from source to target frame.
+    // This also considers the time shift between source and target.
+    transform = tf_buffer->lookupTransform(
+      target_frame_id, target_time,
+      source_frame_id, source_time,
+      fixed_frame_id, transform_tolerance);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("getTransform"),
+      "Failed to get \"%s\"->\"%s\" frame transform: %s",
+      source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
+    return false;
+  }
+
+  // Convert TransformStamped to TF2 transform
+  tf2::fromMsg(transform.transform, tf2_transform);
+  return true;
+}
+
+bool validateTwist(const geometry_msgs::msg::Twist & msg)
+{
+  if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) {
+    return false;
+  }
+
+  if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) {
+    return false;
+  }
+
+  if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) {
+    return false;
+  }
+
+  if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) {
+    return false;
+  }
+
+  if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) {
+    return false;
+  }
+
+  if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) {
+    return false;
+  }
+
+  return true;
+}
+
+}  // end namespace nav2_util

navigation2/nav2_util/src/string_utils.cpp

@@ -0,0 +1,48 @@
+// 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 "nav2_util/string_utils.hpp"
+#include <string>
+
+using std::string;
+
+namespace nav2_util
+{
+
+std::string strip_leading_slash(const string & in)
+{
+  string out = in;
+
+  if ((!in.empty()) && (in[0] == '/')) {
+    out.erase(0, 1);
+  }
+
+  return out;
+}
+
+Tokens split(const string & tokenstring, char delimiter)
+{
+  Tokens tokens;
+
+  size_t current_pos = 0;
+  size_t pos = 0;
+  while ((pos = tokenstring.find(delimiter, current_pos)) != string::npos) {
+    tokens.push_back(tokenstring.substr(current_pos, pos - current_pos));
+    current_pos = pos + 1;
+  }
+  tokens.push_back(tokenstring.substr(current_pos));
+  return tokens;
+}
+
+}  // namespace nav2_util

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