diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index f1c70f02..4cb78d40 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -87,8 +87,17 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration std::vector> imu_sensor_data_; - /// \brief The current effort forces applied to the joints + /// \brief The ROS2 Control state interface for the current imu readings std::vector> imu_state_; + + /// \brief handles to the FT sensors from within Gazebo + std::vector sim_ft_sensors_; + + /// \brief An array per FT sensor for 3D force and torquee + std::vector> ft_sensor_data_; + + /// \brief The ROS2 Control state interface for the current FT sensor readings + std::vector> ft_sensor_state_; }; namespace gazebo_ros2_control @@ -208,10 +217,32 @@ void GazeboSystem::registerSensors( // Collect gazebo sensor handles size_t n_sensors = hardware_info.sensors.size(); std::vector imu_components_; + std::vector ft_sensor_components_; + + // This is split in two steps: Count the number and type of sensor and associate the interfaces + // So we have resize only once the structures where the data will be stored, and we can safely + // use pointers to the structures for (unsigned int j = 0; j < n_sensors; j++) { hardware_interface::ComponentInfo component = hardware_info.sensors[j]; std::string sensor_name = component.name; - std::vector gz_sensor_names = parent_model->SensorScopedName(sensor_name); + + // This can't be used, because it only looks for sensor in links, but force_torque_sensor + // must be in a joint, as in not found by SensorScopedName + // std::vector gz_sensor_names = parent_model->SensorScopedName(sensor_name); + + // Workaround to find sensors whose parent is a link or joint of parent_model + std::vector gz_sensor_names; + for (const auto & s : gazebo::sensors::SensorManager::Instance()->GetSensors()) { + const std::string sensor_parent = s->ParentName(); + if (s->Name() != sensor_name) { + continue; + } + if ((parent_model->GetJoint(sensor_parent) != nullptr) || + parent_model->GetLink(sensor_parent) != nullptr) + { + gz_sensor_names.push_back(s->ScopedName()); + } + } if (gz_sensor_names.empty()) { RCLCPP_WARN_STREAM( this->nh_->get_logger(), "Skipping sensor in the URDF named '" << sensor_name << @@ -244,13 +275,23 @@ void GazeboSystem::registerSensors( } imu_components_.push_back(component); this->dataPtr->sim_imu_sensors_.push_back(imu_sensor); + } else if (simsensor->Type() == "force_torque") { + gazebo::sensors::ForceTorqueSensorPtr ft_sensor = + std::dynamic_pointer_cast(simsensor); + if (!ft_sensor) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "Error retrieving casting sensor '" << sensor_name << " to ForceTorqueSensor"); + continue; + } + ft_sensor_components_.push_back(component); + this->dataPtr->sim_ft_sensors_.push_back(ft_sensor); } } - // This is split in two steps: Count the number and type of sensor and associate the interfaces - // So we have resize only once the structures where the data will be stored, and we can safely - // use pointers to the structures + this->dataPtr->imu_sensor_data_.resize(this->dataPtr->sim_imu_sensors_.size()); + this->dataPtr->ft_sensor_data_.resize(this->dataPtr->sim_ft_sensors_.size()); for (unsigned int i = 0; i < imu_components_.size(); i++) { const std::string & sensor_name = imu_components_[i].name; @@ -278,6 +319,28 @@ void GazeboSystem::registerSensors( sensor_name, state_interface.name, &this->dataPtr->imu_sensor_data_[i][data_index])); } } + for (unsigned int i = 0; i < ft_sensor_components_.size(); i++) { + const std::string & sensor_name = ft_sensor_components_[i].name; + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << sensor_name); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\tState:"); + for (const auto & state_interface : ft_sensor_components_[i].state_interfaces) { + 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} + }; + 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->ft_sensor_state_.emplace_back( + std::make_shared( + sensor_name, state_interface.name, &this->dataPtr->ft_sensor_data_[i][data_index])); + } + } } hardware_interface::return_type @@ -318,6 +381,9 @@ GazeboSystem::export_state_interfaces() for (unsigned int i = 0; i < this->dataPtr->imu_state_.size(); i++) { state_interfaces.emplace_back(*this->dataPtr->imu_state_[i]); } + for (unsigned int i = 0; i < this->dataPtr->ft_sensor_state_.size(); i++) { + state_interfaces.emplace_back(*this->dataPtr->ft_sensor_state_[i]); + } return state_interfaces; } @@ -385,6 +451,17 @@ hardware_interface::return_type GazeboSystem::read() this->dataPtr->imu_sensor_data_[j][8] = sim_imu->LinearAcceleration().Y(); this->dataPtr->imu_sensor_data_[j][9] = sim_imu->LinearAcceleration().Z(); } + + for (unsigned int j = 0; j < this->dataPtr->sim_ft_sensors_.size(); j++) { + auto sim_ft_sensor = this->dataPtr->sim_ft_sensors_[j]; + this->dataPtr->imu_sensor_data_[j][0] = sim_ft_sensor->Force().X(); + this->dataPtr->imu_sensor_data_[j][1] = sim_ft_sensor->Force().Y(); + this->dataPtr->imu_sensor_data_[j][2] = sim_ft_sensor->Force().Z(); + + this->dataPtr->imu_sensor_data_[j][3] = sim_ft_sensor->Torque().X(); + this->dataPtr->imu_sensor_data_[j][4] = sim_ft_sensor->Torque().Y(); + this->dataPtr->imu_sensor_data_[j][5] = sim_ft_sensor->Torque().Z(); + } return hardware_interface::return_type::OK; }