Update controllers adding dt in to update as in ros2_control

ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp

@@ -22,6 +22,8 @@
 
 #include "controller_interface/controller_interface.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/duration.hpp"
 
 namespace ur_controllers
 {
@@ -67,7 +69,7 @@ class ForceTorqueStateBroadcaster : public controller_interface::ControllerInter
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
   CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
 

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -36,6 +36,8 @@
 #include "ur_dashboard_msgs/msg/safety_mode.hpp"
 #include "ur_msgs/srv/set_io.hpp"
 #include "ur_msgs/srv/set_speed_slider_fraction.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/duration.hpp"
 
 namespace ur_controllers
 {
@@ -74,7 +76,7 @@ class GPIOController : public controller_interface::ControllerInterface
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
   CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
 

ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp

@@ -27,6 +27,8 @@
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/duration.hpp"
 
 namespace ur_controllers
 {
@@ -42,7 +44,7 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
 
   CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
 
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
 protected:
   struct TimeData

ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp

@@ -31,6 +31,8 @@
 #include "controller_interface/controller_interface.hpp"
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/duration.hpp"
 #include "std_msgs/msg/float64.hpp"
 
 namespace ur_controllers
@@ -46,7 +48,7 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
   CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
 
@@ -58,7 +60,6 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte
 
 protected:
   std::vector<std::string> sensor_names_;
-  rclcpp::Time last_publish_time_;
   double publish_rate_;
 
   std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>> speed_scaling_state_publisher_;

ur_controllers/src/force_torque_sensor_broadcaster.cpp

@@ -57,7 +57,8 @@ controller_interface::InterfaceConfiguration ForceTorqueStateBroadcaster::state_
   return config;
 }
 
-controller_interface::return_type ur_controllers::ForceTorqueStateBroadcaster::update()
+controller_interface::return_type
+ur_controllers::ForceTorqueStateBroadcaster::update(const rclcpp::Time& time, const rclcpp::Duration& /*period*/)
 {
   geometry_msgs::msg::Vector3 f_vec;
   geometry_msgs::msg::Vector3 t_vec;
@@ -90,7 +91,7 @@ controller_interface::return_type ur_controllers::ForceTorqueStateBroadcaster::u
     }
   }
 
-  wrench_state_msg_.header.stamp = get_node()->get_clock()->now();
+  wrench_state_msg_.header.stamp = time;
   wrench_state_msg_.header.frame_id = fts_params_.frame_id;
 
   // update wrench state message

ur_controllers/src/gpio_controller.cpp

@@ -111,7 +111,8 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
   return config;
 }
 
-controller_interface::return_type ur_controllers::GPIOController::update()
+controller_interface::return_type ur_controllers::GPIOController::update(const rclcpp::Time& /*time*/,
+                                                                         const rclcpp::Duration& /*period*/)
 {
   publishIO();
   publishToolData();

ur_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -46,7 +46,8 @@ CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecyc
   return JointTrajectoryController::on_activate(state);
 }
 
-controller_interface::return_type ScaledJointTrajectoryController::update()
+controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
+                                                                          const rclcpp::Duration& /*period*/)
 {
   if (state_interfaces_.back().get_name() == "speed_scaling") {
     scaling_factor_ = state_interfaces_.back().get_value();
@@ -132,7 +133,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update()
     // Main Speed scaling difference...
     // Adjust time with scaling factor
     TimeData time_data;
-    time_data.time = node_->now();
+    time_data.time = time;
     rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
     time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period);
     time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
@@ -185,7 +186,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update()
       if (active_goal) {
         // send feedback
         auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
-        feedback->header.stamp = node_->now();
+        feedback->header.stamp = time;
         feedback->joint_names = joint_names_;
 
         feedback->actual = state_current;
@@ -224,7 +225,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update()
 
             // TODO(anyone): This will break in speed scaling we have to discuss how to handle the goal
             // time when the robot scales itself down.
-            const double difference = node_->now().seconds() - traj_end.seconds();
+            const double difference = time.seconds() - traj_end.seconds();
             if (difference > default_tolerances_.goal_time_tolerance) {
               auto result = std::make_shared<FollowJTrajAction::Result>();
               result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);

ur_controllers/src/speed_scaling_state_broadcaster.cpp

@@ -93,7 +93,6 @@ SpeedScalingStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*prev
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 SpeedScalingStateBroadcaster::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
-  last_publish_time_ = node_->now();
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -103,15 +102,15 @@ SpeedScalingStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State& /*pre
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type SpeedScalingStateBroadcaster::update()
+controller_interface::return_type SpeedScalingStateBroadcaster::update(const rclcpp::Time& /*time*/,
+                                                                       const rclcpp::Duration& period)
 {
-  if (publish_rate_ > 0.0 && (node_->now() - last_publish_time_) > rclcpp::Duration(1.0 / publish_rate_, 0.0)) {
+  if (publish_rate_ > 0.0 && period > rclcpp::Duration(1.0 / publish_rate_, 0.0)) {
     // Speed scaling is the only interface of the controller
     speed_scaling_state_msg_.data = state_interfaces_[0].get_value() * 100.0;
 
     // publish
     speed_scaling_state_publisher_->publish(speed_scaling_state_msg_);
-    last_publish_time_ = node_->now();
   }
   return controller_interface::return_type::OK;
 }