Temporary nav2_lifecycle and an example lifecycle node (#612)

* Temporary lifecycle directory and first lifecycle class
* Update the lifecycle readme
* Address Matt's review feedback
* Make sure the base class has a virtual destructor so things get cleaned up properly

nav2_lifecycle/CMakeLists.txt

+cmake_minimum_required(VERSION 3.5)
+project(nav2_lifecycle)
+
+find_package(ament_cmake REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(library_name ${PROJECT_NAME}_core)
+
+add_library(${library_name} SHARED
+  src/lifecycle_node.cpp
+)
+
+set(dependencies
+  lifecycle_msgs
+  nav2_common
+  nav2_util
+  rclcpp
+  rclcpp_lifecycle
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+install(TARGETS
+  ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  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()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+ament_package()

nav2_lifecycle/README.md

+# Lifecycle
+
+The Lifecycle project provides temporary functionality for implementing lifecycle nodes in the Navigation 2 system. This directory will be removed once lifecycle-related issues are fixed in various ROS2 dependencies.
+
+## Overview
+
+The motivation for integrating [ROS2 managed nodes](http://design.ros2.org/articles/node_lifecycle.html) into the Navigation 2 system is to 
+
+* Ensure that the nav2 system starts up and shuts down reliably
+* Clearly separate the non-deterministic code from the deterministic code (running in the Active state), setting us up for a subsequent project enable the nav2 software for deterministic (real-time) performance.
+* Simplify the model for node configuration
+
+### Reliable start-up and shutdown
+
+Using lifecycle nodes, the system can be launched in a controlled fashion, verifying each step of the process. Lifecycle nodes can be controlled from a launch script or from a separate controller node (the current implementation uses a controller node).
+
+### Separation of deterministic and non-deterministic code
+
+During the `Active` state of the lifecycle node, the node's execution is intended to be deterministic. The other start-up state transitions allow a node to configure itself and allocate its resources in preparation for running in the active state.
+
+### Node configuration
+
+Lifecycle nodes have an explicit `Configuring` state transition where the node can read its configuration parameters and configure itself. Reconfiguring a node can consist of taking the node back to the `Unconfigured` (or `Inactive`) state, updating the node parameters, and transitioning the node through the intermediate states back to the `Active` state. ROS 1-style dynamic parameters, where a node is reconfigured on-the-fly while it is running, should be reserved for simple parameter updates that don't result in non-deterministic behavior, such as allocating buffers.
+
+## Implementation
+
+This project provides two classes, `LifecycleNode` and `LifecycleHelperInterface`. 
+
+The first, `nav2_lifecycle::LifecycleNode` class is a wrapper for ROS's `rclcpp_lifecycle::LifecycleNode`. This class provides an option, **use_rclcpp_node**, to create a non-lifecycle ROS2 node:
+
+```cpp
+class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  LifecycleNode(
+    const std::string & node_name,
+    const std::string & namespace_ = "",
+    bool use_rclcpp_node = false,
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  virtual ~LifecycleNode();
+
+protected:
+  // Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't
+  // yet support lifecycle nodes
+  bool use_rclcpp_node_;
+
+  // The local node
+  rclcpp::Node::SharedPtr rclcpp_node_;
+
+  // When creating a local node, this class will launch a separate thread created to spin the node
+  std::unique_ptr<std::thread> rclcpp_thread_;
+};
+```
+
+If **use_rclcpp_node** is set to true, the class will create a non-lifecycle node, setting the rclcpp_node_ member variable. It will also automatically create a thread for this local node which will process messages for the thread, as shown in the LifecycleNode constructor:
+
+
+```cpp
+LifecycleNode::LifecycleNode(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool use_rclcpp_node,
+  const rclcpp::NodeOptions & options)
+: rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options),
+  use_rclcpp_node_(use_rclcpp_node)
+{
+  if (use_rclcpp_node_) {
+    rclcpp_node_ = std::make_shared<rclcpp::Node>(node_name + "_rclcpp_node", namespace_);
+    rclcpp_thread_ = std::make_unique<std::thread>(
+      [](rclcpp::Node::SharedPtr node) {rclcpp::spin(node);}, rclcpp_node_
+    );
+  }
+}
+```
+Any lifecycle node classes derived from nav2_lifecycle::LifecycleNode and set use_rclcpp_node in the constructor will have this rclcpp_node_ available to use to interface to any ROS2 classes that don't yet support lifecycle nodes. For example, the following code from the lifecycle version of the AMCL node initializes a tf2_ros::buffer, tf2_ros::TransformListener, and tf2_ros::TransformBroadcaster using the rclcpp_node_:
+
+```cpp
+void
+AmclNode::initTransforms()
+{
+  RCLCPP_INFO(get_logger(), "initTransforms");
+
+  // Initialize transform listener and broadcaster
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(rclcpp_node_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, rclcpp_node_, false);
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(rclcpp_node_);
+
+   ...
+}
+```
+
+The second class, `LifecycleHelperInterface` is an abstract base class that is used by those classes in the nav2 system that aren't themselves nodes, but require a node SharedPtr to create publishers and subscribers. To be in sync with their parent objects, these helper classes also need to walk through the same callbacks as used in the lifecycle nodes. That is, when the parent object is configuring itself in the `OnConfigure` callback, it can dispatch OnConfigure for its child objects as well. For example, the following code shows how a parent object might respond to an on_activate callback by calling helper objects that support the `LifecycleHelperInterface`:
+
+```cpp
+CallbackReturn
+Parent::on_activate(const rclcpp_lifecycle::State & state)
+{
+  // Parent does its own activation
+
+  // Delegates the on_activate message to is contained classes
+  // Helpers could throw an exception upon failure
+  helper1__->on_activate(state);
+  helper2__->on_activate(state);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+```

nav2_lifecycle/include/nav2_lifecycle/lifecycle_helper_interface.hpp

+// 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__LIFECYCLE_HELPER_INTERFACE_HPP_
+#define NAV2_LIFECYCLE__LIFECYCLE_HELPER_INTERFACE_HPP_
+
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+namespace nav2_lifecycle
+{
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+// A lifecycle interface minus onError and onShutdown, which are only relevant for nodes. This
+// has been proposed for rclcpp: https://github.com/ros2/rclcpp/issues/654 and hopefully can be
+// added as rclcpp_lifecycle::LifecycleHelperInterface. If not, we can remove this interface
+// and just use the whole rclcpp_lifecycle::LifecycleNodeInterface.
+
+class LifecycleHelperInterface
+{
+public:
+  virtual ~LifecycleHelperInterface() {}
+
+  virtual CallbackReturn on_configure(const rclcpp_lifecycle::State & state) = 0;
+  virtual CallbackReturn on_activate(const rclcpp_lifecycle::State & state) = 0;
+  virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) = 0;
+  virtual CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) = 0;
+};
+
+}  // namespace nav2_lifecycle
+
+#endif  // NAV2_LIFECYCLE__LIFECYCLE_HELPER_INTERFACE_HPP_

nav2_lifecycle/include/nav2_lifecycle/lifecycle_node.hpp

+// 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__LIFECYCLE_NODE_HPP_
+#define NAV2_LIFECYCLE__LIFECYCLE_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "nav2_lifecycle/lifecycle_helper_interface.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_lifecycle
+{
+
+// The following is a temporary wrapper for rclcpp_lifecycle::LifecycleNode. This class
+// adds the optional creation of an rclcpp::Node that can be used by derived classes
+// to interface to classes, such as MessageFilter and TransformListener, that don't yet
+// support lifecycle nodes. Once we get the fixes into ROS2, this class will be removed.
+
+class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  LifecycleNode(
+    const std::string & node_name,
+    const std::string & namespace_ = "",
+    bool use_rclcpp_node = false,
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  virtual ~LifecycleNode();
+
+protected:
+  // Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't
+  // yet support lifecycle nodes
+  bool use_rclcpp_node_;
+
+  // The local node
+  rclcpp::Node::SharedPtr rclcpp_node_;
+
+  // When creating a local node, this class will launch a separate thread created to spin the node
+  std::unique_ptr<std::thread> rclcpp_thread_;
+};
+
+}  // namespace nav2_lifecycle
+
+#endif  // NAV2_LIFECYCLE__LIFECYCLE_NODE_HPP_

nav2_lifecycle/package.xml

+<?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</name>
+  <version>0.1.5</version>
+  <description>Temporary convenience classes for working with lifecycle nodes</description>
+  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>lifecycle_msgs</build_depend>
+  <build_depend>nav2_common</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_lifecycle</build_depend>
+
+  <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>nav2_common</exec_depend>
+  <exec_depend>nav2_util</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_lifecycle</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

nav2_lifecycle/src/lifecycle_node.cpp

+// 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/lifecycle_node.hpp"
+
+#include <memory>
+#include <string>
+
+namespace nav2_lifecycle
+{
+
+// The nav2_lifecycle::LifecycleNode class is temporary until we get the
+// required support for lifecycle nodes in MessageFilter, TransformListener,
+// and TransforBroadcaster. We have submitted issues for these and will
+// be submitting PRs to add the fixes:
+//
+//     https://github.com/ros2/geometry2/issues/95
+//     https://github.com/ros2/geometry2/issues/94
+//     https://github.com/ros2/geometry2/issues/70
+//
+// Until then, this class can provide a normal ROS node that has a thread
+// that processes the node's messages. If a derived class needs to interface
+// to one of these classes - MessageFilter, etc. - that don't yet support
+// lifecycle nodes, it can simply set the use_rclcpp_node flag in the constructor
+// and then provide the rclcpp_node_ to the helper classes, like MessageFilter.
+//
+
+LifecycleNode::LifecycleNode(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool use_rclcpp_node,
+  const rclcpp::NodeOptions & options)
+: rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options),
+  use_rclcpp_node_(use_rclcpp_node)
+{
+  if (use_rclcpp_node_) {
+    rclcpp_node_ = std::make_shared<rclcpp::Node>(node_name + "_rclcpp_node", namespace_);
+    rclcpp_thread_ = std::make_unique<std::thread>(
+      [](rclcpp::Node::SharedPtr node) {rclcpp::spin(node);}, rclcpp_node_
+    );
+  }
+}
+
+LifecycleNode::~LifecycleNode()
+{
+  if (use_rclcpp_node_) {
+    rclcpp_thread_->join();
+  }
+}
+
+}  // namespace nav2_lifecycle