Merge pull request #220 from ipa-vsp/feature/timeout-config

Add timeouts

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: 2000ms).
 
 Device Section
 --------------

canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp

@@ -35,13 +35,25 @@ void NodeCanopenBaseDriver<NODETYPE>::init(bool called_from_base)
 template <>
 void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool called_from_base)
 {
+  try
+  {
+    this->non_transmit_timeout_ =
+      std::chrono::milliseconds(this->config_["non_transmit_timeout"].as<int>());
+  }
+  catch (...)
+  {
+  }
+  RCLCPP_INFO_STREAM(
+    this->node_->get_logger(),
+    "Non transmit timeout" << static_cast<int>(this->non_transmit_timeout_.count()) << "ms");
+
   try
   {
     polling_ = this->config_["polling"].as<bool>();
   }
   catch (...)
   {
-    RCLCPP_ERROR(this->node_->get_logger(), "Could not polling from config, setting to true.");
+    RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true.");
     polling_ = true;
   }
   if (polling_)
@@ -52,7 +64,7 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
     }
     catch (...)
     {
-      RCLCPP_ERROR(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
+      RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
       period_ms_ = 10;
     }
   }
@@ -64,7 +76,7 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
   }
   catch (...)
   {
-    RCLCPP_ERROR(
+    RCLCPP_WARN(
       this->node_->get_logger(),
       "Could not read enable diagnostics from config, setting to false.");
     diagnostic_enabled_ = false;
@@ -90,13 +102,25 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
 template <>
 void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
 {
+  try
+  {
+    this->non_transmit_timeout_ =
+      std::chrono::milliseconds(this->config_["non_transmit_timeout"].as<int>());
+  }
+  catch (...)
+  {
+  }
+  RCLCPP_INFO_STREAM(
+    this->node_->get_logger(),
+    "Non transmit timeout" << static_cast<int>(this->non_transmit_timeout_.count()) << "ms");
+
   try
   {
     polling_ = this->config_["polling"].as<bool>();
   }
   catch (...)
   {
-    RCLCPP_ERROR(this->node_->get_logger(), "Could not polling from config, setting to true.");
+    RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true.");
     polling_ = true;
   }
   if (polling_)
@@ -107,7 +131,7 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
     }
     catch (...)
     {
-      RCLCPP_ERROR(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
+      RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
       period_ms_ = 10;
     }
   }
@@ -119,7 +143,7 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
   }
   catch (...)
   {
-    RCLCPP_ERROR(
+    RCLCPP_WARN(
       this->node_->get_logger(),
       "Could not read enable diagnostics from config, setting to false.");
     diagnostic_enabled_ = false;
@@ -132,7 +156,7 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
     }
     catch (...)
     {
-      RCLCPP_ERROR(
+      RCLCPP_WARN(
         this->node_->get_logger(),
         "Could not read diagnostics period from config, setting to 1000ms");
       diagnostic_period_ms_ = 1000;

canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp

@@ -85,6 +85,7 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface
   std::string master_dcf_;
   std::string master_bin_;
   std::string can_interface_name_;
+  uint32_t timeout_;
 
   std::thread spinner_;
 
@@ -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(2000);
+    RCLCPP_INFO_STREAM(
+      this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms.");
+  }
 
   /**
    * @brief Activate the driver

canopen_master_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_);
 }