device_container.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_CONTAINER_NODE_HPP
#define LIFECYCLE_DEVICE_CONTAINER_NODE_HPP
 
#include <chrono>
#include <memory>
#include <rclcpp/executors.hpp>
#include <rclcpp_components/component_manager.hpp>
#include <vector>
#include "canopen_core/configuration_manager.hpp"
#include "canopen_core/driver_node.hpp"
#include "canopen_core/lifecycle_manager.hpp"
#include "canopen_core/master_node.hpp"
#include "canopen_interfaces/srv/co_node.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
 
using namespace std::chrono_literals;
 
namespace ros2_canopen
{
class DeviceContainer : public rclcpp_components::ComponentManager
{
public:
  DeviceContainer(
    std::weak_ptr<rclcpp::Executor> executor =
      std::weak_ptr<rclcpp::executors::MultiThreadedExecutor>(),
    std::string node_name = "device_container",
    const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : rclcpp_components::ComponentManager(executor, node_name, node_options)
  {
    executor_ = executor;
    this->declare_parameter<std::string>("can_interface_name", "");
    this->declare_parameter<std::string>("master_config", "");
    this->declare_parameter<std::string>("bus_config", "");
    this->declare_parameter<std::string>("master_bin", "");
    client_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    init_driver_service_ = this->create_service<canopen_interfaces::srv::CONode>(
      "~/init_driver",
      std::bind(
        &DeviceContainer::on_init_driver, this, std::placeholders::_1, std::placeholders::_2),
      rmw_qos_profile_services_default, client_cbg_);
 
    this->loadNode_srv_.reset();
    this->unloadNode_srv_.reset();
    lifecycle_operation_ = false;
  }
 
  void init();
 
  void init(
    const std::string & can_interface_name, const std::string & master_config,
    const std::string & bus_config, const std::string & master_bin = "");
 
  virtual void configure();
  virtual bool load_drivers();
  virtual bool load_master();
  virtual bool load_manager();
 
  virtual bool load_component(
    const std::string package_name, const std::string driver_name, const uint16_t node_id,
    const std::string node_name, std::vector<rclcpp::Parameter> & params,
    const std::string node_namespace = "");
 
  virtual void shutdown()
  {
    for (auto it = registered_drivers_.begin(); it != registered_drivers_.end(); ++it)
    {
      try
      {
        it->second->shutdown();
      }
      catch (const std::exception & e)
      {
        std::cerr << e.what() << '\n';
      }
    }
    try
    {
      can_master_->shutdown();
    }
    catch (const std::exception & e)
    {
      std::cerr << e.what() << '\n';
    }
  }
  virtual void on_list_nodes(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<ListNodes::Request> request,
    std::shared_ptr<ListNodes::Response> response) override;
 
  virtual std::map<uint16_t, std::shared_ptr<CanopenDriverInterface>> get_registered_drivers()
  {
    return registered_drivers_;
  }
 
  virtual size_t count_drivers() { return registered_drivers_.size(); }
 
  std::vector<uint16_t> get_ids_of_drivers_with_type(std::string type)
  {
    std::vector<std::string> devices;
    std::vector<uint16_t> ids;
    uint32_t count = this->config_->get_all_devices(devices);
    if (count == 0)
    {
      return ids;
    }
 
    for (auto it = devices.begin(); it != devices.end(); it++)
    {
      auto driver_name = config_->get_entry<std::string>(*it, "driver");
      if (driver_name.has_value())
      {
        std::string name = driver_name.value();
        if (name.compare(type) == 0)
        {
          auto node_id = config_->get_entry<uint16_t>(*it, "node_id");
          ids.push_back(node_id.value());
        }
      }
    }
    return ids;
  }
  std::string get_driver_type(uint16_t id)
  {
    std::vector<std::string> devices;
    uint32_t count = this->config_->get_all_devices(devices);
    if (count == 0)
    {
      return "";
    }
    for (auto it = devices.begin(); it != devices.end(); it++)
    {
      auto node_id = config_->get_entry<uint16_t>(*it, "node_id");
      if (node_id.has_value() && node_id.value() == id)
      {
        auto driver_name = config_->get_entry<std::string>(*it, "driver");
        return driver_name.value();
      }
    }
  }
 
protected:
  // Components
  std::map<uint16_t, std::shared_ptr<CanopenDriverInterface>>
    registered_drivers_;  
  std::shared_ptr<ros2_canopen::CanopenMasterInterface>
    can_master_;  
  uint16_t can_master_id_;
  std::unique_ptr<ros2_canopen::LifecycleManager> lifecycle_manager_;
 
  // Configuration
  std::shared_ptr<ros2_canopen::ConfigurationManager>
    config_;                        
  std::string dcf_txt_;             
  std::string bus_config_;          
  std::string dcf_bin_;             
  std::string can_interface_name_;  
  bool lifecycle_operation_;
 
  // ROS Objects
  std::weak_ptr<rclcpp::Executor> executor_;  
  rclcpp::Service<canopen_interfaces::srv::CONode>::SharedPtr
    init_driver_service_;                        
  rclcpp::CallbackGroup::SharedPtr client_cbg_;  
 
  void set_executor(const std::weak_ptr<rclcpp::Executor> executor);
 
  bool init_driver(uint16_t node_id);
 
  void on_init_driver(
    const canopen_interfaces::srv::CONode::Request::SharedPtr request,
    canopen_interfaces::srv::CONode::Response::SharedPtr response)
  {
    response->success = init_driver(request->nodeid);
  }
 
  std::map<uint16_t, std::string> list_components();
 
  virtual void add_node_to_executor(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface)
  {
    if (auto exec = executor_.lock())
    {
      RCLCPP_INFO(
        this->get_logger(), "Added %s to executor", node_interface->get_fully_qualified_name());
      exec->add_node(node_interface, true);
    }
    else
    {
      RCLCPP_ERROR(
        this->get_logger(), "Failed to add component %s",
        node_interface->get_fully_qualified_name());
    }
  }
};
 
}  // namespace ros2_canopen
#endif  // LIFECYCLE_DEVICE_CONTAINER_NODE_HPP