Skip to content

fix: reset header stamps to zero in reset methods to prevent timeout #2197

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

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

fix: reset header stamps to zero in reset methods to prevent timeout #2197

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
d433d58
fix: reset header stamps to zero in reset methods to prevent timeout …
Ishan1923 Mar 4, 2026
50b2493
fix: change reset_controller_reference_msg signature to remove unused…
Ishan1923 Mar 27, 2026
cc93ee3
style: pre-commit run
Ishan1923 Mar 27, 2026
42104e3
test: halt robot to clear wheel feedback before odometry reset test
Ishan1923 Apr 2, 2026
1eb067b
Merge branch 'master' into fix/1774-reset-header-stamps
christophfroehlich Apr 17, 2026
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
10 changes: 4 additions & 6 deletions admittance_controller/src/admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,9 @@ void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint &
}

// called from RT control loop
void reset_wrench_msg(
geometry_msgs::msg::WrenchStamped & msg,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
void reset_wrench_msg(geometry_msgs::msg::WrenchStamped & msg)
{
msg.header.stamp = node->now();
msg.header.stamp = rclcpp::Time(0);
msg.wrench = geometry_msgs::msg::Wrench();
}

Expand Down Expand Up @@ -403,7 +401,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
}
}
reset_controller_reference_msg(joint_command_msg_);
reset_wrench_msg(wrench_command_msg_, get_node());
reset_wrench_msg(wrench_command_msg_);

// Use current joint_state as a default reference
last_reference_ = joint_state_;
Expand Down Expand Up @@ -519,7 +517,7 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
admittance_->reset(num_joints_);

reset_controller_reference_msg(joint_command_msg_);
reset_wrench_msg(wrench_command_msg_, get_node());
reset_wrench_msg(wrench_command_msg_);

return CallbackReturn::SUCCESS;
}
Expand Down
2 changes: 1 addition & 1 deletion diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -659,7 +659,7 @@ void DiffDriveController::reset_buffers()

// Fill RealtimeBox with NaNs so it will contain a known value
// but still indicate that no command has yet been sent.
command_msg_.header.stamp = get_node()->now();
command_msg_.header.stamp = rclcpp::Time(0);
command_msg_.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
command_msg_.twist.linear.y = std::numeric_limits<double>::quiet_NaN();
command_msg_.twist.linear.z = std::numeric_limits<double>::quiet_NaN();
Expand Down
9 changes: 4 additions & 5 deletions gpio_controllers/src/gpio_command_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,9 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces
}

// called from RT control loop
void reset_controller_reference_msg(
gpio_controllers::CmdType & msg, const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
void reset_controller_reference_msg(gpio_controllers::CmdType & msg)
{
msg.header.stamp = node->now();
msg.header.stamp = rclcpp::Time(0);
msg.interface_groups.clear();
msg.interface_values.clear();
}
Expand Down Expand Up @@ -147,7 +146,7 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State

initialize_gpio_state_msg();
// Set default value in command
reset_controller_reference_msg(gpio_commands_, get_node());
reset_controller_reference_msg(gpio_commands_);
rt_command_.try_set(gpio_commands_);
RCLCPP_INFO(get_node()->get_logger(), "activate successful");
return CallbackReturn::SUCCESS;
Expand All @@ -156,7 +155,7 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State
CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &)
{
// Set default value in command
reset_controller_reference_msg(gpio_commands_, get_node());
reset_controller_reference_msg(gpio_commands_);
rt_command_.try_set(gpio_commands_);
return CallbackReturn::SUCCESS;
}
Expand Down
11 changes: 5 additions & 6 deletions mecanum_drive_controller/src/mecanum_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,9 @@ using ControllerReferenceMsg =
mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg;

// called from RT control loop
void reset_controller_reference_msg(
ControllerReferenceMsg & msg, const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
void reset_controller_reference_msg(ControllerReferenceMsg & msg)
{
msg.header.stamp = node->now();
msg.header.stamp = rclcpp::Time(0);
msg.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
msg.twist.linear.y = std::numeric_limits<double>::quiet_NaN();
msg.twist.linear.z = std::numeric_limits<double>::quiet_NaN();
Expand Down Expand Up @@ -130,7 +129,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
"~/reference", subscribers_qos,
std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1));

reset_controller_reference_msg(current_ref_, get_node());
reset_controller_reference_msg(current_ref_);
input_ref_.set(current_ref_);

// deprecation warning if tf_frame_prefix_enable set to non-default value
Expand Down Expand Up @@ -297,7 +296,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptr<Controller
ref_timeout_.seconds());

