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.

Sign up for GitHub

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 reference timeout parameter to controllers templates #111

Open
wants to merge 17 commits into
base: master
Choose a base branch
from
Open

Add reference timeout parameter to controllers templates #111

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
df36359
added ref_timeout a parameter
Mar 14, 2023
0cef88d
added ref_timeout related
Mar 14, 2023
62ec6f3
both ref_time related and improvements from mecanum controller
Mar 14, 2023
8b5c1c9
correction
Mar 14, 2023
77e1b53
added tests (new names) onl ref_timeout specific
Mar 14, 2023
27014c9
chainable added missing
Mar 14, 2023
64c8f1d
regular controller added missing
Mar 14, 2023
e2cc41e
regular controller added missing tests
Mar 14, 2023
3f27a6e
corrections
Mar 14, 2023
524e334
all tests pass except load_controller_test
Mar 23, 2023
508ce0e
all tests pass except for load_controller in chaiable controller
Mar 23, 2023
40aef82
correction
Mar 23, 2023
72605dc
corrections
Mar 23, 2023
c02dd31
ran pre.commit
Mar 23, 2023
0fb82f6
/commands to /reference
Mar 29, 2023
74b4d11
removed duplicate code for age_of_last_command
Mar 29, 2023
9692b17
finishing the incomplete test when_ref_timeout_zero_for_reference_cal…
Mar 29, 2023
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
85 changes: 64 additions & 21 deletions templates/ros2_control/controller/dummy_chainable_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -41,8 +41,10 @@ using ControllerReferenceMsg = dummy_package_namespace::DummyClassName::Controll

// called from RT control loop
void reset_controller_reference_msg(
const std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names)
const std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
{
msg->header.stamp = node->now();
msg->joint_names = joint_names;
msg->displacements.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
msg->velocities.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
Expand Down Expand Up @@ -93,13 +95,15 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
subscribers_qos.keep_last(1);
subscribers_qos.best_effort();

ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);

// Reference Subscriber
ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
"~/reference", subscribers_qos,
std::bind(&DummyClassName::reference_callback, this, std::placeholders::_1));

std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
reset_controller_reference_msg(msg, params_.joints);
reset_controller_reference_msg(msg, params_.joints, get_node());
input_ref_.writeFromNonRT(msg);

auto set_slow_mode_service_callback =
Expand Down Expand Up @@ -139,6 +143,30 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
return controller_interface::CallbackReturn::SUCCESS;
}

void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
Copy link
Contributor Author

Choose a reason for hiding this comment

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

saw that ref_callback is called in different order in standard and chainable controller, hence adjusting it to be consistent with standard controller

Copy link
Member

Choose a reason for hiding this comment

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

in the chainable controller,
this callback is not used at all. I mean, you can call it, but nothing will happen.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

sorry for the confusion i should have written "ref_callback is defined in different order in standard and chain-able controller classes, hence adjusting it to be consistent with standard controller"

its trivial change, just changed the order

{
const auto age_of_last_command = get_node()->now() - msg->header.stamp;
if (msg->joint_names.size() == params_.joints.size()) {
if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) {
input_ref_.writeFromNonRT(msg);
Copy link
Member

Choose a reason for hiding this comment

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

Should we not have here default value of the header is timestamp is "0"?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

you are referring to this handling of case if header.stamp ==0 right?

this is included in the improvements from mecanum PR, you can see the line in the below link, should i also implement this in this ref_timeout PR?

https://github.com/StoglRobotics/ros_team_workspace/pull/106/files#diff-6e6a1012850b4b6040c5339b00ffad68f776d9edb72dbbe3e1d84482d52c4081R189-R197

} else {
RCLCPP_ERROR(
get_node()->get_logger(),
"Received message has timestamp %.10f older for %.10f which "
"is more then allowed timeout "
"(%.4f).",
rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
ref_timeout_.seconds());
reset_controller_reference_msg(msg, params_.joints, get_node());
}
} else {
RCLCPP_ERROR(
get_node()->get_logger(),
"Received %zu , but expected %zu joints in command. Ignoring message.",
msg->joint_names.size(), params_.joints.size());
}
}

