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),
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(
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