15 #ifndef CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_
16 #define CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_
20 #include <lely/co/dcf.hpp>
21 #include <lely/co/dev.hpp>
22 #include <lely/co/obj.hpp>
23 #include <lely/coapp/fiber_driver.hpp>
24 #include <lely/coapp/master.hpp>
25 #include <lely/coapp/sdo_error.hpp>
26 #include <lely/ev/co_task.hpp>
27 #include <lely/ev/future.hpp>
30 #include <condition_variable>
34 #include <rclcpp/rclcpp.hpp>
36 #include <system_error>
40 #include <boost/lockfree/queue.hpp>
41 #include <boost/optional.hpp>
42 #include <boost/thread.hpp>
46 using namespace std::chrono_literals;
57 typedef std::map<uint16_t, std::map<uint8_t, pdo_mapping>>
PDOMap;
69 std::shared_ptr<PDOMap> pdo_map = std::make_shared<PDOMap>();
113 for (
int index = 0; index < 256; index++)
115 for (
int subindex = 1; subindex < 9; subindex++)
117 auto obj = find(0x1600 + index, subindex);
124 data = obj->getVal<CO_DEFTYPE_UNSIGNED32>();
126 uint8_t tmps = (data >> 8) & 0xFF;
127 uint16_t tmpi = (data >> 16) & 0xFFFF;
135 (*map)[tmpi][tmps] = mapping;
136 std::cout <<
"Found rpdo mapped object: index=" << std::hex << (int)tmpi
137 <<
" subindex=" << (
int)tmps << std::endl;
143 for (
int index = 0; index < 256; index++)
145 for (
int subindex = 1; subindex < 9; subindex++)
147 auto obj = find(0x1A00 + index, subindex);
154 data = obj->getVal<CO_DEFTYPE_UNSIGNED32>();
156 uint8_t tmps = (data >> 8) & 0xFF;
157 uint16_t tmpi = (data >> 16) & 0xFFFF;
166 (*map)[tmpi][tmps] = mapping;
167 std::cout <<
"Found tpdo mapped object: index=" << std::hex << (int)tmpi
168 <<
" subindex=" << (
int)tmps << std::endl;
177 for (
int i = 0; i < 256; i++)
179 if (this->checkObjInPDO(i, 0x1600, idx, subidx))
191 for (
int i = 0; i < 256; i++)
193 if (this->checkObjInPDO(i, 0x1A00, idx, subidx))
201 bool checkObjInPDO(uint8_t pdo, uint16_t mapping_idx, uint16_t idx, uint8_t subindex)
203 for (
int i = 1; i < 9; i++)
205 auto obj = find(mapping_idx + pdo, i);
212 data = obj->getVal<CO_DEFTYPE_UNSIGNED32>();
214 uint8_t tmps = (data >> 8) & 0xFF;
215 uint16_t tmpi = (data >> 16) & 0xFFFF;
217 if (tmps == subindex && tmpi == idx)
219 std::cout <<
"Found object in pdo: " << (int)pdo << std::endl;
249 const char *
name() const noexcept override;
250 std::
string message(
int ev) const override;
332 void OnSync(uint8_t cnt,
const time_point & t) noexcept
override
334 if (on_sync_function_ !=
nullptr)
354 void OnState(canopen::NmtState state) noexcept
override;
366 virtual void OnBoot(canopen::NmtState st,
char es, const ::std::string & what) noexcept
override;
388 void OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept
override;
391 using FiberDriver::FiberDriver;
405 ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t
id, std::string name, std::string eds,
406 std::string bin, std::chrono::milliseconds timeout = 20ms)
407 : FiberDriver(exec, master, id),
414 dictionary_ = std::make_unique<DriverDictionary>(eds.c_str());
416 if (stat(bin.c_str(), &buffer) == 0)
418 co_unsigned16_t * a = NULL;
419 co_unsigned16_t * b = NULL;
420 dictionary_->readDCF(a, b, bin.c_str());
422 pdo_map_ = dictionary_->createPDOMapping();
423 sdo_timeout = timeout;
441 template <
typename T>
444 std::unique_lock<std::mutex> lck(sdo_mutex);
451 auto prom = std::make_shared<std::promise<bool>>();
452 lely::COSub * sub = this->dictionary_->find(idx, subidx);
455 std::cout <<
"async_sdo_write_typed: id=" << (
unsigned int)this->get_id() <<
" index=0x"
456 << std::hex << (
unsigned int)idx <<
" subindex=" << (
unsigned int)subidx
457 <<
" object does not exist" << std::endl;
458 prom->set_value(
false);
459 this->running =
false;
460 this->sdo_cond.notify_one();
461 return prom->get_future();
466 [
this, value, prom](uint8_t
id, uint16_t idx, uint8_t subidx, ::std::error_code ec)
mutable
471 lely::canopen::make_sdo_exception_ptr(
id, idx, subidx, ec,
"AsyncDownload"));
475 std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
476 this->dictionary_->setVal<T>(idx, subidx, value);
477 prom->set_value(
true);
479 std::unique_lock<std::mutex> lck(this->sdo_mutex);
480 this->running =
false;
481 this->sdo_cond.notify_one();
484 return prom->get_future();
487 template <
typename T>
489 uint16_t idx, uint8_t subidx, T value, std::chrono::milliseconds timeout)
491 auto fut = async_sdo_write_typed(idx, subidx, value);
492 auto wait_res = fut.wait_for(timeout);
493 if (wait_res == std::future_status::timeout)
495 std::cout <<
"sync_sdo_write_typed: id=" << (
unsigned int)this->get_id() <<
" index=0x"
496 << std::hex << (
unsigned int)idx <<
" subindex=" << (
unsigned int)subidx
497 <<
" timed out." << std::endl;
505 catch (std::exception & e)
507 RCLCPP_ERROR(rclcpp::get_logger(name_), e.what());
527 template <
typename T>
530 std::unique_lock<std::mutex> lck(sdo_mutex);
537 auto prom = std::make_shared<std::promise<T>>();
538 lely::COSub * sub = this->dictionary_->find(idx, subidx);
541 std::cout <<
"async_sdo_read: id=" << (
unsigned int)this->get_id() <<
" index=0x" << std::hex
542 << (
unsigned int)idx <<
" subindex=" << (
unsigned int)subidx
543 <<
" object does not exist" << std::endl;
546 throw lely::canopen::SdoError(this->get_id(), idx, subidx, lely::canopen::SdoErrc::NO_OBJ);
550 prom->set_exception(std::current_exception());
552 this->running =
false;
553 this->sdo_cond.notify_one();
554 return prom->get_future();
558 [
this, prom](uint8_t
id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value)
mutable
563 lely::canopen::make_sdo_exception_ptr(
id, idx, subidx, ec,
"AsyncUpload"));
567 std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
568 this->dictionary_->setVal<T>(idx, subidx, value);
569 prom->set_value(value);
571 std::unique_lock<std::mutex> lck(this->sdo_mutex);
572 this->running =
false;
573 this->sdo_cond.notify_one();
576 return prom->get_future();
579 template <
typename T>
581 uint16_t idx, uint8_t subidx, T & value, std::chrono::milliseconds timeout)
583 auto fut = async_sdo_read_typed<T>(idx, subidx);
584 auto wait_res = fut.wait_for(timeout);
585 if (wait_res == std::future_status::timeout)
587 std::cout <<
"sync_sdo_read_typed: id=" << (
unsigned int)this->get_id() <<
" index=0x"
588 << std::hex << (
unsigned int)idx <<
" subindex=" << (
unsigned int)subidx
589 <<
" timed out." << std::endl;
598 catch (std::exception & e)
600 RCLCPP_ERROR(rclcpp::get_logger(name_), e.what());
679 std::unique_lock<std::mutex> lck(boot_mtex);
681 if ((boot_status != 0) && (boot_status !=
'L'))
695 on_sync_function_ = on_sync_function;
718 template <
typename T>
722 std::memcpy(&value, &data.
data_,
sizeof(value));
726 [
this, value](uint8_t
id, uint16_t idx, uint8_t subidx, ::std::error_code ec)
mutable
730 this->sdo_write_data_promise->set_exception(
731 lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec,
"AsyncDownload"));
735 std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
736 this->dictionary_->setVal<T>(idx, subidx, value);
737 this->sdo_write_data_promise->set_value(true);
739 std::unique_lock<std::mutex> lck(this->sdo_mutex);
740 this->running =
false;
741 this->sdo_cond.notify_one();
746 template <
typename T>
751 [
this](uint8_t
id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value)
mutable
755 this->sdo_read_data_promise->set_exception(
756 lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec,
"AsyncUpload"));
760 std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
761 this->dictionary_->setVal<T>(idx, subidx, value);
762 COData d = {idx, subidx, 0};
763 std::memcpy(&d.data_, &value,
sizeof(T));
764 this->sdo_read_data_promise->set_value(d);
766 std::unique_lock<std::mutex> lck(this->sdo_mutex);
767 this->running =
false;
768 this->sdo_cond.notify_one();
773 template <
typename T>
777 bool is_tpdo =
false;
778 if (this->pdo_map_->find(index) != this->pdo_map_->end())
780 auto object = this->pdo_map_->at(index);
781 if (
object.find(subindex) !=
object.end())
783 auto entry =
object.at(subindex);
784 is_tpdo = entry.is_tpdo;
789 if (sync_sdo_read_typed<T>(index, subindex, value, this->sdo_timeout))
795 std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
796 if (
typeid(T) ==
typeid(uint8_t))
798 value = this->dictionary_->getVal<CO_DEFTYPE_UNSIGNED8>(index, subindex);
800 if (
typeid(T) ==
typeid(uint16_t))
802 value = this->dictionary_->getVal<CO_DEFTYPE_UNSIGNED16>(index, subindex);
804 if (
typeid(T) ==
typeid(uint32_t))
806 value = this->dictionary_->getVal<CO_DEFTYPE_UNSIGNED32>(index, subindex);
808 if (
typeid(T) ==
typeid(int8_t))
810 value = this->dictionary_->getVal<CO_DEFTYPE_INTEGER8>(index, subindex);
812 if (
typeid(T) ==
typeid(int16_t))
814 value = this->dictionary_->getVal<CO_DEFTYPE_INTEGER16>(index, subindex);
816 if (
typeid(T) ==
typeid(int32_t))
818 value = this->dictionary_->getVal<CO_DEFTYPE_INTEGER32>(index, subindex);
824 template <
typename T>
827 bool is_rpdo =
false;
828 if (this->pdo_map_->find(index) != this->pdo_map_->end())
830 auto object = this->pdo_map_->at(index);
831 if (
object.find(subindex) !=
object.end())
833 auto entry =
object.at(subindex);
834 is_rpdo = entry.is_rpdo;
839 std::scoped_lock<std::mutex> lck(this->dictionary_mutex_);
840 this->dictionary_->setVal<T>(index, subindex, value);
841 this->tpdo_mapped[index][subindex] = value;
842 this->tpdo_mapped[index][subindex].WriteEvent();
846 sync_sdo_write_typed(index, subindex, value, this->sdo_timeout);
Definition: lely_driver_bridge.hpp:59
~DriverDictionary()
Definition: lely_driver_bridge.hpp:62
DriverDictionary(std::string eds_file)
Definition: lely_driver_bridge.hpp:61
bool checkObjTPDO(uint16_t idx, uint8_t subidx)
Definition: lely_driver_bridge.hpp:187
bool checkObjInPDO(uint8_t pdo, uint16_t mapping_idx, uint16_t idx, uint8_t subindex)
Definition: lely_driver_bridge.hpp:201
std::shared_ptr< PDOMap > createPDOMapping()
Definition: lely_driver_bridge.hpp:67
void fetchTPDO(std::shared_ptr< PDOMap > map)
Definition: lely_driver_bridge.hpp:141
void fetchRPDO(std::shared_ptr< PDOMap > map)
Definition: lely_driver_bridge.hpp:111
bool checkObjRPDO(uint16_t idx, uint8_t subidx)
Definition: lely_driver_bridge.hpp:173
Lely Driver Bridge.
Definition: lely_driver_bridge.hpp:276
std::atomic< bool > emcy_is_set
Definition: lely_driver_bridge.hpp:303
std::mutex sdo_mutex
Definition: lely_driver_bridge.hpp:286
void universal_set_value(uint16_t index, uint8_t subindex, T value)
Definition: lely_driver_bridge.hpp:825
bool is_booted()
Indicates if Device is booted.
Definition: lely_driver_bridge.hpp:716
std::mutex boot_mtex
Definition: lely_driver_bridge.hpp:313
std::shared_ptr< SafeQueue< COEmcy > > get_emcy_queue()
Asynchronous request for EMCY.
bool sync_sdo_write_typed(uint16_t idx, uint8_t subidx, T value, std::chrono::milliseconds timeout)
Definition: lely_driver_bridge.hpp:488
std::promise< COData > rpdo_promise
Definition: lely_driver_bridge.hpp:296
bool running
Definition: lely_driver_bridge.hpp:287
bool sync_sdo_read_typed(uint16_t idx, uint8_t subidx, T &value, std::chrono::milliseconds timeout)
Definition: lely_driver_bridge.hpp:580
std::shared_ptr< std::promise< bool > > sdo_write_data_promise
Definition: lely_driver_bridge.hpp:285
std::future< canopen::NmtState > async_request_nmt()
Asynchronous request for NMT.
std::shared_ptr< PDOMap > pdo_map_
Definition: lely_driver_bridge.hpp:281
std::unique_ptr< DriverDictionary > dictionary_
Definition: lely_driver_bridge.hpp:279
std::atomic< bool > rpdo_is_set
Definition: lely_driver_bridge.hpp:297
std::condition_variable sdo_cond
Definition: lely_driver_bridge.hpp:288
void unset_sync_function()
Definition: lely_driver_bridge.hpp:698
std::mutex emcy_mtex
Definition: lely_driver_bridge.hpp:304
void OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept override
OnRpdoWrite Callback.
virtual void OnBoot(canopen::NmtState st, char es, const ::std::string &what) noexcept override
OnBoot Callback This callback is called when the Boot process of the slave that was initiated by the ...
void nmt_command(canopen::NmtCommand command)
Executes a NMT Command.
void set_sync_function(std::function< void()> on_sync_function)
Definition: lely_driver_bridge.hpp:693
std::string boot_what
Definition: lely_driver_bridge.hpp:310
std::mutex nmt_mtex
Definition: lely_driver_bridge.hpp:293
std::promise< canopen::NmtState > nmt_state_promise
Definition: lely_driver_bridge.hpp:291
std::chrono::milliseconds sdo_timeout
Definition: lely_driver_bridge.hpp:318
void submit_read(COData data)
Definition: lely_driver_bridge.hpp:747
std::atomic< bool > nmt_state_is_set
Definition: lely_driver_bridge.hpp:292
std::function< void()> on_sync_function_
Definition: lely_driver_bridge.hpp:320
uint8_t get_id()
Get the nodeid.
void OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept override
void submit_write(COData data)
Definition: lely_driver_bridge.hpp:719
std::future< bool > async_sdo_write(COData data)
Asynchronous SDO Write.
std::shared_ptr< SafeQueue< COData > > get_rpdo_queue()
Asynchronous request for RPDO.
const T universal_get_value(uint16_t index, uint8_t subindex)
Definition: lely_driver_bridge.hpp:774
std::shared_ptr< SafeQueue< COData > > rpdo_queue
Definition: lely_driver_bridge.hpp:299
std::future< bool > async_sdo_write_typed(uint16_t idx, uint8_t subidx, T value)
Definition: lely_driver_bridge.hpp:442
std::mutex pdo_mtex
Definition: lely_driver_bridge.hpp:298
void Boot()
Request master to boot device.
Definition: lely_driver_bridge.hpp:704
std::future< COData > async_sdo_read(COData data)
Aynchronous SDO Read.
bool wait_for_boot()
Wait for device to be booted.
Definition: lely_driver_bridge.hpp:673
LelyDriverBridge(ev_exec_t *exec, canopen::AsyncMaster &master, uint8_t id, std::string name, std::string eds, std::string bin, std::chrono::milliseconds timeout=20ms)
Construct a new Lely Bridge object.
Definition: lely_driver_bridge.hpp:404
std::atomic< bool > booted
Definition: lely_driver_bridge.hpp:308
std::shared_ptr< std::promise< COData > > sdo_read_data_promise
Definition: lely_driver_bridge.hpp:284
std::promise< COEmcy > emcy_promise
Definition: lely_driver_bridge.hpp:302
std::condition_variable boot_cond
Definition: lely_driver_bridge.hpp:312
void tpdo_transmit(COData data)
Executes a TPDO transmission.
char boot_status
Definition: lely_driver_bridge.hpp:309
std::shared_ptr< SafeQueue< COEmcy > > emcy_queue
Definition: lely_driver_bridge.hpp:305
void OnSync(uint8_t cnt, const time_point &t) noexcept override
Definition: lely_driver_bridge.hpp:332
std::mutex dictionary_mutex_
Definition: lely_driver_bridge.hpp:280
std::future< T > async_sdo_read_typed(uint16_t idx, uint8_t subidx)
Definition: lely_driver_bridge.hpp:528
canopen::NmtState boot_state
Definition: lely_driver_bridge.hpp:311
uint8_t nodeid
Definition: lely_driver_bridge.hpp:315
std::string name_
Definition: lely_driver_bridge.hpp:316
void OnState(canopen::NmtState state) noexcept override
OnState Callback.
Thread Safe Queue for CANOpen Data Exchange.
Definition: exchange.hpp:49
Definition: configuration_manager.hpp:28
std::map< uint16_t, std::map< uint8_t, pdo_mapping > > PDOMap
Definition: lely_driver_bridge.hpp:57
LelyBridgeErrc
Definition: lely_driver_bridge.hpp:229
@ InconsistentProgramDownload
@ ConfigurationDownloadFailed
@ StartErrorControlFailed
@ NmtSlaveInitiallyOperational
Definition: lely_driver_bridge.hpp:255
std::error_code make_error_code(ros2_canopen::LelyBridgeErrc)
Definition: exchange.hpp:26
uint32_t data_
Definition: exchange.hpp:30
uint8_t subindex_
Definition: exchange.hpp:29
uint16_t index_
Definition: exchange.hpp:28
Definition: exchange.hpp:34
Definition: lely_driver_bridge.hpp:248
const char * name() const noexcept override
Definition: lely_driver_bridge.hpp:52
bool is_rpdo
Definition: lely_driver_bridge.hpp:54
bool is_tpdo
Definition: lely_driver_bridge.hpp:53