timeout for booting slave

ros-industrial · Nov 3, 2023 · e6fcd6d · e6fcd6d
1 parent 90cd582
commit e6fcd6d
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 3 deletions.
diff --git a/canopen/sphinx/user-guide/configuration.rst b/canopen/sphinx/user-guide/configuration.rst
@@ -111,6 +111,7 @@ but come from the lely core library. Below you find a list of possible configura
   reset_all_nodes;	Specifies whether all slaves shall be reset in case of an error event on a mandatory slave (default: false, see bit 4 in object 1F80).
   stop_all_nodes;	Specifies whether all slaves shall be stopped in case of an error event on a mandatory slave (default: false, see bit 6 in object 1F80).
   boot_time;	The timeout for booting mandatory slaves in ms (default: 0, see object 1F89).
+  boot_timeout;	The timeout for booting all slaves in ms (default: 100ms).
 
 Device Section
 --------------

diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp
@@ -85,7 +85,8 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface
   std::string master_dcf_;
   std::string master_bin_;
   std::string can_interface_name_;
-
+  uint32_t timeout_;
+
   std::thread spinner_;
 
 public:
@@ -166,7 +167,23 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface
     this->configured_.store(true);
   }
 
-  virtual void configure(bool called_from_base) {}
+  virtual void configure(bool called_from_base) 
+  {
+    std::optional<int> timeout;
+    try
+    {
+      timeout = this->config_["boot_timeout"].as<int>();
+    }
+    catch(...)
+    {
+      RCLCPP_WARN(
+        this->node_->get_logger(),
+        "No timeout parameter found in config file. Using default value of 100ms.");
+    }
+    this->timeout_ = timeout.value_or(100);
+    RCLCPP_INFO_STREAM(
+      this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms.");
+  }
 
   /**
    * @brief Activate the driver

diff --git a/...r_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp b/...r_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp
@@ -16,7 +16,6 @@
 #ifndef NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_
 #define NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_
 
-#include "canopen_core/node_interfaces/node_canopen_master.hpp"
 #include "canopen_master_driver/lely_master_bridge.hpp"
 #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp"
 
@@ -28,6 +27,7 @@ void NodeCanopenBasicMaster<NODETYPE>::activate(bool called_from_base)
   master_bridge_ = std::make_shared<LelyMasterBridge>(
     *(this->exec_), *(this->timer_), *(this->chan_), this->master_dcf_, this->master_bin_,
     this->node_id_);
+  master_bridge_->SetTimeout(std::chrono::milliseconds(this->timeout_));
   this->master_ = std::static_pointer_cast<lely::canopen::AsyncMaster>(master_bridge_);
 }