ros2_canopen  master
C++ ROS CANopen Library
device_container.hpp
Go to the documentation of this file.
1 // Copyright 2022 Harshavadan Deshpande
2 // Christoph Hellmann Santos
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef LIFECYCLE_DEVICE_CONTAINER_NODE_HPP
17 #define LIFECYCLE_DEVICE_CONTAINER_NODE_HPP
18 
19 #include <chrono>
20 #include <memory>
21 #include <rclcpp/executors.hpp>
22 #include <rclcpp_components/component_manager.hpp>
23 #include <vector>
28 #include "canopen_interfaces/srv/co_node.hpp"
29 #include "rcl_interfaces/srv/set_parameters.hpp"
30 
31 using namespace std::chrono_literals;
32 
33 namespace ros2_canopen
34 {
44 class DeviceContainer : public rclcpp_components::ComponentManager
45 {
46 public:
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)
60  {
61  executor_ = executor;
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>(
68  "~/init_driver",
69  std::bind(
70  &DeviceContainer::on_init_driver, this, std::placeholders::_1, std::placeholders::_2),
71  rmw_qos_profile_services_default, client_cbg_);
72 
73  this->loadNode_srv_.reset();
74  this->unloadNode_srv_.reset();
75  lifecycle_operation_ = false;
76  }
77 
88  void init();
89 
100  void init(
101  const std::string & can_interface_name, const std::string & master_config,
102  const std::string & bus_config, const std::string & master_bin = "");
103 
104  virtual void configure();
111  virtual bool load_drivers();
118  virtual bool load_master();
125  virtual bool load_manager();
126 
137  virtual bool load_component(
138  std::string & package_name, std::string & driver_name, uint16_t node_id,
139  std::string & node_name, std::vector<rclcpp::Parameter> & params);
140 
149  virtual void shutdown()
150  {
151  for (auto it = registered_drivers_.begin(); it != registered_drivers_.end(); ++it)
152  {
153  try
154  {
155  it->second->shutdown();
156  }
157  catch (const std::exception & e)
158  {
159  std::cerr << e.what() << '\n';
160  }
161  }
162  try
163  {
164  can_master_->shutdown();
165  }
166  catch (const std::exception & e)
167  {
168  std::cerr << e.what() << '\n';
169  }
170  }
178  virtual void on_list_nodes(
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;
182 
191  virtual std::map<uint16_t, std::shared_ptr<CanopenDriverInterface>> get_registered_drivers()
192  {
193  return registered_drivers_;
194  }
195 
201  virtual size_t count_drivers() { return registered_drivers_.size(); }
202 
213  std::vector<uint16_t> get_ids_of_drivers_with_type(std::string type)
214  {
215  std::vector<std::string> devices;
216  std::vector<uint16_t> ids;
217  uint32_t count = this->config_->get_all_devices(devices);
218  if (count == 0)
219  {
220  return ids;
221  }
222 
223  for (auto it = devices.begin(); it != devices.end(); it++)
224  {
225  auto driver_name = config_->get_entry<std::string>(*it, "driver");
226  if (driver_name.has_value())
227  {
228  std::string name = driver_name.value();
229  if (name.compare(type) == 0)
230  {
231  auto node_id = config_->get_entry<uint16_t>(*it, "node_id");
232  ids.push_back(node_id.value());
233  }
234  }
235  }
236  return ids;
237  }
247  std::string get_driver_type(uint16_t id)
248  {
249  std::vector<std::string> devices;
250  uint32_t count = this->config_->get_all_devices(devices);
251  if (count == 0)
252  {
253  return "";
254  }
255  for (auto it = devices.begin(); it != devices.end(); it++)
256  {
257  auto node_id = config_->get_entry<uint16_t>(*it, "node_id");
258  if (node_id.has_value() && node_id.value() == id)
259  {
260  auto driver_name = config_->get_entry<std::string>(*it, "driver");
261  return driver_name.value();
262  }
263  }
264  }
265 
266 protected:
267  // Components
268  std::map<uint16_t, std::shared_ptr<CanopenDriverInterface>>
270  std::shared_ptr<ros2_canopen::CanopenMasterInterface>
272  uint16_t can_master_id_;
273  std::unique_ptr<ros2_canopen::LifecycleManager> lifecycle_manager_;
274 
275  // Configuration
276  std::shared_ptr<ros2_canopen::ConfigurationManager>
278  std::string dcf_txt_;
279  std::string bus_config_;
280  std::string dcf_bin_;
281  std::string can_interface_name_;
283 
284  // ROS Objects
285  std::weak_ptr<rclcpp::Executor> executor_;
286  rclcpp::Service<canopen_interfaces::srv::CONode>::SharedPtr
288  rclcpp::CallbackGroup::SharedPtr client_cbg_;
289 
295  void set_executor(const std::weak_ptr<rclcpp::Executor> executor);
296 
309  bool init_driver(uint16_t node_id);
310 
318  const canopen_interfaces::srv::CONode::Request::SharedPtr request,
319  canopen_interfaces::srv::CONode::Response::SharedPtr response)
320  {
321  response->success = init_driver(request->nodeid);
322  }
323 
329  std::map<uint16_t, std::string> list_components();
330 
336  virtual void add_node_to_executor(
337  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface)
338  {
339  if (auto exec = executor_.lock())
340  {
341  RCLCPP_INFO(
342  this->get_logger(), "Added %s to executor", node_interface->get_fully_qualified_name());
343  exec->add_node(node_interface, true);
344  }
345  else
346  {
347  RCLCPP_ERROR(
348  this->get_logger(), "Failed to add component %s",
349  node_interface->get_fully_qualified_name());
350  }
351  }
352 };
353 
354 } // namespace ros2_canopen
355 #endif // LIFECYCLE_DEVICE_CONTAINER_NODE_HPP
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 > &params)
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