ros2_canopen  master
C++ ROS CANopen Library
node_canopen_driver.hpp
Go to the documentation of this file.
1 // Copyright 2022 Christoph Hellmann Santos
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef NODE_CANOPEN_DRIVER_HPP_
16 #define NODE_CANOPEN_DRIVER_HPP_
17 
18 #include <yaml-cpp/yaml.h>
19 #include <atomic>
20 #include <lely/coapp/driver.hpp>
21 #include <map>
22 #include <memory>
23 #include <rclcpp/node_interfaces/node_base_interface.hpp>
24 #include <rclcpp/node_interfaces/node_clock_interface.hpp>
25 #include <rclcpp/node_interfaces/node_graph_interface.hpp>
26 #include <rclcpp/node_interfaces/node_logging_interface.hpp>
27 #include <rclcpp/node_interfaces/node_parameters_interface.hpp>
28 #include <rclcpp/node_interfaces/node_services_interface.hpp>
29 #include <rclcpp/node_interfaces/node_time_source_interface.hpp>
30 #include <rclcpp/node_interfaces/node_timers_interface.hpp>
31 #include <rclcpp/node_interfaces/node_topics_interface.hpp>
32 #include <rclcpp/node_interfaces/node_waitables_interface.hpp>
33 #include <rclcpp/rclcpp.hpp>
34 #include <rclcpp_lifecycle/lifecycle_node.hpp>
35 #include <type_traits>
36 #include <vector>
40 #include "canopen_interfaces/srv/co_node.hpp"
41 
42 using namespace rclcpp;
43 namespace ros2_canopen
44 {
45 namespace node_interfaces
46 {
58 template <class NODETYPE>
60 {
61  static_assert(
62  std::is_base_of<rclcpp::Node, NODETYPE>::value ||
63  std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
64  "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");
65 
66 protected:
67  NODETYPE * node_;
68 
69  std::shared_ptr<lely::ev::Executor> exec_;
70  std::shared_ptr<lely::canopen::AsyncMaster> master_;
71  std::shared_ptr<lely::canopen::BasicDriver> driver_;
72 
73  std::chrono::milliseconds non_transmit_timeout_;
74  YAML::Node config_;
75  uint8_t node_id_;
76  std::string container_name_;
77  std::string eds_;
78  std::string bin_;
79 
80  rclcpp::CallbackGroup::SharedPtr client_cbg_;
81  rclcpp::CallbackGroup::SharedPtr timer_cbg_;
82 
83  std::atomic<bool> master_set_;
84  std::atomic<bool> initialised_;
85  std::atomic<bool> configured_;
86  std::atomic<bool> activated_;
87 
88 public:
89  NodeCanopenDriver(NODETYPE * node)
90  : master_set_(false), initialised_(false), configured_(false), activated_(false)
91  {
92  node_ = node;
93  }
94 
102  virtual void set_master(
103  std::shared_ptr<lely::ev::Executor> exec, std::shared_ptr<lely::canopen::AsyncMaster> master)
104  {
105  RCLCPP_DEBUG(node_->get_logger(), "set_master_start");
106  if (!configured_.load())
107  {
108  throw DriverException("Set Master: driver is not configured");
109  }
110  if (activated_.load())
111  {
112  throw DriverException("Set Master: driver is not activated");
113  }
114  this->exec_ = exec;
115  this->master_ = master;
116  this->master_set_.store(true);
117  RCLCPP_DEBUG(node_->get_logger(), "set_master_end");
118  }
119 
127  void init()
128  {
129  RCLCPP_DEBUG(node_->get_logger(), "init_start");
130  if (configured_.load())
131  {
132  throw DriverException("Init: Driver is already configured");
133  }
134  if (activated_.load())
135  {
136  throw DriverException("Init: Driver is already activated");
137  }
138  client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
139  timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
140 
141  node_->declare_parameter("container_name", "");
142  node_->declare_parameter("node_id", 0);
143  node_->declare_parameter("non_transmit_timeout", 100);
144  node_->declare_parameter("config", "");
145  this->init(true);
146  this->initialised_.store(true);
147  RCLCPP_DEBUG(node_->get_logger(), "init_end");
148  }
149 
162  virtual void init(bool called_from_base) {}
163 
175  void configure()
176  {
177  RCLCPP_DEBUG(node_->get_logger(), "configure_start");
178  if (!initialised_.load())
179  {
180  throw DriverException("Configure: driver is not initialised");
181  }
182  if (configured_.load())
183  {
184  throw DriverException("Configure: driver is already configured");
185  }
186  if (activated_.load())
187  {
188  throw DriverException("Configure: driver is already activated");
189  }
190  int non_transmit_timeout;
191  std::string config;
192  node_->get_parameter("container_name", container_name_);
193  node_->get_parameter("non_transmit_timeout", non_transmit_timeout);
194  node_->get_parameter("node_id", this->node_id_);
195  node_->get_parameter("config", config);
196  this->config_ = YAML::Load(config);
197  this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout);
198  auto path = this->config_["dcf_path"].as<std::string>();
199  auto dcf = this->config_["dcf"].as<std::string>();
200  auto name = this->node_->get_name();
201  eds_ = path + "/" + dcf;
202  bin_ = path + "/" + name + ".bin";
203  this->configure(true);
204  this->configured_.store(true);
205  RCLCPP_DEBUG(node_->get_logger(), "configure_end");
206  }
214  virtual void configure(bool called_from_base) {}
215 
223  void activate()
224  {
225  RCLCPP_DEBUG(node_->get_logger(), "activate_start");
226  if (!master_set_.load())
227  {
228  throw DriverException("Activate: master is not set");
229  }
230  if (!initialised_.load())
231  {
232  throw DriverException("Activate: driver is not initialised");
233  }
234  if (!configured_.load())
235  {
236  throw DriverException("Activate: driver is not configured");
237  }
238  if (activated_.load())
239  {
240  throw DriverException("Activate: driver is already activated");
241  }
242  this->add_to_master();
243  this->activate(true);
244  this->activated_.store(true);
245  RCLCPP_DEBUG(node_->get_logger(), "activate_end");
246  }
247 
256  virtual void activate(bool called_from_base) {}
257 
265  void deactivate()
266  {
267  RCLCPP_DEBUG(node_->get_logger(), "deactivate_start");
268  if (!master_set_.load())
269  {
270  throw DriverException("Activate: master is not set");
271  }
272  if (!initialised_.load())
273  {
274  throw DriverException("Deactivate: driver is not initialised");
275  }
276  if (!configured_.load())
277  {
278  throw DriverException("Deactivate: driver is not configured");
279  }
280  if (!activated_.load())
281  {
282  throw DriverException("Deactivate: driver is not activated");
283  }
284  this->activated_.store(false);
285  this->remove_from_master();
286  this->deactivate(true);
287  RCLCPP_DEBUG(node_->get_logger(), "deactivate_end");
288  }
296  virtual void deactivate(bool called_from_base) {}
297 
305  void cleanup()
306  {
307  if (!initialised_.load())
308  {
309  throw DriverException("Cleanup: driver is not initialised");
310  }
311  if (!configured_.load())
312  {
313  throw DriverException("Cleanup: driver is not configured");
314  }
315  if (activated_.load())
316  {
317  throw DriverException("Cleanup: driver is still activated");
318  }
319  this->cleanup(true);
320  this->configured_.store(false);
321  }
322 
330  virtual void cleanup(bool called_from_base)
331  {
332  RCLCPP_INFO(this->node_->get_logger(), "Cleanup");
333  this->exec_.reset();
334  this->master_.reset();
335  this->master_set_.store(false);
336  }
337 
344  void shutdown()
345  {
346  RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down.");
347  if (this->activated_)
348  {
349  this->deactivate();
350  }
351  if (this->configured_)
352  {
353  this->cleanup();
354  }
355  shutdown(true);
356  this->master_set_.store(false);
357  this->initialised_.store(false);
358  this->configured_.store(false);
359  this->activated_.store(false);
360  }
368  virtual void shutdown(bool called_from_base) {}
369 
370  virtual void demand_set_master();
371 
372 protected:
377  virtual void add_to_master() { throw DriverException("Add to master not implemented."); }
378 
383  virtual void remove_from_master()
384  {
385  throw DriverException("Remove from master not implemented.");
386  }
387 };
388 } // namespace node_interfaces
389 } // namespace ros2_canopen
390 
391 #endif
Driver Exception.
Definition: driver_error.hpp:31
Node Canopen Driver Interface.
Definition: node_canopen_driver_interface.hpp:33
Node Canopen Driver.
Definition: node_canopen_driver.hpp:60
std::string bin_
Definition: node_canopen_driver.hpp:78
virtual void remove_from_master()
Remove the driver from master.
Definition: node_canopen_driver.hpp:383
std::atomic< bool > initialised_
Definition: node_canopen_driver.hpp:84
void shutdown()
Shutdown the driver.
Definition: node_canopen_driver.hpp:344
virtual void init(bool called_from_base)
Initialises the driver.
Definition: node_canopen_driver.hpp:162
std::atomic< bool > master_set_
Definition: node_canopen_driver.hpp:83
std::string eds_
Definition: node_canopen_driver.hpp:77
void deactivate()
Deactivate the driver.
Definition: node_canopen_driver.hpp:265
std::atomic< bool > configured_
Definition: node_canopen_driver.hpp:85
void cleanup()
Cleanup the driver.
Definition: node_canopen_driver.hpp:305
void activate()
Activate the driver.
Definition: node_canopen_driver.hpp:223
std::string container_name_
Definition: node_canopen_driver.hpp:76
NODETYPE * node_
Definition: node_canopen_driver.hpp:64
virtual void demand_set_master()
Demand set Master.
uint8_t node_id_
Definition: node_canopen_driver.hpp:75
std::atomic< bool > activated_
Definition: node_canopen_driver.hpp:86
virtual void set_master(std::shared_ptr< lely::ev::Executor > exec, std::shared_ptr< lely::canopen::AsyncMaster > master)
Set Master.
Definition: node_canopen_driver.hpp:102
rclcpp::CallbackGroup::SharedPtr client_cbg_
Definition: node_canopen_driver.hpp:80
virtual void deactivate(bool called_from_base)
Deactivates the driver.
Definition: node_canopen_driver.hpp:296
std::shared_ptr< lely::canopen::BasicDriver > driver_
Definition: node_canopen_driver.hpp:71
void init()
Initialise the driver.
Definition: node_canopen_driver.hpp:127
void configure()
Configure the driver.
Definition: node_canopen_driver.hpp:175
virtual void cleanup(bool called_from_base)
Cleanup the driver.
Definition: node_canopen_driver.hpp:330
rclcpp::CallbackGroup::SharedPtr timer_cbg_
Definition: node_canopen_driver.hpp:81
virtual void add_to_master()
Add the driver to master.
Definition: node_canopen_driver.hpp:377
virtual void shutdown(bool called_from_base)
Shuts down the driver.
Definition: node_canopen_driver.hpp:368
virtual void activate(bool called_from_base)
Activates the driver.
Definition: node_canopen_driver.hpp:256
std::chrono::milliseconds non_transmit_timeout_
Definition: node_canopen_driver.hpp:73
std::shared_ptr< lely::canopen::AsyncMaster > master_
Definition: node_canopen_driver.hpp:70
YAML::Node config_
Definition: node_canopen_driver.hpp:74
NodeCanopenDriver(NODETYPE *node)
Definition: node_canopen_driver.hpp:89
std::shared_ptr< lely::ev::Executor > exec_
Definition: node_canopen_driver.hpp:69
virtual void configure(bool called_from_base)
Configure the driver.
Definition: node_canopen_driver.hpp:214
Definition: configuration_manager.hpp:28