lifecycle_manager.hpp

Go to the documentation of this file.

//    Copyright 2022 Harshavadan Deshpande
//                   Christoph Hellmann Santos
//
//    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 LIFECYCLE_DEVICE_MANAGER_NODE_HPP
#define LIFECYCLE_DEVICE_MANAGER_NODE_HPP
 
#include <chrono>
#include <memory>
#include <string>
#include <thread>
 
#include "lifecycle_msgs/msg/state.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
//#include "std_srvs/srv/trigger.hpp"
 
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
#include "canopen_core/configuration_manager.hpp"
#include "canopen_interfaces/srv/co_node.hpp"
 
using namespace std::chrono_literals;
/*
 
*/
namespace ros2_canopen
{
class LifecycleManager : public rclcpp_lifecycle::LifecycleNode
{
public:
  LifecycleManager(const rclcpp::NodeOptions & node_options)
  : rclcpp_lifecycle::LifecycleNode("lifecycle_manager", node_options)
  {
    this->declare_parameter("container_name", "");
    cbg_clients = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
  }
 
  void init(std::shared_ptr<ros2_canopen::ConfigurationManager> config);
 
protected:
  rclcpp::CallbackGroup::SharedPtr cbg_clients;
  std::shared_ptr<ros2_canopen::ConfigurationManager> config_;
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & state);
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & state);
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & state);
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
    const rclcpp_lifecycle::State & state);
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
    const rclcpp_lifecycle::State & state);
 
  std::map<uint8_t, rclcpp::Client<lifecycle_msgs::srv::GetState>::SharedPtr>
    drivers_get_state_clients;  
  std::map<uint8_t, rclcpp::Client<lifecycle_msgs::srv::ChangeState>::SharedPtr>
    drivers_change_state_clients;                      
  std::map<std::string, uint8_t> device_names_to_ids;  // stores device_name and node_id
 
  rclcpp::Client<canopen_interfaces::srv::CONode>::SharedPtr
    add_driver_client_;  
  rclcpp::Client<canopen_interfaces::srv::CONode>::SharedPtr
    remove_driver_client_;  
 
  uint8_t master_id_;           
  std::string container_name_;  
 
protected:
  template <typename FutureT, typename WaitTimeT>
  std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait)
  {
    auto end = std::chrono::steady_clock::now() + time_to_wait;
    std::chrono::milliseconds wait_period(100);
    std::future_status status = std::future_status::timeout;
    do
    {
      auto now = std::chrono::steady_clock::now();
      auto time_left = end - now;
      if (time_left <= std::chrono::seconds(0))
      {
        break;
      }
      status = future.wait_for((time_left < wait_period) ? time_left : wait_period);
    } while (rclcpp::ok() && status != std::future_status::ready);
    return status;
  }
 
  virtual unsigned int get_state(uint8_t node_id, std::chrono::seconds time_out);
 
  virtual bool change_state(uint8_t node_id, uint8_t transition, std::chrono::seconds time_out);
 
  virtual bool bring_up_all();
 
  virtual bool bring_down_all();
 
  virtual bool bring_up_master();
 
  virtual bool bring_down_master();
 
  virtual bool bring_up_driver(std::string device_name);
 
  virtual bool bring_down_driver(std::string device_name);
 
  virtual bool load_from_config();
};
}  // namespace ros2_canopen
 
#endif