started adding FTS info

gz_ros2_control/src/gz_system.cpp

@@ -26,6 +26,7 @@
 
 #include <gz/sim/components/AngularVelocity.hh>
 #include <gz/sim/components/Imu.hh>
+#include <gz/sim/components/ForceTorque.hh>
 #include <gz/sim/components/JointForce.hh>
 #include <gz/sim/components/JointForceCmd.hh>
 #include <gz/sim/components/JointPosition.hh>
@@ -46,6 +47,7 @@
 
 #include <ignition/gazebo/components/AngularVelocity.hh>
 #include <ignition/gazebo/components/Imu.hh>
+#include <ignition/gazebo/components/ForceTorque.hh>
 #include <ignition/gazebo/components/JointForce.hh>
 #include <ignition/gazebo/components/JointForceCmd.hh>
 #include <ignition/gazebo/components/JointPosition.hh>
@@ -108,6 +110,25 @@ struct MimicJoint
   std::vector<std::string> interfaces_to_mimic;
 };
 
+class ForceTorqueData
+{
+public:
+  /// \brief imu's name.
+  std::string name{};
+
+  /// \brief imu's topic name.
+  std::string topicName{};
+
+  /// \brief handles to the force torque from within Gazebo
+  sim::Entity sim_ft_sensors_ = sim::kNullEntity;
+
+  /// \brief An array per FT
+  std::array<double, 6> ft_sensor_data_;
+
+  /// \brief callback to get the Force Torque topic values
+  //void OnForceTorque(const GZ_MSGS_NAMESPACE IMU & _msg); // TODO
+};
+
 class ImuData
 {
 public:
@@ -159,6 +180,8 @@ class gz_ros2_control::GazeboSimSystemPrivate
   /// \brief vector with the imus .
   std::vector<std::shared_ptr<ImuData>> imus_;
 
+  std::vector<std::shared_ptr<ForceTorqueData>> ft_sensors_;
+
   /// \brief state interfaces that will be exported to the Resource Manager
   std::vector<hardware_interface::StateInterface> state_interfaces_;
 
@@ -462,8 +485,12 @@ void GazeboSimSystem::registerSensors(
   size_t n_sensors = hardware_info.sensors.size();
   std::vector<hardware_interface::ComponentInfo> sensor_components_;
 
+  RCLCPP_WARN(this->nh_->get_logger(), "======================> RegisterSensors");
+  RCLCPP_WARN(this->nh_->get_logger(), "======================> n_sensors: %lu", n_sensors);
+
   for (unsigned int j = 0; j < n_sensors; j++) {
     hardware_interface::ComponentInfo component = hardware_info.sensors[j];
+    RCLCPP_WARN(this->nh_->get_logger(), "======================> Sensor name: %s", hardware_info.sensors[j].name.c_str());
     sensor_components_.push_back(component);
   }
   // This is split in two steps: Count the number and type of sensor and associate the interfaces
@@ -522,6 +549,56 @@ void GazeboSimSystem::registerSensors(
       this->dataPtr->imus_.push_back(imuData);
       return true;
     });
+
+  this->dataPtr->ecm->Each<sim::components::ForceTorque,
+    sim::components::Name>(
+    [&](const sim::Entity & _entity,
+    const sim::components::ForceTorque *,
+    const sim::components::Name * _name) -> bool
+    {
+      auto ftData = std::make_shared<ForceTorqueData>();
+      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data());
+
+      auto sensorTopicComp = this->dataPtr->ecm->Component<
+        sim::components::SensorTopic>(_entity);
+      if (sensorTopicComp) {
+        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data());
+      }
+
+      RCLCPP_INFO_STREAM(
+          this->nh_->get_logger(), "\tState:");
+      ftData->name = _name->Data();
+      ftData->sim_ft_sensors_ = _entity;
+
+      hardware_interface::ComponentInfo component;
+      for (auto & comp : sensor_components_) {
+        if (comp.name == _name->Data()) {
+          component = comp;
+        }
+      }
+
+      static const std::map<std::string, size_t> interface_name_map = {
+        {"force.x", 0},
+        {"force.y", 1},
+        {"force.z", 2},
+        {"torque.x", 3},
+        {"torque.y", 4},
+        {"torque.z", 5},
+      };
+
+      for (const auto & state_interface : component.state_interfaces) {
+        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name);
+
+        size_t data_index = interface_name_map.at(state_interface.name);
+        this->dataPtr->state_interfaces_.emplace_back(
+          ftData->name,
+          state_interface.name,
+          &ftData->ft_sensor_data_[data_index]);
+      }
+      this->dataPtr->ft_sensors_.push_back(ftData);
+      return true;
+    });
+
 }
 
 CallbackReturn