[FakeSlave] Extend slaves to provide period data for properly testing PDOs

canopen_fake_slaves/config/simple_slave.eds

@@ -267,4 +267,4 @@ PDOMapping=1
 ParameterName=UNSIGNED32 sent from slave
 DataType=0x0007
 AccessType=rwr
-PDOMapping=1
+PDOMapping=1

canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp

@@ -36,11 +36,19 @@ class SimpleSlave : public canopen::BasicSlave
 {
 public:
   using BasicSlave::BasicSlave;
+  ~SimpleSlave()
+  {
+    if (message_thread.joinable())
+    {
+      message_thread.join();
+    }
+  }
 
 protected:
+  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.
+   * 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.
    */
@@ -49,6 +57,21 @@ class SimpleSlave : public canopen::BasicSlave
     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));
+  }
+
+  void fake_periodic_messages()
+  {
+    // If ros is running, send messages
+    while (rclcpp::ok())
+    {
+      uint32_t val = 0x1122;
+      (*this)[0x4004][0] = val;
+      this->TpdoEvent(0);
+      // 40 ms sleep - 25 Hz
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
   }
 };
 

canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp

@@ -26,6 +26,7 @@
 
 #include <string>
 #include <vector>
+#include <queue>
 
 #include "canopen_core/device_container.hpp"
 #include "canopen_proxy_driver/proxy_driver.hpp"
@@ -140,6 +141,38 @@ struct CanopenNodeData
 
   WORos2ControlCoData rsdo;  // write-only
   WORos2ControlCoData wsdo;  // write-only
+
+  // Define a FIFO queue for read-only data 
+  std::queue<RORos2ControlCOData> 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<RORos2ControlCOData> empty;
+    std::swap(rpdo_data_queue, empty);
+  }
+
+  // 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();
+    return rpdo_data_tmp;
+  }
 };
 
 class CanopenSystem : public hardware_interface::SystemInterface

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 = 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;
 }
 

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=11
+SupportedObjects=13
 1=0x1003
 2=0x1005
 3=0x1014
@@ -60,14 +60,17 @@ SupportedObjects=11
 8=0x1400
 9=0x1600
 10=0x1800
-11=0x1A00
+11=0x1801
+12=0x1A00
+13=0x1A01
 
 [ManufacturerObjects]
-SupportedObjects=4
+SupportedObjects=5
 1=0x4000
 2=0x4001
 3=0x4002
 4=0x4003
+5=0x4004
 
 [1000]
 ParameterName=Device type
@@ -248,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
@@ -259,6 +305,17 @@ CompactSubObj=1
 NrOfEntries=1
 1=0x40010020
 
+[1A01]
+ParameterName=TPDO mapping parameter 2
+ObjectType=0x08
+DataType=0x0007
+AccessType=rw
+CompactSubObj=1
+
+[1A01Value]
+NrOfEntries=1
+1=0x40040020
+
 [4000]
 ParameterName=UNSIGNED32 received by slave
 DataType=0x0007
@@ -280,3 +337,9 @@ AccessType=rw
 ParameterName=INTEGER16 test
 DataType=0x0003
 AccessType=rw
+
+[4004]
+ParameterName=UNSIGNED32 sent from slave 2
+DataType=0x0007
+AccessType=rwr
+PDOMapping=1