add humble-navigation2

navigation2/nav2_lifecycle_manager/CMakeLists.txt

@@ -0,0 +1,95 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_lifecycle_manager)
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(bondcpp REQUIRED)
+find_package(diagnostic_updater REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(library_name ${PROJECT_NAME}_core)
+
+add_library(${library_name} SHARED
+  src/lifecycle_manager.cpp
+  src/lifecycle_manager_client.cpp
+)
+
+set(dependencies
+  geometry_msgs
+  lifecycle_msgs
+  nav2_msgs
+  nav2_util
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  rclcpp_components
+  std_msgs
+  std_srvs
+  tf2_geometry_msgs
+  bondcpp
+  diagnostic_updater
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+add_executable(lifecycle_manager
+  src/main.cpp
+)
+
+target_link_libraries(lifecycle_manager
+  ${library_name}
+)
+
+ament_target_dependencies(lifecycle_manager
+  ${dependencies}
+)
+
+rclcpp_components_register_nodes(${library_name} "nav2_lifecycle_manager::LifecycleManager")
+
+install(TARGETS
+  ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS
+  lifecycle_manager
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/ DESTINATION include/)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+ament_package()

navigation2/nav2_lifecycle_manager/README.md

@@ -0,0 +1,18 @@
+### Background on lifecycle enabled nodes
+Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in Nav2, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```.
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions.
+
+### nav2_lifecycle_manager
+Nav2's lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.), but it is meant to be called on bringup through a production system application.
+
+In order to start the navigation stack and be able to navigate, the necessary nodes must be configured and activated. Thus, for example when _startup_ is requested from the lifecycle manager's manage_nodes service, the lifecycle managers calls _configure()_ and _activate()_ on the lifecycle enabled nodes in the node list. These are all transitioned in ordered groups for bringup transitions, and reverse ordered groups for shutdown transitions.
+
+The lifecycle manager has a default nodes list for all the nodes that it manages. This list can be changed using the lifecycle manager’s _“node_names”_ parameter.
+
+The diagram below shows an _example_ of a list of managed nodes, and how it interfaces with the lifecycle manager.
+<img src="./doc/diagram_lifecycle_manager.JPG" title="" width="100%" align="middle">
+
+The UML diagram below shows the sequence of service calls once the _startup_ is requested from the lifecycle manager.
+
+<img src="./doc/uml_lifecycle_manager.JPG" title="Lifecycle manager UML diagram" width="100%" align="middle">

navigation2/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -0,0 +1,237 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2022 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_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
+#define NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+#include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
+#include "std_srvs/srv/trigger.hpp"
+#include "bondcpp/bond.hpp"
+#include "diagnostic_updater/diagnostic_updater.hpp"
+
+
+namespace nav2_lifecycle_manager
+{
+using namespace std::chrono_literals;  // NOLINT
+
+using nav2_msgs::srv::ManageLifecycleNodes;
+/**
+ * @class nav2_lifecycle_manager::LifecycleManager
+ * @brief Implements service interface to transition the lifecycle nodes of
+ * Nav2 stack. It receives transition request and then uses lifecycle
+ * interface to change lifecycle node's state.
+ */
+class LifecycleManager : public rclcpp::Node
+{
+public:
+  /**
+   * @brief A constructor for nav2_lifecycle_manager::LifecycleManager
+   * @param options Additional options to control creation of the node.
+   */
+  explicit LifecycleManager(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief A destructor for nav2_lifecycle_manager::LifecycleManager
+   */
+  ~LifecycleManager();
+
+protected:
+  // Callback group used by services and timers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  std::unique_ptr<nav2_util::NodeThread> service_thread_;
+
+  // The services provided by this node
+  rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;
+  /**
+   * @brief Lifecycle node manager callback function
+   * @param request_header Header of the service request
+   * @param request Service request
+   * @param reponse Service response
+   */
+  void managerCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<ManageLifecycleNodes::Request> request,
+    std::shared_ptr<ManageLifecycleNodes::Response> response);
+  /**
+   * @brief Trigger callback function checks if the managed nodes are in active
+   * state.
+   * @param request_header Header of the request
+   * @param request Service request
+   * @param reponse Service response
+   */
+  void isActiveCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+
+  // Support functions for the service calls
+  /**
+   * @brief Start up managed nodes.
+   * @return true or false
+   */
+  bool startup();
+  /**
+   * @brief Deactivate, clean up and shut down all the managed nodes.
+   * @return true or false
+   */
+  bool shutdown();
+  /**
+   * @brief Reset all the managed nodes.
+   * @return true or false
+   */
+  bool reset(bool hard_reset = false);
+  /**
+   * @brief Pause all the managed nodes.
+   * @return true or false
+   */
+  bool pause();
+  /**
+   * @brief Resume all the managed nodes.
+   * @return true or false
+   */
+  bool resume();
+
+  /**
+   * @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 or shutdown().
+   */
+  void onRclPreshutdown();
+
+  // Support function for creating service clients
+  /**
+   * @brief Support function for creating service clients
+   */
+  void createLifecycleServiceClients();
+
+  // Support functions for shutdown
+  /**
+   * @brief Support function for shutdown
+   */
+  void shutdownAllNodes();
+  /**
+   * @brief Destroy all the lifecycle service clients.
+   */
+  void destroyLifecycleServiceClients();
+
+  // Support function for creating bond timer
+  /**
+   * @brief Support function for creating bond timer
+   */
+  void createBondTimer();
+
+  // Support function for creating bond connection
+  /**
+   * @brief Support function for creating bond connections
+   */
+  bool createBondConnection(const std::string & node_name);
+
+  // Support function for killing bond connections
+  /**
+   * @brief Support function for killing bond connections
+   */
+  void destroyBondTimer();
+
+  // Support function for checking on bond connections
+  /**
+   * @ brief Support function for checking on bond connections
+   * will take down system if there's something non-responsive
+   */
+  void checkBondConnections();
+
+  // Support function for checking if bond connections come back after respawn
+  /**
+   * @ brief Support function for checking on bond connections
+   * will bring back the system if something goes from non-responsive to responsive
+   */
+  void checkBondRespawnConnection();
+
+  /**
+   * @brief For a node, transition to the new target state
+   */
+  bool changeStateForNode(
+    const std::string & node_name,
+    std::uint8_t transition);
+
+  /**
+   * @brief For each node in the map, transition to the new target state
+   */
+  bool changeStateForAllNodes(std::uint8_t transition, bool hard_change = false);
+
+  // Convenience function to highlight the output on the console
+  /**
+   * @brief Helper function to highlight the output on the console
+   */
+  void message(const std::string & msg);
+
+  // Diagnostics functions
+  /**
+   * @brief function to check if the Nav2 system is active
+   */
+  void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  /**
+   * 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 or the
+   * shutdown() instance function.
+   */
+  void registerRclPreshutdownCallback();
+
+  // Timer thread to look at bond connections
+  rclcpp::TimerBase::SharedPtr init_timer_;
+  rclcpp::TimerBase::SharedPtr bond_timer_;
+  rclcpp::TimerBase::SharedPtr bond_respawn_timer_;
+  std::chrono::milliseconds bond_timeout_;
+
+  // A map of all nodes to check bond connection
+  std::map<std::string, std::shared_ptr<bond::Bond>> bond_map_;
+
+  // A map of all nodes to be controlled
+  std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
+
+  std::map<std::uint8_t, std::string> transition_label_map_;
+
+  // A map of the expected transitions to primary states
+  std::unordered_map<std::uint8_t, std::uint8_t> transition_state_map_;
+
+  // The names of the nodes to be managed, in the order of desired bring-up
+  std::vector<std::string> node_names_;
+
+  // Whether to automatically start up the system
+  bool autostart_;
+  bool attempt_respawn_reconnection_;
+
+  bool system_active_{false};
+  diagnostic_updater::Updater diagnostics_updater_;
+
+  rclcpp::Time bond_respawn_start_time_{0};
+  rclcpp::Duration bond_respawn_max_duration_{10s};
+};
+
+}  // namespace nav2_lifecycle_manager
+
+#endif  // NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_

navigation2/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -0,0 +1,109 @@
+// 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_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
+#define NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
+
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
+#include "std_srvs/srv/trigger.hpp"
+#include "nav2_util/service_client.hpp"
+
+namespace nav2_lifecycle_manager
+{
+/**
+ * @enum nav2_lifecycle_manager::SystemStatus
+ * @brief Enum class representing the status of the system.
+ */
+enum class SystemStatus {ACTIVE, INACTIVE, TIMEOUT};
+/**
+ * @class nav2_lifecycle_manager::LifeCycleMangerClient
+ * @brief The LifecycleManagerClient sends requests to the LifecycleManager to
+ * control the lifecycle state of the navigation modules.
+ */
+class LifecycleManagerClient
+{
+public:
+  /**
+   * @brief A constructor for LifeCycleMangerClient
+   * @param name Managed node name
+   * @param parent_node Node that execute the service calls
+   */
+  explicit LifecycleManagerClient(
+    const std::string & name,
+    std::shared_ptr<rclcpp::Node> parent_node);
+
+  // Client-side interface to the Nav2 lifecycle manager
+  /**
+   * @brief Make start up service call
+   * @return true or false
+   */
+  bool startup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Make shutdown service call
+   * @return true or false
+   */
+  bool shutdown(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Make pause service call
+   * @return true or false
+   */
+  bool pause(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Make resume service call
+   * @return true or false
+   */
+  bool resume(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Make reset service call
+   * @return true or false
+   */
+  bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Check if lifecycle node manager server is active
+   * @return ACTIVE or INACTIVE or TIMEOUT
+   */
+  SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+protected:
+  using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes;
+
+  /**
+   * @brief A generic method used to call startup, shutdown, etc.
+   * @param command
+   */
+  bool callService(
+    uint8_t command,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  // The node to use for the service call
+  rclcpp::Node::SharedPtr node_;
+
+  std::shared_ptr<nav2_util::ServiceClient<ManageLifecycleNodes>> manager_client_;
+  std::shared_ptr<nav2_util::ServiceClient<std_srvs::srv::Trigger>> is_active_client_;
+  std::string manage_service_name_;
+  std::string active_service_name_;
+};
+
+}  // namespace nav2_lifecycle_manager
+
+#endif  // NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_

navigation2/nav2_lifecycle_manager/package.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_lifecycle_manager</name>
+  <version>1.1.18</version>
+  <description>A controller/manager for the lifecycle nodes of the Navigation 2 system</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>lifecycle_msgs</build_depend>
+  <build_depend>nav2_msgs</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>rclcpp_action</build_depend>
+  <build_depend>rclcpp_lifecycle</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>bondcpp</build_depend>
+  <build_depend>nav2_common</build_depend>
+  <build_depend>diagnostic_updater</build_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>nav2_util</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
+  <exec_depend>rclcpp_lifecycle</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>bondcpp</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>diagnostic_updater</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation2/nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -0,0 +1,510 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2022 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 "nav2_lifecycle_manager/lifecycle_manager.hpp"
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+using lifecycle_msgs::msg::Transition;
+using lifecycle_msgs::msg::State;
+using nav2_util::LifecycleServiceClient;
+
+namespace nav2_lifecycle_manager
+{
+
+LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
+: Node("lifecycle_manager", options), diagnostics_updater_(this)
+{
+  RCLCPP_INFO(get_logger(), "Creating");
+
+  // The list of names is parameterized, allowing this module to be used with a different set
+  // of nodes
+  declare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY);
+  declare_parameter("autostart", rclcpp::ParameterValue(false));
+  declare_parameter("bond_timeout", 4.0);
+  declare_parameter("bond_respawn_max_duration", 10.0);
+  declare_parameter("attempt_respawn_reconnection", true);
+
+  registerRclPreshutdownCallback();
+
+  node_names_ = get_parameter("node_names").as_string_array();
+  get_parameter("autostart", autostart_);
+  double bond_timeout_s;
+  get_parameter("bond_timeout", bond_timeout_s);
+  bond_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(
+    std::chrono::duration<double>(bond_timeout_s));
+
+  double respawn_timeout_s;
+  get_parameter("bond_respawn_max_duration", respawn_timeout_s);
+  bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s);
+
+  get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_);
+
+  callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  manager_srv_ = create_service<ManageLifecycleNodes>(
+    get_name() + std::string("/manage_nodes"),
+    std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),
+    rclcpp::ServicesQoS().get_rmw_qos_profile(),
+    callback_group_);
+
+  is_active_srv_ = create_service<std_srvs::srv::Trigger>(
+    get_name() + std::string("/is_active"),
+    std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),
+    rclcpp::ServicesQoS().get_rmw_qos_profile(),
+    callback_group_);
+
+  transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
+  transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
+  transition_state_map_[Transition::TRANSITION_ACTIVATE] = State::PRIMARY_STATE_ACTIVE;
+  transition_state_map_[Transition::TRANSITION_DEACTIVATE] = State::PRIMARY_STATE_INACTIVE;
+  transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
+    State::PRIMARY_STATE_FINALIZED;
+
+  transition_label_map_[Transition::TRANSITION_CONFIGURE] = std::string("Configuring ");
+  transition_label_map_[Transition::TRANSITION_CLEANUP] = std::string("Cleaning up ");
+  transition_label_map_[Transition::TRANSITION_ACTIVATE] = std::string("Activating ");
+  transition_label_map_[Transition::TRANSITION_DEACTIVATE] = std::string("Deactivating ");
+  transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
+    std::string("Shutting down ");
+
+  init_timer_ = this->create_wall_timer(
+    0s,
+    [this]() -> void {
+      init_timer_->cancel();
+      createLifecycleServiceClients();
+      if (autostart_) {
+        init_timer_ = this->create_wall_timer(
+          0s,
+          [this]() -> void {
+            init_timer_->cancel();
+            startup();
+          },
+          callback_group_);
+      }
+      auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      executor->add_callback_group(callback_group_, get_node_base_interface());
+      service_thread_ = std::make_unique<nav2_util::NodeThread>(executor);
+    });
+  diagnostics_updater_.setHardwareID("Nav2");
+  diagnostics_updater_.add("Nav2 Health", this, &LifecycleManager::CreateActiveDiagnostic);
+}
+
+LifecycleManager::~LifecycleManager()
+{
+  RCLCPP_INFO(get_logger(), "Destroying %s", get_name());
+  service_thread_.reset();
+}
+
+void
+LifecycleManager::managerCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<ManageLifecycleNodes::Request> request,
+  std::shared_ptr<ManageLifecycleNodes::Response> response)
+{
+  switch (request->command) {
+    case ManageLifecycleNodes::Request::STARTUP:
+      response->success = startup();
+      break;
+    case ManageLifecycleNodes::Request::RESET:
+      response->success = reset();
+      break;
+    case ManageLifecycleNodes::Request::SHUTDOWN:
+      response->success = shutdown();
+      break;
+    case ManageLifecycleNodes::Request::PAUSE:
+      response->success = pause();
+      break;
+    case ManageLifecycleNodes::Request::RESUME:
+      response->success = resume();
+      break;
+  }
+}
+
+void
+LifecycleManager::isActiveCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<std_srvs::srv::Trigger::Request>/*request*/,
+  std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+{
+  response->success = system_active_;
+}
+
+void
+LifecycleManager::CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  if (system_active_) {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 is active");
+  } else {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 is inactive");
+  }
+}
+
+void
+LifecycleManager::createLifecycleServiceClients()
+{
+  message("Creating and initializing lifecycle service clients");
+  for (auto & node_name : node_names_) {
+    node_map_[node_name] =
+      std::make_shared<LifecycleServiceClient>(node_name, shared_from_this());
+  }
+}
+
+void
+LifecycleManager::destroyLifecycleServiceClients()
+{
+  message("Destroying lifecycle service clients");
+  for (auto & kv : node_map_) {
+    kv.second.reset();
+  }
+}
+
+bool
+LifecycleManager::createBondConnection(const std::string & node_name)
+{
+  const double timeout_ns =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(bond_timeout_).count();
+  const double timeout_s = timeout_ns / 1e9;
+
+  if (bond_map_.find(node_name) == bond_map_.end() && bond_timeout_.count() > 0.0) {
+    bond_map_[node_name] =
+      std::make_shared<bond::Bond>("bond", node_name, shared_from_this());
+    bond_map_[node_name]->setHeartbeatTimeout(timeout_s);
+    bond_map_[node_name]->setHeartbeatPeriod(0.10);
+    bond_map_[node_name]->start();
+    if (
+      !bond_map_[node_name]->waitUntilFormed(
+        rclcpp::Duration(rclcpp::Duration::from_nanoseconds(timeout_ns / 2))))
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Server %s was unable to be reached after %0.2fs by bond. "
+        "This server may be misconfigured.",
+        node_name.c_str(), timeout_s);
+      return false;
+    }
+    RCLCPP_INFO(get_logger(), "Server %s connected with bond.", node_name.c_str());
+  }
+
+  return true;
+}
+
+bool
+LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition)
+{
+  message(transition_label_map_[transition] + node_name);
+
+  if (!node_map_[node_name]->change_state(transition) ||
+    !(node_map_[node_name]->get_state() == transition_state_map_[transition]))
+  {
+    RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", node_name.c_str());
+    return false;
+  }
+
+  if (transition == Transition::TRANSITION_ACTIVATE) {
+    return createBondConnection(node_name);
+  } else if (transition == Transition::TRANSITION_DEACTIVATE) {
+    bond_map_.erase(node_name);
+  }
+
+  return true;
+}
+
+bool
+LifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool hard_change)
+{
+  // Hard change will continue even if a node fails
+  if (transition == Transition::TRANSITION_CONFIGURE ||
+    transition == Transition::TRANSITION_ACTIVATE)
+  {
+    for (auto & node_name : node_names_) {
+      try {
+        if (!changeStateForNode(node_name, transition) && !hard_change) {
+          return false;
+        }
+      } catch (const std::runtime_error & e) {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Failed to change state for node: %s. Exception: %s.", node_name.c_str(), e.what());
+        return false;
+      }
+    }
+  } else {
+    std::vector<std::string>::reverse_iterator rit;
+    for (rit = node_names_.rbegin(); rit != node_names_.rend(); ++rit) {
+      try {
+        if (!changeStateForNode(*rit, transition) && !hard_change) {
+          return false;
+        }
+      } catch (const std::runtime_error & e) {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Failed to change state for node: %s. Exception: %s.", (*rit).c_str(), e.what());
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+void
+LifecycleManager::shutdownAllNodes()
+{
+  message("Deactivate, cleanup, and shutdown nodes");
+  changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE);
+  changeStateForAllNodes(Transition::TRANSITION_CLEANUP);
+  changeStateForAllNodes(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
+}
+
+bool
+LifecycleManager::startup()
+{
+  message("Starting managed nodes bringup...");
+  if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE) ||
+    !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))
+  {
+    RCLCPP_ERROR(get_logger(), "Failed to bring up all requested nodes. Aborting bringup.");
+    return false;
+  }
+  message("Managed nodes are active");
+  system_active_ = true;
+  createBondTimer();
+  return true;
+}
+
+bool
+LifecycleManager::shutdown()
+{
+  system_active_ = false;
+  destroyBondTimer();
+
+  message("Shutting down managed nodes...");
+  shutdownAllNodes();
+  destroyLifecycleServiceClients();
+  message("Managed nodes have been shut down");
+  return true;
+}
+
+bool
+LifecycleManager::reset(bool hard_reset)
+{
+  system_active_ = false;
+  destroyBondTimer();
+
+  message("Resetting managed nodes...");
+  // Should transition in reverse order
+  if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE, hard_reset) ||
+    !changeStateForAllNodes(Transition::TRANSITION_CLEANUP, hard_reset))
+  {
+    if (!hard_reset) {
+      RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset");
+      return false;
+    }
+  }
+
+  message("Managed nodes have been reset");
+  return true;
+}
+
+bool
+LifecycleManager::pause()
+{
+  system_active_ = false;
+  destroyBondTimer();
+
+  message("Pausing managed nodes...");
+  if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) {
+    RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause");
+    return false;
+  }
+
+  message("Managed nodes have been paused");
+  return true;
+}
+
+bool
+LifecycleManager::resume()
+{
+  message("Resuming managed nodes...");
+  if (!changeStateForAllNodes(Transition::TRANSITION_ACTIVATE)) {
+    RCLCPP_ERROR(get_logger(), "Failed to resume nodes: aborting resume");
+    return false;
+  }
+
+  message("Managed nodes are active");
+  system_active_ = true;
+  createBondTimer();
+  return true;
+}
+
+void
+LifecycleManager::createBondTimer()
+{
+  if (bond_timeout_.count() <= 0) {
+    return;
+  }
+
+  message("Creating bond timer...");
+  bond_timer_ = this->create_wall_timer(
+    200ms,
+    std::bind(&LifecycleManager::checkBondConnections, this),
+    callback_group_);
+}
+
+void
+LifecycleManager::destroyBondTimer()
+{
+  if (bond_timer_) {
+    message("Terminating bond timer...");
+    bond_timer_->cancel();
+    bond_timer_.reset();
+  }
+}
+
+void
+LifecycleManager::onRclPreshutdown()
+{
+  RCLCPP_INFO(
+    get_logger(), "Running Nav2 LifecycleManager rcl preshutdown (%s)",
+    this->get_name());
+
+  destroyBondTimer();
+
+  /*
+   * Dropping the bond map is what we really need here, but we drop the others
+   * to prevent the bond map being used. Likewise, squash the service thread.
+   */
+  service_thread_.reset();
+  node_names_.clear();
+  node_map_.clear();
+  bond_map_.clear();
+}
+
+void
+LifecycleManager::registerRclPreshutdownCallback()
+{
+  rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+
+  context->add_pre_shutdown_callback(
+    std::bind(&LifecycleManager::onRclPreshutdown, this)
+  );
+}
+
+void
+LifecycleManager::checkBondConnections()
+{
+  if (!system_active_ || !rclcpp::ok() || bond_map_.empty()) {
+    return;
+  }
+
+  for (auto & node_name : node_names_) {
+    if (!rclcpp::ok()) {
+      return;
+    }
+
+    if (bond_map_[node_name]->isBroken()) {
+      message(
+        std::string(
+          "Have not received a heartbeat from " + node_name + "."));
+
+      // if one is down, bring them all down
+      RCLCPP_ERROR(
+        get_logger(),
+        "CRITICAL FAILURE: SERVER %s IS DOWN after not receiving a heartbeat for %i ms."
+        " Shutting down related nodes.",
+        node_name.c_str(), static_cast<int>(bond_timeout_.count()));
+      reset(true);  // hard reset to transition all still active down
+      // if a server crashed, it won't get cleared due to failed transition, clear manually
+      bond_map_.clear();
+
+      // Initialize the bond respawn timer to check if server comes back online
+      // after a failure, within a maximum timeout period.
+      if (attempt_respawn_reconnection_) {
+        bond_respawn_timer_ = this->create_wall_timer(
+          1s,
+          std::bind(&LifecycleManager::checkBondRespawnConnection, this),
+          callback_group_);
+      }
+      return;
+    }
+  }
+}
+
+void
+LifecycleManager::checkBondRespawnConnection()
+{
+  // First attempt in respawn, start maximum duration to respawn
+  if (bond_respawn_start_time_.nanoseconds() == 0) {
+    bond_respawn_start_time_ = now();
+  }
+
+  // Note: system_active_ is inverted since this should be in a failure
+  // condition. If another outside user actives the system again, this should not process.
+  if (system_active_ || !rclcpp::ok() || node_names_.empty()) {
+    bond_respawn_start_time_ = rclcpp::Time(0);
+    bond_respawn_timer_.reset();
+    return;
+  }
+
+  // Check number of live connections after a bond failure
+  int live_servers = 0;
+  const int max_live_servers = node_names_.size();
+  for (auto & node_name : node_names_) {
+    if (!rclcpp::ok()) {
+      return;
+    }
+
+    try {
+      node_map_[node_name]->get_state();  // Only won't throw if the server exists
+      live_servers++;
+    } catch (...) {
+      break;
+    }
+  }
+
+  // If all are alive, kill timer and retransition system to active
+  // Else, check if maximum timeout has occurred
+  if (live_servers == max_live_servers) {
+    message("Successfully re-established connections from server respawns, starting back up.");
+    bond_respawn_start_time_ = rclcpp::Time(0);
+    bond_respawn_timer_.reset();
+    startup();
+  } else if (now() - bond_respawn_start_time_ >= bond_respawn_max_duration_) {
+    message("Failed to re-establish connection from a server crash after maximum timeout.");
+    bond_respawn_start_time_ = rclcpp::Time(0);
+    bond_respawn_timer_.reset();
+  }
+}
+
+#define ANSI_COLOR_RESET    "\x1b[0m"
+#define ANSI_COLOR_BLUE     "\x1b[34m"
+
+void
+LifecycleManager::message(const std::string & msg)
+{
+  RCLCPP_INFO(get_logger(), ANSI_COLOR_BLUE "\33[1m%s\33[0m" ANSI_COLOR_RESET, msg.c_str());
+}
+
+}  // namespace nav2_lifecycle_manager
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_lifecycle_manager::LifecycleManager)

navigation2/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

@@ -0,0 +1,136 @@
+// 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_lifecycle_manager/lifecycle_manager_client.hpp"
+
+#include <cmath>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+namespace nav2_lifecycle_manager
+{
+using nav2_util::geometry_utils::orientationAroundZAxis;
+
+LifecycleManagerClient::LifecycleManagerClient(
+  const std::string & name,
+  std::shared_ptr<rclcpp::Node> parent_node)
+{
+  manage_service_name_ = name + std::string("/manage_nodes");
+  active_service_name_ = name + std::string("/is_active");
+
+  // Use parent node for service call and logging
+  node_ = parent_node;
+
+  // Create the service clients
+  manager_client_ = std::make_shared<nav2_util::ServiceClient<ManageLifecycleNodes>>(
+    manage_service_name_, node_);
+  is_active_client_ = std::make_shared<nav2_util::ServiceClient<std_srvs::srv::Trigger>>(
+    active_service_name_, node_);
+}
+
+bool
+LifecycleManagerClient::startup(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::STARTUP, timeout);
+}
+
+bool
+LifecycleManagerClient::shutdown(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::SHUTDOWN, timeout);
+}
+
+bool
+LifecycleManagerClient::pause(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::PAUSE, timeout);
+}
+
+bool
+LifecycleManagerClient::resume(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::RESUME, timeout);
+}
+
+bool
+LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::RESET, timeout);
+}
+
+SystemStatus
+LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout)
+{
+  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto response = std::make_shared<std_srvs::srv::Trigger::Response>();
+
+  RCLCPP_DEBUG(
+    node_->get_logger(), "Waiting for the %s service...",
+    active_service_name_.c_str());
+
+  if (!is_active_client_->wait_for_service(std::chrono::seconds(1))) {
+    return SystemStatus::TIMEOUT;
+  }
+
+  RCLCPP_DEBUG(
+    node_->get_logger(), "Sending %s request",
+    active_service_name_.c_str());
+
+  try {
+    response = is_active_client_->invoke(request, timeout);
+  } catch (std::runtime_error &) {
+    return SystemStatus::TIMEOUT;
+  }
+
+  if (response->success) {
+    return SystemStatus::ACTIVE;
+  } else {
+    return SystemStatus::INACTIVE;
+  }
+}
+
+bool
+LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseconds timeout)
+{
+  auto request = std::make_shared<ManageLifecycleNodes::Request>();
+  request->command = command;
+
+  RCLCPP_DEBUG(
+    node_->get_logger(), "Waiting for the %s service...",
+    manage_service_name_.c_str());
+
+  while (!manager_client_->wait_for_service(timeout)) {
+    if (!rclcpp::ok()) {
+      RCLCPP_ERROR(node_->get_logger(), "Client interrupted while waiting for service to appear");
+      return false;
+    }
+    RCLCPP_DEBUG(node_->get_logger(), "Waiting for service to appear...");
+  }
+
+  RCLCPP_DEBUG(
+    node_->get_logger(), "Sending %s request",
+    manage_service_name_.c_str());
+  try {
+    auto future_result = manager_client_->invoke(request, timeout);
+    return future_result->success;
+  } catch (std::runtime_error &) {
+    return false;
+  }
+}
+
+}  // namespace nav2_lifecycle_manager

navigation2/nav2_lifecycle_manager/src/main.cpp

@@ -0,0 +1,28 @@
+// 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_lifecycle_manager/lifecycle_manager.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_lifecycle_manager::LifecycleManager>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_lifecycle_manager/test/CMakeLists.txt

@@ -0,0 +1,41 @@
+ament_add_gtest_executable(test_lifecycle_gtest
+  test_lifecycle_manager.cpp
+)
+
+target_link_libraries(test_lifecycle_gtest
+  ${library_name}
+)
+
+ament_target_dependencies(test_lifecycle_gtest
+  ${dependencies}
+)
+
+ament_add_test(test_lifecycle
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 20
+  ENV
+    TEST_EXECUTABLE=$<TARGET_FILE:test_lifecycle_gtest>
+)
+
+ament_add_gtest_executable(test_bond_gtest
+  test_bond.cpp
+)
+
+target_link_libraries(test_bond_gtest
+  ${library_name}
+)
+
+ament_target_dependencies(test_bond_gtest
+  ${dependencies}
+)
+
+ament_add_test(test_bond
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 20
+  ENV
+    TEST_EXECUTABLE=$<TARGET_FILE:test_bond_gtest>
+)

navigation2/nav2_lifecycle_manager/test/launch_bond_test.py

@@ -0,0 +1,57 @@
+#! /usr/bin/env python3
+# Copyright (c) 2020 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_test',
+            output='screen',
+            parameters=[{'use_sim_time': False},
+                        {'autostart': False},
+                        {'node_names': ['bond_tester']}]),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_bond_gtest',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_lifecycle_manager/test/launch_lifecycle_test.py

@@ -0,0 +1,58 @@
+#! /usr/bin/env python3
+# Copyright (c) 2020 Shivang Patel
+#
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_test',
+            output='screen',
+            parameters=[{'use_sim_time': False},
+                        {'autostart': False},
+                        {'bond_timeout': 0.0},
+                        {'node_names': ['lifecycle_node_test']}]),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_lifecycle_node_gtest',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_lifecycle_manager/test/test_bond.cpp

@@ -0,0 +1,199 @@
+// Copyright (c) 2020 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 <gtest/gtest.h>
+#include <memory>
+#include <chrono>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "nav2_lifecycle_manager/lifecycle_manager.hpp"
+#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+// minimal lifecycle node implementing bond as in rest of navigation servers
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+  TestLifecycleNode(bool bond, std::string name)
+  : nav2_util::LifecycleNode(name)
+  {
+    state = "";
+    enable_bond = bond;
+  }
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!");
+    state = "configured";
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!");
+    state = "activated";
+    if (enable_bond) {
+      createBond();
+    }
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!");
+    state = "deactivated";
+    if (enable_bond) {
+      destroyBond();
+    }
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!");
+    state = "cleaned up";
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!");
+    state = "shut down";
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!");
+    state = "errored";
+    return CallbackReturn::SUCCESS;
+  }
+
+  bool bondAllocated()
+  {
+    return bond_ ? true : false;
+  }
+
+  void breakBond()
+  {
+    bond_->breakBond();
+  }
+
+  std::string getState()
+  {
+    return state;
+  }
+
+  bool isBondEnabled()
+  {
+    return enable_bond;
+  }
+
+  bool isBondConnected()
+  {
+    return bondAllocated() ? !bond_->isBroken() : false;
+  }
+
+  std::string state;
+  bool enable_bond;
+};
+
+class TestFixture
+{
+public:
+  TestFixture(bool bond, std::string node_name)
+  {
+    lf_node_ = std::make_shared<TestLifecycleNode>(bond, node_name);
+    lf_thread_ = std::make_unique<nav2_util::NodeThread>(lf_node_->get_node_base_interface());
+  }
+
+  std::shared_ptr<TestLifecycleNode> lf_node_;
+  std::unique_ptr<nav2_util::NodeThread> lf_thread_;
+};
+
+TEST(LifecycleBondTest, POSITIVE)
+{
+  // let the lifecycle server come up
+  rclcpp::Rate(1).sleep();
+
+  auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
+  nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
+
+  // create node, should be up now
+  auto fixture = TestFixture(true, "bond_tester");
+  auto bond_tester = fixture.lf_node_;
+
+  EXPECT_TRUE(client.startup());
+
+  // check if bond is connected after being activated
+  rclcpp::Rate(5).sleep();
+  EXPECT_TRUE(bond_tester->isBondConnected());
+  EXPECT_EQ(bond_tester->getState(), "activated");
+
+  bond_tester->breakBond();
+
+  // bond should be disconnected now and lifecycle manager should know and react to reset
+  rclcpp::Rate(5).sleep();
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::INACTIVE,
+    client.is_active(std::chrono::nanoseconds(1000000000)));
+  EXPECT_FALSE(bond_tester->isBondConnected());
+  EXPECT_EQ(bond_tester->getState(), "cleaned up");
+
+  // check that bringing up again is OK
+  EXPECT_TRUE(client.startup());
+  EXPECT_EQ(bond_tester->getState(), "activated");
+  EXPECT_TRUE(bond_tester->isBondConnected());
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::ACTIVE,
+    client.is_active(std::chrono::nanoseconds(1000000000)));
+
+  // clean state for next test.
+  EXPECT_TRUE(client.reset());
+  EXPECT_FALSE(bond_tester->isBondConnected());
+  EXPECT_EQ(bond_tester->getState(), "cleaned up");
+}
+
+TEST(LifecycleBondTest, NEGATIVE)
+{
+  auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
+  nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
+
+  // create node, now without bond setup to connect to. Should fail because no bond
+  auto fixture = TestFixture(false, "bond_tester");
+  auto bond_tester = fixture.lf_node_;
+  EXPECT_FALSE(client.startup());
+  EXPECT_FALSE(bond_tester->isBondEnabled());
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::INACTIVE,
+    client.is_active(std::chrono::nanoseconds(1000000000)));
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp

@@ -0,0 +1,128 @@
+// Copyright (c) 2020 Shivang Patel
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <chrono>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class LifecycleNodeTest : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  LifecycleNodeTest()
+  : rclcpp_lifecycle::LifecycleNode("lifecycle_node_test") {}
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!");
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!");
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!");
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!");
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!");
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override
+  {
+    RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!");
+    return CallbackReturn::SUCCESS;
+  }
+};
+
+class LifecycleClientTestFixture
+{
+public:
+  LifecycleClientTestFixture()
+  {
+    lf_node_ = std::make_shared<LifecycleNodeTest>();
+    lf_thread_ = std::make_unique<nav2_util::NodeThread>(lf_node_->get_node_base_interface());
+  }
+
+private:
+  std::shared_ptr<LifecycleNodeTest> lf_node_;
+  std::unique_ptr<nav2_util::NodeThread> lf_thread_;
+};
+
+TEST(LifecycleClientTest, BasicTest)
+{
+  LifecycleClientTestFixture fix;
+  auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
+  nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::TIMEOUT,
+    client.is_active(std::chrono::nanoseconds(1000)));
+  EXPECT_TRUE(client.startup());
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::ACTIVE,
+    client.is_active(std::chrono::nanoseconds(1000000000)));
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::ACTIVE,
+    client.is_active());
+  EXPECT_TRUE(client.pause());
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::INACTIVE,
+    client.is_active(std::chrono::nanoseconds(1000000000)));
+  EXPECT_TRUE(client.resume());
+  EXPECT_TRUE(client.reset());
+  EXPECT_TRUE(client.shutdown());
+}
+
+TEST(LifecycleClientTest, WithoutFixture)
+{
+  auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
+  nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
+  EXPECT_EQ(
+    nav2_lifecycle_manager::SystemStatus::TIMEOUT,
+    client.is_active(std::chrono::nanoseconds(1000)));
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}