ros2_canopen  master
C++ ROS CANopen Library
node_canopen_master.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_MASTER_HPP_
16 #define NODE_CANOPEN_MASTER_HPP_
17 
18 #include <yaml-cpp/yaml.h>
19 #include <atomic>
20 #include <lely/coapp/master.hpp>
21 #include <lely/coapp/slave.hpp>
22 #include <lely/ev/exec.hpp>
23 #include <lely/ev/loop.hpp>
24 #include <lely/io2/linux/can.hpp>
25 #include <lely/io2/posix/poll.hpp>
26 #include <lely/io2/sys/io.hpp>
27 #include <lely/io2/sys/sigset.hpp>
28 #include <lely/io2/sys/timer.hpp>
29 #include <rclcpp/rclcpp.hpp>
30 #include <rclcpp_lifecycle/lifecycle_node.hpp>
31 #include <thread>
34 
35 namespace ros2_canopen
36 {
37 namespace node_interfaces
38 {
50 template <class NODETYPE>
52 {
53  static_assert(
54  std::is_base_of<rclcpp::Node, NODETYPE>::value ||
55  std::is_base_of<rclcpp_lifecycle::LifecycleNode, NODETYPE>::value,
56  "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode");
57 
58 protected:
59  NODETYPE * node_;
60 
61  std::atomic<bool> initialised_;
62  std::atomic<bool> configured_;
63  std::atomic<bool> activated_;
64  std::atomic<bool> master_set_;
65 
66  std::shared_ptr<lely::canopen::AsyncMaster> master_;
67  std::shared_ptr<lely::ev::Executor> exec_;
68 
69  std::unique_ptr<lely::io::IoGuard> io_guard_;
70  std::unique_ptr<lely::io::Context> ctx_;
71  std::unique_ptr<lely::io::Poll> poll_;
72  std::unique_ptr<lely::ev::Loop> loop_;
73  std::unique_ptr<lely::io::Timer> timer_;
74  std::unique_ptr<lely::io::CanController> ctrl_;
75  std::unique_ptr<lely::io::CanChannel> chan_;
76  std::unique_ptr<lely::io::SignalSet> sigset_;
77 
78  rclcpp::CallbackGroup::SharedPtr client_cbg_;
79  rclcpp::CallbackGroup::SharedPtr timer_cbg_;
80 
81  YAML::Node config_;
82  uint8_t node_id_;
83  std::chrono::milliseconds non_transmit_timeout_;
84  std::string container_name_;
85  std::string master_dcf_;
86  std::string master_bin_;
87  std::string can_interface_name_;
88  uint32_t timeout_;
89 
90  std::thread spinner_;
91 
92 public:
93  NodeCanopenMaster(NODETYPE * node)
94  : initialised_(false), configured_(false), activated_(false), master_set_(false)
95  {
96  node_ = node;
97  }
98 
103  void init() override
104  {
105  RCLCPP_DEBUG(node_->get_logger(), "init_start");
106  if (configured_.load())
107  {
108  throw MasterException("Init: Master is already configured.");
109  }
110  if (activated_.load())
111  {
112  throw MasterException("Init: Master is already activated.");
113  }
114  client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
115  timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
116 
117  node_->declare_parameter("container_name", "");
118  node_->declare_parameter("master_dcf", "");
119  node_->declare_parameter("master_bin", "");
120  node_->declare_parameter("can_interface_name", "vcan0");
121  node_->declare_parameter("node_id", 0);
122  node_->declare_parameter("non_transmit_timeout", 100);
123  node_->declare_parameter("config", "");
124  this->init(true);
125  this->initialised_.store(true);
126  RCLCPP_DEBUG(node_->get_logger(), "init_end");
127  }
128  virtual void init(bool called_from_base) {}
129 
137  void configure() override
138  {
139  if (!initialised_.load())
140  {
141  throw MasterException("Configure: Master is not initialised.");
142  }
143  if (configured_.load())
144  {
145  throw MasterException("Configure: Master is already configured.");
146  }
147  if (activated_.load())
148  {
149  throw MasterException("Configure: Master is already activated.");
150  }
151 
152  int non_transmit_timeout;
153  std::string config;
154 
155  node_->get_parameter("container_name", container_name_);
156  node_->get_parameter("master_dcf", master_dcf_);
157  node_->get_parameter("master_bin", master_bin_);
158  node_->get_parameter("can_interface_name", can_interface_name_);
159  node_->get_parameter("node_id", node_id_);
160  node_->get_parameter("non_transmit_timeout", non_transmit_timeout);
161  node_->get_parameter("config", config);
162 
163  this->config_ = YAML::Load(config);
164  this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout);
165 
166  this->configure(true);
167  this->configured_.store(true);
168  }
169 
170  virtual void configure(bool called_from_base)
171  {
172  std::optional<int> timeout;
173  try
174  {
175  timeout = this->config_["boot_timeout"].as<int>();
176  }
177  catch (...)
178  {
179  RCLCPP_WARN(
180  this->node_->get_logger(),
181  "No timeout parameter found in config file. Using default value of 100ms.");
182  }
183  this->timeout_ = timeout.value_or(2000);
184  RCLCPP_INFO_STREAM(
185  this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms.");
186  }
187 
195  void activate() override
196  {
197  RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate start");
198  if (!initialised_.load())
199  {
200  throw MasterException("Activate: master is not initialised");
201  }
202  if (!configured_.load())
203  {
204  throw MasterException("Activate: master is not configured");
205  }
206  if (activated_.load())
207  {
208  throw MasterException("Activate: master is already activated");
209  }
210 
211  io_guard_ = std::make_unique<lely::io::IoGuard>();
212  ctx_ = std::make_unique<lely::io::Context>();
213  poll_ = std::make_unique<lely::io::Poll>(*ctx_);
214  loop_ = std::make_unique<lely::ev::Loop>(poll_->get_poll());
215 
216  exec_ = std::make_shared<lely::ev::Executor>(loop_->get_executor());
217  timer_ = std::make_unique<lely::io::Timer>(*poll_, *exec_, CLOCK_MONOTONIC);
218  ctrl_ = std::make_unique<lely::io::CanController>(can_interface_name_.c_str());
219  chan_ = std::make_unique<lely::io::CanChannel>(*poll_, *exec_);
220  chan_->open(*ctrl_);
221 
222  this->activate(true);
223  if (!master_)
224  {
225  throw MasterException("Activate: master not set");
226  }
227  this->master_set_.store(true);
228  this->master_->Reset();
229  this->spinner_ = std::thread(
230  [this]()
231  {
232  try
233  {
234  loop_->run();
235  }
236  catch (const std::exception & e)
237  {
238  RCLCPP_INFO(this->node_->get_logger(), e.what());
239  }
240  RCLCPP_INFO(this->node_->get_logger(), "Canopen master loop stopped");
241  });
242  this->activated_.store(true);
243  RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate end");
244  }
254  virtual void activate(bool called_from_base) {}
255 
263  void deactivate() override
264  {
265  if (!initialised_.load())
266  {
267  throw MasterException("Deactivate: master is not initialised");
268  }
269  if (!configured_.load())
270  {
271  throw MasterException("Deactivate: master is not configured");
272  }
273  if (!activated_.load())
274  {
275  throw MasterException("Deactivate: master is not activated");
276  }
277  exec_->post(
278  [this]()
279  {
280  RCLCPP_INFO(node_->get_logger(), "Lely Core Context Shutdown");
281  ctx_->shutdown();
282  });
283  this->spinner_.join();
284 
285  this->deactivate(true);
286  this->activated_.store(false);
287  }
288 
297  virtual void deactivate(bool called_from_base) {}
298 
306  void cleanup() override
307  {
308  if (!initialised_.load())
309  {
310  throw MasterException("Cleanup: master is not initialised.");
311  }
312  if (!configured_.load())
313  {
314  throw MasterException("Cleanup: master is not configured");
315  }
316  if (activated_.load())
317  {
318  throw MasterException("Cleanup: master is still active");
319  }
320  this->cleanup(true);
321  io_guard_.reset();
322  ctx_.reset();
323  poll_.reset();
324  loop_.reset();
325 
326  exec_.reset();
327  timer_.reset();
328  ctrl_.reset();
329  chan_.reset();
330  this->configured_.store(false);
331  this->master_set_.store(false);
332  }
333  virtual void cleanup(bool called_from_base) {}
334 
341  void shutdown() override
342  {
343  RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down.");
344  if (this->activated_)
345  {
346  this->deactivate();
347  }
348 
349  if (this->configured_)
350  {
351  this->cleanup();
352  }
353  shutdown(true);
354 
355  this->master_set_.store(false);
356  this->initialised_.store(false);
357  this->configured_.store(false);
358  this->activated_.store(false);
359  }
360  virtual void shutdown(bool called_from_base) {}
361 
367  virtual std::shared_ptr<lely::canopen::AsyncMaster> get_master()
368  {
369  if (!master_set_.load())
370  {
371  throw MasterException("Get Master: Master is not set.");
372  }
373  return master_;
374  }
380  virtual std::shared_ptr<lely::ev::Executor> get_executor()
381  {
382  if (!master_set_.load())
383  {
384  throw MasterException("Get Executor: master is not set");
385  }
386  return exec_;
387  }
388 };
389 } // namespace node_interfaces
390 } // namespace ros2_canopen
391 
392 #endif
Master Exception.
Definition: master_error.hpp:30
Node Canopen Master Interface.
Definition: node_canopen_master_interface.hpp:34
Node Canopen Master.
Definition: node_canopen_master.hpp:52
YAML::Node config_
Definition: node_canopen_master.hpp:81
void shutdown() override
Shutdown the driver.
Definition: node_canopen_master.hpp:341
std::unique_ptr< lely::ev::Loop > loop_
Definition: node_canopen_master.hpp:72
rclcpp::CallbackGroup::SharedPtr client_cbg_
Definition: node_canopen_master.hpp:78
virtual void deactivate(bool called_from_base)
Deactivate hook for derived classes.
Definition: node_canopen_master.hpp:297
void configure() override
Configure the driver.
Definition: node_canopen_master.hpp:137
virtual void init(bool called_from_base)
Definition: node_canopen_master.hpp:128
virtual void shutdown(bool called_from_base)
Definition: node_canopen_master.hpp:360
std::string can_interface_name_
Definition: node_canopen_master.hpp:87
std::string master_bin_
Definition: node_canopen_master.hpp:86
std::shared_ptr< lely::ev::Executor > exec_
Definition: node_canopen_master.hpp:67
std::unique_ptr< lely::io::CanChannel > chan_
Definition: node_canopen_master.hpp:75
void init() override
Initialise Master.
Definition: node_canopen_master.hpp:103
virtual std::shared_ptr< lely::canopen::AsyncMaster > get_master()
Get the master object.
Definition: node_canopen_master.hpp:367
virtual void activate(bool called_from_base)
Activate hook for derived classes.
Definition: node_canopen_master.hpp:254
std::atomic< bool > master_set_
Definition: node_canopen_master.hpp:64
std::unique_ptr< lely::io::Context > ctx_
Definition: node_canopen_master.hpp:70
std::unique_ptr< lely::io::IoGuard > io_guard_
Definition: node_canopen_master.hpp:69
std::unique_ptr< lely::io::Poll > poll_
Definition: node_canopen_master.hpp:71
std::string container_name_
Definition: node_canopen_master.hpp:84
virtual std::shared_ptr< lely::ev::Executor > get_executor()
Get the executor object.
Definition: node_canopen_master.hpp:380
std::chrono::milliseconds non_transmit_timeout_
Definition: node_canopen_master.hpp:83
NODETYPE * node_
Definition: node_canopen_master.hpp:56
virtual void configure(bool called_from_base)
Definition: node_canopen_master.hpp:170
std::unique_ptr< lely::io::CanController > ctrl_
Definition: node_canopen_master.hpp:74
std::shared_ptr< lely::canopen::AsyncMaster > master_
Definition: node_canopen_master.hpp:66
void deactivate() override
Deactivate the driver.
Definition: node_canopen_master.hpp:263
void cleanup() override
Cleanup the driver.
Definition: node_canopen_master.hpp:306
rclcpp::CallbackGroup::SharedPtr timer_cbg_
Definition: node_canopen_master.hpp:79
std::unique_ptr< lely::io::Timer > timer_
Definition: node_canopen_master.hpp:73
std::atomic< bool > activated_
Definition: node_canopen_master.hpp:63
virtual void cleanup(bool called_from_base)
Definition: node_canopen_master.hpp:333
void activate() override
Activate the driver.
Definition: node_canopen_master.hpp:195
std::atomic< bool > initialised_
Definition: node_canopen_master.hpp:61
std::thread spinner_
Definition: node_canopen_master.hpp:90
std::unique_ptr< lely::io::SignalSet > sigset_
Definition: node_canopen_master.hpp:76
NodeCanopenMaster(NODETYPE *node)
Definition: node_canopen_master.hpp:93
uint32_t timeout_
Definition: node_canopen_master.hpp:88
std::string master_dcf_
Definition: node_canopen_master.hpp:85
uint8_t node_id_
Definition: node_canopen_master.hpp:82
std::atomic< bool > configured_
Definition: node_canopen_master.hpp:62
Definition: configuration_manager.hpp:28