From f3dc1838cc0e541ec2ec03af9070bf6895c1d22b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 19 Aug 2020 13:32:26 +0200 Subject: [PATCH 01/18] Fix world example and add new parameter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../worlds/optical_tactile_sensor_plugin.sdf | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index d3532c0e06..7b35a4ed67 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -185,7 +185,7 @@ 0.030 - 0.065 + 10.0 @@ -201,6 +201,7 @@ filename="libignition-gazebo-opticaltactileplugin-system.so" name="ignition::gazebo::systems::OpticalTactilePlugin"> true + optical_tactile_plugin 15 true true @@ -211,30 +212,22 @@ 0 0 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SurgicalTrolleyMed - + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SurgicalTrolleyMed 0 0 0.7 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke - + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke 0 -0.7 0 0 0 3.14 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue - + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue -1.5 0 0 0 0 1.57 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine - + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine From a9402fd1bf9900f57413b3d9d719ef8c447f468d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 19 Aug 2020 13:57:53 +0200 Subject: [PATCH 02/18] Add enable service and normal forces topic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 92 +++++++++++++++++-- .../OpticalTactilePlugin.hh | 12 ++- .../optical_tactile_plugin/Visualization.cc | 25 ++++- .../optical_tactile_plugin/Visualization.hh | 3 + 4 files changed, 119 insertions(+), 13 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 58ca03e3cb..9d48b67a3b 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -52,8 +52,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] _enable Whether to enable the plugin or disable it. + public: void Enable(const bool _enable); /// \brief Callback for the depth camera /// \param[in] _msg Message from the subscribed topic @@ -163,6 +163,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"}; }; ////////////////////////////////////////////////// @@ -273,6 +279,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, } } + if (!_sdf->HasElement("namespace")) + { + ignlog << "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 @@ -298,6 +314,32 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, } } } + + // Advertise topics 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; + return; + } + + // Advertise enabling service + std::string enableService{"/" + this->dataPtr->ns + "/enable"}; + std::function enableCb = + [this](const msgs::Boolean &_req) + { + this->dataPtr->Enable(_req.data()); + }; + + if (!this->dataPtr->node.Advertise(enableService, enableCb)) + { + ignerr << "Error advertising service [" << enableService << "]" + << std::endl; + return; + } } ////////////////////////////////////////////////// @@ -307,7 +349,7 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, IGN_PROFILE("TouchPluginPrivate::PreUpdate"); // Nothing left to do if paused - if (_info.paused) + if (_info.paused || !this->dataPtr->enabled) return; if (!this->dataPtr->initialized) @@ -353,7 +395,7 @@ void OpticalTactilePlugin::PostUpdate( IGN_PROFILE("TouchPlugin::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 @@ -514,10 +556,18 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool _value) +void OpticalTactilePluginPrivate::Enable(const bool _enable) { - // todo(mcres) Implement method - _value; + { + std::lock_guard lock(this->serviceMutex); + this->enabled = _enable; + } + + if (!_enable) + { + // We don't remove the sensor marker because that's a plugin parameter + this->visualizePtr->RemoveNormalForcesMarkers(); + } } ////////////////////////////////////////////////// @@ -526,7 +576,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 @@ -638,6 +688,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(); + auto *normalForcesBuffer = new char[bufferSize]; + uint32_t bufferIndex; + // Marker messages representing the normal forces ignition::msgs::Marker positionMarkerMsg; ignition::msgs::Marker forceMarkerMsg; @@ -668,8 +729,14 @@ 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[bufferIndex] = normalForce.X(); + normalForcesBuffer[bufferIndex + sizeof(float)] = normalForce.Y(); + normalForcesBuffer[bufferIndex + 2 * sizeof(float)] = normalForce.Z(); + if (!_visualizeForces) continue; @@ -679,6 +746,11 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( } } + // Publish message + normalsMsg.set_data(normalForcesBuffer, + 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..08b4c31266 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -42,10 +42,20 @@ namespace systems /// /// Parameters: /// - /// (todo) Set this to true so the plugin works from the start and + /// 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. diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index ef59688788..47ff254cf1 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -48,6 +48,11 @@ OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( { } +////////////////////////////////////////////////// +OpticalTactilePluginVisualization::~OpticalTactilePluginVisualization() +{ +} + ////////////////////////////////////////////////// void OpticalTactilePluginVisualization::InitializeSensorMarkerMsg( ignition::msgs::Marker &_sensorMarkerMsg) @@ -79,8 +84,8 @@ void OpticalTactilePluginVisualization::RequestSensorMarkerMsg( ignition::msgs::Set(sensorMarkerMsg.mutable_pose(), ignition::math::Pose3d(_sensorPose.Pos().X(), _sensorPose.Pos().Y(), _sensorPose.Pos().Z(), - _sensorPose.Rot().X(), _sensorPose.Rot().Y(), - _sensorPose.Rot().Z(), _sensorPose.Rot().W())); + _sensorPose.Rot().W(), _sensorPose.Rot().X(), + _sensorPose.Rot().Y(), _sensorPose.Rot().Z())); this->node.Request("/marker", sensorMarkerMsg); } @@ -203,6 +208,22 @@ void OpticalTactilePluginVisualization::RequestNormalForcesMarkerMsgs( this->normalForcesMsgsAreInitialized = false; } +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RemoveNormalForcesMarkers() +{ + ignition::msgs::Marker positionMarkerMsg; + ignition::msgs::Marker forceMarkerMsg; + + 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); + + node.Request("/marker", forceMarkerMsg); + node.Request("/marker", positionMarkerMsg); +} + } } } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index 8c6df3144e..dc6353ca68 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -110,6 +110,9 @@ namespace optical_tactile_sensor ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg); + /// \brief Remove normal forces markers currently in the scene + public: void RemoveNormalForcesMarkers(); + /// \brief Transport node to request the /marker service private: ignition::transport::Node node; From 17c5b87b9785070c0ac3e4a268b30747649d13f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 19 Aug 2020 13:58:21 +0200 Subject: [PATCH 03/18] Add test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- test/integration/CMakeLists.txt | 1 + .../optical_tactile_sensor_plugin.cc | 146 ++++++++++++++++++ test/worlds/optical_tactile_sensor_plugin.sdf | 93 +++++++++++ 3 files changed, 240 insertions(+) create mode 100644 test/integration/optical_tactile_sensor_plugin.cc create mode 100644 test/worlds/optical_tactile_sensor_plugin.sdf diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index c205ee29c1..f23821aba9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -46,6 +46,7 @@ set(tests set(tests_needing_display depth_camera.cc gpu_lidar.cc + optical_tactile_sensor_plugin.cc rgbd_camera.cc sensors_system.cc ) diff --git a/test/integration/optical_tactile_sensor_plugin.cc b/test/integration/optical_tactile_sensor_plugin.cc new file mode 100644 index 0000000000..0bbc46f5e5 --- /dev/null +++ b/test/integration/optical_tactile_sensor_plugin.cc @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2019 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 "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(std::string(PROJECT_SOURCE_PATH) + _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 +TEST_F(OpticalTactilePluginTest, ForcesOnPlane) +{ + this->StartServer("/test/worlds/optical_tactile_sensor_plugin.sdf"); + + 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); + }); + + 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 plugin generate data + server->Run(true, 1100, false); + // Check that there are no contacts nor 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, 1100, false); + + // Check the values of the contacts and 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_sensor_plugin.sdf b/test/worlds/optical_tactile_sensor_plugin.sdf new file mode 100644 index 0000000000..9cf06e2d3f --- /dev/null +++ b/test/worlds/optical_tactile_sensor_plugin.sdf @@ -0,0 +1,93 @@ + + + + + + + + + + ogre2 + + + + 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 + 1 + true + true + 0.01 + 0.05 + + + + + From ee79d64f663d6388510010e8dd5c524dcd3c80a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Fri, 21 Aug 2020 20:39:15 +0200 Subject: [PATCH 04/18] Visualize contacts MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../worlds/optical_tactile_sensor_plugin.sdf | 3 +- .../OpticalTactilePlugin.cc | 30 ++++++++- .../OpticalTactilePlugin.hh | 3 + .../optical_tactile_plugin/Visualization.cc | 62 ++++++++++++++++++- .../optical_tactile_plugin/Visualization.hh | 24 ++++++- 5 files changed, 113 insertions(+), 9 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 7b35a4ed67..f54fc0d0d8 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -196,7 +196,7 @@ - true + false @@ -207,6 +207,7 @@ true 0.01 0.001 + true diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 9d48b67a3b..a874b71595 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -94,6 +94,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}; @@ -233,6 +236,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->visualizeForces = _sdf->Get("visualize_forces"); } + if (!_sdf->HasElement("visualize_contacts")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->visualizeContacts << std::endl; + } + else + { + this->dataPtr->visualizeContacts = _sdf->Get("visualize_contacts"); + } + if (!_sdf->HasElement("extended_sensing")) { ignlog << "Missing parameter , " @@ -390,7 +403,7 @@ 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("TouchPlugin::PostUpdate"); @@ -400,6 +413,18 @@ void OpticalTactilePlugin::PostUpdate( // TODO(anyone) Get ContactSensor data and merge it with DepthCamera data + if (this->dataPtr->visualizeContacts) + { + auto* contacts = + _ecm.Component( + this->dataPtr->sensorCollisionEntity); + + if (contacts) + { + this->dataPtr->visualizePtr->RequestContactsMarkerMsg(contacts); + } + } + // Process camera message if it's new { std::lock_guard lock(this->dataPtr->serviceMutex); @@ -565,8 +590,7 @@ void OpticalTactilePluginPrivate::Enable(const bool _enable) if (!_enable) { - // We don't remove the sensor marker because that's a plugin parameter - this->visualizePtr->RemoveNormalForcesMarkers(); + this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); } } diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 08b4c31266..b123c9198a 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -75,6 +75,9 @@ namespace systems /// /// 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 or not. This element is optional, and the default value is false. class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : public System, diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 47ff254cf1..aeb3d61947 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -90,6 +90,57 @@ void OpticalTactilePluginVisualization::RequestSensorMarkerMsg( this->node.Request("/marker", sensorMarkerMsg); } +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::InitializeContactsMarkerMsg( + ignition::msgs::Marker &_contactsMarkerMsg) +{ + // Initialize the marker for visualizing the 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 message + ignition::math::Vector3d contactNormal(0, 0, 0.03); + + for (auto const &position : _contact.position()) + { + ignition::math::Vector3d firstPoint = ignition::msgs::Convert(position); + ignition::math::Vector3d secondPoint = firstPoint + contactNormal; + + ignition::msgs::Set(_contactMarkerMsg.add_point(), firstPoint); + ignition::msgs::Set(_contactMarkerMsg.add_point(), secondPoint); + } +} + +////////////////////////////////////////////////// +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, @@ -201,18 +252,19 @@ 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::RemoveNormalForcesMarkers() +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); @@ -220,8 +272,12 @@ void OpticalTactilePluginVisualization::RemoveNormalForcesMarkers() forceMarkerMsg.set_ns("forces_" + this->modelName); forceMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - node.Request("/marker", forceMarkerMsg); + contactMarkerMsg.set_ns("contacts_" + this->modelName); + contactMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + node.Request("/marker", positionMarkerMsg); + node.Request("/marker", forceMarkerMsg); + node.Request("/marker", contactMarkerMsg); } } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index dc6353ca68..42f0e890cd 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -24,6 +24,7 @@ #include #include #include +#include "ignition/gazebo/components/ContactSensorData.hh" namespace ignition { @@ -72,6 +73,25 @@ namespace optical_tactile_sensor public: void RequestSensorMarkerMsg( 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 + /// \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 /// positions @@ -110,8 +130,8 @@ namespace optical_tactile_sensor ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg); - /// \brief Remove normal forces markers currently in the scene - public: void RemoveNormalForcesMarkers(); + /// \brief Remove normal forces and contact markers currently in the scene + public: void RemoveNormalForcesAndContactsMarkers(); /// \brief Transport node to request the /marker service private: ignition::transport::Node node; From b7e7056bb058e96da37d4035217f984ecfcb7e00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Fri, 21 Aug 2020 20:52:33 +0200 Subject: [PATCH 05/18] Fix test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- test/integration/optical_tactile_sensor_plugin.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/integration/optical_tactile_sensor_plugin.cc b/test/integration/optical_tactile_sensor_plugin.cc index 0bbc46f5e5..56e3362912 100644 --- a/test/integration/optical_tactile_sensor_plugin.cc +++ b/test/integration/optical_tactile_sensor_plugin.cc @@ -120,9 +120,9 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); - // Let the plugin generate data - server->Run(true, 1100, false); - // Check that there are no contacts nor forces before the plugin is enabled + // Let the depth camera generate data + server->Run(true, 1000, 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); @@ -136,9 +136,9 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) EXPECT_TRUE(executed); // Let the plugin generate data again - server->Run(true, 1100, false); + server->Run(true, 2000, false); - // Check the values of the contacts and forces + // 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); From 90c29b5e8b6d25fb8ce73e82ff60cf2cfe127389 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Fri, 21 Aug 2020 21:12:22 +0200 Subject: [PATCH 06/18] Fix conflicts MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- examples/worlds/optical_tactile_sensor_plugin.sdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index f54fc0d0d8..1e2c436514 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -223,11 +223,11 @@ 0 -0.7 0 0 0 3.14 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue - -1.5 0 0 0 0 1.57 + -1.5 0 0 0 0 0 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine From 2db11bc9cabf753fd9838cd1182fd0e0f65ca372 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Tue, 25 Aug 2020 13:59:24 +0200 Subject: [PATCH 07/18] PR Feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 6 ++-- .../optical_tactile_plugin/Visualization.cc | 32 +++++++++---------- test/integration/CMakeLists.txt | 2 +- ...or_plugin.cc => optical_tactile_plugin.cc} | 4 +-- ..._plugin.sdf => optical_tactile_plugin.sdf} | 5 ++- 5 files changed, 23 insertions(+), 26 deletions(-) rename test/integration/{optical_tactile_sensor_plugin.cc => optical_tactile_plugin.cc} (97%) rename test/worlds/{optical_tactile_sensor_plugin.sdf => optical_tactile_plugin.sdf} (96%) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index a874b71595..c65bbbfe34 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -328,7 +328,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, } } - // Advertise topics for normal forces + // Advertise topic for normal forces std::string normalForcesTopic = "/" + this->dataPtr->ns + "/normal_forces"; this->dataPtr->normalForcesPub = this->dataPtr->node.Advertise(normalForcesTopic); @@ -336,7 +336,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, { ignerr << "Error advertising topic [" << normalForcesTopic << "]" << std::endl; - return; } // Advertise enabling service @@ -351,7 +350,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, { ignerr << "Error advertising service [" << enableService << "]" << std::endl; - return; } } @@ -756,7 +754,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // 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); + bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); normalForcesBuffer[bufferIndex] = normalForce.X(); normalForcesBuffer[bufferIndex + sizeof(float)] = normalForce.Y(); normalForcesBuffer[bufferIndex + 2 * sizeof(float)] = normalForce.Z(); diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index aeb3d61947..c2d6c926fe 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -118,11 +118,11 @@ void OpticalTactilePluginVisualization::AddContactToMarkerMsg( for (auto const &position : _contact.position()) { - ignition::math::Vector3d firstPoint = ignition::msgs::Convert(position); - ignition::math::Vector3d secondPoint = firstPoint + contactNormal; + ignition::math::Vector3d startPoint = ignition::msgs::Convert(position); + ignition::math::Vector3d endPoint = startPoint + contactNormal; - ignition::msgs::Set(_contactMarkerMsg.add_point(), firstPoint); - ignition::msgs::Set(_contactMarkerMsg.add_point(), secondPoint); + ignition::msgs::Set(_contactMarkerMsg.add_point(), startPoint); + ignition::msgs::Set(_contactMarkerMsg.add_point(), endPoint); } } @@ -211,12 +211,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); @@ -224,27 +224,27 @@ 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 (startPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0) || + endPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0)) return; // Add points to the messages ignition::msgs::Set(_positionMarkerMsg.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(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())); } ////////////////////////////////////////////////// diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f23821aba9..db3593ebd5 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -46,7 +46,7 @@ set(tests set(tests_needing_display depth_camera.cc gpu_lidar.cc - optical_tactile_sensor_plugin.cc + optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc ) diff --git a/test/integration/optical_tactile_sensor_plugin.cc b/test/integration/optical_tactile_plugin.cc similarity index 97% rename from test/integration/optical_tactile_sensor_plugin.cc rename to test/integration/optical_tactile_plugin.cc index 56e3362912..9257996adf 100644 --- a/test/integration/optical_tactile_sensor_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -93,7 +93,7 @@ class OpticalTactilePluginTest : public ::testing::Test // The test checks the normal forces on the corners of the box-shaped sensor TEST_F(OpticalTactilePluginTest, ForcesOnPlane) { - this->StartServer("/test/worlds/optical_tactile_sensor_plugin.sdf"); + this->StartServer("/test/worlds/optical_tactile_plugin.sdf"); math::Vector3f upperLeftNormalForce(0, 0, 0); math::Vector3f upperRightNormalForce(0, 0, 0); diff --git a/test/worlds/optical_tactile_sensor_plugin.sdf b/test/worlds/optical_tactile_plugin.sdf similarity index 96% rename from test/worlds/optical_tactile_sensor_plugin.sdf rename to test/worlds/optical_tactile_plugin.sdf index 9cf06e2d3f..19266180fd 100644 --- a/test/worlds/optical_tactile_sensor_plugin.sdf +++ b/test/worlds/optical_tactile_plugin.sdf @@ -44,7 +44,7 @@ 0 0 0.0030 0 1.57 -1.57 - + @@ -55,7 +55,7 @@ 1 depth_camera - -0.05 0 0 0 0 0 + -0.05 0 0 0 0 0 640 @@ -73,7 +73,6 @@ collision - false Date: Fri, 29 Jan 2021 01:21:45 -0500 Subject: [PATCH 08/18] fix dtor error Signed-off-by: Mabel Zhang --- src/systems/optical_tactile_plugin/Visualization.cc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index b70407385a..0aaef41734 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -48,11 +48,6 @@ OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( { } -////////////////////////////////////////////////// -OpticalTactilePluginVisualization::~OpticalTactilePluginVisualization() -{ -} - ////////////////////////////////////////////////// void OpticalTactilePluginVisualization::InitializeSensorMarkerMsg( ignition::msgs::Marker &_sensorMarkerMsg) From 60744580f6c7d7230dd3b36f337556327937bcaa Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 29 Jan 2021 02:16:20 -0500 Subject: [PATCH 09/18] style Signed-off-by: Mabel Zhang --- .../OpticalTactilePlugin.hh | 148 +++++++++--------- .../optical_tactile_plugin/Visualization.cc | 21 +-- .../optical_tactile_plugin/Visualization.hh | 52 +++--- 3 files changed, 106 insertions(+), 115 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index b123c9198a..8372ba789e 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -32,86 +32,90 @@ 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: - /// - /// 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 - /// - /// 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. - /// - /// Whether to visualize the contacts from the contact - /// 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. + /// + /// 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. + /// + /// 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. + /// + /// Whether to visualize the contacts from the contact + /// sensor or not. 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 0aaef41734..5276bcd21c 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -19,17 +19,10 @@ #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( @@ -276,9 +269,3 @@ void OpticalTactilePluginVisualization::RemoveNormalForcesAndContactsMarkers() node.Request("/marker", forceMarkerMsg); node.Request("/marker", contactMarkerMsg); } - -} -} -} -} -} diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index 995370ec8b..b215680824 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -50,44 +50,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); + ignition::msgs::Marker &_contactsMarkerMsg); /// \brief Add a contact to the marker message representing the contacts /// from the contact sensor /// \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); + 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); + components::ContactSensorData const *_contacts); /// \brief Initialize the marker messages representing the normal forces /// \param[out] _positionMarkerMsg Message for visualizing the contact @@ -95,8 +95,8 @@ 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 @@ -111,11 +111,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 @@ -124,8 +124,8 @@ 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 normal forces and contact markers currently in the scene public: void RemoveNormalForcesAndContactsMarkers(); @@ -154,10 +154,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 From cb947f089c7e42451b83a461750b7fd2668a99c0 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 29 Jan 2021 02:51:58 -0500 Subject: [PATCH 10/18] style review Signed-off-by: Mabel Zhang --- .../optical_tactile_plugin/Visualization.cc | 21 ++++++++++++++----- .../optical_tactile_plugin/Visualization.hh | 4 +++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 5276bcd21c..78360238ea 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -15,6 +15,8 @@ * */ +#include +#include #include #include "Visualization.hh" @@ -45,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); @@ -82,6 +86,9 @@ void OpticalTactilePluginVisualization::RequestSensorMarkerMsg( void OpticalTactilePluginVisualization::InitializeContactsMarkerMsg( ignition::msgs::Marker &_contactsMarkerMsg) { + // Reset all fields + _contactsMarkerMsg = ignition::msgs::Marker(); + // Initialize the marker for visualizing the contacts as red lines _contactsMarkerMsg.set_ns("contacts_" + this->modelName); _contactsMarkerMsg.set_id(1); @@ -101,9 +108,11 @@ void OpticalTactilePluginVisualization::AddContactToMarkerMsg( ignition::msgs::Contact const &_contact, ignition::msgs::Marker &_contactMarkerMsg) { - // todo(anyone) once available, use normal field in the message + // 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); @@ -134,10 +143,10 @@ 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 _positionMarkerMsg.set_ns("positions_" + this->modelName); _positionMarkerMsg.set_id(1); @@ -154,6 +163,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()-> @@ -161,6 +171,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()-> diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index b215680824..fcf36f725e 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -24,6 +24,8 @@ #include #include #include +#include + #include "ignition/gazebo/components/ContactSensorData.hh" namespace ignition @@ -127,7 +129,7 @@ namespace optical_tactile_sensor ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg); - /// \brief Remove normal forces and contact markers currently in the scene + /// \brief Remove all normal forces and contact markers public: void RemoveNormalForcesAndContactsMarkers(); /// \brief Transport node to request the /marker service From e9f84960f359df8d97a55d82f8fd8ea1e2dc34ca Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 29 Jan 2021 03:09:07 -0500 Subject: [PATCH 11/18] reviewed optical_tactile_plugin.cc Signed-off-by: Mabel Zhang --- .../OpticalTactilePlugin.cc | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 5ef5cdcb1d..3c515a30c5 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -56,7 +56,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Actual function that enables the plugin. /// \param[in] _enable Whether to enable the plugin or disable it. - public: void Enable(const bool _enable); + public: void Enable(const ignition::msgs::Boolean &_req); /// \brief Callback for the depth camera /// \param[in] _msg Message from the subscribed topic @@ -306,6 +306,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Get the size of the sensor from the SDF // If there's no specified inside , a default one // is set + this->dataPtr->sensorSize = ignition::math::Vector3d(0.005, 0.02, 0.02); if (_sdf->GetParent() != nullptr) { if (_sdf->GetParent()->GetElement("link") != nullptr) @@ -340,14 +341,9 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, } // Advertise enabling service - std::string enableService{"/" + this->dataPtr->ns + "/enable"}; - std::function enableCb = - [this](const msgs::Boolean &_req) - { - this->dataPtr->Enable(_req.data()); - }; - - if (!this->dataPtr->node.Advertise(enableService, enableCb)) + 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; @@ -615,14 +611,14 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool _enable) +void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req) { { std::lock_guard lock(this->serviceMutex); - this->enabled = _enable; + this->enabled = _req.data(); } - if (!_enable) + if (!_req.data()) { this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); } From b4b8bc511c249cb848a59e4b6a1e4a6131c85f21 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 29 Jan 2021 20:29:36 -0500 Subject: [PATCH 12/18] attempt to fix mac CI Signed-off-by: Mabel Zhang --- test/worlds/optical_tactile_plugin.sdf | 1 - 1 file changed, 1 deletion(-) diff --git a/test/worlds/optical_tactile_plugin.sdf b/test/worlds/optical_tactile_plugin.sdf index 19266180fd..8a7b4aa90e 100644 --- a/test/worlds/optical_tactile_plugin.sdf +++ b/test/worlds/optical_tactile_plugin.sdf @@ -13,7 +13,6 @@ - ogre2 From f0c28b359ad64302abcb2268d86b199e3cf4b175 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 29 Jan 2021 21:51:14 -0500 Subject: [PATCH 13/18] smart pointer; system-free path separator; printouts for debugging Mac CI Signed-off-by: Mabel Zhang --- .../OpticalTactilePlugin.cc | 10 +++++----- test/integration/optical_tactile_plugin.cc | 19 ++++++++++++++++--- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 3c515a30c5..23443c037c 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -755,7 +755,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( normalsMsg.set_pixel_format_type(ignition::msgs::PixelFormatType::R_FLOAT32); uint32_t bufferSize = 3 * sizeof(float) * _msg.width() * _msg.height(); - auto *normalForcesBuffer = new char[bufferSize]; + std::shared_ptr normalForcesBuffer(new char[bufferSize]); uint32_t bufferIndex; // Marker messages representing the normal forces @@ -792,9 +792,9 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // 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[bufferIndex] = normalForce.X(); - normalForcesBuffer[bufferIndex + sizeof(float)] = normalForce.Y(); - normalForcesBuffer[bufferIndex + 2 * sizeof(float)] = normalForce.Z(); + normalForcesBuffer.get()[bufferIndex] = normalForce.X(); + normalForcesBuffer.get()[bufferIndex + sizeof(float)] = normalForce.Y(); + normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = normalForce.Z(); if (!_visualizeForces) continue; @@ -807,7 +807,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( } // Publish message - normalsMsg.set_data(normalForcesBuffer, + normalsMsg.set_data(normalForcesBuffer.get(), 3 * sizeof(float) * _msg.width() * _msg.height()); this->normalForcesPub.Publish(normalsMsg); diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index 9257996adf..7911c28267 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -48,7 +49,7 @@ class OpticalTactilePluginTest : public ::testing::Test public: void StartServer(const std::string &_sdfFile) { ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + _sdfFile); + serverConfig.SetSdfFile(_sdfFile); server = std::make_unique(serverConfig); using namespace std::chrono_literals; server->SetUpdatePeriod(0ns); @@ -93,7 +94,12 @@ class OpticalTactilePluginTest : public ::testing::Test // The test checks the normal forces on the corners of the box-shaped sensor TEST_F(OpticalTactilePluginTest, ForcesOnPlane) { - this->StartServer("/test/worlds/optical_tactile_plugin.sdf"); + ignerr << "1111111111111111111111\n"; + + // World with moving entities + const auto sdfPath = common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", "optical_tactile_plugin.sdf"); + this->StartServer(sdfPath); math::Vector3f upperLeftNormalForce(0, 0, 0); math::Vector3f upperRightNormalForce(0, 0, 0); @@ -120,6 +126,8 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); + ignerr << "2222222222222222222222\n"; + // Let the depth camera generate data server->Run(true, 1000, false); // Check that there are no forces before the plugin is enabled @@ -128,16 +136,21 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); + ignerr << "3333333333333333333333\n"; + // Enable the plugin msgs::Boolean req; req.set_data(true); bool executed = node.Request("/optical_tactile_sensor/enable", req); - EXPECT_TRUE(executed); + ignerr << "4444444444444444444444\n"; + // Let the plugin generate data again server->Run(true, 2000, false); + ignerr << "5555555555555555555555\n"; + // Check the values of the forces EXPECT_EQ(math::Vector3f(-1, 0, 0), upperRightNormalForce); EXPECT_EQ(math::Vector3f(-1, 0, 0), upperLeftNormalForce); From 4baf8daeadd4b8af3b969bd11a3b0e7271c10acb Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 29 Jan 2021 23:44:28 -0500 Subject: [PATCH 14/18] test Mac CI: turn off visualization Signed-off-by: Mabel Zhang --- test/integration/optical_tactile_plugin.cc | 1 + test/worlds/optical_tactile_plugin.sdf | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index 7911c28267..279ab8ded6 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -128,6 +128,7 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) ignerr << "2222222222222222222222\n"; + // TODO: Mac CI has ogre 2 error here. // Let the depth camera generate data server->Run(true, 1000, false); // Check that there are no forces before the plugin is enabled diff --git a/test/worlds/optical_tactile_plugin.sdf b/test/worlds/optical_tactile_plugin.sdf index 8a7b4aa90e..932f24bd19 100644 --- a/test/worlds/optical_tactile_plugin.sdf +++ b/test/worlds/optical_tactile_plugin.sdf @@ -80,8 +80,8 @@ false optical_tactile_sensor 1 - true - true + false + false 0.01 0.05 From 1b46c2ea4f0e19d4f37a4eeb53065c4189047cbd Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Sat, 30 Jan 2021 01:10:07 -0500 Subject: [PATCH 15/18] add clarifications Signed-off-by: Mabel Zhang --- .../worlds/optical_tactile_sensor_plugin.sdf | 6 +++--- .../OpticalTactilePlugin.cc | 3 ++- .../OpticalTactilePlugin.hh | 17 +++++++++++------ .../optical_tactile_plugin/Visualization.cc | 11 +++++------ .../optical_tactile_plugin/Visualization.hh | 13 +++++++++---- test/integration/optical_tactile_plugin.cc | 1 + test/worlds/optical_tactile_plugin.sdf | 5 +++-- 7 files changed, 34 insertions(+), 22 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 412875554d..00f6a5c6db 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -202,12 +202,12 @@ name="ignition::gazebo::systems::OpticalTactilePlugin"> true optical_tactile_plugin - 15 - true true + true + true + 15 0.01 0.001 - true diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 23443c037c..68822ebaf1 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -794,7 +794,8 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( 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(); + normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = + normalForce.Z(); if (!_visualizeForces) continue; diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 8372ba789e..57cc5aa008 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -39,6 +39,11 @@ namespace systems /// /// 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: /// @@ -63,10 +68,6 @@ namespace systems /// 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. @@ -80,8 +81,12 @@ namespace systems /// is optional, and the default value is false. /// /// Whether to visualize the contacts from the contact - /// sensor or not. This element is optional, and the - /// default value is false. + /// 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, diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 78360238ea..c53827de7d 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -89,7 +89,7 @@ void OpticalTactilePluginVisualization::InitializeContactsMarkerMsg( // Reset all fields _contactsMarkerMsg = ignition::msgs::Marker(); - // Initialize the marker for visualizing the contacts as red lines + // 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); @@ -148,6 +148,7 @@ void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( // 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); @@ -229,20 +230,18 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs( normalForcePoseFromWorld.Pos(); // Check invalid points to avoid data transfer overhead - if (startPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0) || - endPointFromWorld == 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(startPointFromWorld.X(), startPointFromWorld.Y(), startPointFromWorld.Z())); + // Normal ignition::msgs::Set(_forceMarkerMsg.add_point(), ignition::math::Vector3d(startPointFromWorld.X(), startPointFromWorld.Y(), startPointFromWorld.Z())); - ignition::msgs::Set(_forceMarkerMsg.add_point(), ignition::math::Vector3d(endPointFromWorld.X(), endPointFromWorld.Y(), endPointFromWorld.Z())); diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index fcf36f725e..5b48268e10 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -40,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 @@ -79,7 +84,7 @@ namespace optical_tactile_sensor ignition::msgs::Marker &_contactsMarkerMsg); /// \brief Add a contact to the marker message representing the contacts - /// from the contact sensor + /// 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( @@ -100,8 +105,8 @@ namespace optical_tactile_sensor 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 diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index 279ab8ded6..dff5db81f1 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -94,6 +94,7 @@ class OpticalTactilePluginTest : public ::testing::Test // The test checks the normal forces on the corners of the box-shaped sensor TEST_F(OpticalTactilePluginTest, ForcesOnPlane) { + // TEMPORARY: Debugging Mac CI Ogre2 error ignerr << "1111111111111111111111\n"; // World with moving entities diff --git a/test/worlds/optical_tactile_plugin.sdf b/test/worlds/optical_tactile_plugin.sdf index 932f24bd19..8c8a5770b5 100644 --- a/test/worlds/optical_tactile_plugin.sdf +++ b/test/worlds/optical_tactile_plugin.sdf @@ -79,9 +79,10 @@ name="ignition::gazebo::systems::OpticalTactilePlugin"> false optical_tactile_sensor - 1 - false false + false + false + 1 0.01 0.05 From 50f425099605b408abc42f20f85091e02251ff7a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 28 Apr 2021 19:07:38 -0700 Subject: [PATCH 16/18] Small tweaks Signed-off-by: Louise Poubel --- .../worlds/optical_tactile_sensor_plugin.sdf | 18 ++++++++-- .../OpticalTactilePlugin.cc | 34 +++++++++++++------ .../optical_tactile_plugin/Visualization.cc | 6 ++-- 3 files changed, 43 insertions(+), 15 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 00f6a5c6db..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 + @@ -225,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 68822ebaf1..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,7 +56,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: void Load(const EntityComponentManager &_ecm); /// \brief Actual function that enables the plugin. - /// \param[in] _enable Whether to enable the plugin or disable it. + /// \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 @@ -115,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}; @@ -153,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. @@ -295,7 +296,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("namespace")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->ns << std::endl; } else @@ -306,7 +307,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Get the size of the sensor from the SDF // If there's no specified inside , a default one // is set - this->dataPtr->sensorSize = ignition::math::Vector3d(0.005, 0.02, 0.02); if (_sdf->GetParent() != nullptr) { if (_sdf->GetParent()->GetElement("link") != nullptr) @@ -324,6 +324,8 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, _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; } } } @@ -339,6 +341,11 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, 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"; @@ -348,6 +355,11 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, ignerr << "Error advertising service [" << enableService << "]" << std::endl; } + else + { + ignmsg << "Service to enable tactile sensor [" << enableService << "]" + << std::endl; + } } ////////////////////////////////////////////////// @@ -411,14 +423,13 @@ void OpticalTactilePlugin::PostUpdate( return; // TODO(anyone) Get ContactSensor data and merge it with DepthCamera data - if (this->dataPtr->visualizeContacts) { - auto* contacts = + auto *contacts = _ecm.Component( this->dataPtr->sensorCollisionEntity); - if (contacts) + if (nullptr != contacts) { this->dataPtr->visualizePtr->RequestContactsMarkerMsg(contacts); } @@ -613,11 +624,14 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req) { + if (_req.data() != this->enabled) { - std::lock_guard lock(this->serviceMutex); - this->enabled = _req.data(); + ignmsg << "Enabling optical tactile sensor with namespace [" << this->ns + << "]: " << _req.data() << std::endl; } + this->enabled = _req.data(); + if (!_req.data()) { this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index c53827de7d..16c5664a38 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -275,7 +275,7 @@ void OpticalTactilePluginVisualization::RemoveNormalForcesAndContactsMarkers() contactMarkerMsg.set_ns("contacts_" + this->modelName); contactMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - node.Request("/marker", positionMarkerMsg); - node.Request("/marker", forceMarkerMsg); - node.Request("/marker", contactMarkerMsg); + this->node.Request("/marker", positionMarkerMsg); + this->node.Request("/marker", forceMarkerMsg); + this->node.Request("/marker", contactMarkerMsg); } From 25f12446e4825a9f73f235c52c96c08c99542aec Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 30 Apr 2021 15:54:38 -0700 Subject: [PATCH 17/18] Give messages time to propagate Signed-off-by: Louise Poubel --- test/integration/optical_tactile_plugin.cc | 36 +++++++++++++++------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index dff5db81f1..69b640125f 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -94,14 +94,12 @@ class OpticalTactilePluginTest : public ::testing::Test // The test checks the normal forces on the corners of the box-shaped sensor TEST_F(OpticalTactilePluginTest, ForcesOnPlane) { - // TEMPORARY: Debugging Mac CI Ogre2 error - ignerr << "1111111111111111111111\n"; - // 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); @@ -116,6 +114,7 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) lowerLeftNormalForce = this->MapPointCloudData(1, (_image.height() - 2)); lowerRightNormalForce = this->MapPointCloudData( _image.width() - 2, _image.height() - 2); + receivedMsg = true; }); transport::Node node; @@ -127,31 +126,46 @@ TEST_F(OpticalTactilePluginTest, ForcesOnPlane) EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); - ignerr << "2222222222222222222222\n"; - - // TODO: Mac CI has ogre 2 error here. // 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); - ignerr << "3333333333333333333333\n"; - // Enable the plugin msgs::Boolean req; req.set_data(true); bool executed = node.Request("/optical_tactile_sensor/enable", req); EXPECT_TRUE(executed); - ignerr << "4444444444444444444444\n"; - // Let the plugin generate data again server->Run(true, 2000, false); - ignerr << "5555555555555555555555\n"; + // 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); From 4ffad4dcf564e6281161b62e8751401e0d604f13 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 3 May 2021 13:27:39 -0700 Subject: [PATCH 18/18] Disable test on macOS Signed-off-by: Louise Poubel --- test/integration/optical_tactile_plugin.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index 69b640125f..ea6ec02e85 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -23,8 +23,9 @@ #include #include -#include #include +#include +#include #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -92,7 +93,9 @@ class OpticalTactilePluginTest : public ::testing::Test ///////////////////////////////////////////////// // The test checks the normal forces on the corners of the box-shaped sensor -TEST_F(OpticalTactilePluginTest, ForcesOnPlane) +// 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(