From b184ce70477312fec4353c005442f499900f0a54 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 17 Jun 2024 19:18:09 +0100 Subject: [PATCH] Still fill desired/actual deprecated fields --- .../src/joint_trajectory_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7705f7870c..f23cd970fb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1180,6 +1180,12 @@ void JointTrajectoryController::publish_state( state_publisher_->msg_.reference.velocities = desired_state.velocities; state_publisher_->msg_.reference.accelerations = desired_state.accelerations; state_publisher_->msg_.feedback.positions = current_state.positions; + // DESIRED and ACTUAL are deprecated in the message but we are still + // reporting on them + state_publisher_legacy_->msg_.desired.positions = desired_state.positions; + state_publisher_legacy_->msg_.desired.velocities = desired_state.velocities; + state_publisher_legacy_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_legacy_->msg_.actual.positions = current_state.positions; state_publisher_->msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) {