From e6fcd6d55d878e86f8c22e6bc19dbe3f5c74029e Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu Date: Fri, 3 Nov 2023 10:48:43 +0100 Subject: [PATCH] timeout for booting slave --- canopen/sphinx/user-guide/configuration.rst | 1 + .../node_interfaces/node_canopen_master.hpp | 21 +++++++++++++++++-- .../node_canopen_basic_master_impl.hpp | 2 +- 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/canopen/sphinx/user-guide/configuration.rst b/canopen/sphinx/user-guide/configuration.rst index abdc3579..25136279 100644 --- 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 index 89794cc8..a308fee7 100644 --- 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 timeout; + try + { + timeout = this->config_["boot_timeout"].as(); + } + 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/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp index 3fd638dc..970bfedd 100644 --- a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp +++ b/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::activate(bool called_from_base) master_bridge_ = std::make_shared( *(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(master_bridge_); }