Merge pull request #261 from gsalinas/configurable-sdo-timeout

Configurable SDO timeout
ros-industrial · Apr 18, 2024 · 5dcebad · 5dcebad
2 parents 93ebb7d + 508f08f
commit 5dcebad
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 8 deletions.
diff --git a/canopen/sphinx/user-guide/configuration.rst b/canopen/sphinx/user-guide/configuration.rst
@@ -152,6 +152,7 @@ device.
   software_version;	The expected software version (default: 0x00000000, see object 1F55).
   configuration_file;	The name of the file containing the configuration (default: "<dcf_path>/<name>.bin" (where <name> is the section name), see object 1F22).
   restore_configuration;	The sub-index of object 1011 to be used when restoring the configuration (default: 0x00).
+  sdo_timeout_ms; The timeout to use for SDO reads/writes to this device. (default: 20ms) 
   sdo;	Additional SDO requests to be sent during configuration (see below).
 
 

diff --git a/canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp b/canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp
@@ -314,6 +314,8 @@ class LelyDriverBridge : public canopen::FiberDriver
 
   uint8_t nodeid;
   std::string name_;
+
+  std::chrono::milliseconds sdo_timeout;
 
   std::function<void()> on_sync_function_;
 
@@ -396,11 +398,12 @@ class LelyDriverBridge : public canopen::FiberDriver
    * @param [in] id       NodeId to connect to
    * @param [in] eds      EDS file
    * @param [in] bin      BIN file (concise dcf)
+   * @param [in] timeout  Timeout in milliseconds for SDO reads/writes
    *
    */
   LelyDriverBridge(
     ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t id, std::string name, std::string eds,
-    std::string bin)
+    std::string bin, std::chrono::milliseconds timeout = 20ms)
   : FiberDriver(exec, master, id),
     rpdo_queue(new SafeQueue<COData>()),
     emcy_queue(new SafeQueue<COEmcy>())
@@ -417,6 +420,7 @@ class LelyDriverBridge : public canopen::FiberDriver
       dictionary_->readDCF(a, b, bin.c_str());
     }
     pdo_map_ = dictionary_->createPDOMapping();
+    sdo_timeout = timeout;
   }
 
   /**
@@ -476,7 +480,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
     return prom->get_future();
   }
 
@@ -568,7 +572,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
     return prom->get_future();
   }
 
@@ -736,7 +740,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
   }
 
   template <typename T>
@@ -763,7 +767,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
   }
 
   template <typename T>
@@ -782,7 +786,7 @@ class LelyDriverBridge : public canopen::FiberDriver
     }
     if (!is_tpdo)
     {
-      if (sync_sdo_read_typed<T>(index, subindex, value, std::chrono::milliseconds(20)))
+      if (sync_sdo_read_typed<T>(index, subindex, value, this->sdo_timeout))
       {
         return value;
       }
@@ -839,7 +843,7 @@ class LelyDriverBridge : public canopen::FiberDriver
     }
     else
     {
-      sync_sdo_write_typed(index, subindex, value, std::chrono::milliseconds(20));
+      sync_sdo_write_typed(index, subindex, value, this->sdo_timeout);
     }
   }
 };

diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp
@@ -44,6 +44,7 @@ class NodeCanopenBaseDriver : public NodeCanopenDriver<NODETYPE>
   std::mutex driver_mutex_;
   std::shared_ptr<ros2_canopen::LelyDriverBridge> lely_driver_;
   uint32_t period_ms_;
+  int sdo_timeout_ms_;
   bool polling_;
 
   // nmt state callback

diff --git a/...base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/...base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp
@@ -98,6 +98,16 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
     diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
     diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
   }
+
+  std::optional<int> sdo_timeout_ms;
+  try
+  {
+    sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as<int>());
+  }
+  catch(...)
+  {
+  }
+  sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
 }
 template <>
 void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
@@ -165,6 +175,16 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
     diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
     diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
   }
+
+  std::optional<int> sdo_timeout_ms;
+  try
+  {
+    sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as<int>());
+  }
+  catch(...)
+  {
+  }
+  sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
 }
 
 template <class NODETYPE>
@@ -235,7 +255,7 @@ void NodeCanopenBaseDriver<NODETYPE>::add_to_master()
       std::scoped_lock<std::mutex> lock(this->driver_mutex_);
       this->lely_driver_ = std::make_shared<ros2_canopen::LelyDriverBridge>(
         *(this->exec_), *(this->master_), this->node_id_, this->node_->get_name(), this->eds_,
-        this->bin_);
+        this->bin_, std::chrono::milliseconds(this->sdo_timeout_ms_));
       this->driver_ = std::static_pointer_cast<lely::canopen::BasicDriver>(this->lely_driver_);
       prom->set_value(lely_driver_);
     });