diff --git a/gz_ros2_control/include/gz_ros2_control/gz_system.hpp b/gz_ros2_control/include/gz_ros2_control/gz_system.hpp index c9ad741b..6b57f9d6 100644 --- a/gz_ros2_control/include/gz_ros2_control/gz_system.hpp +++ b/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 dataPtr; }; diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index b476fb50..2c80fc42 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/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( - this->dataPtr->controller_manager_); this->dataPtr->controller_manager_->read(sim_time_ros, sim_period); this->dataPtr->controller_manager_->update(sim_time_ros, sim_period); } diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 6975f530..1b23753a 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -88,6 +88,14 @@ struct MimicJoint std::vector interfaces_to_mimic; }; +struct GPIOData +{ + std::string name; + std::vector state_interface_names; + std::vector states; + std::vector mock_commands; +}; + class ForceTorqueData { public: @@ -165,6 +173,9 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief vector with the joint's names. std::vector joints_; + /// \brief + std::vector gpios_; + /// \brief vector with the imus . std::vector> 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; }