diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 519dfefa..e0746959 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -23,9 +23,11 @@ #ifdef GZ_HEADERS #include +#include #include #include +#include #include #include #include @@ -43,9 +45,11 @@ #define GZ_MSGS_NAMESPACE gz::msgs:: #else #include +#include #include #include +#include #include #include #include @@ -108,6 +112,35 @@ struct MimicJoint std::vector interfaces_to_mimic; }; +class ForceTorqueData +{ +public: + /// \brief imu's name. + std::string name{}; + + /// \brief imu's topic name. + std::string topicName{}; + + /// \brief handles to the force torque from within Gazebo + sim::Entity sim_ft_sensors_ = sim::kNullEntity; + + /// \brief An array per FT + std::array ft_sensor_data_; + + /// \brief callback to get the Force Torque topic values + void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); +}; + +void ForceTorqueData::OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg) +{ + this->ft_sensor_data_[0] = _msg.force().x(); + this->ft_sensor_data_[1] = _msg.force().y(); + this->ft_sensor_data_[2] = _msg.force().z(); + this->ft_sensor_data_[3] = _msg.torque().x(); + this->ft_sensor_data_[4] = _msg.torque().y(); + this->ft_sensor_data_[5] = _msg.torque().z(); +} + class ImuData { public: @@ -159,6 +192,8 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief vector with the imus . std::vector> imus_; + std::vector> ft_sensors_; + /// \brief state interfaces that will be exported to the Resource Manager std::vector state_interfaces_; @@ -522,6 +557,55 @@ void GazeboSimSystem::registerSensors( this->dataPtr->imus_.push_back(imuData); return true; }); + + this->dataPtr->ecm->Each( + [&](const sim::Entity & _entity, + const sim::components::ForceTorque *, + const sim::components::Name * _name) -> bool + { + auto ftData = std::make_shared(); + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(_entity); + if (sensorTopicComp) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\tState:"); + ftData->name = _name->Data(); + ftData->sim_ft_sensors_ = _entity; + + hardware_interface::ComponentInfo component; + for (auto & comp : sensor_components_) { + if (comp.name == _name->Data()) { + component = comp; + } + } + + static const std::map interface_name_map = { + {"force.x", 0}, + {"force.y", 1}, + {"force.z", 2}, + {"torque.x", 3}, + {"torque.y", 4}, + {"torque.z", 5}, + }; + + for (const auto & state_interface : component.state_interfaces) { + 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->state_interfaces_.emplace_back( + ftData->name, + state_interface.name, + &ftData->ft_sensor_data_[data_index]); + } + this->dataPtr->ft_sensors_.push_back(ftData); + return true; + }); } CallbackReturn @@ -609,6 +693,24 @@ hardware_interface::return_type GazeboSimSystem::read( } } } + + for (unsigned int i = 0; i < this->dataPtr->ft_sensors_.size(); ++i) { + if (this->dataPtr->ft_sensors_[i]->topicName.empty()) { + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(this->dataPtr->ft_sensors_[i]->sim_ft_sensors_); + if (sensorTopicComp) { + this->dataPtr->ft_sensors_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + + this->dataPtr->node.Subscribe( + this->dataPtr->ft_sensors_[i]->topicName, &ForceTorqueData::OnForceTorque, + this->dataPtr->ft_sensors_[i].get()); + } + } + } + return hardware_interface::return_type::OK; }