diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 6975f530..89349dec 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -165,6 +165,10 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief vector with the joint's names. std::vector joints_; + /// \brief + std::vector gpio_states_; + std::vector gpio_commands_; + /// \brief vector with the imus . std::vector> imus_; @@ -430,6 +434,45 @@ bool GazeboSimSystem::initSim( registerSensors(hardware_info); + + // Register GPIOs + size_t n_gpios = hardware_info.gpios.size(); + std::cout << "=======> GPIO size: "<< n_gpios << std::endl; + this->dataPtr->gpio_states_.resize(n_gpios); + + for (unsigned int j = 0; j < n_gpios; j++) { + hardware_interface::ComponentInfo component = hardware_info.gpios[j]; + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading GPIO: " << component.name); + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); + for (const auto & state_interface : component.state_interfaces) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); + + // get initial value + this->dataPtr->gpio_states_[j] = std::stod(state_interface.initial_value); + RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", this->dataPtr->gpio_states_[j]); + + // register state interface + this->dataPtr->state_interfaces_.emplace_back( + component.name, + state_interface.name.c_str(), + &this->dataPtr->gpio_states_[j]); + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); + for (const auto & command_interface : component.command_interfaces) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << command_interface.name); + + // register state interface + this->dataPtr->command_interfaces_.emplace_back( + component.name, + command_interface.name.c_str(), + &this->dataPtr->gpio_commands_[j]); + } + } + + return true; }