Program Listing for File device_container.hpp

↰ Return to documentation for file (/tmp/ws/src/ros2_canopen/canopen_core/include/canopen_core/device_container.hpp)

//    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),
      rclcpp::QoS(10), 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(
    std::string & package_name, std::string & driver_name, uint16_t node_id,
    std::string & node_name, std::vector<rclcpp::Parameter> & params);

  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