add humble-navigation2

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