From 7b154311bcba8e9a391a9b4a05376217ea0401a6 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 10 Oct 2023 14:39:50 +0200 Subject: [PATCH] use reset+initRT due to missing writeFromRT MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 8430a75bf5..24cec2ff20 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -210,7 +210,8 @@ controller_interface::return_type JointTrajectoryController::update( time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period); - time_data_.writeFromNonRT(time_data); + time_data_.reset(); + time_data_.initRT(time_data); bool first_sample = false; // if sampling the first time, set the point before you sample