Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add timeouts #220

Merged
merged 5 commits into from
Mar 11, 2024
Merged
Show file tree
Hide file tree
Changes from 4 commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/docker.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions canopen/sphinx/user-guide/configuration.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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).
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

100ms seems very short fot a default. I would say go with something like 2 seconds.


Device Section
--------------
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_)
Expand All @@ -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;
}
}
Expand All @@ -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;
Expand All @@ -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_)
Expand All @@ -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;
}
}
Expand All @@ -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;
Expand All @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand Down Expand Up @@ -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.");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Change to boot timeout, otherwise it is hard to get.

}
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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -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_);
}

Expand Down
Loading