ros2_canopen  master
C++ ROS CANopen Library
node_canopen_base_driver_impl.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_BASE_DRIVER_IMPL
16 #define NODE_CANOPEN_BASE_DRIVER_IMPL
19 
20 using namespace ros2_canopen::node_interfaces;
21 
22 template <class NODETYPE>
24 : ros2_canopen::node_interfaces::NodeCanopenDriver<NODETYPE>(node),
25  diagnostic_enabled_(false),
26  diagnostic_collector_(new DiagnosticsCollector())
27 {
28 }
29 
30 template <class NODETYPE>
31 void NodeCanopenBaseDriver<NODETYPE>::init(bool called_from_base)
32 {
33 }
34 
35 template <>
37 {
38  try
39  {
40  this->non_transmit_timeout_ =
41  std::chrono::milliseconds(this->config_["non_transmit_timeout"].as<int>());
42  }
43  catch (...)
44  {
45  }
46  RCLCPP_INFO_STREAM(
47  this->node_->get_logger(),
48  "Non transmit timeout" << static_cast<int>(this->non_transmit_timeout_.count()) << "ms");
49 
50  try
51  {
52  polling_ = this->config_["polling"].as<bool>();
53  }
54  catch (...)
55  {
56  RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true.");
57  polling_ = true;
58  }
59  if (polling_)
60  {
61  try
62  {
63  period_ms_ = this->config_["period"].as<std::uint32_t>();
64  }
65  catch (...)
66  {
67  RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
68  period_ms_ = 10;
69  }
70  }
71 
72  // Diagnostic components
73  try
74  {
75  diagnostic_enabled_ = this->config_["diagnostics"]["enable"].as<bool>();
76  }
77  catch (...)
78  {
79  RCLCPP_WARN(
80  this->node_->get_logger(),
81  "Could not read enable diagnostics from config, setting to false.");
82  diagnostic_enabled_ = false;
83  }
84  if (diagnostic_enabled_.load())
85  {
86  try
87  {
88  diagnostic_period_ms_ = this->config_["diagnostics"]["period"].as<std::uint32_t>();
89  }
90  catch (...)
91  {
92  RCLCPP_ERROR(
93  this->node_->get_logger(),
94  "Could not read diagnostics period from config, setting to 1000ms");
95  diagnostic_period_ms_ = 1000;
96  }
97 
98  diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
99  diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
100  }
101 
102  std::optional<int> sdo_timeout_ms;
103  try
104  {
105  sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as<int>());
106  }
107  catch (...)
108  {
109  }
110  sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
111 }
112 template <>
113 void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
114 {
115  try
116  {
117  this->non_transmit_timeout_ =
118  std::chrono::milliseconds(this->config_["non_transmit_timeout"].as<int>());
119  }
120  catch (...)
121  {
122  }
123  RCLCPP_INFO_STREAM(
124  this->node_->get_logger(),
125  "Non transmit timeout" << static_cast<int>(this->non_transmit_timeout_.count()) << "ms");
126 
127  try
128  {
129  polling_ = this->config_["polling"].as<bool>();
130  }
131  catch (...)
132  {
133  RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true.");
134  polling_ = true;
135  }
136  if (polling_)
137  {
138  try
139  {
140  period_ms_ = this->config_["period"].as<std::uint32_t>();
141  }
142  catch (...)
143  {
144  RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
145  period_ms_ = 10;
146  }
147  }
148 
149  // Diagnostic components
150  try
151  {
152  diagnostic_enabled_ = this->config_["diagnostics"]["enable"].as<bool>();
153  }
154  catch (...)
155  {
156  RCLCPP_WARN(
157  this->node_->get_logger(),
158  "Could not read enable diagnostics from config, setting to false.");
159  diagnostic_enabled_ = false;
160  }
161  if (diagnostic_enabled_.load())
162  {
163  try
164  {
165  diagnostic_period_ms_ = this->config_["diagnostics"]["period"].as<std::uint32_t>();
166  }
167  catch (...)
168  {
169  RCLCPP_WARN(
170  this->node_->get_logger(),
171  "Could not read diagnostics period from config, setting to 1000ms");
172  diagnostic_period_ms_ = 1000;
173  }
174 
175  diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
176  diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
177  }
178 
179  std::optional<int> sdo_timeout_ms;
180  try
181  {
182  sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as<int>());
183  }
184  catch (...)
185  {
186  }
187  sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
188 }
189 
190 template <class NODETYPE>
191 void NodeCanopenBaseDriver<NODETYPE>::activate(bool called_from_base)
192 {
193  nmt_state_publisher_thread_ =
194  std::thread(std::bind(&NodeCanopenBaseDriver<NODETYPE>::nmt_listener, this));
195  emcy_queue_ = this->lely_driver_->get_emcy_queue();
196  rpdo_queue_ = this->lely_driver_->get_rpdo_queue();
197  if (polling_)
198  {
199  RCLCPP_INFO(this->node_->get_logger(), "Starting with polling mode.");
200  poll_timer_ = this->node_->create_wall_timer(
201  std::chrono::milliseconds(period_ms_),
202  std::bind(&NodeCanopenBaseDriver<NODETYPE>::poll_timer_callback, this), this->timer_cbg_);
203  }
204  else
205  {
206  RCLCPP_INFO(this->node_->get_logger(), "Starting with event mode.");
207  this->lely_driver_->set_sync_function(
209  }
210 
211  if (diagnostic_enabled_.load())
212  {
213  RCLCPP_INFO(this->node_->get_logger(), "Starting with diagnostics enabled.");
214  diagnostic_updater_->add(
215  "diagnostic updater", this, &NodeCanopenBaseDriver<NODETYPE>::diagnostic_callback);
216  }
217 }
218 
219 template <class NODETYPE>
221 {
222  nmt_state_publisher_thread_.join();
223  poll_timer_->cancel();
224  emcy_queue_.reset();
225  rpdo_queue_.reset();
226  if (diagnostic_enabled_.load())
227  {
228  diagnostic_updater_->removeByName("diagnostic updater");
229  }
230 }
231 
232 template <class NODETYPE>
233 void NodeCanopenBaseDriver<NODETYPE>::cleanup(bool called_from_base)
234 {
235  NodeCanopenDriver<NODETYPE>::cleanup(called_from_base);
236  // this->lely_driver_->unset_sync_function();
237 }
238 
239 template <class NODETYPE>
240 void NodeCanopenBaseDriver<NODETYPE>::shutdown(bool called_from_base)
241 {
242 }
243 
244 template <class NODETYPE>
246 {
247  RCLCPP_INFO(this->node_->get_logger(), "eds file %s", this->eds_.c_str());
248  RCLCPP_INFO(this->node_->get_logger(), "bin file %s", this->bin_.c_str());
249  std::shared_ptr<std::promise<std::shared_ptr<ros2_canopen::LelyDriverBridge>>> prom;
250  prom = std::make_shared<std::promise<std::shared_ptr<ros2_canopen::LelyDriverBridge>>>();
251  std::future<std::shared_ptr<ros2_canopen::LelyDriverBridge>> f = prom->get_future();
252  this->exec_->post(
253  [this, prom]()
254  {
255  std::scoped_lock<std::mutex> lock(this->driver_mutex_);
256  this->lely_driver_ = std::make_shared<ros2_canopen::LelyDriverBridge>(
257  *(this->exec_), *(this->master_), this->node_id_, this->node_->get_name(), this->eds_,
258  this->bin_, std::chrono::milliseconds(this->sdo_timeout_ms_));
259  this->driver_ = std::static_pointer_cast<lely::canopen::BasicDriver>(this->lely_driver_);
260  prom->set_value(lely_driver_);
261  });
262 
263  auto future_status = f.wait_for(this->non_transmit_timeout_);
264  if (future_status != std::future_status::ready)
265  {
266  RCLCPP_ERROR(this->node_->get_logger(), "Adding timed out.");
267  throw DriverException("add_to_master: adding timed out");
268  }
269  this->lely_driver_ = f.get();
270  this->driver_ = std::static_pointer_cast<lely::canopen::BasicDriver>(this->lely_driver_);
271  if (!this->lely_driver_->IsReady())
272  {
273  RCLCPP_WARN(this->node_->get_logger(), "Wait for device to boot.");
274  try
275  {
276  this->lely_driver_->wait_for_boot();
277  }
278  catch (const std::exception & e)
279  {
280  RCLCPP_ERROR(this->node_->get_logger(), e.what());
281  }
282  }
283  RCLCPP_INFO(this->node_->get_logger(), "Driver booted and ready.");
284 
285  if (diagnostic_enabled_.load())
286  {
287  diagnostic_collector_->updateAll(
288  diagnostic_msgs::msg::DiagnosticStatus::OK, "Device ready", "DEVICE", "Added to master.");
289  }
290 }
291 
292 template <class NODETYPE>
294 {
295  std::shared_ptr<std::promise<void>> prom = std::make_shared<std::promise<void>>();
296  auto f = prom->get_future();
297  this->exec_->post(
298  [this, prom]()
299  {
300  this->driver_.reset();
301  this->lely_driver_.reset();
302  prom->set_value();
303  });
304 
305  auto future_status = f.wait_for(this->non_transmit_timeout_);
306  if (future_status != std::future_status::ready)
307  {
308  throw DriverException("remove_from_master: removing timed out");
309  }
310  if (diagnostic_enabled_.load())
311  {
312  diagnostic_collector_->updateAll(
313  diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Device removed", "DEVICE",
314  "Removed from master.");
315  }
316 }
317 template <class NODETYPE>
319 {
320  while (rclcpp::ok())
321  {
322  std::future<lely::canopen::NmtState> f;
323  {
324  std::scoped_lock<std::mutex> lock(this->driver_mutex_);
325  f = this->lely_driver_->async_request_nmt();
326  }
327  while (f.wait_for(this->non_transmit_timeout_) != std::future_status::ready)
328  {
329  if (!this->activated_.load()) return;
330  }
331  try
332  {
333  auto state = f.get();
334  if (nmt_state_cb_)
335  {
336  nmt_state_cb_(state, this->lely_driver_->get_id());
337  }
338  on_nmt(state);
339  }
340  catch (const std::future_error & e)
341  {
342  break;
343  }
344  }
345 }
346 template <class NODETYPE>
347 void NodeCanopenBaseDriver<NODETYPE>::on_nmt(canopen::NmtState nmt_state)
348 {
349 }
350 
351 template <class NODETYPE>
353 {
354 }
355 
356 template <class NODETYPE>
358 {
359  diagnostic_collector_->summary(
360  diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Emergency message received");
361  std::string emcy_msg = "Emergency message: ";
362  emcy_msg.append("eec: ");
363  emcy_msg.append(std::to_string(emcy.eec));
364  emcy_msg.append(" er: ");
365  emcy_msg.append(std::to_string(emcy.er));
366  emcy_msg.append(" msef: ");
367  for (auto & msef : emcy.msef)
368  {
369  emcy_msg.append(std::to_string(msef));
370  emcy_msg.append(" ");
371  }
372  diagnostic_collector_->add("EMCY", emcy_msg);
373 }
374 
375 template <class NODETYPE>
377 {
378  RCLCPP_INFO(this->node_->get_logger(), "Starting RPDO Listener");
379  auto q = lely_driver_->get_rpdo_queue();
380  while (rclcpp::ok())
381  {
383  while (!q->wait_and_pop_for(this->non_transmit_timeout_, rpdo))
384  {
385  if (!this->activated_.load()) return;
386  }
387  try
388  {
389  if (rpdo_cb_)
390  {
391  rpdo_cb_(rpdo, this->lely_driver_->get_id());
392  }
393  on_rpdo(rpdo);
394  }
395  catch (const std::exception & e)
396  {
397  RCLCPP_ERROR_STREAM(this->node_->get_logger(), "RPDO Listener error: " << e.what());
398  break;
399  }
400  }
401 }
402 template <class NODETYPE>
404 {
405  for (int i = 0; i < 10; i++)
406  {
407  auto opt = emcy_queue_->try_pop();
408  if (!opt.has_value())
409  {
410  break;
411  }
412  try
413  {
414  if (emcy_cb_)
415  {
416  emcy_cb_(opt.value(), this->lely_driver_->get_id());
417  }
418  on_emcy(opt.value());
419  }
420  catch (const std::exception & e)
421  {
422  RCLCPP_ERROR_STREAM(this->node_->get_logger(), "EMCY poll error: " << e.what());
423  break;
424  }
425  }
426  for (int i = 0; i < 10; i++)
427  {
428  auto opt = rpdo_queue_->try_pop();
429  if (!opt.has_value())
430  {
431  break;
432  }
433  try
434  {
435  if (rpdo_cb_)
436  {
437  rpdo_cb_(opt.value(), this->lely_driver_->get_id());
438  }
439  on_rpdo(opt.value());
440  }
441  catch (const std::exception & e)
442  {
443  RCLCPP_ERROR_STREAM(this->node_->get_logger(), "RPDO Poll error: " << e.what());
444  break;
445  }
446  }
447 }
448 
449 template <class NODETYPE>
451 {
452  RCLCPP_INFO(this->node_->get_logger(), "Starting EMCY Listener");
453  auto q = lely_driver_->get_emcy_queue();
454  while (rclcpp::ok())
455  {
457  while (!q->wait_and_pop_for(this->non_transmit_timeout_, emcy))
458  {
459  if (!this->activated_.load()) return;
460  }
461  try
462  {
463  if (emcy_cb_)
464  {
465  emcy_cb_(emcy, this->lely_driver_->get_id());
466  }
467  on_emcy(emcy);
468  }
469  catch (const std::exception & e)
470  {
471  RCLCPP_ERROR_STREAM(this->node_->get_logger(), "EMCY Listener error: " << e.what());
472  break;
473  }
474  }
475 }
476 
477 template <class NODETYPE>
479  diagnostic_updater::DiagnosticStatusWrapper & stat)
480 {
481 }
482 
483 #endif
A class to collect diagnostic information.
Definition: diagnostic_collector.hpp:34
Driver Exception.
Definition: driver_error.hpp:31
Definition: node_canopen_base_driver.hpp:34
void rdpo_listener()
Definition: node_canopen_base_driver_impl.hpp:376
virtual void add_to_master()
Add the driver to master.
Definition: node_canopen_base_driver_impl.hpp:245
void nmt_listener()
Definition: node_canopen_base_driver_impl.hpp:318
virtual void on_rpdo(COData data)
Definition: node_canopen_base_driver_impl.hpp:352
virtual void poll_timer_callback()
Definition: node_canopen_base_driver_impl.hpp:403
NodeCanopenBaseDriver(NODETYPE *node)
Definition: node_canopen_base_driver_impl.hpp:23
virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: node_canopen_base_driver_impl.hpp:478
virtual void remove_from_master()
Remove the driver from master.
Definition: node_canopen_base_driver_impl.hpp:293
void emcy_listener()
Definition: node_canopen_base_driver_impl.hpp:450
virtual void on_nmt(canopen::NmtState nmt_state)
Definition: node_canopen_base_driver_impl.hpp:347
virtual void on_emcy(COEmcy emcy)
Definition: node_canopen_base_driver_impl.hpp:357
Node Canopen Driver.
Definition: node_canopen_driver.hpp:60
void shutdown()
Shutdown the driver.
Definition: node_canopen_driver.hpp:344
void deactivate()
Deactivate the driver.
Definition: node_canopen_driver.hpp:265
void cleanup()
Cleanup the driver.
Definition: node_canopen_driver.hpp:305
void activate()
Activate the driver.
Definition: node_canopen_driver.hpp:223
void init()
Initialise the driver.
Definition: node_canopen_driver.hpp:127
void configure()
Configure the driver.
Definition: node_canopen_driver.hpp:175
Definition: node_canopen_driver.hpp:46
Definition: configuration_manager.hpp:28
Definition: exchange.hpp:26
Definition: exchange.hpp:34
uint16_t eec
Definition: exchange.hpp:36
uint8_t er
Definition: exchange.hpp:37
uint8_t msef[5]
Definition: exchange.hpp:38