controller_interface::InterfaceConfiguration DummyClassName::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration command_interfaces_config;
Expand All @@ -165,18 +193,6 @@ controller_interface::InterfaceConfiguration DummyClassName::state_interface_con
return state_interfaces_config;
}

void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
{
if (msg->joint_names.size() == params_.joints.size()) {
input_ref_.writeFromNonRT(msg);
} else {
RCLCPP_ERROR(
get_node()->get_logger(),
"Received %zu , but expected %zu joints in command. Ignoring message.",
msg->joint_names.size(), params_.joints.size());
}
}

std::vector<hardware_interface::CommandInterface> DummyClassName::on_export_reference_interfaces()
{
reference_interfaces_.resize(state_joints_.size(), std::numeric_limits<double>::quiet_NaN());
Expand All @@ -203,7 +219,7 @@ controller_interface::CallbackReturn DummyClassName::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// Set default value in command
reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_);
reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_, get_node());

return controller_interface::CallbackReturn::SUCCESS;
}
Expand All @@ -222,14 +238,32 @@ controller_interface::CallbackReturn DummyClassName::on_deactivate(
controller_interface::return_type DummyClassName::update_reference_from_subscribers()
{
auto current_ref = input_ref_.readFromRT();
const auto age_of_last_command = get_node()->now() - (*current_ref)->header.stamp;

// TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
// instead of a loop
for (size_t i = 0; i < reference_interfaces_.size(); ++i) {
if (!std::isnan((*current_ref)->displacements[i])) {
reference_interfaces_[i] = (*current_ref)->displacements[i];
// send message only if there is no timeout
if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
if (!std::isnan((*current_ref)->displacements[i])) {
if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
(*current_ref)->displacements[i] /= 2;
}
reference_interfaces_[i] = (*current_ref)->displacements[i];
if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
}
}
} else {
if (!std::isnan((*current_ref)->displacements[i])) {
reference_interfaces_[i] = 0.0;

(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
}
}
}
return controller_interface::return_type::OK;
Expand All @@ -238,16 +272,22 @@ controller_interface::return_type DummyClassName::update_reference_from_subscrib
controller_interface::return_type DummyClassName::update_and_write_commands(
const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
{
auto current_ref = input_ref_.readFromRT();
// TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
// instead of a loop
for (size_t i = 0; i < command_interfaces_.size(); ++i) {
// send message only if there is no timeout
if (!std::isnan(reference_interfaces_[i])) {
if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
reference_interfaces_[i] /= 2;
}
command_interfaces_[i].set_value(reference_interfaces_[i]);

reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
}
}
}

Expand All @@ -258,6 +298,9 @@ controller_interface::return_type DummyClassName::update_and_write_commands(
state_publisher_->unlockAndPublish();
}

for (size_t i = 0; i < reference_interfaces_.size(); ++i) {
reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
}
return controller_interface::return_type::OK;
}

Expand Down
48 changes: 38 additions & 10 deletions templates/ros2_control/controller/dummy_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -41,8 +41,10 @@ using ControllerReferenceMsg = dummy_package_namespace::DummyClassName::Controll

// called from RT control loop
void reset_controller_reference_msg(
std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names)
const std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
{
msg->header.stamp = node->now();
msg->joint_names = joint_names;
msg->displacements.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
msg->velocities.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
Expand Down Expand Up @@ -93,13 +95,15 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
subscribers_qos.keep_last(1);
subscribers_qos.best_effort();

ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);

// Reference Subscriber
ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
"~/reference", subscribers_qos,
std::bind(&DummyClassName::reference_callback, this, std::placeholders::_1));

std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
reset_controller_reference_msg(msg, params_.joints);
reset_controller_reference_msg(msg, params_.joints, get_node());
input_ref_.writeFromNonRT(msg);

