added dt to controller interface and controller manager #438

controller_interface/include/controller_interface/controller_interface.hpp

@@ -95,7 +95,7 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   virtual return_type init(const std::string & controller_name);
 
   CONTROLLER_INTERFACE_PUBLIC
-  virtual return_type update() = 0;
+  virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
   CONTROLLER_INTERFACE_PUBLIC
   std::shared_ptr<rclcpp::Node> get_node();

controller_interface/test/test_controller_with_options.hpp

@@ -80,7 +80,8 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
       controller_interface::interface_configuration_type::NONE};
   }
 
-  controller_interface::return_type update() override
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override
   {
     return controller_interface::return_type::OK;
   }

controller_manager/include/controller_manager/controller_manager.hpp

@@ -129,7 +129,8 @@ class ControllerManager : public rclcpp::Node
   void read();
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::return_type update();
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period);
 
   CONTROLLER_MANAGER_PUBLIC
   void write();

controller_manager/src/controller_manager.cpp

@@ -1146,7 +1146,8 @@ std::vector<std::string> ControllerManager::get_controller_names()
 
 void ControllerManager::read() { resource_manager_->read(); }
 
-controller_interface::return_type ControllerManager::update()
+controller_interface::return_type ControllerManager::update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
@@ -1158,7 +1159,7 @@ controller_interface::return_type ControllerManager::update()
     // https://github.com/ros-controls/ros2_control/issues/153
     if (is_controller_running(*loaded_controller.c))
     {
-      auto controller_ret = loaded_controller.c->update();
+      auto controller_ret = loaded_controller.c->update(time, period);
       if (controller_ret != controller_interface::return_type::OK)
       {
         ret = controller_ret;

controller_manager/src/ros2_control_node.cpp

@@ -48,11 +48,18 @@ int main(int argc, char ** argv)
     }
     RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", update_rate);
 
+    std::chrono::system_clock::time_point timepoint_start = std::chrono::system_clock::now();
+    std::chrono::system_clock::time_point begin = timepoint_start;
+    std::this_thread::sleep_for(std::chrono::nanoseconds(1000000000 / update_rate));
     while (rclcpp::ok())
     {
-      std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
+      std::chrono::system_clock::time_point begin_last = begin;
+      begin = std::chrono::system_clock::now();
       cm->read();
-      cm->update();
+      cm->update(
+        rclcpp::Time(
+          std::chrono::duration_cast<std::chrono::nanoseconds>(begin - timepoint_start).count()),
+        rclcpp::Duration(std::chrono::duration_cast<std::chrono::nanoseconds>(begin - begin_last)));
       cm->write();
       std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
       std::this_thread::sleep_for(std::max(

controller_manager/test/controller_manager_test_common.hpp

@@ -63,7 +63,7 @@ class ControllerManagerFixture : public ::testing::Test
     updater_ = std::thread([&](void) -> void {
       while (run_updater_)
       {
-        cm_->update();
+        cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
       }
     });

controller_manager/test/test_controller/test_controller.cpp

@@ -27,7 +27,8 @@ TestController::TestController()
 {
 }
 
-controller_interface::return_type TestController::update()
+controller_interface::return_type TestController::update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   ++internal_counter;
   return controller_interface::return_type::OK;

controller_manager/test/test_controller/test_controller.hpp

@@ -49,7 +49,8 @@ class TestController : public controller_interface::ControllerInterface
   }
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   CONTROLLER_MANAGER_PUBLIC
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;

controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp

@@ -37,7 +37,8 @@ controller_interface::return_type TestControllerFailedInit::init(
   return controller_interface::return_type::ERROR;
 }
 
-controller_interface::return_type TestControllerFailedInit::update()
+controller_interface::return_type TestControllerFailedInit::update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   return controller_interface::return_type::OK;
 }

controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp

@@ -54,7 +54,8 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac
   }
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   CONTROLLER_MANAGER_PUBLIC
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;

controller_manager/test/test_controller_manager.cpp

@@ -39,7 +39,9 @@ TEST_F(TestControllerManager, controller_lifecycle)
   EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
   EXPECT_EQ(2, test_controller.use_count());
 
-  EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(0u, test_controller->internal_counter)
     << "Update should not reach an unconfigured controller";
 
@@ -48,7 +50,9 @@ TEST_F(TestControllerManager, controller_lifecycle)
 
   // configure controller
   cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
-  EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
 
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
@@ -63,15 +67,19 @@ TEST_F(TestControllerManager, controller_lifecycle)
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
-  EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
   {
     ControllerManagerRunner cm_runner(this);
     EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
   }
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
 
-  EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_GE(test_controller->internal_counter, 1u);
   auto last_internal_counter = test_controller->internal_counter;
 
@@ -85,7 +93,9 @@ TEST_F(TestControllerManager, controller_lifecycle)
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
-  EXPECT_EQ(controller_interface::return_type::OK, cm_->update());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter)
     << "Controller is stopped at the end of update, so it should have done one more update";
   {

controller_manager/test/test_controller_manager_srvs.cpp

@@ -42,7 +42,7 @@ class TestControllerManagerSrvs : public ControllerManagerFixture
 
     update_timer_ = cm_->create_wall_timer(std::chrono::milliseconds(10), [&]() {
       cm_->read();
-      cm_->update();
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
       cm_->write();
     });
 
@@ -69,7 +69,7 @@ class TestControllerManagerSrvs : public ControllerManagerFixture
       while (service_executor.spin_until_future_complete(result, 50ms) !=
              rclcpp::FutureReturnCode::SUCCESS)
       {
-        cm_->update();
+        cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
       }
     }
     else

controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp

@@ -32,7 +32,8 @@ TestControllerWithInterfaces::on_init()
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type TestControllerWithInterfaces::update()
+controller_interface::return_type TestControllerWithInterfaces::update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   return controller_interface::return_type::OK;
 }

controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp

@@ -51,7 +51,8 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte
   }
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::return_type update() override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   CONTROLLER_MANAGER_PUBLIC
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;

controller_manager/test/test_spawner_unspawner.cpp

@@ -36,7 +36,7 @@ class TestLoadController : public ControllerManagerFixture
 
     update_timer_ = cm_->create_wall_timer(std::chrono::milliseconds(10), [&]() {
       cm_->read();
-      cm_->update();
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
       cm_->write();
     });