Add time and period to update function (ros-navigation#241)

* Add time and period to update function

Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com>

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -61,7 +61,8 @@ class DiffDriveController : public controller_interface::ControllerInterface
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
   DIFF_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   DIFF_DRIVE_CONTROLLER_PUBLIC
   CallbackReturn on_init() override;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -132,7 +132,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons
   return {interface_configuration_type::INDIVIDUAL, conf_names};
 }
 
-controller_interface::return_type DiffDriveController::update()
+controller_interface::return_type DiffDriveController::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto logger = node_->get_logger();
   if (get_state().id() == State::PRIMARY_STATE_INACTIVE)
@@ -145,29 +146,29 @@ controller_interface::return_type DiffDriveController::update()
     return controller_interface::return_type::OK;
   }
 
-  const auto current_time = node_->get_clock()->now();
+  const auto current_time = time;
 
-  std::shared_ptr<Twist> last_msg;
-  received_velocity_msg_ptr_.get(last_msg);
+  std::shared_ptr<Twist> last_command_msg;
+  received_velocity_msg_ptr_.get(last_command_msg);
 
-  if (last_msg == nullptr)
+  if (last_command_msg == nullptr)
   {
     RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
     return controller_interface::return_type::ERROR;
   }
 
-  const auto dt = current_time - last_msg->header.stamp;
+  const auto age_of_last_command = current_time - last_command_msg->header.stamp;
   // Brake if cmd_vel has timeout, override the stored command
-  if (dt > cmd_vel_timeout_)
+  if (age_of_last_command > cmd_vel_timeout_)
   {
-    last_msg->twist.linear.x = 0.0;
-    last_msg->twist.angular.z = 0.0;
+    last_command_msg->twist.linear.x = 0.0;
+    last_command_msg->twist.angular.z = 0.0;
   }
 
   // linear_command and angular_command may be limited further by SpeedLimit,
   // without affecting the stored twist command
-  double linear_command = last_msg->twist.linear.x;
-  double angular_command = last_msg->twist.angular.z;
+  double linear_command = last_command_msg->twist.linear.x;
+  double angular_command = last_command_msg->twist.angular.z;
 
   // Apply (possibly new) multipliers:
   const auto wheels = wheel_params_;
@@ -247,7 +248,7 @@ controller_interface::return_type DiffDriveController::update()
     angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds());
 
   previous_commands_.pop();
-  previous_commands_.emplace(*last_msg);
+  previous_commands_.emplace(*last_command_msg);
 
   //    Publish limited velocity
   if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock())

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -285,11 +285,15 @@ TEST_F(TestDiffDriveController, cleanup)
   publish(linear, angular);
   controller_->wait_for_twist(executor);
 
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   state = controller_->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   state = controller_->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
@@ -333,7 +337,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
   // wait for msg is be published to the system
   ASSERT_TRUE(controller_->wait_for_twist(executor));
 
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
   EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value());
   EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value());
 
@@ -342,7 +348,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
   std::this_thread::sleep_for(std::chrono::milliseconds(500));
   state = controller_->deactivate();
   ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";
   EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";

effort_controllers/test/test_joint_group_effort_controller.cpp

@@ -114,7 +114,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // update successful though no command has been send yet
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
   ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
@@ -127,7 +129,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
   controller_->rt_command_ptr_.writeFromNonRT(command_ptr);
 
   // update successful, command received
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check joint commands have been modified
   ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
@@ -147,7 +151,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
   controller_->rt_command_ptr_.writeFromNonRT(command_ptr);
 
   // update failed, command size does not match number of joints
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::ERROR);
 
   // check joint commands are still the default ones
   ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
@@ -162,7 +168,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // update successful, no command received yet
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
   ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
@@ -201,7 +209,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
   rclcpp::spin_some(controller_->get_node()->get_node_base_interface());
 
   // update successful
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check command in handle was set
   ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

@@ -59,7 +59,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
   std::string sensor_name_;

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -156,11 +156,12 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate(
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type ForceTorqueSensorBroadcaster::update()
+controller_interface::return_type ForceTorqueSensorBroadcaster::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   if (realtime_publisher_ && realtime_publisher_->trylock())
   {
-    realtime_publisher_->msg_.header.stamp = node_->now();
+    realtime_publisher_->msg_.header.stamp = time;
     force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench);
     realtime_publisher_->unlockAndPublish();
   }

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -88,7 +88,9 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message(
     "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback);
 
   // call update to publish the test value
-  ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // wait for message to be passed
   ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
@@ -220,7 +222,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success)
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
-  ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success)
@@ -234,7 +238,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success)
 
   ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success)

