Add ForceTorqueSensor parsing to gazebo_system

gazebo_ros2_control/src/gazebo_system.cpp

@@ -87,8 +87,17 @@ class gazebo_ros2_control::GazeboSystemPrivate
   /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration
   std::vector<std::array<double, 10>> imu_sensor_data_;
 
-  /// \brief The current effort forces applied to the joints
+  /// \brief The ROS2 Control state interface for the current imu readings
   std::vector<std::shared_ptr<hardware_interface::StateInterface>> imu_state_;
+
+  /// \brief handles to the FT sensors from within Gazebo
+  std::vector<gazebo::sensors::ForceTorqueSensorPtr> sim_ft_sensors_;
+
+  /// \brief An array per FT sensor for 3D force and torquee
+  std::vector<std::array<double, 6>> ft_sensor_data_;
+
+  /// \brief The ROS2 Control state interface for the current FT sensor readings
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> ft_sensor_state_;
 };
 
 namespace gazebo_ros2_control
@@ -208,10 +217,32 @@ void GazeboSystem::registerSensors(
   // Collect gazebo sensor handles
   size_t n_sensors = hardware_info.sensors.size();
   std::vector<hardware_interface::ComponentInfo> imu_components_;
+  std::vector<hardware_interface::ComponentInfo> ft_sensor_components_;
+
+  // This is split in two steps: Count the number and type of sensor and associate the interfaces
+  // So we have resize only once the structures where the data will be stored, and we can safely
+  // use pointers to the structures
   for (unsigned int j = 0; j < n_sensors; j++) {
     hardware_interface::ComponentInfo component = hardware_info.sensors[j];
     std::string sensor_name = component.name;
-    std::vector<std::string> gz_sensor_names = parent_model->SensorScopedName(sensor_name);
+
+    // This can't be used, because it only looks for sensor in links, but force_torque_sensor
+    // must be in a joint, as in not found by SensorScopedName
+    // std::vector<std::string> gz_sensor_names = parent_model->SensorScopedName(sensor_name);
+
+    // Workaround to find sensors whose parent is a link or joint of parent_model
+    std::vector<std::string> gz_sensor_names;
+    for (const auto & s : gazebo::sensors::SensorManager::Instance()->GetSensors()) {
+      const std::string sensor_parent = s->ParentName();
+      if (s->Name() != sensor_name) {
+        continue;
+      }
+      if ((parent_model->GetJoint(sensor_parent) != nullptr) ||
+        parent_model->GetLink(sensor_parent) != nullptr)
+      {
+        gz_sensor_names.push_back(s->ScopedName());
+      }
+    }
     if (gz_sensor_names.empty()) {
       RCLCPP_WARN_STREAM(
         this->nh_->get_logger(), "Skipping sensor in the URDF named '" << sensor_name <<
@@ -244,13 +275,23 @@ void GazeboSystem::registerSensors(
       }
       imu_components_.push_back(component);
       this->dataPtr->sim_imu_sensors_.push_back(imu_sensor);
+    } else if (simsensor->Type() == "force_torque") {
+      gazebo::sensors::ForceTorqueSensorPtr ft_sensor =
+        std::dynamic_pointer_cast<gazebo::sensors::ForceTorqueSensor>(simsensor);
+      if (!ft_sensor) {
+        RCLCPP_ERROR_STREAM(
+          this->nh_->get_logger(),
+          "Error retrieving casting sensor '" << sensor_name << " to ForceTorqueSensor");
+        continue;
+      }
+      ft_sensor_components_.push_back(component);
+      this->dataPtr->sim_ft_sensors_.push_back(ft_sensor);
     }
   }
 
-  // This is split in two steps: Count the number and type of sensor and associate the interfaces
-  // So we have resize only once the structures where the data will be stored, and we can safely
-  // use pointers to the structures
+
   this->dataPtr->imu_sensor_data_.resize(this->dataPtr->sim_imu_sensors_.size());
+  this->dataPtr->ft_sensor_data_.resize(this->dataPtr->sim_ft_sensors_.size());
 
   for (unsigned int i = 0; i < imu_components_.size(); i++) {
     const std::string & sensor_name = imu_components_[i].name;
@@ -278,6 +319,28 @@ void GazeboSystem::registerSensors(
           sensor_name, state_interface.name, &this->dataPtr->imu_sensor_data_[i][data_index]));
     }
   }
+  for (unsigned int i = 0; i < ft_sensor_components_.size(); i++) {
+    const std::string & sensor_name = ft_sensor_components_[i].name;
+    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << sensor_name);
+    RCLCPP_INFO_STREAM(
+      this->nh_->get_logger(), "\tState:");
+    for (const auto & state_interface : ft_sensor_components_[i].state_interfaces) {
+      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}
+      };
+      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->ft_sensor_state_.emplace_back(
+        std::make_shared<hardware_interface::StateInterface>(
+          sensor_name, state_interface.name, &this->dataPtr->ft_sensor_data_[i][data_index]));
+    }
+  }
 }
 
 hardware_interface::return_type
@@ -318,6 +381,9 @@ GazeboSystem::export_state_interfaces()
   for (unsigned int i = 0; i < this->dataPtr->imu_state_.size(); i++) {
     state_interfaces.emplace_back(*this->dataPtr->imu_state_[i]);
   }
+  for (unsigned int i = 0; i < this->dataPtr->ft_sensor_state_.size(); i++) {
+    state_interfaces.emplace_back(*this->dataPtr->ft_sensor_state_[i]);
+  }
   return state_interfaces;
 }
 
@@ -385,6 +451,17 @@ hardware_interface::return_type GazeboSystem::read()
     this->dataPtr->imu_sensor_data_[j][8] = sim_imu->LinearAcceleration().Y();
     this->dataPtr->imu_sensor_data_[j][9] = sim_imu->LinearAcceleration().Z();
   }
+
+  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->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();
+  }
   return hardware_interface::return_type::OK;
 }