Program Listing for File lifecycle_manager.hpp

↰ Return to documentation for file (include/nav2_lifecycle_manager/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_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;

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_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_;
  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 functions for shutdown
  void shutdownAllNodes();
  void destroyLifecycleServiceClients();

  // 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();

  bool isActive();

  // 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_;

  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_