From f342ba27a52d2c830a0771cb2becc1fdb4f97874 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 8 Apr 2024 10:26:23 +0200 Subject: [PATCH] Fix incorrect force-torque sensor vec population (#296) (#300) (cherry picked from commit fdcd7aa8c67ea57f44bbf2f8fba90a28d7f04b5d) Co-authored-by: Mateus Menezes --- gazebo_ros2_control/src/gazebo_system.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 2b76adc9..153ec0b4 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -561,13 +561,13 @@ hardware_interface::return_type GazeboSystem::read( for (unsigned int j = 0; j < this->dataPtr->sim_ft_sensors_.size(); j++) { auto sim_ft_sensor = this->dataPtr->sim_ft_sensors_[j]; - this->dataPtr->imu_sensor_data_[j][0] = sim_ft_sensor->Force().X(); - this->dataPtr->imu_sensor_data_[j][1] = sim_ft_sensor->Force().Y(); - this->dataPtr->imu_sensor_data_[j][2] = sim_ft_sensor->Force().Z(); + this->dataPtr->ft_sensor_data_[j][0] = sim_ft_sensor->Force().X(); + this->dataPtr->ft_sensor_data_[j][1] = sim_ft_sensor->Force().Y(); + this->dataPtr->ft_sensor_data_[j][2] = sim_ft_sensor->Force().Z(); - this->dataPtr->imu_sensor_data_[j][3] = sim_ft_sensor->Torque().X(); - this->dataPtr->imu_sensor_data_[j][4] = sim_ft_sensor->Torque().Y(); - this->dataPtr->imu_sensor_data_[j][5] = sim_ft_sensor->Torque().Z(); + this->dataPtr->ft_sensor_data_[j][3] = sim_ft_sensor->Torque().X(); + this->dataPtr->ft_sensor_data_[j][4] = sim_ft_sensor->Torque().Y(); + this->dataPtr->ft_sensor_data_[j][5] = sim_ft_sensor->Torque().Z(); } return hardware_interface::return_type::OK; }