forward_command_controller/include/forward_command_controller/forward_command_controller.hpp

@@ -69,7 +69,8 @@ class ForwardCommandController : public controller_interface::ControllerInterfac
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
   std::vector<std::string> joint_names_;

forward_command_controller/src/forward_command_controller.cpp

@@ -153,7 +153,8 @@ CallbackReturn ForwardCommandController::on_deactivate(
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type ForwardCommandController::update()
+controller_interface::return_type ForwardCommandController::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto joint_commands = rt_command_ptr_.readFromRT();
 

forward_command_controller/test/test_forward_command_controller.cpp

@@ -178,7 +178,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // update successful though no command has been send yet
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
   ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
@@ -191,7 +193,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest)
   controller_->rt_command_ptr_.writeFromNonRT(command_ptr);
 
   // update successful, command received
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check joint commands have been modified
   ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
@@ -215,7 +219,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest)
   controller_->rt_command_ptr_.writeFromNonRT(command_ptr);
 
   // update failed, command size does not match number of joints
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::ERROR);
 
   // check joint commands are still the default ones
   ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
@@ -233,7 +239,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // update successful, no command received yet
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
   ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
@@ -274,7 +282,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
   rclcpp::spin_some(controller_->get_node()->get_node_base_interface());
 
   // update successful
-  ASSERT_EQ(controller_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // check command in handle was set
   ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);

gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp

@@ -88,7 +88,8 @@ class GripperActionController : public controller_interface::ControllerInterface
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
   GRIPPER_ACTION_CONTROLLER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   GRIPPER_ACTION_CONTROLLER_PUBLIC
   CallbackReturn on_init() override;

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -61,7 +61,8 @@ CallbackReturn GripperActionController<HardwareInterface>::on_init()
 }
 
 template <const char * HardwareInterface>
-controller_interface::return_type GripperActionController<HardwareInterface>::update()
+controller_interface::return_type GripperActionController<HardwareInterface>::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   command_struct_rt_ = *(command_.readFromRT());
 

imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp

@@ -56,7 +56,8 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   IMU_SENSOR_BROADCASTER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
   std::string sensor_name_;

imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

@@ -112,11 +112,12 @@ CallbackReturn IMUSensorBroadcaster::on_deactivate(
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type IMUSensorBroadcaster::update()
+controller_interface::return_type IMUSensorBroadcaster::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   if (realtime_publisher_ && realtime_publisher_->trylock())
   {
-    realtime_publisher_->msg_.header.stamp = node_->now();
+    realtime_publisher_->msg_.header.stamp = time;
     imu_sensor_->get_values_as_message(realtime_publisher_->msg_);
     realtime_publisher_->unlockAndPublish();
   }

imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp

@@ -90,7 +90,9 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu &
     "/test_imu_sensor_broadcaster/imu", 10, subs_callback);
 
   // call update to publish the test value
-  ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 
   // wait for message to be passed
   ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
@@ -171,7 +173,9 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success)
   ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
-  ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK);
+  ASSERT_EQ(
+    imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
 }
 
 TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success)

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -44,7 +44,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
   JOINT_STATE_BROADCASTER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   JOINT_STATE_BROADCASTER_PUBLIC
   CallbackReturn on_init() override;

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -229,7 +229,8 @@ double get_value(
   }
 }
 
-controller_interface::return_type JointStateBroadcaster::update()
+controller_interface::return_type JointStateBroadcaster::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   for (const auto & state_interface : state_interfaces_)
   {
@@ -240,8 +241,8 @@ controller_interface::return_type JointStateBroadcaster::update()
       state_interface.get_interface_name().c_str(), state_interface.get_value());
   }
 
-  joint_state_msg_.header.stamp = get_node()->get_clock()->now();
-  dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now();
+  joint_state_msg_.header.stamp = time;
+  dynamic_joint_state_msg_.header.stamp = time;
 
   // update joint state message and dynamic joint state message
   for (auto i = 0ul; i < joint_names_.size(); ++i)