[WIP] Add gpio support in gz system

gz_ros2_control/include/gz_ros2_control/gz_system.hpp

@@ -82,10 +82,16 @@ class GazeboSimSystem : public GazeboSimSystemInterface
 private:
   // Register a sensor (for now just IMUs)
   // \param[in] hardware_info hardware information where the data of
-  // the sensors is extract.
+  // the sensors is extracted.
   void registerSensors(
     const hardware_interface::HardwareInfo & hardware_info);
 
+  // Register GPIOs (for now, only mimics command to state)
+  // \param[in] hardware_info hardware information where the data of
+  // the GPIOs is extracted.
+  void registerGPIOs(
+    const hardware_interface::HardwareInfo & hardware_info);
+
   /// \brief Private data class
   std::unique_ptr<GazeboSimSystemPrivate> dataPtr;
 };

gz_ros2_control/src/gz_ros2_control_plugin.cpp

@@ -485,9 +485,6 @@ void GazeboSimROS2ControlPlugin::PostUpdate(
 
   if (sim_period >= this->dataPtr->control_period_) {
     this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;
-    auto gz_controller_manager =
-      std::dynamic_pointer_cast<gz_ros2_control::GazeboSimSystemInterface>(
-      this->dataPtr->controller_manager_);
     this->dataPtr->controller_manager_->read(sim_time_ros, sim_period);
     this->dataPtr->controller_manager_->update(sim_time_ros, sim_period);
   }

gz_ros2_control/src/gz_system.cpp

@@ -88,6 +88,14 @@ struct MimicJoint
   std::vector<std::string> interfaces_to_mimic;
 };
 
+struct GPIOData
+{
+  std::string name;
+  std::vector<std::string> state_interface_names;
+  std::vector<double> states;
+  std::vector<double> mock_commands;
+};
+
 class ForceTorqueData
 {
 public:
@@ -165,6 +173,9 @@ class gz_ros2_control::GazeboSimSystemPrivate
   /// \brief vector with the joint's names.
   std::vector<struct jointData> joints_;
 
+  /// \brief
+  std::vector<struct GPIOData> gpios_;
+
   /// \brief vector with the imus .
   std::vector<std::shared_ptr<ImuData>> imus_;
 
@@ -429,10 +440,60 @@ bool GazeboSimSystem::initSim(
   }
 
   registerSensors(hardware_info);
+  registerGPIOs(hardware_info);
 
   return true;
 }
 
+void GazeboSimSystem::registerGPIOs(
+  const hardware_interface::HardwareInfo & hardware_info)
+{
+  size_t n_gpios = hardware_info.gpios.size();
+  this->dataPtr->gpios_.resize(n_gpios);
+
+  for (unsigned int j = 0; j < n_gpios; j++) {
+    hardware_interface::ComponentInfo component = hardware_info.gpios[j];
+
+    this->dataPtr->gpios_[j].name = component.name;
+    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading GPIO: " << this->dataPtr->gpios_[j].name);
+
+    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
+    for (const auto & state_interface : component.state_interfaces) {
+      // get interface name
+      auto state_interface_name = state_interface.name;
+      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name);
+      this->dataPtr->gpios_[j].state_interface_names.push_back(state_interface_name);
+      // get initial value
+      double initial_value = 0.0;
+      if(!state_interface.initial_value.empty()){
+        initial_value = hardware_interface::stod(state_interface.initial_value);
+        RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value);
+      }      
+      this->dataPtr->gpios_[j].states.push_back(initial_value);  
+    }
+
+    // register state interfaces
+    auto n_state_interfaces = this->dataPtr->gpios_[j].state_interface_names.size();
+    for (size_t i = 0; i < n_state_interfaces; i++)
+      this->dataPtr->state_interfaces_.emplace_back(
+        this->dataPtr->gpios_[j].name,
+        this->dataPtr->gpios_[j].state_interface_names[i],
+        &this->dataPtr->gpios_[j].states[i]); 
+
+    // register command interfaces
+    this->dataPtr->gpios_[j].mock_commands.resize(n_state_interfaces);
+    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand: (mocked)");
+    for (size_t i = 0; i < n_state_interfaces; i++){
+      // mock gpio - register command interfaces under the same names as state
+      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << this->dataPtr->gpios_[j].state_interface_names[i]);
+      this->dataPtr->command_interfaces_.emplace_back(
+        this->dataPtr->gpios_[j].name,
+        this->dataPtr->gpios_[j].state_interface_names[i],
+        &this->dataPtr->gpios_[j].mock_commands[i]); 
+    }    
+  }
+}
+
 void GazeboSimSystem::registerSensors(
   const hardware_interface::HardwareInfo & hardware_info)
 {
@@ -657,6 +718,11 @@ hardware_interface::return_type GazeboSimSystem::read(
     }
   }
 
+  // mirror gpio commands to states
+  for (unsigned int i = 0; i < this->dataPtr->gpios_.size(); ++i) {
+    this->dataPtr->gpios_[i].states = this->dataPtr->gpios_[i].mock_commands;
+  }
+
   return hardware_interface::return_type::OK;
 }