ControllerReferenceMsg emtpy_msg;
reset_controller_reference_msg(emtpy_msg, get_node());
reset_controller_reference_msg(emtpy_msg);
input_ref_.set(emtpy_msg);
}
}
Expand Down Expand Up @@ -363,7 +362,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate(
// Try to set default value in command.
// If this fails, then another command will be received soon anyways.
ControllerReferenceMsg emtpy_msg;
reset_controller_reference_msg(emtpy_msg, get_node());
reset_controller_reference_msg(emtpy_msg);
input_ref_.try_set(emtpy_msg);
return controller_interface::CallbackReturn::SUCCESS;
}
Expand Down
2 changes: 1 addition & 1 deletion mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -440,7 +440,7 @@ TEST_F(
controller_->wait_for_commands(executor);
reference = controller_->input_ref_.get();

ASSERT_EQ(old_timestamp.sec, reference.header.stamp.sec);
EXPECT_GT(reference.header.stamp.sec, old_timestamp.sec);
EXPECT_FALSE(std::isnan(reference.twist.linear.x));
EXPECT_FALSE(std::isnan(reference.twist.angular.z));
EXPECT_EQ(reference.twist.linear.x, 1.5);
Expand Down
2 changes: 1 addition & 1 deletion omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -566,7 +566,7 @@ void OmniWheelDriveController::reset_buffers()

// Fill RealtimeBox with NaNs so it will contain a known value
// but still indicate that no command has yet been sent.
command_msg_.header.stamp = get_node()->now();
command_msg_.header.stamp = rclcpp::Time(0);
command_msg_.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
command_msg_.twist.linear.y = std::numeric_limits<double>::quiet_NaN();
command_msg_.twist.linear.z = std::numeric_limits<double>::quiet_NaN();
Expand Down
32 changes: 25 additions & 7 deletions steering_controllers_library/src/steering_controllers_library.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include "steering_controllers_library/steering_controllers_library.hpp"

#include <cmath>
#include <limits>
#include <memory>
#include <string>
Expand All @@ -31,10 +32,9 @@ using ControllerTwistReferenceMsg =
steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg;

// called from RT control loop
void reset_controller_reference_msg(
ControllerTwistReferenceMsg & msg, const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
void reset_controller_reference_msg(ControllerTwistReferenceMsg & msg)
{
msg.header.stamp = node->now();
msg.header.stamp = rclcpp::Time(0);
msg.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
msg.twist.linear.y = std::numeric_limits<double>::quiet_NaN();
msg.twist.linear.z = std::numeric_limits<double>::quiet_NaN();
Expand Down Expand Up @@ -263,7 +263,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
"~/reference", subscribers_qos,
std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1));

reset_controller_reference_msg(current_ref_, get_node());
reset_controller_reference_msg(current_ref_);
input_ref_.set(current_ref_);

try
Expand Down Expand Up @@ -470,7 +470,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate(
{
// Try to set default value in command.
// If this fails, then another command will be received soon anyways.
reset_controller_reference_msg(current_ref_, get_node());
reset_controller_reference_msg(current_ref_);
input_ref_.try_set(current_ref_);

return controller_interface::CallbackReturn::SUCCESS;
Expand Down Expand Up @@ -727,10 +727,28 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
controller_state_msg_.steering_angle_command.push_back(command_interface_value_op.value());
}
}

controller_state_publisher_->try_publish(controller_state_msg_);
}

// store current ref (for open loop odometry) and update odometry
Copy link
Copy Markdown
Contributor

@thedevmystic thedevmystic Mar 28, 2026

Choose a reason for hiding this comment

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

The edits in steering_controllers_library.cpp is not related to the fix, right?

So, why here?

Copy link
Copy Markdown
Contributor Author

@Ishan1923 Ishan1923 Apr 2, 2026

Choose a reason for hiding this comment

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

In the conversation #1721 ; @saikishor explicitly mentioned about setting the message time to 0, and @christophfroehlich agreed to resolve this in future pr, which also includes the steering_controllers_library.cpp as it was mentioned to remove the node parameter from the reset_controller_reference_msg, so I had to remove the node parameter in the steering_controllers_library.cpp as well.

if (std::isfinite(reference_interfaces_[0]))
{
last_linear_velocity_ = reference_interfaces_[0];
}
else
{
last_linear_velocity_ = 0.0;
}
if (std::isfinite(reference_interfaces_[1]))
{
last_angular_velocity_ = reference_interfaces_[1];
}
else
{
last_angular_velocity_ = 0.0;
}
update_odometry(period);

reference_interfaces_[0] = std::numeric_limits<double>::quiet_NaN();
reference_interfaces_[1] = std::numeric_limits<double>::quiet_NaN();

Expand Down Expand Up @@ -763,7 +781,7 @@ bool SteeringControllersLibrary::reset()
{
odometry_.set_odometry(0.0, 0.0, 0.0);

reset_controller_reference_msg(current_ref_, get_node());
reset_controller_reference_msg(current_ref_);
input_ref_.set(current_ref_);

last_linear_velocity_ = std::numeric_limits<double>::quiet_NaN();
Expand Down
2 changes: 2 additions & 0 deletions steering_controllers_library/test/test_steering_controllers_library.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -426,6 +426,8 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service)
for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0);
ASSERT_GT(controller_->odometry_.get_x(), 0.0);

move_robot(0.0, 0.0, 0.0);

// 2. Call the odometry set service
auto set_request = std::make_shared<control_msgs::srv::SetOdometry::Request>();
auto set_response = std::make_shared<control_msgs::srv::SetOdometry::Response>();
Expand Down
Loading