add humble-navigation2

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_