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 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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