diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index bf3d6f86f7..08a900120f 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -1,4 +1,17 @@ + @@ -38,7 +51,8 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - -6 0 6 0 0.5 0 + 0.35 0.23 0.94 0 0.05 -2.53 + @@ -196,14 +210,16 @@ - true + false true - 15 - true + optical_tactile_plugin true + true + true + 15 0.01 0.001 @@ -223,7 +239,7 @@ 0 -0.7 0 0 0 3.14 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue - + -1.5 0 0 0 0 0 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 8885973829..492666b0ec 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include #include @@ -55,8 +56,8 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: void Load(const EntityComponentManager &_ecm); /// \brief Actual function that enables the plugin. - /// \param[in] _value True to enable plugin. - public: void Enable(const bool _value); + /// \param[in] _req Whether to enable the plugin or disable it. + public: void Enable(const ignition::msgs::Boolean &_req); /// \brief Callback for the depth camera /// \param[in] _msg Message from the subscribed topic @@ -98,6 +99,9 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Whether to visualize the normal forces. public: bool visualizeForces{false}; + /// \brief Whether to visualize the contacts. + public: bool visualizeContacts{false}; + /// \brief Model interface. public: Model model{kNullEntity}; @@ -112,7 +116,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: ignition::gazebo::Entity objectCollisionEntity; /// \brief Whether the plugin is enabled. - public: bool enabled{true}; + public: std::atomic enabled{true}; /// \brief Initialization flag. public: bool initialized{false}; @@ -150,7 +154,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: bool visualizeSensor{false}; /// \brief Size of the contact sensor - public: ignition::math::Vector3d sensorSize; + public: ignition::math::Vector3d sensorSize{0.005, 0.02, 0.02}; /// \brief Extended sensing distance. The sensor will output data coming from /// its volume plus this distance. @@ -164,6 +168,12 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Flag for allowing the plugin to output error/warning only once public: bool initErrorPrinted{false}; + + /// \brief Normal forces publisher + public: ignition::transport::Node::Publisher normalForcesPub; + + /// \brief Namespace for transport topics + public: std::string ns{"optical_tactile_sensor"}; }; ////////////////////////////////////////////////// @@ -228,6 +238,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->visualizeForces = _sdf->Get("visualize_forces"); } + if (!_sdf->HasElement("visualize_contacts")) + { + igndbg << "Missing parameter , " + << "setting to " << this->dataPtr->visualizeContacts << std::endl; + } + else + { + this->dataPtr->visualizeContacts = _sdf->Get("visualize_contacts"); + } + if (!_sdf->HasElement("extended_sensing")) { igndbg << "Missing parameter , " @@ -273,6 +293,73 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->forceLength = _sdf->Get("force_length"); } } + + if (!_sdf->HasElement("namespace")) + { + igndbg << "Missing parameter , " + << "setting to " << this->dataPtr->ns << std::endl; + } + else + { + this->dataPtr->ns = _sdf->Get("namespace"); + } + + // Get the size of the sensor from the SDF + // If there's no specified inside , a default one + // is set + if (_sdf->GetParent() != nullptr) + { + if (_sdf->GetParent()->GetElement("link") != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision") + != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry") != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box") != nullptr) + { + this->dataPtr->sensorSize = + _sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box")-> + Get("size"); + igndbg << "Setting sensor size to box collision size: [" + << this->dataPtr->sensorSize << "]" << std::endl; + } + } + } + } + } + + // Advertise topic for normal forces + std::string normalForcesTopic = "/" + this->dataPtr->ns + "/normal_forces"; + this->dataPtr->normalForcesPub = + this->dataPtr->node.Advertise(normalForcesTopic); + if (!this->dataPtr->normalForcesPub) + { + ignerr << "Error advertising topic [" << normalForcesTopic << "]" + << std::endl; + } + else + { + ignmsg << "Topic publishing normal forces [" << normalForcesTopic << "]" + << std::endl; + } + + // Advertise enabling service + std::string enableService = "/" + this->dataPtr->ns + "/enable"; + if (!this->dataPtr->node.Advertise(enableService, + &OpticalTactilePluginPrivate::Enable, this->dataPtr.get())) + { + ignerr << "Error advertising service [" << enableService << "]" + << std::endl; + } + else + { + ignmsg << "Service to enable tactile sensor [" << enableService << "]" + << std::endl; + } } ////////////////////////////////////////////////// @@ -282,7 +369,7 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, IGN_PROFILE("OpticalTactilePlugin::PreUpdate"); // Nothing left to do if paused - if (_info.paused) + if (_info.paused || !this->dataPtr->enabled) return; if (!this->dataPtr->initialized) @@ -327,15 +414,26 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void OpticalTactilePlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) + const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("OpticalTactilePlugin::PostUpdate"); // Nothing left to do if paused or failed to initialize. - if (_info.paused || !this->dataPtr->initialized) + if (_info.paused || !this->dataPtr->initialized || !this->dataPtr->enabled) return; // TODO(anyone) Get ContactSensor data and merge it with DepthCamera data + if (this->dataPtr->visualizeContacts) + { + auto *contacts = + _ecm.Component( + this->dataPtr->sensorCollisionEntity); + + if (nullptr != contacts) + { + this->dataPtr->visualizePtr->RequestContactsMarkerMsg(contacts); + } + } // Process camera message if it's new { @@ -524,9 +622,20 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool /*_value*/) +void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req) { - // todo(mcres) Implement method + if (_req.data() != this->enabled) + { + ignmsg << "Enabling optical tactile sensor with namespace [" << this->ns + << "]: " << _req.data() << std::endl; + } + + this->enabled = _req.data(); + + if (!_req.data()) + { + this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); + } } ////////////////////////////////////////////////// @@ -535,7 +644,7 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( { // This check avoids running the callback at t=0 and getting // unexpected markers in the scene - if (!this->initialized) + if (!this->initialized || !this->enabled) return; // Check whether DepthCamera returns FLOAT32 data @@ -652,6 +761,17 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Declare variables for storing the XYZ points ignition::math::Vector3f p1, p2, p3, p4, markerPosition; + // Message for publishing normal forces + ignition::msgs::Image normalsMsg; + normalsMsg.set_width(_msg.width()); + normalsMsg.set_height(_msg.height()); + normalsMsg.set_step(3 * sizeof(float) * _msg.width()); + normalsMsg.set_pixel_format_type(ignition::msgs::PixelFormatType::R_FLOAT32); + + uint32_t bufferSize = 3 * sizeof(float) * _msg.width() * _msg.height(); + std::shared_ptr normalForcesBuffer(new char[bufferSize]); + uint32_t bufferIndex; + // Marker messages representing the normal forces ignition::msgs::Marker positionMarkerMsg; ignition::msgs::Marker forceMarkerMsg; @@ -682,8 +802,15 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // https://github.com/ignitionrobotics/ign-math/issues/144 ignition::math::Vector3f normalForce = direction.Normalized(); - // todo(mcres) Normal forces are computed even if visualization - // is turned off. These forces should be published in the future. + // Add force to buffer + // Forces buffer is composed of XYZ coordinates, while _msg buffer is + // made up of XYZRGB values + bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); + normalForcesBuffer.get()[bufferIndex] = normalForce.X(); + normalForcesBuffer.get()[bufferIndex + sizeof(float)] = normalForce.Y(); + normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = + normalForce.Z(); + if (!_visualizeForces) continue; @@ -694,6 +821,11 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( } } + // Publish message + normalsMsg.set_data(normalForcesBuffer.get(), + 3 * sizeof(float) * _msg.width() * _msg.height()); + this->normalForcesPub.Publish(normalsMsg); + if (_visualizeForces) { this->visualizePtr->RequestNormalForcesMarkerMsgs(positionMarkerMsg, diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index fbefd9e05a..57cc5aa008 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -32,73 +32,95 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - // Forward declaration - class OpticalTactilePluginPrivate; + // Forward declaration + class OpticalTactilePluginPrivate; - /// \brief Plugin that implements an optical tactile sensor - /// - /// It requires that contact sensor and depth camera be placed in at least - /// one link on the model on which this plugin is attached. - /// - /// Parameters: - /// - /// (todo) Set this to true so the plugin works from the start and - /// doesn't need to be enabled. This element is optional, and the - /// default value is true. - /// - /// Number n of pixels to skip when visualizing - /// the forces. One vector representing a normal force is computed for - /// every nth pixel. This element must be positive and it is optional. - /// The default value is 30 - /// - /// Set this to true so the plugin visualizes the normal - /// forces in the 3D world. This element is optional, and the - /// default value is false. - /// - /// Length in meters of the forces visualized if - /// is set to true. This parameter is optional, and the - /// default value is 0.01. - /// - /// Extended sensing distance in meters. The sensor will - /// output data coming from its collision geometry plus this distance. This - /// element is optional, and the default value is 0.001. - /// - /// Whether to visualize the sensor or not. This element - /// is optional, and the default value is false. + /// \brief Plugin that implements an optical tactile sensor + /// + /// It requires that contact sensor and depth camera be placed in at least + /// one link on the model on which this plugin is attached. + /// TODO: + /// Currently, the contacts returned from the physics engine (which tends to + /// be sparse) and the normal forces separately computed from the depth + /// camera (which is dense, resolution adjustable) are disjoint. It is + /// left as future work to combine the two sets of data. + /// + /// Parameters: + /// + /// Set this to true so the plugin works from the start and + /// doesn't need to be enabled. This element is optional, and the + /// default value is true. + /// + /// Namespace for transport topics/services. If there are more + /// than one optical tactile plugins, their namespaces should + /// be different. + /// This element is optional, and the default value is + /// "optical_tactile_sensor". + /// //enable : Service used to enable and disable the + /// plugin. + /// //normal_forces : Topic where a message is + /// published each time the + /// normal forces are computed + /// + /// Number n of pixels to skip when visualizing + /// the forces. One vector representing a normal + /// force is computed for every nth pixel. This + /// element must be positive and it is optional. + /// The default value is 30. + /// + /// Length in meters of the forces visualized if + /// is set to true. This parameter is + /// optional, and the default value is 0.01. + /// + /// Extended sensing distance in meters. The sensor will + /// output data coming from its collision geometry plus + /// this distance. This element is optional, and the + /// default value is 0.001. + /// + /// Whether to visualize the sensor or not. This element + /// is optional, and the default value is false. + /// + /// Whether to visualize the contacts from the contact + /// sensor based on physics. This element is optional, + /// and the default value is false. + /// + /// Whether to visualize normal forces computed from the + /// depth camera. This element is optional, and the + /// default value is false. - class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : - public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate - { - /// \brief Constructor - public: OpticalTactilePlugin(); + class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: OpticalTactilePlugin(); - /// \brief Destructor - public: ~OpticalTactilePlugin() override = default; + /// \brief Destructor + public: ~OpticalTactilePlugin() override = default; - // Documentation inherited - public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; - /// Documentation inherited - public: void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; - // Documentation inherited - public: void PostUpdate( + // Documentation inherited + public: void PostUpdate( const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) override; - /// \brief Private data pointer - private: std::unique_ptr dataPtr; - }; -} -} -} -} + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition #endif diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 0610f97479..16c5664a38 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -15,21 +15,16 @@ * */ +#include +#include #include #include "Visualization.hh" -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ -namespace systems -{ -namespace optical_tactile_sensor -{ +using namespace ignition; +using namespace gazebo; +using namespace systems; +using namespace optical_tactile_sensor; ////////////////////////////////////////////////// OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( @@ -52,8 +47,10 @@ OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( void OpticalTactilePluginVisualization::InitializeSensorMarkerMsg( ignition::msgs::Marker &_sensorMarkerMsg) { - // Initialize the marker for visualizing the sensor as a grey transparent box + // Reset all fields + _sensorMarkerMsg = ignition::msgs::Marker(); + // Initialize the marker for visualizing the sensor as a grey transparent box _sensorMarkerMsg.set_ns("sensor_" + this->modelName); _sensorMarkerMsg.set_id(1); _sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); @@ -85,16 +82,73 @@ void OpticalTactilePluginVisualization::RequestSensorMarkerMsg( this->node.Request("/marker", sensorMarkerMsg); } +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::InitializeContactsMarkerMsg( + ignition::msgs::Marker &_contactsMarkerMsg) +{ + // Reset all fields + _contactsMarkerMsg = ignition::msgs::Marker(); + + // Initialize the marker for visualizing the physical contacts as red lines + _contactsMarkerMsg.set_ns("contacts_" + this->modelName); + _contactsMarkerMsg.set_id(1); + _contactsMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + _contactsMarkerMsg.set_type(ignition::msgs::Marker::LINE_LIST); + _contactsMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + ignition::msgs::Set(_contactsMarkerMsg.mutable_material()-> + mutable_ambient(), math::Color(1, 0, 0, 1)); + ignition::msgs::Set(_contactsMarkerMsg.mutable_material()-> + mutable_diffuse(), math::Color(1, 0, 0, 1)); + _contactsMarkerMsg.mutable_lifetime()->set_sec(1); +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::AddContactToMarkerMsg( + ignition::msgs::Contact const &_contact, + ignition::msgs::Marker &_contactMarkerMsg) +{ + // todo(anyone) once available, use normal field in the Contact message + ignition::math::Vector3d contactNormal(0, 0, 0.03); + + // For each contact, add a line marker starting from the contact position, + // ending at the endpoint of the normal. + for (auto const &position : _contact.position()) + { + ignition::math::Vector3d startPoint = ignition::msgs::Convert(position); + ignition::math::Vector3d endPoint = startPoint + contactNormal; + + ignition::msgs::Set(_contactMarkerMsg.add_point(), startPoint); + ignition::msgs::Set(_contactMarkerMsg.add_point(), endPoint); + } +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RequestContactsMarkerMsg( + const components::ContactSensorData *_contacts) +{ + ignition::msgs::Marker contactsMarkerMsg; + this->InitializeContactsMarkerMsg(contactsMarkerMsg); + + for (const auto &contact : _contacts->Data().contact()) + { + this->AddContactToMarkerMsg(contact, contactsMarkerMsg); + } + + this->node.Request("/marker", contactsMarkerMsg); +} + ////////////////////////////////////////////////// void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg) { - // Initialize marker messages for position and force of the contacts + _positionMarkerMsg = ignition::msgs::Marker(); + _forceMarkerMsg = ignition::msgs::Marker(); - // Blue points for positions - // Green lines for forces + // Initialize marker messages for position and force of the contacts + // Positions computed from camera _positionMarkerMsg.set_ns("positions_" + this->modelName); _positionMarkerMsg.set_id(1); _positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); @@ -110,6 +164,7 @@ void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( _forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); // Set material properties and lifetime + // Blue points for positions ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> mutable_ambient(), math::Color(0, 0, 1, 1)); ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> @@ -117,6 +172,7 @@ void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( _positionMarkerMsg.mutable_lifetime()->set_nsec( this->cameraUpdateRate * 1000000000); + // Green lines for forces ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> mutable_ambient(), math::Color(0, 1, 0, 1)); ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> @@ -157,12 +213,12 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs( (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose; normalForcePoseFromWorld.Correct(); - // Get the first point of the normal force - ignition::math::Vector3f firstPointFromWorld = + // Get the start point of the normal force + ignition::math::Vector3f startPointFromWorld = normalForcePoseFromWorld.Pos(); // Move the normal force pose a distance of forceLength along the direction - // of _normalForce and get the second point + // of _normalForce and get the end point normalForcePoseFromSensor.Set(normalForcePositionFromSensor + _normalForce * this->forceLength, normalForceOrientationFromSensor); @@ -170,27 +226,25 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs( (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose; normalForcePoseFromWorld.Correct(); - ignition::math::Vector3f secondPointFromWorld = + ignition::math::Vector3f endPointFromWorld = normalForcePoseFromWorld.Pos(); // Check invalid points to avoid data transfer overhead - if (firstPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0) || - secondPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0)) + if (abs(startPointFromWorld.Distance(endPointFromWorld)) < 1e-6) return; - // Add points to the messages - + // Position ignition::msgs::Set(_positionMarkerMsg.add_point(), - ignition::math::Vector3d(firstPointFromWorld.X(), - firstPointFromWorld.Y(), firstPointFromWorld.Z())); + ignition::math::Vector3d(startPointFromWorld.X(), + startPointFromWorld.Y(), startPointFromWorld.Z())); + // Normal ignition::msgs::Set(_forceMarkerMsg.add_point(), - ignition::math::Vector3d(firstPointFromWorld.X(), - firstPointFromWorld.Y(), firstPointFromWorld.Z())); - + ignition::math::Vector3d(startPointFromWorld.X(), + startPointFromWorld.Y(), startPointFromWorld.Z())); ignition::msgs::Set(_forceMarkerMsg.add_point(), - ignition::math::Vector3d(secondPointFromWorld.X(), - secondPointFromWorld.Y(), secondPointFromWorld.Z())); + ignition::math::Vector3d(endPointFromWorld.X(), + endPointFromWorld.Y(), endPointFromWorld.Z())); } ////////////////////////////////////////////////// @@ -198,15 +252,30 @@ void OpticalTactilePluginVisualization::RequestNormalForcesMarkerMsgs( ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg) { - this->node.Request("/marker", _forceMarkerMsg); this->node.Request("/marker", _positionMarkerMsg); + this->node.Request("/marker", _forceMarkerMsg); // Let the messages be initialized again this->normalForcesMsgsAreInitialized = false; } -} -} -} -} +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RemoveNormalForcesAndContactsMarkers() +{ + ignition::msgs::Marker positionMarkerMsg; + ignition::msgs::Marker forceMarkerMsg; + ignition::msgs::Marker contactMarkerMsg; + + positionMarkerMsg.set_ns("positions_" + this->modelName); + positionMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + + forceMarkerMsg.set_ns("forces_" + this->modelName); + forceMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + + contactMarkerMsg.set_ns("contacts_" + this->modelName); + contactMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + + this->node.Request("/marker", positionMarkerMsg); + this->node.Request("/marker", forceMarkerMsg); + this->node.Request("/marker", contactMarkerMsg); } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index c1dbfb0b13..5b48268e10 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -24,6 +24,9 @@ #include #include #include +#include + +#include "ignition/gazebo/components/ContactSensorData.hh" namespace ignition { @@ -37,7 +40,12 @@ namespace systems namespace optical_tactile_sensor { // This class handles the configuration and process of visualizing the - // different elements needed for the Optical Tactile Sensor plugin + // different elements needed for the Optical Tactile Sensor plugin. + // Terminology: + // "Contacts" refers to data from the contact sensor based on physics. + // "Normal forces" refers to post-processed data from the depth camera based + // purely on imagery. + // These two sets of data are currently disjoint and visualized separately. class OpticalTactilePluginVisualization { /// \brief Constructor @@ -49,25 +57,44 @@ namespace optical_tactile_sensor /// \param[in] _visualizationResolution Value of the /// visualizationResolution attribute public: OpticalTactilePluginVisualization( - std::string &_modelName, - ignition::math::Vector3d &_sensorSize, - double &_forceLength, - float &_cameraUpdateRate, - ignition::math::Pose3f &_depthCameraOffset, - int &_visualizationResolution); + std::string &_modelName, + ignition::math::Vector3d &_sensorSize, + double &_forceLength, + float &_cameraUpdateRate, + ignition::math::Pose3f &_depthCameraOffset, + int &_visualizationResolution); /// \brief Initialize the marker message representing the optical tactile /// sensor /// \param[out] _sensorMarkerMsg Message for visualizing the sensor private: void InitializeSensorMarkerMsg( - ignition::msgs::Marker &_sensorMarkerMsg); + ignition::msgs::Marker &_sensorMarkerMsg); /// \brief Request the "/marker" service for the sensor marker. /// This can be helpful when debbuging, given that there shouldn't be a /// visual tag in the plugin's model /// \param[in] _sensorPose Pose of the optical tactile sensor public: void RequestSensorMarkerMsg( - ignition::math::Pose3f const &_sensorPose); + ignition::math::Pose3f const &_sensorPose); + + /// \brief Initialize the marker message representing the contacts from the + /// contact sensor + /// \param[out] _contactsMarkerMsg Message for visualizing the contacts + private: void InitializeContactsMarkerMsg( + ignition::msgs::Marker &_contactsMarkerMsg); + + /// \brief Add a contact to the marker message representing the contacts + /// from the contact sensor based on physics + /// \param[in] _contact Contact to be added + /// \param[out] _contactsMarkerMsg Message for visualizing the contacts + public: void AddContactToMarkerMsg( + ignition::msgs::Contact const &_contact, + ignition::msgs::Marker &_contactsMarkerMsg); + + /// \brief Request the "/marker" service for the contacts marker. + /// \param[in] _contacts Contacts to visualize + public: void RequestContactsMarkerMsg( + components::ContactSensorData const *_contacts); /// \brief Initialize the marker messages representing the normal forces /// \param[out] _positionMarkerMsg Message for visualizing the contact @@ -75,11 +102,11 @@ namespace optical_tactile_sensor /// \param[out] _forceMarkerMsg Message for visualizing the contact /// normal forces private: void InitializeNormalForcesMarkerMsgs( - ignition::msgs::Marker &_positionMarkerMsg, - ignition::msgs::Marker &_forceMarkerMsg); + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg); - /// \brief Add a normal force to the marker messages representing the - /// normal forces computed + /// \brief Create a marker messages representing the normal force computed + /// from depth camera /// \param[out] _positionMarkerMsg Message for visualizing the contact /// positions /// \param[out] _forceMarkerMsg Message for visualizing the contact @@ -91,11 +118,11 @@ namespace optical_tactile_sensor /// \param[in] _sensorWorldPose Pose of the plugin's model referenced to /// the world's origin public: void AddNormalForceToMarkerMsgs( - ignition::msgs::Marker &_positionMarkerMsg, - ignition::msgs::Marker &_forceMarkerMsg, - ignition::math::Vector3f &_position, - ignition::math::Vector3f &_normalForce, - ignition::math::Pose3f &_sensorWorldPose); + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg, + ignition::math::Vector3f &_position, + ignition::math::Vector3f &_normalForce, + ignition::math::Pose3f &_sensorWorldPose); /// \brief Request the "/marker" service for the marker messages /// representing the normal forces @@ -104,8 +131,11 @@ namespace optical_tactile_sensor /// \param[in] _forceMarkerMsg Message for visualizing the contact /// normal forces public: void RequestNormalForcesMarkerMsgs( - ignition::msgs::Marker &_positionMarkerMsg, - ignition::msgs::Marker &_forceMarkerMsg); + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg); + + /// \brief Remove all normal forces and contact markers + public: void RemoveNormalForcesAndContactsMarkers(); /// \brief Transport node to request the /marker service private: ignition::transport::Node node; @@ -131,10 +161,10 @@ namespace optical_tactile_sensor /// \brief Whether the normal forces messages are initialized or not private: bool normalForcesMsgsAreInitialized{false}; }; -} -} -} -} -} +} // namespace optical_tactile_sensor +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition #endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index c423de553c..d3c20875e3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -62,6 +62,7 @@ set(tests_needing_display camera_video_record_system.cc depth_camera.cc gpu_lidar.cc + optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc thermal_system.cc diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc new file mode 100644 index 0000000000..ea6ec02e85 --- /dev/null +++ b/test/integration/optical_tactile_plugin.cc @@ -0,0 +1,178 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "plugins/MockSystem.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test OpticalTactilePlugin system +class OpticalTactilePluginTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + + public: void StartServer(const std::string &_sdfFile) + { + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + server = std::make_unique(serverConfig); + using namespace std::chrono_literals; + server->SetUpdatePeriod(0ns); + + EXPECT_FALSE(server->Running()); + EXPECT_FALSE(*server->Running(0)); + } + + public: ignition::math::Vector3f MapPointCloudData( + const uint64_t &_i, + const uint64_t &_j) + { + // Initialize return variable + ignition::math::Vector3f measuredPoint(0, 0, 0); + + std::string data = this->normalForces.data(); + char *msgBuffer = data.data(); + + // Number of bytes from the beginning of the pointer (image coordinates at + // 0,0) to the desired (i,j) position + uint32_t msgBufferIndex = + _j * this->normalForces.step() + _i * 3 * sizeof(float); + + measuredPoint.X() = static_cast( + msgBuffer[msgBufferIndex]); + + measuredPoint.Y() = static_cast( + msgBuffer[msgBufferIndex + sizeof(float)]); + + measuredPoint.Z() = static_cast( + msgBuffer[msgBufferIndex + 2*sizeof(float)]); + + return measuredPoint; + } + + public: msgs::Image normalForces; + + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +// The test checks the normal forces on the corners of the box-shaped sensor +// Fails to load Ogre plugin on macOS +TEST_F(OpticalTactilePluginTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ForcesOnPlane)) +{ + // World with moving entities + const auto sdfPath = common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", "optical_tactile_plugin.sdf"); + this->StartServer(sdfPath); + + bool receivedMsg{false}; + math::Vector3f upperLeftNormalForce(0, 0, 0); + math::Vector3f upperRightNormalForce(0, 0, 0); + math::Vector3f lowerLeftNormalForce(0, 0, 0); + math::Vector3f lowerRightNormalForce(0, 0, 0); + auto normalForcesCb = std::function( + [&](const msgs::Image &_image) + { + this->normalForces = _image; + + upperRightNormalForce = this->MapPointCloudData(1, 1); + upperLeftNormalForce = this->MapPointCloudData((_image.width() - 2), 1); + lowerLeftNormalForce = this->MapPointCloudData(1, (_image.height() - 2)); + lowerRightNormalForce = this->MapPointCloudData( + _image.width() - 2, _image.height() - 2); + receivedMsg = true; + }); + + transport::Node node; + node.Subscribe("/optical_tactile_sensor/normal_forces", normalForcesCb); + + // Check that there are no contacts nor forces yet + EXPECT_EQ(math::Vector3f(0, 0, 0), upperRightNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), upperLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); + + // Let the depth camera generate data + server->Run(true, 1000, false); + + // Give some time for messages to propagate + int sleep{0}; + int maxSleep{10}; + while (!receivedMsg && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + sleep++; + } + EXPECT_EQ(maxSleep, sleep); + EXPECT_FALSE(receivedMsg); + receivedMsg = false; + + // Check that there are no forces before the plugin is enabled + EXPECT_EQ(math::Vector3f(0, 0, 0), upperRightNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), upperLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); + + // Enable the plugin + msgs::Boolean req; + req.set_data(true); + bool executed = node.Request("/optical_tactile_sensor/enable", req); + EXPECT_TRUE(executed); + + // Let the plugin generate data again + server->Run(true, 2000, false); + + // Give some time for messages to propagate + sleep = 0; + while (!receivedMsg && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + sleep++; + } + EXPECT_NE(maxSleep, sleep); + EXPECT_TRUE(receivedMsg); + receivedMsg = false; + + // Check the values of the forces + EXPECT_EQ(math::Vector3f(-1, 0, 0), upperRightNormalForce); + EXPECT_EQ(math::Vector3f(-1, 0, 0), upperLeftNormalForce); + EXPECT_EQ(math::Vector3f(-1, 0, 0), lowerLeftNormalForce); + EXPECT_EQ(math::Vector3f(-1, 0, 0), lowerRightNormalForce); +} diff --git a/test/worlds/optical_tactile_plugin.sdf b/test/worlds/optical_tactile_plugin.sdf new file mode 100644 index 0000000000..8c8a5770b5 --- /dev/null +++ b/test/worlds/optical_tactile_plugin.sdf @@ -0,0 +1,92 @@ + + + + + + + + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.0030 0 1.57 -1.57 + + + + + 0.005 0.02 0.02 + + + + + 1 + depth_camera + -0.05 0 0 0 0 0 + + + 640 + 480 + R_FLOAT32 + + + 0.030 + 10.0 + + + + + + collision + + + + false + + false + optical_tactile_sensor + false + false + false + 1 + 0.01 + 0.05 + + + + +