auto set_slow_mode_service_callback =
Expand Down Expand Up @@ -141,8 +145,20 @@ controller_interface::CallbackReturn DummyClassName::on_configure(

void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
{
const auto age_of_last_command = get_node()->now() - msg->header.stamp;
if (msg->joint_names.size() == params_.joints.size()) {
input_ref_.writeFromNonRT(msg);
if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) {
input_ref_.writeFromNonRT(msg);
} else {
RCLCPP_ERROR(
get_node()->get_logger(),
"Received message has timestamp %.10f older for %.10f which "
"is more then allowed timeout "
"(%.4f).",
rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
ref_timeout_.seconds());
reset_controller_reference_msg(msg, params_.joints, get_node());
}
} else {
RCLCPP_ERROR(
get_node()->get_logger(),
Expand Down Expand Up @@ -185,7 +201,7 @@ controller_interface::CallbackReturn DummyClassName::on_activate(
// `controller_interface::get_ordered_interfaces` helper function

// Set default value in command
reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints);
reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints, get_node());

return controller_interface::CallbackReturn::SUCCESS;
}
Expand All @@ -205,17 +221,29 @@ controller_interface::return_type DummyClassName::update(
const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
{
auto current_ref = input_ref_.readFromRT();
const auto age_of_last_command = get_node()->now() - (*current_ref)->header.stamp;

// TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
// instead of a loop
for (size_t i = 0; i < command_interfaces_.size(); ++i) {
if (!std::isnan((*current_ref)->displacements[i])) {
if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
(*current_ref)->displacements[i] /= 2;
// send message only if there is no timeout
if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
if (!std::isnan((*current_ref)->displacements[i])) {
if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
(*current_ref)->displacements[i] /= 2;
}
command_interfaces_[i].set_value((*current_ref)->displacements[i]);
if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
}
}
command_interfaces_[i].set_value((*current_ref)->displacements[i]);

} else {
command_interfaces_[i].set_value(0.0);
Copy link
Member

Choose a reason for hiding this comment

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

here under is missing resetting of refrence interfaces...

Copy link
Contributor Author

Choose a reason for hiding this comment

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

this is standard controller and in my understanding we dont have reference_interfaces in standard controller. Please correct me if i am wrong

(*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
(*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
}
}

Expand Down
7 changes: 7 additions & 0 deletions templates/ros2_control/controller/dummy_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,3 +29,10 @@ dummy_controller:
forbidden_interface_name_prefix: null
}
}
reference_timeout: {
type: double,
default_value: 0.0,
description: "Timeout for controller references after which they will be reset.
This is especially useful for controllers that can cause unwanted and dangerous behaviour
if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.",
}
2 changes: 2 additions & 0 deletions templates/ros2_control/controller/dummy_controller_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,3 +5,5 @@ test_dummy_controller:
- joint1

interface_name: acceleration

reference_timeout: 0.1
2 changes: 2 additions & 0 deletions templates/ros2_control/controller/dummy_controller_preceeding_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,5 @@ test_dummy_controller:

state_joints:
- joint1state

reference_timeout: 0.1
4 changes: 3 additions & 1 deletion templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -95,6 +95,8 @@ class DummyClassName : public controller_interface::ChainableControllerInterface
rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;

rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms

rclcpp::Service<ControllerModeSrvType>::SharedPtr set_slow_control_mode_service_;
realtime_tools::RealtimeBuffer<control_mode_type> control_mode_;

Expand Down
4 changes: 3 additions & 1 deletion templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -92,6 +92,8 @@ class DummyClassName : public controller_interface::ControllerInterface
rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;

rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms

rclcpp::Service<ControllerModeSrvType>::SharedPtr set_slow_control_mode_service_;
realtime_tools::RealtimeBuffer<control_mode_type> control_mode_;

Expand Down
2 changes: 1 addition & 1 deletion .../ros2_control/controller/dummy_package_namespace/validate_dummy_controller_parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down
2 changes: 1 addition & 1 deletion templates/ros2_control/controller/dummy_package_namespace/visibility_control.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down
Loading