15 #ifndef NODE_CANOPEN_BASE_DRIVER_IMPL
16 #define NODE_CANOPEN_BASE_DRIVER_IMPL
22 template <
class NODETYPE>
25 diagnostic_enabled_(false),
30 template <
class NODETYPE>
40 this->non_transmit_timeout_ =
41 std::chrono::milliseconds(this->config_[
"non_transmit_timeout"].as<int>());
47 this->node_->get_logger(),
48 "Non transmit timeout" <<
static_cast<int>(this->non_transmit_timeout_.count()) <<
"ms");
52 polling_ = this->config_[
"polling"].as<
bool>();
56 RCLCPP_WARN(this->node_->get_logger(),
"Could not polling from config, setting to true.");
63 period_ms_ = this->config_[
"period"].as<std::uint32_t>();
67 RCLCPP_WARN(this->node_->get_logger(),
"Could not read period from config, setting to 10ms");
75 diagnostic_enabled_ = this->config_[
"diagnostics"][
"enable"].as<
bool>();
80 this->node_->get_logger(),
81 "Could not read enable diagnostics from config, setting to false.");
82 diagnostic_enabled_ =
false;
84 if (diagnostic_enabled_.load())
88 diagnostic_period_ms_ = this->config_[
"diagnostics"][
"period"].as<std::uint32_t>();
93 this->node_->get_logger(),
94 "Could not read diagnostics period from config, setting to 1000ms");
95 diagnostic_period_ms_ = 1000;
98 diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
99 diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
102 std::optional<int> sdo_timeout_ms;
105 sdo_timeout_ms = std::optional(this->config_[
"sdo_timeout_ms"].as<int>());
110 sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
117 this->non_transmit_timeout_ =
118 std::chrono::milliseconds(this->config_[
"non_transmit_timeout"].as<int>());
124 this->node_->get_logger(),
125 "Non transmit timeout" <<
static_cast<int>(this->non_transmit_timeout_.count()) <<
"ms");
129 polling_ = this->config_[
"polling"].as<
bool>();
133 RCLCPP_WARN(this->node_->get_logger(),
"Could not polling from config, setting to true.");
140 period_ms_ = this->config_[
"period"].as<std::uint32_t>();
144 RCLCPP_WARN(this->node_->get_logger(),
"Could not read period from config, setting to 10ms");
152 diagnostic_enabled_ = this->config_[
"diagnostics"][
"enable"].as<
bool>();
157 this->node_->get_logger(),
158 "Could not read enable diagnostics from config, setting to false.");
159 diagnostic_enabled_ =
false;
161 if (diagnostic_enabled_.load())
165 diagnostic_period_ms_ = this->config_[
"diagnostics"][
"period"].as<std::uint32_t>();
170 this->node_->get_logger(),
171 "Could not read diagnostics period from config, setting to 1000ms");
172 diagnostic_period_ms_ = 1000;
175 diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
176 diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
179 std::optional<int> sdo_timeout_ms;
182 sdo_timeout_ms = std::optional(this->config_[
"sdo_timeout_ms"].as<int>());
187 sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
190 template <
class NODETYPE>
193 nmt_state_publisher_thread_ =
195 emcy_queue_ = this->lely_driver_->get_emcy_queue();
196 rpdo_queue_ = this->lely_driver_->get_rpdo_queue();
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_),
206 RCLCPP_INFO(this->node_->get_logger(),
"Starting with event mode.");
207 this->lely_driver_->set_sync_function(
211 if (diagnostic_enabled_.load())
213 RCLCPP_INFO(this->node_->get_logger(),
"Starting with diagnostics enabled.");
214 diagnostic_updater_->add(
219 template <
class NODETYPE>
222 nmt_state_publisher_thread_.join();
223 poll_timer_->cancel();
226 if (diagnostic_enabled_.load())
228 diagnostic_updater_->removeByName(
"diagnostic updater");
232 template <
class NODETYPE>
239 template <
class NODETYPE>
244 template <
class NODETYPE>
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();
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_);
263 auto future_status = f.wait_for(this->non_transmit_timeout_);
264 if (future_status != std::future_status::ready)
266 RCLCPP_ERROR(this->node_->get_logger(),
"Adding timed out.");
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())
273 RCLCPP_WARN(this->node_->get_logger(),
"Wait for device to boot.");
276 this->lely_driver_->wait_for_boot();
278 catch (
const std::exception & e)
280 RCLCPP_ERROR(this->node_->get_logger(), e.what());
283 RCLCPP_INFO(this->node_->get_logger(),
"Driver booted and ready.");
285 if (diagnostic_enabled_.load())
287 diagnostic_collector_->updateAll(
288 diagnostic_msgs::msg::DiagnosticStatus::OK,
"Device ready",
"DEVICE",
"Added to master.");
292 template <
class NODETYPE>
295 std::shared_ptr<std::promise<void>> prom = std::make_shared<std::promise<void>>();
296 auto f = prom->get_future();
300 this->driver_.reset();
301 this->lely_driver_.reset();
305 auto future_status = f.wait_for(this->non_transmit_timeout_);
306 if (future_status != std::future_status::ready)
310 if (diagnostic_enabled_.load())
312 diagnostic_collector_->updateAll(
313 diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"Device removed",
"DEVICE",
314 "Removed from master.");
317 template <
class NODETYPE>
322 std::future<lely::canopen::NmtState> f;
324 std::scoped_lock<std::mutex> lock(this->driver_mutex_);
325 f = this->lely_driver_->async_request_nmt();
327 while (f.wait_for(this->non_transmit_timeout_) != std::future_status::ready)
329 if (!this->activated_.load())
return;
333 auto state = f.get();
336 nmt_state_cb_(state, this->lely_driver_->get_id());
340 catch (
const std::future_error & e)
346 template <
class NODETYPE>
351 template <
class NODETYPE>
356 template <
class NODETYPE>
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)
369 emcy_msg.append(std::to_string(msef));
370 emcy_msg.append(
" ");
372 diagnostic_collector_->add(
"EMCY", emcy_msg);
375 template <
class NODETYPE>
378 RCLCPP_INFO(this->node_->get_logger(),
"Starting RPDO Listener");
379 auto q = lely_driver_->get_rpdo_queue();
383 while (!q->wait_and_pop_for(this->non_transmit_timeout_, rpdo))
385 if (!this->activated_.load())
return;
391 rpdo_cb_(rpdo, this->lely_driver_->get_id());
395 catch (
const std::exception & e)
397 RCLCPP_ERROR_STREAM(this->node_->get_logger(),
"RPDO Listener error: " << e.what());
402 template <
class NODETYPE>
405 for (
int i = 0; i < 10; i++)
407 auto opt = emcy_queue_->try_pop();
408 if (!opt.has_value())
416 emcy_cb_(opt.value(), this->lely_driver_->get_id());
418 on_emcy(opt.value());
420 catch (
const std::exception & e)
422 RCLCPP_ERROR_STREAM(this->node_->get_logger(),
"EMCY poll error: " << e.what());
426 for (
int i = 0; i < 10; i++)
428 auto opt = rpdo_queue_->try_pop();
429 if (!opt.has_value())
437 rpdo_cb_(opt.value(), this->lely_driver_->get_id());
439 on_rpdo(opt.value());
441 catch (
const std::exception & e)
443 RCLCPP_ERROR_STREAM(this->node_->get_logger(),
"RPDO Poll error: " << e.what());
449 template <
class NODETYPE>
452 RCLCPP_INFO(this->node_->get_logger(),
"Starting EMCY Listener");
453 auto q = lely_driver_->get_emcy_queue();
457 while (!q->wait_and_pop_for(this->non_transmit_timeout_, emcy))
459 if (!this->activated_.load())
return;
465 emcy_cb_(emcy, this->lely_driver_->get_id());
469 catch (
const std::exception & e)
471 RCLCPP_ERROR_STREAM(this->node_->get_logger(),
"EMCY Listener error: " << e.what());
477 template <
class NODETYPE>
479 diagnostic_updater::DiagnosticStatusWrapper & stat)
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