From 37d3d226c7b94719e58c10e1120252ee03240f3b Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Sun, 9 Jul 2023 18:53:24 +0200 Subject: [PATCH 01/18] WIP: Extend the rpdo to have a queue (FIFO) to save the values. The read function take the latest value out of the queue and assign to the system interface. Need tests. --- .../canopen_ros2_control/canopen_system.hpp | 28 +++++++++++++++++++ canopen_ros2_control/src/canopen_system.cpp | 14 ++++++++-- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index f2aa4fab..446b95e3 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -26,6 +26,7 @@ #include #include +#include #include "canopen_core/device_container.hpp" #include "canopen_proxy_driver/proxy_driver.hpp" @@ -140,6 +141,33 @@ struct CanopenNodeData WORos2ControlCoData rsdo; // write-only WORos2ControlCoData wsdo; // write-only + + // Define a FIFO queue for read-only data + std::queue rpdo_data_queue; + + // Push data to the queue - FIFO + void set_rpdo_data(ros2_canopen::COData d) + { + RORos2ControlCOData rpdo_data_tmp; + rpdo_data_tmp.set_data(d); + rpdo_data_queue.push(rpdo_data_tmp); + } + + // Clear the queue + void clear_rpdo_data() + { + std::queue empty; + std::swap(rpdo_data_queue, empty); + } + + // Pop data from the queue + RORos2ControlCOData get_rpdo_data() + { + RORos2ControlCOData rpdo_data_tmp; + rpdo_data_tmp = rpdo_data_queue.front(); + rpdo_data_queue.pop(); + return rpdo_data_tmp; + } }; class CanopenSystem : public hardware_interface::SystemInterface diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index 268d709c..65ecd7c7 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -140,8 +140,9 @@ void CanopenSystem::initDeviceContainer() // register callback proxy_driver->register_nmt_state_cb(nmt_state_cb); + // The id here refer to the node id auto rpdo_cb = [&](ros2_canopen::COData data, uint8_t id) - { canopen_data_[id].rpdo_data.set_data(data); }; + { canopen_data_[id].set_rpdo_data(data); }; // register callback proxy_driver->register_rpdo_cb(rpdo_cb); @@ -246,8 +247,17 @@ hardware_interface::return_type CanopenSystem::read( // nmt state is set via Ros2ControlNmtState::set_state within nmt_state_cb - // rpdo is set via RORos2ControlCOData::set_data within rpdo_cb + // rpdo has a queue of messages, we read the latest one + for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) + { + const auto rpdo_data = canopen_data_[node_id].get_latest_rpdo_data(); + // Assign the latest rpdo data to the state interface + canopen_data_[node_id].rpdo_data.index = rpdo_data.index; + canopen_data_[node_id].rpdo_data.subindex = rpdo_data.subindex; + canopen_data_[node_id].rpdo_data.data = rpdo_data.data; + } + return hardware_interface::return_type::OK; } From 399161cb6a535183843b411f8ae4e7c031ec227e Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Tue, 11 Jul 2023 10:15:25 +0200 Subject: [PATCH 02/18] Fix bug in state interface indexing.. --- canopen_ros2_control/src/canopen_system.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index 65ecd7c7..c8787b96 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -250,12 +250,13 @@ hardware_interface::return_type CanopenSystem::read( // rpdo has a queue of messages, we read the latest one for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) { - const auto rpdo_data = canopen_data_[node_id].get_latest_rpdo_data(); + const auto rpdo_data = it->second.get_latest_rpdo_data(); + // Assign the latest rpdo data to the state interface - canopen_data_[node_id].rpdo_data.index = rpdo_data.index; - canopen_data_[node_id].rpdo_data.subindex = rpdo_data.subindex; - canopen_data_[node_id].rpdo_data.data = rpdo_data.data; + it->second.rpdo_data.index = rpdo_data.index; + it->second.rpdo_data.subindex = rpdo_data.subindex; + it->second.rpdo_data.data = rpdo_data.data; } return hardware_interface::return_type::OK; From ef0ac0c3c12ec084c0969397f7f3b7aa1c7809e0 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Tue, 11 Jul 2023 10:29:16 +0200 Subject: [PATCH 03/18] Use proper function to get rpdo data --- canopen_ros2_control/src/canopen_system.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index c8787b96..8c2cdbfc 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -250,8 +250,7 @@ hardware_interface::return_type CanopenSystem::read( // rpdo has a queue of messages, we read the latest one for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) { - - const auto rpdo_data = it->second.get_latest_rpdo_data(); + const auto rpdo_data = it->second.get_rpdo_data(); // Assign the latest rpdo data to the state interface it->second.rpdo_data.index = rpdo_data.index; From 817945414a094886b69d65bbdaf5810af0391a8e Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Sun, 16 Jul 2023 15:29:11 +0200 Subject: [PATCH 04/18] Extend fake_slaves to publish messages via rpdo --- canopen_fake_slaves/config/simple_slave.eds | 6 +++++ .../canopen_fake_slaves/basic_slave.hpp | 23 ++++++++++++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/canopen_fake_slaves/config/simple_slave.eds b/canopen_fake_slaves/config/simple_slave.eds index 77126767..743f242e 100644 --- a/canopen_fake_slaves/config/simple_slave.eds +++ b/canopen_fake_slaves/config/simple_slave.eds @@ -268,3 +268,9 @@ ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr PDOMapping=1 + +[4002] +ParameterName=UNSIGNED32 sent from slave in fixed period +DataType=0x0007 +AccessType=rwr +PDOMapping=1 \ No newline at end of file diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index 62a6dc97..7fff4ee2 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -36,11 +36,19 @@ class SimpleSlave : public canopen::BasicSlave { public: using BasicSlave::BasicSlave; + explicit SimpleSlave( + io::TimerBase & timer, io::CanChannelBase & chan, const ::std::string & dcf_txt, + const ::std::string & dcf_bin = "", uint8_t id = 0xff) + : canopen::BasicSlave(timer, chan, dcf_txt, dcf_bin, id) { + + // Keep sending via TPDO, frequency is 25 Hz + std::thread(std::bind(&SimpleSlave::periodic_messages, this)); + } protected: /** * @brief This function is called when a value is written to the local object dictionary by an SDO - * or RPDO. Also copies the RPDO value to TPDO. + * or RPDO. Also copies the RPDO value to TPDO. A function from the class Device * @param idx The index of the PDO. * @param subidx The subindex of the PDO. */ @@ -50,6 +58,19 @@ class SimpleSlave : public canopen::BasicSlave (*this)[0x4001][0] = val; this->TpdoEvent(0); } + + /** + * @brief This function triggers the sending of a PDO. Frequency is 25 Hz. + */ + void periodic_messages() + { + uint32_t val = 0; + (*this)[0x4002][0] = val; + this->TpdoEvent(0); + + // 40 ms sleep - 25 Hz + std::this_thread::sleep_for(std::chrono::milliseconds(40)); + } }; class BasicSlave : public BaseSlave From 25c3d9e7a74d1f33e6f53c2c37fe0aaa32c97bd1 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Sun, 16 Jul 2023 17:13:36 +0200 Subject: [PATCH 05/18] Put the periodic messages in OnWrite --- .../canopen_fake_slaves/basic_slave.hpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index 7fff4ee2..6a6badd1 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -36,16 +36,17 @@ class SimpleSlave : public canopen::BasicSlave { public: using BasicSlave::BasicSlave; - explicit SimpleSlave( - io::TimerBase & timer, io::CanChannelBase & chan, const ::std::string & dcf_txt, - const ::std::string & dcf_bin = "", uint8_t id = 0xff) - : canopen::BasicSlave(timer, chan, dcf_txt, dcf_bin, id) { - - // Keep sending via TPDO, frequency is 25 Hz - std::thread(std::bind(&SimpleSlave::periodic_messages, this)); + ~SimpleSlave() + { + if (message_thread.joinable()) + { + message_thread.join(); + } } protected: + std::thread message_thread; + int write_count = 0; /** * @brief This function is called when a value is written to the local object dictionary by an SDO * or RPDO. Also copies the RPDO value to TPDO. A function from the class Device @@ -57,19 +58,21 @@ class SimpleSlave : public canopen::BasicSlave uint32_t val = (*this)[idx][subidx]; (*this)[0x4001][0] = val; this->TpdoEvent(0); + write_count++; + message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); } - /** - * @brief This function triggers the sending of a PDO. Frequency is 25 Hz. - */ - void periodic_messages() + void fake_periodic_messages() { - uint32_t val = 0; - (*this)[0x4002][0] = val; - this->TpdoEvent(0); - - // 40 ms sleep - 25 Hz - std::this_thread::sleep_for(std::chrono::milliseconds(40)); + // If ros is running, send messages + while (rclcpp::ok()) + { + uint32_t val = 0; + (*this)[0x4002][0] = val; + this->TpdoEvent(0); + // 40 ms sleep - 25 Hz + std::this_thread::sleep_for(std::chrono::milliseconds(40)); + } } }; From 56be2a23335ff83ef1a823d9bcc63c264915a71e Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Sun, 16 Jul 2023 17:33:34 +0200 Subject: [PATCH 06/18] Fix the bug that the rpdo queue keeps poping although it is empty. --- .../include/canopen_ros2_control/canopen_system.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index 446b95e3..95dea9ac 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -163,6 +163,12 @@ struct CanopenNodeData // Pop data from the queue RORos2ControlCOData get_rpdo_data() { + // If there is nothing new in the queue, return the old data + if (rpdo_data_queue.empty()) + { + return rpdo_data; + } + RORos2ControlCOData rpdo_data_tmp; rpdo_data_tmp = rpdo_data_queue.front(); rpdo_data_queue.pop(); From 6cf29b7ba455789234620a7a8b86618d9d0400d5 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Sun, 16 Jul 2023 17:40:33 +0200 Subject: [PATCH 07/18] Working logic. Still have to work on the edf file. --- .../include/canopen_fake_slaves/basic_slave.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index 6a6badd1..df3cd697 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -67,9 +67,9 @@ class SimpleSlave : public canopen::BasicSlave // If ros is running, send messages while (rclcpp::ok()) { - uint32_t val = 0; + uint32_t val = 1; (*this)[0x4002][0] = val; - this->TpdoEvent(0); + this->TpdoEvent(1); // 40 ms sleep - 25 Hz std::this_thread::sleep_for(std::chrono::milliseconds(40)); } From 65d9530a680158d51c66389d21fa710b15b40c80 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Tue, 18 Jul 2023 09:58:50 +0200 Subject: [PATCH 08/18] Periodic messages sent, but not received properly. --- canopen_fake_slaves/config/simple_slave.eds | 6 ----- .../canopen_fake_slaves/basic_slave.hpp | 13 +++++------ .../canopen_ros2_control/canopen_system.hpp | 1 - .../config/canopen_system/simple.eds | 23 +++++++++++++++++-- 4 files changed, 27 insertions(+), 16 deletions(-) diff --git a/canopen_fake_slaves/config/simple_slave.eds b/canopen_fake_slaves/config/simple_slave.eds index 743f242e..80c20bac 100644 --- a/canopen_fake_slaves/config/simple_slave.eds +++ b/canopen_fake_slaves/config/simple_slave.eds @@ -267,10 +267,4 @@ PDOMapping=1 ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr -PDOMapping=1 - -[4002] -ParameterName=UNSIGNED32 sent from slave in fixed period -DataType=0x0007 -AccessType=rwr PDOMapping=1 \ No newline at end of file diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index df3cd697..eedee63e 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -46,7 +46,6 @@ class SimpleSlave : public canopen::BasicSlave protected: std::thread message_thread; - int write_count = 0; /** * @brief This function is called when a value is written to the local object dictionary by an SDO * or RPDO. Also copies the RPDO value to TPDO. A function from the class Device @@ -55,10 +54,10 @@ class SimpleSlave : public canopen::BasicSlave */ void OnWrite(uint16_t idx, uint8_t subidx) noexcept override { - uint32_t val = (*this)[idx][subidx]; - (*this)[0x4001][0] = val; - this->TpdoEvent(0); - write_count++; + // uint32_t val = (*this)[idx][subidx]; + // (*this)[0x4001][0] = val; + // this->TpdoEvent(0); + // Publish periodic message message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); } @@ -67,8 +66,8 @@ class SimpleSlave : public canopen::BasicSlave // If ros is running, send messages while (rclcpp::ok()) { - uint32_t val = 1; - (*this)[0x4002][0] = val; + uint32_t val = 0x1122; + (*this)[0x4004][0] = val; this->TpdoEvent(1); // 40 ms sleep - 25 Hz std::this_thread::sleep_for(std::chrono::milliseconds(40)); diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index 95dea9ac..393b10a7 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -168,7 +168,6 @@ struct CanopenNodeData { return rpdo_data; } - RORos2ControlCOData rpdo_data_tmp; rpdo_data_tmp = rpdo_data_queue.front(); rpdo_data_queue.pop(); diff --git a/canopen_tests/config/canopen_system/simple.eds b/canopen_tests/config/canopen_system/simple.eds index 68d9bb2a..931bbe60 100644 --- a/canopen_tests/config/canopen_system/simple.eds +++ b/canopen_tests/config/canopen_system/simple.eds @@ -49,7 +49,7 @@ SupportedObjects=3 3=0x1018 [OptionalObjects] -SupportedObjects=11 +SupportedObjects=12 1=0x1003 2=0x1005 3=0x1014 @@ -61,13 +61,15 @@ SupportedObjects=11 9=0x1600 10=0x1800 11=0x1A00 +12=0x2A00 [ManufacturerObjects] -SupportedObjects=4 +SupportedObjects=5 1=0x4000 2=0x4001 3=0x4002 4=0x4003 +5=0x4004 [1000] ParameterName=Device type @@ -259,6 +261,17 @@ CompactSubObj=1 NrOfEntries=1 1=0x40010020 +[2A00] +ParameterName=TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[2A00Value] +NrOfEntries=1 +1=0x40040020 + [4000] ParameterName=UNSIGNED32 received by slave DataType=0x0007 @@ -280,3 +293,9 @@ AccessType=rw ParameterName=INTEGER16 test DataType=0x0003 AccessType=rw + +[4004] +ParameterName=UNSIGNED32 sent from slave for fixed period +DataType=0x0007 +AccessType=rwr +PDOMapping=1 \ No newline at end of file From 1dedd9ed04a163876f9c0c5ef19d65031ec5f03a Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Tue, 18 Jul 2023 21:52:31 +0200 Subject: [PATCH 09/18] Working version. --- .../canopen_fake_slaves/basic_slave.hpp | 10 ++-- .../config/canopen_system/simple.eds | 60 ++++++++++++++++--- 2 files changed, 57 insertions(+), 13 deletions(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index eedee63e..ccf73fb5 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -54,9 +54,9 @@ class SimpleSlave : public canopen::BasicSlave */ void OnWrite(uint16_t idx, uint8_t subidx) noexcept override { - // uint32_t val = (*this)[idx][subidx]; - // (*this)[0x4001][0] = val; - // this->TpdoEvent(0); + uint32_t val = (*this)[idx][subidx]; + (*this)[0x4001][0] = val; + this->TpdoEvent(0); // Publish periodic message message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); } @@ -68,9 +68,9 @@ class SimpleSlave : public canopen::BasicSlave { uint32_t val = 0x1122; (*this)[0x4004][0] = val; - this->TpdoEvent(1); + this->TpdoEvent(0); // 40 ms sleep - 25 Hz - std::this_thread::sleep_for(std::chrono::milliseconds(40)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } }; diff --git a/canopen_tests/config/canopen_system/simple.eds b/canopen_tests/config/canopen_system/simple.eds index 931bbe60..ba139683 100644 --- a/canopen_tests/config/canopen_system/simple.eds +++ b/canopen_tests/config/canopen_system/simple.eds @@ -19,7 +19,7 @@ Granularity=1 DynamicChannelsSupported=0 GroupMessaging=0 NrOfRxPDO=1 -NrOfTxPDO=1 +NrOfTxPDO=2 LSS_Supported=1 [DummyUsage] @@ -49,7 +49,7 @@ SupportedObjects=3 3=0x1018 [OptionalObjects] -SupportedObjects=12 +SupportedObjects=13 1=0x1003 2=0x1005 3=0x1014 @@ -60,8 +60,9 @@ SupportedObjects=12 8=0x1400 9=0x1600 10=0x1800 -11=0x1A00 -12=0x2A00 +11=0x1801 +12=0x1A00 +13=0x1A01 [ManufacturerObjects] SupportedObjects=5 @@ -250,6 +251,49 @@ ParameterName=SYNC start value DataType=0x0005 AccessType=rw +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO 2 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x280 + +[1801sub2] +ParameterName=transmission type 2 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time 2 +DataType=0x0006 +AccessType=rw + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer 2 +DataType=0x0006 +AccessType=rw + +[1801sub6] +ParameterName=SYNC start value 2 +DataType=0x0005 +AccessType=rw + [1A00] ParameterName=TPDO mapping parameter ObjectType=0x08 @@ -261,14 +305,14 @@ CompactSubObj=1 NrOfEntries=1 1=0x40010020 -[2A00] -ParameterName=TPDO mapping parameter +[1A01] +ParameterName=TPDO mapping parameter 2 ObjectType=0x08 DataType=0x0007 AccessType=rw CompactSubObj=1 -[2A00Value] +[1A01Value] NrOfEntries=1 1=0x40040020 @@ -295,7 +339,7 @@ DataType=0x0003 AccessType=rw [4004] -ParameterName=UNSIGNED32 sent from slave for fixed period +ParameterName=UNSIGNED32 sent from slave 2 DataType=0x0007 AccessType=rwr PDOMapping=1 \ No newline at end of file From e10338af42adf9a84121d191b00b4cdf040b5e66 Mon Sep 17 00:00:00 2001 From: Xi-Huang Date: Sun, 23 Jul 2023 18:52:37 +0200 Subject: [PATCH 10/18] Apply suggestions from code review Co-authored-by: Dr. Denis --- canopen_fake_slaves/config/simple_slave.eds | 2 +- .../include/canopen_fake_slaves/basic_slave.hpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/canopen_fake_slaves/config/simple_slave.eds b/canopen_fake_slaves/config/simple_slave.eds index 80c20bac..77126767 100644 --- a/canopen_fake_slaves/config/simple_slave.eds +++ b/canopen_fake_slaves/config/simple_slave.eds @@ -267,4 +267,4 @@ PDOMapping=1 ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr -PDOMapping=1 \ No newline at end of file +PDOMapping=1 diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index ccf73fb5..da7f437c 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -45,7 +45,7 @@ class SimpleSlave : public canopen::BasicSlave } protected: - std::thread message_thread; + std::thread message_thread = nullptr; /** * @brief This function is called when a value is written to the local object dictionary by an SDO * or RPDO. Also copies the RPDO value to TPDO. A function from the class Device @@ -58,7 +58,8 @@ class SimpleSlave : public canopen::BasicSlave (*this)[0x4001][0] = val; this->TpdoEvent(0); // Publish periodic message - message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); + if not message_thread: + message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); } void fake_periodic_messages() From e4c7fd240ce3e69e936b3ba6371fd5b9c6e3dce1 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Tue, 8 Aug 2023 21:52:01 +0200 Subject: [PATCH 11/18] Fix the thread issue. --- .../include/canopen_fake_slaves/basic_slave.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index da7f437c..9df82e28 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -45,7 +45,7 @@ class SimpleSlave : public canopen::BasicSlave } protected: - std::thread message_thread = nullptr; + std::thread message_thread; /** * @brief This function is called when a value is written to the local object dictionary by an SDO * or RPDO. Also copies the RPDO value to TPDO. A function from the class Device @@ -58,8 +58,9 @@ class SimpleSlave : public canopen::BasicSlave (*this)[0x4001][0] = val; this->TpdoEvent(0); // Publish periodic message - if not message_thread: - message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); + if (!message_thread.joinable()) { + message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); + } } void fake_periodic_messages() From e13ebd348b920fe78a5f355fd385eedaeb257723 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Tue, 8 Aug 2023 21:54:32 +0200 Subject: [PATCH 12/18] Add comments for the fake slave function --- .../include/canopen_fake_slaves/basic_slave.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index 9df82e28..eed981cb 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -63,6 +63,10 @@ class SimpleSlave : public canopen::BasicSlave } } + /** + * @brief This function is attached to a thread and sends periodic messages + * via 0x4004 + */ void fake_periodic_messages() { // If ros is running, send messages @@ -71,7 +75,7 @@ class SimpleSlave : public canopen::BasicSlave uint32_t val = 0x1122; (*this)[0x4004][0] = val; this->TpdoEvent(0); - // 40 ms sleep - 25 Hz + // 100 ms sleep - 10 Hz std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } From 3180b78ad9f5249a28a799c24d8e9c42e1f8c99d Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Wed, 9 Aug 2023 13:03:33 +0200 Subject: [PATCH 13/18] Add new line to get checking pipeline okay --- canopen_tests/config/canopen_system/simple.eds | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/canopen_tests/config/canopen_system/simple.eds b/canopen_tests/config/canopen_system/simple.eds index ba139683..0f3a3673 100644 --- a/canopen_tests/config/canopen_system/simple.eds +++ b/canopen_tests/config/canopen_system/simple.eds @@ -342,4 +342,4 @@ AccessType=rw ParameterName=UNSIGNED32 sent from slave 2 DataType=0x0007 AccessType=rwr -PDOMapping=1 \ No newline at end of file +PDOMapping=1 From 6d34880fea48c0a880944341b2a2b80fe34848fe Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Wed, 9 Aug 2023 13:50:44 +0200 Subject: [PATCH 14/18] Fix pre-commit --- .../include/canopen_ros2_control/canopen_system.hpp | 2 +- canopen_ros2_control/src/canopen_system.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index 393b10a7..d02d5b70 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -142,7 +142,7 @@ struct CanopenNodeData WORos2ControlCoData rsdo; // write-only WORos2ControlCoData wsdo; // write-only - // Define a FIFO queue for read-only data + // Define a FIFO queue for read-only data std::queue rpdo_data_queue; // Push data to the queue - FIFO diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index 8c2cdbfc..afb5a557 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -140,7 +140,7 @@ void CanopenSystem::initDeviceContainer() // register callback proxy_driver->register_nmt_state_cb(nmt_state_cb); - // The id here refer to the node id + // The id here refer to the node id auto rpdo_cb = [&](ros2_canopen::COData data, uint8_t id) { canopen_data_[id].set_rpdo_data(data); }; // register callback @@ -251,13 +251,13 @@ hardware_interface::return_type CanopenSystem::read( for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) { const auto rpdo_data = it->second.get_rpdo_data(); - + // Assign the latest rpdo data to the state interface it->second.rpdo_data.index = rpdo_data.index; it->second.rpdo_data.subindex = rpdo_data.subindex; it->second.rpdo_data.data = rpdo_data.data; } - + return hardware_interface::return_type::OK; } From d8d18ebe674393692c6a2bc0bc93b3a1c310ca5f Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Wed, 9 Aug 2023 14:19:58 +0200 Subject: [PATCH 15/18] Fix clang format --- .../include/canopen_fake_slaves/basic_slave.hpp | 4 +++- .../include/canopen_ros2_control/canopen_system.hpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index eed981cb..f6a857dc 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -57,8 +57,10 @@ class SimpleSlave : public canopen::BasicSlave uint32_t val = (*this)[idx][subidx]; (*this)[0x4001][0] = val; this->TpdoEvent(0); + // Publish periodic message - if (!message_thread.joinable()) { + if (!message_thread.joinable()) + { message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this)); } } diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index d02d5b70..13052581 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -24,9 +24,9 @@ #ifndef CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ #define CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ +#include #include #include -#include #include "canopen_core/device_container.hpp" #include "canopen_proxy_driver/proxy_driver.hpp" From 57979a302f36cc4f3e3cfca842b80bae33e0b555 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Wed, 16 Aug 2023 21:23:43 +0200 Subject: [PATCH 16/18] Add pdo mapping support --- .../canopen_ros2_control/canopen_system.hpp | 59 +++++++++++++------ canopen_ros2_control/src/canopen_system.cpp | 9 --- 2 files changed, 40 insertions(+), 28 deletions(-) diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index 13052581..570befc9 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -133,6 +133,23 @@ struct Ros2ControlNmtState double start_fbk; }; +struct pair_hash { + template + std::size_t operator () (const std::pair& pair) const { + auto h1 = std::hash{}(pair.first); + auto h2 = std::hash{}(pair.second); + + // // check if T2 is a pair, if so recursively compute hash + // if constexpr (std::is_same>::value) { + // h2 = pair_hash{}(pair.second); + // } else { + // h2 = std::hash{}(pair.second); + // } + + return h1 ^ h2; + } +}; + struct CanopenNodeData { Ros2ControlNmtState nmt_state; // read-write @@ -142,36 +159,40 @@ struct CanopenNodeData WORos2ControlCoData rsdo; // write-only WORos2ControlCoData wsdo; // write-only - // Define a FIFO queue for read-only data - std::queue rpdo_data_queue; + using PDO_INDICES = std::pair; // Index, Subindex + std::unordered_map rpdo_data_map; // Push data to the queue - FIFO void set_rpdo_data(ros2_canopen::COData d) { - RORos2ControlCOData rpdo_data_tmp; - rpdo_data_tmp.set_data(d); - rpdo_data_queue.push(rpdo_data_tmp); - } + rpdo_data.set_data(d); - // Clear the queue - void clear_rpdo_data() - { - std::queue empty; - std::swap(rpdo_data_queue, empty); + PDO_INDICES index_pair(d.index_, d.subindex_); + + // check if the index pair is already in the map + if (rpdo_data_map.find(index_pair) != rpdo_data_map.end()) { + // if it is, update the value + rpdo_data_map[index_pair] = rpdo_data.data; + } else { + // if it is not, add it to the map + rpdo_data_map.emplace(index_pair, rpdo_data.data); + } } // Pop data from the queue - RORos2ControlCOData get_rpdo_data() + double get_rpdo_data(uint16_t index, uint8_t subindex) { - // If there is nothing new in the queue, return the old data - if (rpdo_data_queue.empty()) + PDO_INDICES index_pair(index, subindex); + if (rpdo_data_map.find(index_pair) != rpdo_data_map.end()) + { + return rpdo_data_map[index_pair]; + } + else { - return rpdo_data; + // // Log error + // RCLCPP_WARN(kLogger, "The index pair (%u, %u) is not in the map", index, subindex); + return 0; } - RORos2ControlCOData rpdo_data_tmp; - rpdo_data_tmp = rpdo_data_queue.front(); - rpdo_data_queue.pop(); - return rpdo_data_tmp; } }; diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index afb5a557..7ea588b1 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -248,15 +248,6 @@ hardware_interface::return_type CanopenSystem::read( // nmt state is set via Ros2ControlNmtState::set_state within nmt_state_cb // rpdo has a queue of messages, we read the latest one - for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) - { - const auto rpdo_data = it->second.get_rpdo_data(); - - // Assign the latest rpdo data to the state interface - it->second.rpdo_data.index = rpdo_data.index; - it->second.rpdo_data.subindex = rpdo_data.subindex; - it->second.rpdo_data.data = rpdo_data.data; - } return hardware_interface::return_type::OK; } From 457b4acef6c0d333b4d55e586d0e5e09efe48093 Mon Sep 17 00:00:00 2001 From: Xi-Huang Date: Wed, 17 Jan 2024 18:03:51 +0100 Subject: [PATCH 17/18] Update canopen_system.hpp --- .../include/canopen_ros2_control/canopen_system.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index 570befc9..446fe75e 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -139,13 +139,6 @@ struct pair_hash { auto h1 = std::hash{}(pair.first); auto h2 = std::hash{}(pair.second); - // // check if T2 is a pair, if so recursively compute hash - // if constexpr (std::is_same>::value) { - // h2 = pair_hash{}(pair.second); - // } else { - // h2 = std::hash{}(pair.second); - // } - return h1 ^ h2; } }; From 90508e141e25e13046b9d8128219e79093970566 Mon Sep 17 00:00:00 2001 From: Xi Huang Date: Fri, 19 Jan 2024 17:57:45 +0100 Subject: [PATCH 18/18] Fix clang format --- .../canopen_ros2_control/canopen_system.hpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp index 446fe75e..b9d81fe1 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -133,9 +133,11 @@ struct Ros2ControlNmtState double start_fbk; }; -struct pair_hash { +struct pair_hash +{ template - std::size_t operator () (const std::pair& pair) const { + std::size_t operator()(const std::pair & pair) const + { auto h1 = std::hash{}(pair.first); auto h2 = std::hash{}(pair.second); @@ -152,7 +154,7 @@ struct CanopenNodeData WORos2ControlCoData rsdo; // write-only WORos2ControlCoData wsdo; // write-only - using PDO_INDICES = std::pair; // Index, Subindex + using PDO_INDICES = std::pair; // Index, Subindex std::unordered_map rpdo_data_map; // Push data to the queue - FIFO @@ -163,10 +165,13 @@ struct CanopenNodeData PDO_INDICES index_pair(d.index_, d.subindex_); // check if the index pair is already in the map - if (rpdo_data_map.find(index_pair) != rpdo_data_map.end()) { + if (rpdo_data_map.find(index_pair) != rpdo_data_map.end()) + { // if it is, update the value rpdo_data_map[index_pair] = rpdo_data.data; - } else { + } + else + { // if it is not, add it to the map rpdo_data_map.emplace(index_pair, rpdo_data.data); }