From 63e4d805d0240642b3dc806ca4474e3b85aa643e Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 8 Apr 2024 20:59:49 -0400 Subject: [PATCH 1/4] started adding FTS info --- gz_ros2_control/src/gz_system.cpp | 77 +++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 519dfefa..6095c75a 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -46,6 +47,7 @@ #include #include +#include #include #include #include @@ -108,6 +110,25 @@ 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 IMU & _msg); // TODO +}; + class ImuData { public: @@ -159,6 +180,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_; @@ -462,8 +485,12 @@ void GazeboSimSystem::registerSensors( size_t n_sensors = hardware_info.sensors.size(); std::vector sensor_components_; + RCLCPP_WARN(this->nh_->get_logger(), "======================> RegisterSensors"); + RCLCPP_WARN(this->nh_->get_logger(), "======================> n_sensors: %lu", n_sensors); + for (unsigned int j = 0; j < n_sensors; j++) { hardware_interface::ComponentInfo component = hardware_info.sensors[j]; + RCLCPP_WARN(this->nh_->get_logger(), "======================> Sensor name: %s", hardware_info.sensors[j].name.c_str()); sensor_components_.push_back(component); } // This is split in two steps: Count the number and type of sensor and associate the interfaces @@ -522,6 +549,56 @@ 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 From e75025691f754d156052a4fe625f056cc8bec5be Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 09:44:06 -0400 Subject: [PATCH 2/4] Providing force torque sensor data to controller_manager --- gz_ros2_control/src/gz_system.cpp | 37 ++++++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 6095c75a..b324cac6 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -23,6 +23,7 @@ #ifdef GZ_HEADERS #include +#include #include #include @@ -44,6 +45,7 @@ #define GZ_MSGS_NAMESPACE gz::msgs:: #else #include +#include #include #include @@ -126,9 +128,19 @@ class ForceTorqueData std::array ft_sensor_data_; /// \brief callback to get the Force Torque topic values - //void OnForceTorque(const GZ_MSGS_NAMESPACE IMU & _msg); // TODO + void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); // TODO }; +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: @@ -485,12 +497,8 @@ void GazeboSimSystem::registerSensors( size_t n_sensors = hardware_info.sensors.size(); std::vector sensor_components_; - RCLCPP_WARN(this->nh_->get_logger(), "======================> RegisterSensors"); - RCLCPP_WARN(this->nh_->get_logger(), "======================> n_sensors: %lu", n_sensors); - for (unsigned int j = 0; j < n_sensors; j++) { hardware_interface::ComponentInfo component = hardware_info.sensors[j]; - RCLCPP_WARN(this->nh_->get_logger(), "======================> Sensor name: %s", hardware_info.sensors[j].name.c_str()); sensor_components_.push_back(component); } // This is split in two steps: Count the number and type of sensor and associate the interfaces @@ -598,7 +606,6 @@ void GazeboSimSystem::registerSensors( this->dataPtr->ft_sensors_.push_back(ftData); return true; }); - } CallbackReturn @@ -686,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; } From eee498d7eac889090c70383cc4f2acffc7d6a47d Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 16:45:30 -0400 Subject: [PATCH 3/4] Removed old TODO that was completed --- gz_ros2_control/src/gz_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index b324cac6..93de6a02 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -128,7 +128,7 @@ class ForceTorqueData std::array ft_sensor_data_; /// \brief callback to get the Force Torque topic values - void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); // TODO + void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); }; void ForceTorqueData::OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg) From c3d88c38d09d22be0f1222ca50234d4f4e5dd284 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 16:54:32 -0400 Subject: [PATCH 4/4] Indent spaces fix --- gz_ros2_control/src/gz_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 93de6a02..e0746959 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -574,7 +574,7 @@ void GazeboSimSystem::registerSensors( } RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "\tState:"); + this->nh_->get_logger(), "\tState:"); ftData->name = _name->Data(); ftData->sim_ft_sensors_ = _entity; @@ -701,7 +701,7 @@ hardware_interface::return_type GazeboSimSystem::read( if (sensorTopicComp) { this->dataPtr->ft_sensors_[i]->topicName = sensorTopicComp->Data(); RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << + this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << " has a topic name: " << sensorTopicComp->Data()); this->dataPtr->node.Subscribe(