From e6fcd6d55d878e86f8c22e6bc19dbe3f5c74029e Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu Date: Fri, 3 Nov 2023 10:48:43 +0100 Subject: [PATCH 1/5] 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_); } From 3e53a2f6ef58afb24a28e80ce3dd2b2975771093 Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu Date: Fri, 3 Nov 2023 11:24:27 +0100 Subject: [PATCH 2/5] non transmit time from bus.yml --- .github/workflows/docker.yml | 2 +- .../node_interfaces/node_canopen_base_driver_impl.hpp | 11 +++++++++++ .../node_interfaces/node_canopen_master.hpp | 6 +++--- 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index a57be285..9dc58922 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -25,7 +25,7 @@ jobs: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} password: ${{ secrets.GITHUB_TOKEN }} - + - name: Extract metadata (tags, labels) for Docker id: meta uses: docker/metadata-action@v4 diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 26ebddab..5b73efed 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -35,6 +35,17 @@ void NodeCanopenBaseDriver::init(bool called_from_base) template <> void NodeCanopenBaseDriver::configure(bool called_from_base) { + try + { + this->non_transmit_timeout_ = + std::chrono::milliseconds(this->config_["non_transmit_timeout"].as()); + } + catch (...) + { + } + RCLCPP_INFO( + this->node_->get_logger(), "Non transmit timeout %d", this->non_transmit_timeout_.count()); + try { polling_ = this->config_["polling"].as(); 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 a308fee7..82d403b1 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 @@ -86,7 +86,7 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface std::string master_bin_; std::string can_interface_name_; uint32_t timeout_; - + std::thread spinner_; public: @@ -167,14 +167,14 @@ 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(...) + catch (...) { RCLCPP_WARN( this->node_->get_logger(), From 92ed7f12eb6c10376a06ad82b649d9946a19ed8c Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu Date: Fri, 3 Nov 2023 11:31:54 +0100 Subject: [PATCH 3/5] remove build warings --- .../node_canopen_base_driver_impl.hpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 5b73efed..74e2d03f 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -43,8 +43,9 @@ void NodeCanopenBaseDriver::configure(bool call catch (...) { } - RCLCPP_INFO( - this->node_->get_logger(), "Non transmit timeout %d", this->non_transmit_timeout_.count()); + RCLCPP_INFO_STREAM( + this->node_->get_logger(), + "Non transmit timeout" << static_cast(this->non_transmit_timeout_.count()) << "ms"); try { @@ -101,6 +102,18 @@ void NodeCanopenBaseDriver::configure(bool call template <> void NodeCanopenBaseDriver::configure(bool called_from_base) { + try + { + this->non_transmit_timeout_ = + std::chrono::milliseconds(this->config_["non_transmit_timeout"].as()); + } + catch (...) + { + } + RCLCPP_INFO_STREAM( + this->node_->get_logger(), + "Non transmit timeout" << static_cast(this->non_transmit_timeout_.count()) << "ms"); + try { polling_ = this->config_["polling"].as(); From 7cde31f30ecebee12017c556a8df76e7f712643c Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu Date: Fri, 3 Nov 2023 12:51:09 +0100 Subject: [PATCH 4/5] change error to warn for testing --- .../node_canopen_base_driver_impl.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 74e2d03f..d48e20dd 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -53,7 +53,7 @@ void NodeCanopenBaseDriver::configure(bool call } 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_) @@ -64,7 +64,7 @@ void NodeCanopenBaseDriver::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; } } @@ -76,7 +76,7 @@ void NodeCanopenBaseDriver::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; @@ -120,7 +120,7 @@ void NodeCanopenBaseDriver::configure(bool called_from_base) } 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_) @@ -131,7 +131,7 @@ void NodeCanopenBaseDriver::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; } } @@ -143,7 +143,7 @@ void NodeCanopenBaseDriver::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; @@ -156,7 +156,7 @@ void NodeCanopenBaseDriver::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; From 785ecb8eff0207a3b29732075f9bf8861d34291b Mon Sep 17 00:00:00 2001 From: ipa-vsp Date: Wed, 6 Dec 2023 08:51:45 +0000 Subject: [PATCH 5/5] change from 100ms to 2000ms --- canopen/sphinx/user-guide/configuration.rst | 2 +- .../canopen_core/node_interfaces/node_canopen_master.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/canopen/sphinx/user-guide/configuration.rst b/canopen/sphinx/user-guide/configuration.rst index 25136279..dc915f2e 100644 --- a/canopen/sphinx/user-guide/configuration.rst +++ b/canopen/sphinx/user-guide/configuration.rst @@ -111,7 +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). + boot_timeout; The timeout for booting all slaves in ms (default: 2000ms). 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 82d403b1..29ec2375 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 @@ -180,7 +180,7 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface this->node_->get_logger(), "No timeout parameter found in config file. Using default value of 100ms."); } - this->timeout_ = timeout.value_or(100); + this->timeout_ = timeout.value_or(2000); RCLCPP_INFO_STREAM( this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms."); }