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

admittance_controller/src/admittance_controller.cpp

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

diff_drive_controller/src/diff_drive_controller.cpp

@@ -644,7 +644,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();

gpio_controllers/src/gpio_command_controller.cpp

@@ -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();
 }
@@ -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;
@@ -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;
 }

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -34,10 +34,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();
@@ -131,7 +130,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
@@ -346,7 +345,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);
   }
 }
@@ -413,6 +412,12 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate(
   // influence the behavior of the controller after a deactivate->activate cycle.
   reset_buffers();
 
+  // 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);
+  input_ref_.try_set(emtpy_msg);
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -450,7 +455,7 @@ void MecanumDriveController::reset_buffers()
     std::deque<std::array<double, 3>>{{{0.0, 0.0, 0.0}}, {{0.0, 0.0, 0.0}}});
 
   // Reset the latest received reference back to NaN so no stale command is applied.
-  reset_controller_reference_msg(current_ref_, get_node());
+  reset_controller_reference_msg(current_ref_);
   input_ref_.set(current_ref_);
 }
 

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -441,7 +441,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);

omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp

@@ -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();

steering_controllers_library/src/steering_controllers_library.cpp

@@ -14,6 +14,7 @@
 
 #include "steering_controllers_library/steering_controllers_library.hpp"
 
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <string>
@@ -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();
@@ -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
@@ -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;
@@ -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
+  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();
 
@@ -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();

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -425,6 +425,8 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service)
   for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0);
   ASSERT_GT(controller_->odometry_.get_x(), 0.0);
 
+  move_robot(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>();