Merge pull request #178 from StoglRobotics-forks/extend_fake_slaves

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

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,29 @@ 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())
+    {
+      message_thread = std::thread(std::bind(&SimpleSlave::fake_periodic_messages, this));
+    }
+  }
+
+  /**
+   * @brief This function is attached to a thread and sends periodic messages
+   * via 0x4004
+   */
+  void fake_periodic_messages()
+  {
+    // If ros is running, send messages
+    while (rclcpp::ok())
+    {
+      uint32_t val = 0x1122;
+      (*this)[0x4004][0] = val;
+      this->TpdoEvent(0);
+      // 100 ms sleep - 10 Hz
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
   }
 };
 

canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp

@@ -24,6 +24,7 @@
 #ifndef CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_
 #define CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_
 
+#include <queue>
 #include <string>
 #include <vector>
 
@@ -132,6 +133,18 @@ struct Ros2ControlNmtState
   double start_fbk;
 };
 
+struct pair_hash
+{
+  template <class T1, class T2>
+  std::size_t operator()(const std::pair<T1, T2> & pair) const
+  {
+    auto h1 = std::hash<T1>{}(pair.first);
+    auto h2 = std::hash<T2>{}(pair.second);
+
+    return h1 ^ h2;
+  }
+};
+
 struct CanopenNodeData
 {
   Ros2ControlNmtState nmt_state;  // read-write
@@ -140,6 +153,45 @@ struct CanopenNodeData
 
   WORos2ControlCoData rsdo;  // write-only
   WORos2ControlCoData wsdo;  // write-only
+
+  using PDO_INDICES = std::pair<uint16_t, uint8_t>;  // Index, Subindex
+  std::unordered_map<PDO_INDICES, double, pair_hash> rpdo_data_map;
+
+  // Push data to the queue - FIFO
+  void set_rpdo_data(ros2_canopen::COData d)
+  {
+    rpdo_data.set_data(d);
+
+    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
+  double get_rpdo_data(uint16_t index, uint8_t subindex)
+  {
+    PDO_INDICES index_pair(index, subindex);
+    if (rpdo_data_map.find(index_pair) != rpdo_data_map.end())
+    {
+      return rpdo_data_map[index_pair];
+    }
+    else
+    {
+      // // Log error
+      // RCLCPP_WARN(kLogger, "The index pair (%u, %u) is not in the map", index, subindex);
+      return 0;
+    }
+  }
 };
 
 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,7 +247,7 @@ 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
 
   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