From b8b673c6c39bf28eb6cb1fb6f47a277f7e22c033 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Tue, 13 Aug 2024 12:07:42 +0200 Subject: [PATCH 1/3] register state and command interfaces for gpios --- gz_ros2_control/src/gz_system.cpp | 43 +++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) 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; } From 9db0ab921882f07fd3ca0d5bb3e3c273e09724d2 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Tue, 13 Aug 2024 15:36:05 +0200 Subject: [PATCH 2/3] remove unnecessary ptr cast --- gz_ros2_control/src/gz_ros2_control_plugin.cpp | 3 --- 1 file changed, 3 deletions(-) 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); } From ea14c9a52f1e40c30201bd97070d70f3ad736925 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Tue, 13 Aug 2024 16:58:47 +0200 Subject: [PATCH 3/3] refactor --- .../include/gz_ros2_control/gz_system.hpp | 8 +- gz_ros2_control/src/gz_system.cpp | 87 ++++++++++++------- 2 files changed, 62 insertions(+), 33 deletions(-) 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_system.cpp b/gz_ros2_control/src/gz_system.cpp index 89349dec..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: @@ -166,8 +174,7 @@ class gz_ros2_control::GazeboSimSystemPrivate std::vector joints_; /// \brief - std::vector gpio_states_; - std::vector gpio_commands_; + std::vector gpios_; /// \brief vector with the imus . std::vector> imus_; @@ -433,47 +440,58 @@ bool GazeboSimSystem::initSim( } registerSensors(hardware_info); + registerGPIOs(hardware_info); + return true; +} - // Register GPIOs +void GazeboSimSystem::registerGPIOs( + const hardware_interface::HardwareInfo & hardware_info) +{ size_t n_gpios = hardware_info.gpios.size(); - std::cout << "=======> GPIO size: "<< n_gpios << std::endl; - this->dataPtr->gpio_states_.resize(n_gpios); + this->dataPtr->gpios_.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); - + 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) { - 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); + // 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 interface - this->dataPtr->command_interfaces_.emplace_back( - component.name, - command_interface.name.c_str(), - &this->dataPtr->gpio_commands_[j]); - } + // 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]); + } } - - - return true; } void GazeboSimSystem::registerSensors( @@ -700,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; }