lifecycle_manager.hpp

// 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_ros_common/node_thread.hpp"
#include "nav2_ros_common/publisher.hpp"
#include "nav2_ros_common/service_server.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.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"
#include "nav2_ros_common/lifecycle_node.hpp"
 
 
namespace nav2_lifecycle_manager
{
using namespace std::chrono_literals;  // NOLINT
 
using nav2_msgs::srv::ManageLifecycleNodes;
 
enum NodeState
{
  UNCONFIGURED,
  ACTIVE,
  INACTIVE,
  FINALIZED,
  UNKNOWN,
};
 
class LifecycleManager : public rclcpp::Node
{
public:
  explicit LifecycleManager(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
  ~LifecycleManager();
 
protected:
  // Callback group used by services and timers
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  std::unique_ptr<nav2::NodeThread> service_thread_;
 
  // The services provided by this node
  nav2::ServiceServer<ManageLifecycleNodes>::SharedPtr manager_srv_;
  nav2::ServiceServer<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;
 
  // Latched publisher for is_active status
  nav2::Publisher<std_msgs::msg::Bool>::SharedPtr is_active_pub_;
 
  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);
  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
  bool startup();
  bool configure();
  bool cleanup();
  bool shutdown();
  bool reset(bool hard_reset = false);
  bool pause();
  bool resume();
 
  void onRclPreshutdown();
 
  // Support function for creating service clients
  void createLifecycleServiceClients();
 
  // Support function for creating service servers
  void createLifecycleServiceServers();
 
  // Support function for creating publishers
  void createLifecyclePublishers();
 
  // Support functions for shutdown
  void shutdownAllNodes();
  void destroyLifecycleServiceClients();
  void destroyLifecyclePublishers();
 
  // Support function for creating bond timer
  void createBondTimer();
 
  // Support function for creating bond connection
  bool createBondConnection(const std::string & node_name);
 
  // Support function for killing bond connections
  void destroyBondTimer();
 
  // Support function for checking on bond connections
  void checkBondConnections();
 
  // Support function for checking if bond connections come back after respawn
  void checkBondRespawnConnection();
 
  bool changeStateForNode(
    const std::string & node_name,
    std::uint8_t transition);
 
  bool changeStateForAllNodes(std::uint8_t transition, bool hard_change = false);
 
  // Convenience function to highlight the output on the console
  void message(const std::string & msg);
 
  // Diagnostics functions
  void CreateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
  void registerRclPreshutdownCallback();
 
  void setState(const NodeState & state);
 
  bool isActive();
 
  void publishIsActiveState();
 
  // 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_;
  std::chrono::milliseconds service_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_;
 
  NodeState managed_nodes_state_{NodeState::UNCONFIGURED};
  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_