16 #ifndef LIFECYCLE_DEVICE_CONTAINER_NODE_HPP
17 #define LIFECYCLE_DEVICE_CONTAINER_NODE_HPP
21 #include <rclcpp/executors.hpp>
22 #include <rclcpp_components/component_manager.hpp>
28 #include "canopen_interfaces/srv/co_node.hpp"
29 #include "rcl_interfaces/srv/set_parameters.hpp"
31 using namespace std::chrono_literals;
55 std::weak_ptr<rclcpp::Executor> executor =
56 std::weak_ptr<rclcpp::executors::MultiThreadedExecutor>(),
57 std::string node_name =
"device_container",
58 const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
59 : rclcpp_components::ComponentManager(executor, node_name, node_options)
62 this->declare_parameter<std::string>(
"can_interface_name",
"");
63 this->declare_parameter<std::string>(
"master_config",
"");
64 this->declare_parameter<std::string>(
"bus_config",
"");
65 this->declare_parameter<std::string>(
"master_bin",
"");
66 client_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
67 init_driver_service_ = this->create_service<canopen_interfaces::srv::CONode>(
70 &DeviceContainer::on_init_driver,
this, std::placeholders::_1, std::placeholders::_2),
71 rmw_qos_profile_services_default, client_cbg_);
73 this->loadNode_srv_.reset();
74 this->unloadNode_srv_.reset();
75 lifecycle_operation_ =
false;
101 const std::string & can_interface_name,
const std::string & master_config,
102 const std::string & bus_config,
const std::string & master_bin =
"");
138 std::string & package_name, std::string & driver_name, uint16_t node_id,
139 std::string & node_name, std::vector<rclcpp::Parameter> & params);
151 for (
auto it = registered_drivers_.begin(); it != registered_drivers_.end(); ++it)
155 it->second->shutdown();
157 catch (
const std::exception & e)
159 std::cerr << e.what() <<
'\n';
164 can_master_->shutdown();
166 catch (
const std::exception & e)
168 std::cerr << e.what() <<
'\n';
179 const std::shared_ptr<rmw_request_id_t> request_header,
180 const std::shared_ptr<ListNodes::Request> request,
181 std::shared_ptr<ListNodes::Response> response)
override;
193 return registered_drivers_;
215 std::vector<std::string> devices;
216 std::vector<uint16_t> ids;
217 uint32_t count = this->config_->get_all_devices(devices);
223 for (
auto it = devices.begin(); it != devices.end(); it++)
225 auto driver_name = config_->get_entry<std::string>(*it,
"driver");
226 if (driver_name.has_value())
228 std::string name = driver_name.value();
229 if (name.compare(type) == 0)
231 auto node_id = config_->get_entry<uint16_t>(*it,
"node_id");
232 ids.push_back(node_id.value());
249 std::vector<std::string> devices;
250 uint32_t count = this->config_->get_all_devices(devices);
255 for (
auto it = devices.begin(); it != devices.end(); it++)
257 auto node_id = config_->get_entry<uint16_t>(*it,
"node_id");
258 if (node_id.has_value() && node_id.value() == id)
260 auto driver_name = config_->get_entry<std::string>(*it,
"driver");
261 return driver_name.value();
268 std::map<uint16_t, std::shared_ptr<CanopenDriverInterface>>
270 std::shared_ptr<ros2_canopen::CanopenMasterInterface>
276 std::shared_ptr<ros2_canopen::ConfigurationManager>
286 rclcpp::Service<canopen_interfaces::srv::CONode>::SharedPtr
318 const canopen_interfaces::srv::CONode::Request::SharedPtr request,
319 canopen_interfaces::srv::CONode::Response::SharedPtr response)
321 response->success = init_driver(request->nodeid);
337 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface)
339 if (
auto exec = executor_.lock())
342 this->get_logger(),
"Added %s to executor", node_interface->get_fully_qualified_name());
343 exec->add_node(node_interface,
true);
348 this->get_logger(),
"Failed to add component %s",
349 node_interface->get_fully_qualified_name());
Lifecycle Device Container for CANopen.
Definition: device_container.hpp:45
std::weak_ptr< rclcpp::Executor > executor_
Pointer to ros executor instance.
Definition: device_container.hpp:285
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())
Construct a new Lifecycle Device Container Node object.
Definition: device_container.hpp:54
std::unique_ptr< ros2_canopen::LifecycleManager > lifecycle_manager_
Definition: device_container.hpp:273
virtual void add_node_to_executor(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface)
Add node to executor.
Definition: device_container.hpp:336
virtual size_t count_drivers()
Get the number of registered drivers.
Definition: device_container.hpp:201
virtual void shutdown()
Shutdown all devices.
Definition: device_container.hpp:149
std::map< uint16_t, std::string > list_components()
Returns a list of components.
bool init_driver(uint16_t node_id)
Initialises a driver.
std::string dcf_bin_
Cached value of .bin file parameter.
Definition: device_container.hpp:280
std::string dcf_txt_
Cached value of .dcf file parameter.
Definition: device_container.hpp:278
std::string bus_config_
Cached value of bus.yml file parameter.
Definition: device_container.hpp:279
void init()
Executes the initialisation.
void on_init_driver(const canopen_interfaces::srv::CONode::Request::SharedPtr request, canopen_interfaces::srv::CONode::Response::SharedPtr response)
Callback for init driver service.
Definition: device_container.hpp:317
bool lifecycle_operation_
Definition: device_container.hpp:282
virtual bool load_master()
Loads master from configuration.
std::string can_interface_name_
Cached value of can interface name.
Definition: device_container.hpp:281
rclcpp::CallbackGroup::SharedPtr client_cbg_
Callback group for services.
Definition: device_container.hpp:288
std::shared_ptr< ros2_canopen::ConfigurationManager > config_
Pointer to configuration manager instance.
Definition: device_container.hpp:277
std::map< uint16_t, std::shared_ptr< CanopenDriverInterface > > registered_drivers_
Map of drivers registered in busconfiguration. Name is key.
Definition: device_container.hpp:269
virtual bool load_manager()
Loads the device manager.
rclcpp::Service< canopen_interfaces::srv::CONode >::SharedPtr init_driver_service_
Service object for init_driver service.
Definition: device_container.hpp:287
std::vector< uint16_t > get_ids_of_drivers_with_type(std::string type)
Get node ids of all drivers with type.
Definition: device_container.hpp:213
virtual std::map< uint16_t, std::shared_ptr< CanopenDriverInterface > > get_registered_drivers()
Get the registered drivers object.
Definition: device_container.hpp:191
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
Callback for the listing service.
virtual bool load_component(std::string &package_name, std::string &driver_name, uint16_t node_id, std::string &node_name, std::vector< rclcpp::Parameter > ¶ms)
Load a component.
virtual bool load_drivers()
Loads drivers from configuration.
void set_executor(const std::weak_ptr< rclcpp::Executor > executor)
Set the ROS executor object.
uint16_t can_master_id_
Definition: device_container.hpp:272
void init(const std::string &can_interface_name, const std::string &master_config, const std::string &bus_config, const std::string &master_bin="")
Executes the initialisation.
std::string get_driver_type(uint16_t id)
Get the type of driver by node id.
Definition: device_container.hpp:247
std::shared_ptr< ros2_canopen::CanopenMasterInterface > can_master_
Pointer to can master instance.
Definition: device_container.hpp:271
Definition: configuration_manager.hpp:28