Fix incorrect force-torque sensor vec population (#296) (#299)

(cherry picked from commit fdcd7aa)

Co-authored-by: Mateus Menezes <[email protected]>

Author: mergify[bot]

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;
 }