From 0aca10d84b7ad4741d25c67bd4785fb93268b59d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 4 Jun 2020 15:37:37 +0200 Subject: [PATCH 01/23] Initial commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- src/systems/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 5f7a9cc111..b7ccaaa469 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -88,6 +88,7 @@ add_subdirectory(logical_camera) add_subdirectory(magnetometer) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) +add_subdirectory(optical_tactile_plugin) add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) From 222d65b95e77708d6631b583aafc45b9002145d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 4 Jun 2020 15:39:20 +0200 Subject: [PATCH 02/23] Added plugin files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../optical_tactile_plugin/CMakeLists.txt | 6 ++ .../OpticalTactilePlugin.cc | 84 +++++++++++++++++++ .../OpticalTactilePlugin.hh | 73 ++++++++++++++++ 3 files changed, 163 insertions(+) create mode 100644 src/systems/optical_tactile_plugin/CMakeLists.txt create mode 100644 src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc create mode 100644 src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh diff --git a/src/systems/optical_tactile_plugin/CMakeLists.txt b/src/systems/optical_tactile_plugin/CMakeLists.txt new file mode 100644 index 0000000000..48420a8229 --- /dev/null +++ b/src/systems/optical_tactile_plugin/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(opticaltactileplugin + SOURCES + OpticalTactilePlugin.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) \ No newline at end of file diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc new file mode 100644 index 0000000000..eadfce5c6f --- /dev/null +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -0,0 +1,84 @@ +/* + * 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 + +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/ContactSensorData.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Sensor.hh" + +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "OpticalTactilePlugin.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::OpticalTactilePluginPrivate +{ + /// \brief Load the Contact sensor from an sdf element + /// \param[in] _sdf SDF element describing the Contact sensor + /// \param[in] _topic string with topic name + /// \param[in] _collisionEntities A list of entities that act as contact + /// sensors + public: void Load(const sdf::ElementPtr &_sdf, const std::string &_topic, + const std::vector &_collisionEntities); + + /// \brief Actual function that enables the plugin. + /// \param[in] _value True to enable plugin. + public: void Enable(const bool _value); + + /// \brief Process contact sensor data and determine if a touch event occurs + /// \param[in] _info Simulation update info + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Update(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + +}; + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::Load(const sdf::ElementPtr &_sdf, const std::string &_topic, + const std::vector &_collisionEntities) +{ + +} + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::Enable(const bool _value) +{ + +} + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + +} \ No newline at end of file diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh new file mode 100644 index 0000000000..0db578849a --- /dev/null +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -0,0 +1,73 @@ +/* + * 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. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_OPTICAL_TACTILE_PLUGIN_HH_ +#define IGNITION_GAZEBO_SYSTEMS_OPTICAL_TACTILE_PLUGIN_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ + // Forward declaration + class OpticalTactilePluginPrivate; + + class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: OpticalTactilePlugin(); + + /// \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 PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + // 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; + + }; +} +} +} +} + +#endif \ No newline at end of file From 7be1771ff5e054f526e7089573e238bd9fc86532 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Mon, 8 Jun 2020 11:01:46 +0200 Subject: [PATCH 03/23] Initial .sdf file 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 | 188 ++++++++++++++++++ 1 file changed, 188 insertions(+) create mode 100644 examples/worlds/optical_tactile_sensor_plugin.sdf diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf new file mode 100644 index 0000000000..0018b03906 --- /dev/null +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -0,0 +1,188 @@ + + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/touch/control + /world/touch/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/touch/stats + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 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 1.25 0 0 0 + + + + + 0.5 0.5 0.5 + + + + + + + 0.5 0.5 0.5 + + + + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 0 0.5 0 0 0 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + collision + + + + + + + true + 5 + "Gaussian" + + + + + + From c3d91fbd7c97f77ef9c3a179ff1a2b4debcb3157 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Mon, 8 Jun 2020 11:04:15 +0200 Subject: [PATCH 04/23] Pipeline for getting sensor contacts MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 199 +++++++++++++++++- .../OpticalTactilePlugin.hh | 10 + 2 files changed, 199 insertions(+), 10 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index eadfce5c6f..0d86a5597e 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -45,40 +45,219 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate { /// \brief Load the Contact sensor from an sdf element /// \param[in] _sdf SDF element describing the Contact sensor - /// \param[in] _topic string with topic name - /// \param[in] _collisionEntities A list of entities that act as contact - /// sensors - public: void Load(const sdf::ElementPtr &_sdf, const std::string &_topic, - const std::vector &_collisionEntities); + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf); /// \brief Actual function that enables the plugin. /// \param[in] _value True to enable plugin. public: void Enable(const bool _value); - /// \brief Process contact sensor data and determine if a touch event occurs + /// \brief Process contact sensor data /// \param[in] _info Simulation update info /// \param[in] _ecm Immutable reference to the EntityComponentManager public: void Update(const UpdateInfo &_info, const EntityComponentManager &_ecm); + /// \brief Filters out new collisions fetched not related to the sensors + public: void FilterOutCollisions(const EntityComponentManager &_ecm, + const std::vector &_entities); + + /// \brief Resolution of the sensor in mm. + public: double resolution; + + /// \brief Model interface. + public: Model model{kNullEntity}; + + /// \brief Transport node to keep services alive. + public: transport::Node node; + + /// \brief Publisher that publishes the sensors' contacts. + public: std::optionalsensorContactsPub; + + /// \brief Collision entities that have been designated as contact sensors. + public: std::vector sensorEntities; + + /// \brief Collision entities of the simulation. + public: std::vector collisionEntities; + + /// \brief Filtered collisions from the simulation that belong to one or more sensors. + public: std::vector filteredCollisionEntities; + + /// \brief Wheter the plugin is enabled. + public: bool enabled{true}; + + /// \brief Initialization flag. + public: bool initialized{false}; + + /// \brief Copy of the sdf configuration used for this plugin. + public: sdf::ElementPtr sdfConfig; + + /// \brief Name of the model in which the sensor(s) is attached. + public: std::string modelName; + + }; ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Load(const sdf::ElementPtr &_sdf, const std::string &_topic, - const std::vector &_collisionEntities) +void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf) { + igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; + + // Get sdf parameters + if (!_sdf->HasElement("resolution")) { + ignerr << "Missing required parameter ." << std::endl; + return; + } + else + { + this->resolution = _sdf->Get("resolution"); + igndbg << "Sensor resolution: " << this->resolution << " mm" << std::endl; + } + + // Get all the sensors + auto allLinks = + _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); + + for (const Entity linkEntity : allLinks) + { + auto linkCollisions= + _ecm.ChildrenByComponents(linkEntity, components::Collision()); + for (const Entity colEntity : linkCollisions) + { + if (_ecm.EntityHasComponentType(colEntity, + components::ContactSensorData::typeId)) + { + this->sensorEntities.push_back(colEntity); + + this->modelName = this->model.Name(_ecm); + + igndbg << "Sensor detected within model " << this->modelName << std::endl; + } + } + } + + } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Enable(const bool _value) { - + igndbg << "OpticalTactilePluginPrivate::Enable" << std::endl; } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, const EntityComponentManager &_ecm) { + IGN_PROFILE("TouchPluginPrivate::Update"); + + // Print collision messages which have the sensor as one collision + for (const Entity colEntity: this->filteredCollisionEntities) + { + auto* contacts = _ecm.Component(colEntity); + if (contacts) + { + for (const auto &contact : contacts->Data().contact()) + { + ignmsg << "Collision between Entity " << contact.collision1().id() + << " and " << contact.collision2().id() << std::endl; + } + } + } +} + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::FilterOutCollisions(const EntityComponentManager &_ecm, + const std::vector &_entities) +{ + if (_entities.empty()) + return; + + // Clear vector that contains old data + this->filteredCollisionEntities.clear(); + + // Get collisions from the sensor + for (Entity entity : _entities) + { + std::string name = scopedName(entity,_ecm); + igndbg << "scopedName: " << name << std::endl; + if (name.find(this->modelName) != std::string::npos) + { + igndbg << "Filtered collision that belongs to a sensor" << std::endl; + this->filteredCollisionEntities.push_back(entity); + } + } + +} + +////////////////////////////////////////////////// +OpticalTactilePlugin::OpticalTactilePlugin() + : System(), dataPtr(std::make_unique()) +{ + igndbg << "OpticalTactilePlugin Constructor" << std::endl; +} + +////////////////////////////////////////////////// +void OpticalTactilePlugin::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + igndbg << "OpticalTactilePlugin::Configure" << std::endl; + this->dataPtr->sdfConfig = _sdf->Clone(); + this->dataPtr->model = Model(_entity); +} + +////////////////////////////////////////////////// +void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("TouchPluginPrivate::PreUpdate"); + + if (!this->dataPtr->initialized) + { + this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); + this->dataPtr->initialized = true; + } + + // Update new collision messages + + // This is not an "else" because "initialized" can be set in the if block + // above + if (this->dataPtr->initialized) + { + // Fetch new collisions from simulation + std::vector newCollisions; + _ecm.EachNew( + [&](const Entity &_entity, const components::Collision *) -> bool + { + igndbg << "Fetched new collision" << std::endl; + newCollisions.push_back(_entity); + return true; + }); + + this->dataPtr->FilterOutCollisions(_ecm, newCollisions); + } +} + +////////////////////////////////////////////////// +void OpticalTactilePlugin::PostUpdate( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TouchPluginPrivate::PostUpdate"); + + this->dataPtr->Update(_info, _ecm); + +} + +IGNITION_ADD_PLUGIN(OpticalTactilePlugin, + ignition::gazebo::System, + OpticalTactilePlugin::ISystemConfigure, + OpticalTactilePlugin::ISystemPreUpdate, + OpticalTactilePlugin::ISystemPostUpdate) -} \ No newline at end of file +IGNITION_ADD_PLUGIN_ALIAS(OpticalTactilePlugin, "ignition::gazebo::systems::OpticalTactilePlugin") \ No newline at end of file diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 0db578849a..44232e9fda 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -34,6 +34,16 @@ namespace systems // Forward declaration class OpticalTactilePluginPrivate; + /// \brief Plugin that implements an optical tactile sensor + /// + /// It requires that contact sensors 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. + class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : public System, public ISystemConfigure, From 16a268e579817cde60d75b87ed04dfceb5cf3ad0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Tue, 9 Jun 2020 16:16:24 +0200 Subject: [PATCH 05/23] Added visualization of postion and forces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 119 +++++++++++++++++- 1 file changed, 115 insertions(+), 4 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 0d86a5597e..e6c8cfeb15 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -35,6 +36,12 @@ #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" +#include +#include +#include + +#include + #include "OpticalTactilePlugin.hh" using namespace ignition; @@ -63,13 +70,19 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: void FilterOutCollisions(const EntityComponentManager &_ecm, const std::vector &_entities); + /// \brief Visualize the sensor's data using the Marker message + public: void VisualizeSensorData(std::vector &positions); + + /// \brief Interpolates contact data + public: void InterpolateData(const ignition::msgs::Contact &contactMsg); + /// \brief Resolution of the sensor in mm. public: double resolution; /// \brief Model interface. public: Model model{kNullEntity}; - /// \brief Transport node to keep services alive. + /// \brief Transport node to visualize data on Gazebo. public: transport::Node node; /// \brief Publisher that publishes the sensors' contacts. @@ -96,6 +109,14 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Name of the model in which the sensor(s) is attached. public: std::string modelName; + /// \brief Message for visualizing contact positions + public: ignition::msgs::Marker positionMarkerMsg; + + /// \brief Message for visualizing contact forces + public: ignition::msgs::Marker forceMarkerMsg; + + /// \brief Position interpolated from the Contact messages + public: std::vector interpolatedPosition; }; @@ -114,7 +135,6 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, else { this->resolution = _sdf->Get("resolution"); - igndbg << "Sensor resolution: " << this->resolution << " mm" << std::endl; } // Get all the sensors @@ -162,9 +182,15 @@ void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, { for (const auto &contact : contacts->Data().contact()) { - ignmsg << "Collision between Entity " << contact.collision1().id() - << " and " << contact.collision2().id() << std::endl; + // Interpolate data returned by the Contact message + this->InterpolateData(contact); + + // Visualize interpolated data + this->VisualizeSensorData(this->interpolatedPosition); + } + + //ignition::common::Time::Sleep(ignition::common::Time(0.1)); } } } @@ -193,6 +219,46 @@ void OpticalTactilePluginPrivate::FilterOutCollisions(const EntityComponentManag } +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::InterpolateData(const ignition::msgs::Contact &contact) +{ + // Delete old positions + this->interpolatedPosition.clear(); + + // Add new ones and interpolate + for (int index = 0; index < contact.position_size(); ++index) + { + this->interpolatedPosition.push_back(contact.position(index)); + } +} + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::VisualizeSensorData(std::vector &positions) +{ + // Delete previous shapes already in simulation + this->positionMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + this->forceMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + this->node.Request("/marker",this->positionMarkerMsg); + this->node.Request("/marker",this->forceMarkerMsg); + + // Add the new ones + this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + for (int index = 0; index < positions.size(); ++index) + { + this->positionMarkerMsg.set_id(index); + this->forceMarkerMsg.set_id(index); + + ignition::msgs::Set(this->positionMarkerMsg.mutable_pose(), + ignition::math::Pose3d(positions[index].x(), positions[index].y(), positions[index].z(), 0, 0, 0)); + ignition::msgs::Set(this->forceMarkerMsg.mutable_pose(), + ignition::math::Pose3d(positions[index].x(), positions[index].y(), positions[index].z(), 0, 0, 0)); + + this->node.Request("/marker",this->positionMarkerMsg); + this->node.Request("/marker",this->forceMarkerMsg); + } +} + ////////////////////////////////////////////////// OpticalTactilePlugin::OpticalTactilePlugin() : System(), dataPtr(std::make_unique()) @@ -209,6 +275,51 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, igndbg << "OpticalTactilePlugin::Configure" << std::endl; this->dataPtr->sdfConfig = _sdf->Clone(); this->dataPtr->model = Model(_entity); + + // Configure Marker messages for position and force of the contacts + // Blue spheres for positions + // Red cylinders for forces + + // Create the marker message + this->dataPtr->positionMarkerMsg.set_ns("positions"); + this->dataPtr->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->positionMarkerMsg.set_type(ignition::msgs::Marker::SPHERE); + this->dataPtr->positionMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + this->dataPtr->forceMarkerMsg.set_ns("forces"); + this->dataPtr->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->forceMarkerMsg.set_type(ignition::msgs::Marker::CYLINDER); + this->dataPtr->forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + // Set material properties + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + this->dataPtr->positionMarkerMsg.mutable_lifetime()->set_sec(2); + this->dataPtr->positionMarkerMsg.mutable_lifetime()->set_nsec(0); + + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + this->dataPtr->forceMarkerMsg.mutable_lifetime()->set_sec(2); + this->dataPtr->forceMarkerMsg.mutable_lifetime()->set_nsec(0); + + // Set scales + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(0.20, 0.20, 0.20)); + + ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), + ignition::math::Vector3d(0.05, 0.05, 0.7)); } ////////////////////////////////////////////////// From 5cd5d5fec660bf18e5e1e918015d339f92fa6824 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Tue, 9 Jun 2020 16:54:43 +0200 Subject: [PATCH 06/23] Added plugin for Transform Control 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, 4 insertions(+) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 0018b03906..a70ba4e437 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -10,6 +10,10 @@ filename="libignition-gazebo-contact-system.so" name="ignition::gazebo::systems::Contact"> + + From cd58ae636a009b388968b9298eada0a31b7992e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 10 Jun 2020 17:04:40 +0200 Subject: [PATCH 07/23] Added initial interpolation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 91 ++++++++++++++++--- 1 file changed, 76 insertions(+), 15 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index e6c8cfeb15..167d8f4baa 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -71,7 +71,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate const std::vector &_entities); /// \brief Visualize the sensor's data using the Marker message - public: void VisualizeSensorData(std::vector &positions); + public: void VisualizeSensorData(std::vector &positions); /// \brief Interpolates contact data public: void InterpolateData(const ignition::msgs::Contact &contactMsg); @@ -112,11 +112,17 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Message for visualizing contact positions public: ignition::msgs::Marker positionMarkerMsg; + /// \brief Radius of the visualized contact sphere + public: double contactRadius{0.20}; + /// \brief Message for visualizing contact forces public: ignition::msgs::Marker forceMarkerMsg; + /// \brief Length of the visualized force cylinder + public: double forceLenght{0.20}; + /// \brief Position interpolated from the Contact messages - public: std::vector interpolatedPosition; + public: std::vector interpolatedPosition; }; @@ -128,13 +134,15 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; // Get sdf parameters - if (!_sdf->HasElement("resolution")) { + if (!_sdf->HasElement("resolution")) + { ignerr << "Missing required parameter ." << std::endl; return; } else { - this->resolution = _sdf->Get("resolution"); + // resolution is specified in mm + this->resolution = _sdf->Get("resolution")/1000; } // Get all the sensors @@ -184,7 +192,7 @@ void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, { // Interpolate data returned by the Contact message this->InterpolateData(contact); - + // Visualize interpolated data this->VisualizeSensorData(this->interpolatedPosition); @@ -225,15 +233,64 @@ void OpticalTactilePluginPrivate::InterpolateData(const ignition::msgs::Contact // Delete old positions this->interpolatedPosition.clear(); - // Add new ones and interpolate - for (int index = 0; index < contact.position_size(); ++index) + // Interpolate new ones + if (contact.position_size() == 4) { - this->interpolatedPosition.push_back(contact.position(index)); - } + igndbg << "Interpolate 4 position" << std::endl; + ignition::math::Vector3d contact1 = ignition::msgs::Convert(contact.position(0)); + ignition::math::Vector3d contact2 = ignition::msgs::Convert(contact.position(1)); + ignition::math::Vector3d contact3 = ignition::msgs::Convert(contact.position(2)); + ignition::math::Vector3d contact4 = ignition::msgs::Convert(contact.position(3)); + + ignition::math::Vector3d direction1 = (contact2 - contact1).Normalized() * this->resolution; + ignition::math::Vector3d direction2 = (contact4 - contact1).Normalized() * this->resolution; + + ignition::math::Vector3d interpolatedVector = contact1; + this->interpolatedPosition.push_back(contact1); + + // auxiliary Vector3d to iterate through contacts + ignition::math::Vector3d tempVector (0.0, 0.0, 0.0); + + long unsigned int steps1 = contact1.Distance(contact2) / this->resolution; + long unsigned int steps2 = contact1.Distance(contact4) / this->resolution; + + igndbg << "\n contact1: " << contact1 << "\n" + << "contact2: " << contact2 << "\n" + << "contact3: " << contact3 << "\n" + << "contact4: " << contact4 << "\n" + << "direction1: " << direction1 << "\n" + << "direction2: " << direction2 << "\n"; + + igndbg << "steps1 = " << steps1 << std::endl; + igndbg << "contact1.Distance(contact2) = " << contact1.Distance(contact2) << std::endl; + igndbg << "this->resolution = " << this->resolution << std::endl; + + igndbg << "steps2 = " << steps2 << std::endl; + igndbg << "contact1.Distance(contact4) = " << contact1.Distance(contact4) << std::endl; + igndbg << "this->resolution = " << this->resolution << std::endl; + igndbg << "interpolatedVector: " << interpolatedVector << std::endl; + for (long unsigned int index1 = 0; index1 <= steps1; ++index1) + { + tempVector = interpolatedVector; + for (long unsigned int index2 = 0; index2 < steps2; ++index2) + { + interpolatedVector += direction2; + igndbg << "interpolatedVector: " << interpolatedVector << std::endl; + this->interpolatedPosition.push_back(interpolatedVector); + } + if (index1 != steps1) + { + interpolatedVector = tempVector; + interpolatedVector += direction1; + igndbg << "interpolatedVector: " << interpolatedVector << std::endl; + this->interpolatedPosition.push_back(interpolatedVector); + } + } + } } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::VisualizeSensorData(std::vector &positions) +void OpticalTactilePluginPrivate::VisualizeSensorData(std::vector &positions) { // Delete previous shapes already in simulation this->positionMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); @@ -244,15 +301,18 @@ void OpticalTactilePluginPrivate::VisualizeSensorData(std::vectorpositionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + for (int index = 0; index < positions.size(); ++index) { this->positionMarkerMsg.set_id(index); this->forceMarkerMsg.set_id(index); ignition::msgs::Set(this->positionMarkerMsg.mutable_pose(), - ignition::math::Pose3d(positions[index].x(), positions[index].y(), positions[index].z(), 0, 0, 0)); + ignition::math::Pose3d(positions[index].X(), positions[index].Y(), + positions[index].Z(), 0, 0, 0)); ignition::msgs::Set(this->forceMarkerMsg.mutable_pose(), - ignition::math::Pose3d(positions[index].x(), positions[index].y(), positions[index].z(), 0, 0, 0)); + ignition::math::Pose3d(positions[index].X(), positions[index].Y(), + positions[index].Z() + this->forceLenght, 0, 0, 0)); this->node.Request("/marker",this->positionMarkerMsg); this->node.Request("/marker",this->forceMarkerMsg); @@ -278,7 +338,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Configure Marker messages for position and force of the contacts // Blue spheres for positions - // Red cylinders for forces + // Green cylinders for forces // Create the marker message this->dataPtr->positionMarkerMsg.set_ns("positions"); @@ -316,10 +376,11 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Set scales ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.20, 0.20, 0.20)); + ignition::math::Vector3d(this->dataPtr->contactRadius, this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.05, 0.05, 0.7)); + ignition::math::Vector3d(0.05, 0.05, this->dataPtr->forceLenght)); } ////////////////////////////////////////////////// From 40a87ef5fd35e46a15235bbb5db055e5a2457500 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 11 Jun 2020 11:59:16 +0200 Subject: [PATCH 08/23] Code check MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 355 ++++++++++-------- .../OpticalTactilePlugin.hh | 47 ++- 2 files changed, 220 insertions(+), 182 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 167d8f4baa..520bbe26ac 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -36,12 +36,6 @@ #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" -#include -#include -#include - -#include - #include "OpticalTactilePlugin.hh" using namespace ignition; @@ -50,96 +44,96 @@ using namespace systems; class ignition::gazebo::systems::OpticalTactilePluginPrivate { - /// \brief Load the Contact sensor from an sdf element - /// \param[in] _sdf SDF element describing the Contact sensor - /// \param[in] _ecm Immutable reference to the EntityComponentManager - public: void Load(const EntityComponentManager &_ecm, - const sdf::ElementPtr &_sdf); + /// \brief Load the Contact sensor from an sdf element + /// \param[in] _sdf SDF element describing the Contact sensor + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf); - /// \brief Actual function that enables the plugin. - /// \param[in] _value True to enable plugin. - public: void Enable(const bool _value); + /// \brief Actual function that enables the plugin. + /// \param[in] _value True to enable plugin. + public: void Enable(const bool _value); - /// \brief Process contact sensor data - /// \param[in] _info Simulation update info - /// \param[in] _ecm Immutable reference to the EntityComponentManager - public: void Update(const UpdateInfo &_info, - const EntityComponentManager &_ecm); + /// \brief Process contact sensor data + /// \param[in] _info Simulation update info + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Update(const UpdateInfo &_info, + const EntityComponentManager &_ecm); - /// \brief Filters out new collisions fetched not related to the sensors - public: void FilterOutCollisions(const EntityComponentManager &_ecm, - const std::vector &_entities); + /// \brief Filters out new collisions fetched not related to the sensors + public: void FilterOutCollisions(const EntityComponentManager &_ecm, + const std::vector &_entities); - /// \brief Visualize the sensor's data using the Marker message - public: void VisualizeSensorData(std::vector &positions); + /// \brief Visualize the sensor's data using the Marker message + public: void VisualizeSensorData( + std::vector &positions); - /// \brief Interpolates contact data - public: void InterpolateData(const ignition::msgs::Contact &contactMsg); + /// \brief Interpolates contact data + public: void InterpolateData(const ignition::msgs::Contact &contactMsg); - /// \brief Resolution of the sensor in mm. - public: double resolution; + /// \brief Resolution of the sensor in mm. + public: double resolution; - /// \brief Model interface. - public: Model model{kNullEntity}; + /// \brief Model interface. + public: Model model{kNullEntity}; - /// \brief Transport node to visualize data on Gazebo. - public: transport::Node node; + /// \brief Transport node to visualize data on Gazebo. + public: transport::Node node; - /// \brief Publisher that publishes the sensors' contacts. - public: std::optionalsensorContactsPub; + /// \brief Publisher that publishes the sensors' contacts. + public: std::optionalsensorContactsPub; - /// \brief Collision entities that have been designated as contact sensors. - public: std::vector sensorEntities; + /// \brief Collision entities that have been designated as contact sensors. + public: std::vector sensorEntities; - /// \brief Collision entities of the simulation. - public: std::vector collisionEntities; + /// \brief Collision entities of the simulation. + public: std::vector collisionEntities; - /// \brief Filtered collisions from the simulation that belong to one or more sensors. - public: std::vector filteredCollisionEntities; + /// \brief Filtered collisions from the simulation that belong to + // one or more sensors. + public: std::vector filteredCollisionEntities; - /// \brief Wheter the plugin is enabled. - public: bool enabled{true}; + /// \brief Wheter the plugin is enabled. + public: bool enabled{true}; - /// \brief Initialization flag. - public: bool initialized{false}; + /// \brief Initialization flag. + public: bool initialized{false}; - /// \brief Copy of the sdf configuration used for this plugin. - public: sdf::ElementPtr sdfConfig; + /// \brief Copy of the sdf configuration used for this plugin. + public: sdf::ElementPtr sdfConfig; - /// \brief Name of the model in which the sensor(s) is attached. - public: std::string modelName; - - /// \brief Message for visualizing contact positions - public: ignition::msgs::Marker positionMarkerMsg; + /// \brief Name of the model in which the sensor(s) is attached. + public: std::string modelName; - /// \brief Radius of the visualized contact sphere - public: double contactRadius{0.20}; + /// \brief Message for visualizing contact positions + public: ignition::msgs::Marker positionMarkerMsg; - /// \brief Message for visualizing contact forces - public: ignition::msgs::Marker forceMarkerMsg; + /// \brief Radius of the visualized contact sphere + public: double contactRadius{0.20}; - /// \brief Length of the visualized force cylinder - public: double forceLenght{0.20}; + /// \brief Message for visualizing contact forces + public: ignition::msgs::Marker forceMarkerMsg; - /// \brief Position interpolated from the Contact messages - public: std::vector interpolatedPosition; + /// \brief Length of the visualized force cylinder + public: double forceLenght{0.20}; + /// \brief Position interpolated from the Contact messages + public: std::vector interpolatedPosition; }; ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, - const sdf::ElementPtr &_sdf) + const sdf::ElementPtr &_sdf) { - igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; // Get sdf parameters - if (!_sdf->HasElement("resolution")) + if (!_sdf->HasElement("resolution")) { ignerr << "Missing required parameter ." << std::endl; return; - } - else + } + else { // resolution is specified in mm this->resolution = _sdf->Get("resolution")/1000; @@ -151,23 +145,22 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, for (const Entity linkEntity : allLinks) { - auto linkCollisions= + auto linkCollisions = _ecm.ChildrenByComponents(linkEntity, components::Collision()); for (const Entity colEntity : linkCollisions) { if (_ecm.EntityHasComponentType(colEntity, - components::ContactSensorData::typeId)) + components::ContactSensorData::typeId)) { this->sensorEntities.push_back(colEntity); this->modelName = this->model.Name(_ecm); - igndbg << "Sensor detected within model " << this->modelName << std::endl; + igndbg << "Sensor detected within model " + << this->modelName << std::endl; } } } - - } ////////////////////////////////////////////////// @@ -181,31 +174,31 @@ void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, const EntityComponentManager &_ecm) { IGN_PROFILE("TouchPluginPrivate::Update"); - + // Print collision messages which have the sensor as one collision - for (const Entity colEntity: this->filteredCollisionEntities) + for (const Entity colEntity : this->filteredCollisionEntities) { - auto* contacts = _ecm.Component(colEntity); + auto* contacts = + _ecm.Component(colEntity); if (contacts) { for (const auto &contact : contacts->Data().contact()) { // Interpolate data returned by the Contact message this->InterpolateData(contact); - + // Visualize interpolated data this->VisualizeSensorData(this->interpolatedPosition); - } - - //ignition::common::Time::Sleep(ignition::common::Time(0.1)); + // ignition::common::Time::Sleep(ignition::common::Time(0.1)); } } } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::FilterOutCollisions(const EntityComponentManager &_ecm, - const std::vector &_entities) +void OpticalTactilePluginPrivate::FilterOutCollisions( + const EntityComponentManager &_ecm, + const std::vector &_entities) { if (_entities.empty()) return; @@ -213,22 +206,23 @@ void OpticalTactilePluginPrivate::FilterOutCollisions(const EntityComponentManag // Clear vector that contains old data this->filteredCollisionEntities.clear(); - // Get collisions from the sensor + // Get collisions from the sensor for (Entity entity : _entities) { - std::string name = scopedName(entity,_ecm); + std::string name = scopedName(entity, _ecm); igndbg << "scopedName: " << name << std::endl; if (name.find(this->modelName) != std::string::npos) { - igndbg << "Filtered collision that belongs to a sensor" << std::endl; + igndbg << "Filtered collision that belongs to a sensor" + << std::endl; this->filteredCollisionEntities.push_back(entity); } } - } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::InterpolateData(const ignition::msgs::Contact &contact) +void OpticalTactilePluginPrivate::InterpolateData( + const ignition::msgs::Contact &contact) { // Delete old positions this->interpolatedPosition.clear(); @@ -237,85 +231,103 @@ void OpticalTactilePluginPrivate::InterpolateData(const ignition::msgs::Contact if (contact.position_size() == 4) { igndbg << "Interpolate 4 position" << std::endl; - ignition::math::Vector3d contact1 = ignition::msgs::Convert(contact.position(0)); - ignition::math::Vector3d contact2 = ignition::msgs::Convert(contact.position(1)); - ignition::math::Vector3d contact3 = ignition::msgs::Convert(contact.position(2)); - ignition::math::Vector3d contact4 = ignition::msgs::Convert(contact.position(3)); - - ignition::math::Vector3d direction1 = (contact2 - contact1).Normalized() * this->resolution; - ignition::math::Vector3d direction2 = (contact4 - contact1).Normalized() * this->resolution; + ignition::math::Vector3d contact1 = + ignition::msgs::Convert(contact.position(0)); + ignition::math::Vector3d contact2 = + ignition::msgs::Convert(contact.position(1)); + ignition::math::Vector3d contact3 = + ignition::msgs::Convert(contact.position(2)); + ignition::math::Vector3d contact4 = + ignition::msgs::Convert(contact.position(3)); + + ignition::math::Vector3d direction1 = + (contact2 - contact1).Normalized() * this->resolution; + ignition::math::Vector3d direction2 = + (contact4 - contact1).Normalized() * this->resolution; ignition::math::Vector3d interpolatedVector = contact1; this->interpolatedPosition.push_back(contact1); // auxiliary Vector3d to iterate through contacts - ignition::math::Vector3d tempVector (0.0, 0.0, 0.0); + ignition::math::Vector3d tempVector(0.0, 0.0, 0.0); + + uint64_t steps1 = + contact1.Distance(contact2) / this->resolution; + uint64_t steps2 = + contact1.Distance(contact4) / this->resolution; - long unsigned int steps1 = contact1.Distance(contact2) / this->resolution; - long unsigned int steps2 = contact1.Distance(contact4) / this->resolution; - igndbg << "\n contact1: " << contact1 << "\n" - << "contact2: " << contact2 << "\n" - << "contact3: " << contact3 << "\n" - << "contact4: " << contact4 << "\n" - << "direction1: " << direction1 << "\n" - << "direction2: " << direction2 << "\n"; - + << "contact2: " << contact2 << "\n" + << "contact2: " << contact2 << "\n" + << "contact2: " << contact2 << "\n" + << "contact2: " << contact2 << "\n" + << "contact2: " << contact2 << "\n" + << "contact3: " << contact3 << "\n" + << "contact4: " << contact4 << "\n" + << "direction1: " << direction1 << "\n" + << "direction2: " << direction2 << "\n"; + igndbg << "steps1 = " << steps1 << std::endl; - igndbg << "contact1.Distance(contact2) = " << contact1.Distance(contact2) << std::endl; - igndbg << "this->resolution = " << this->resolution << std::endl; + igndbg << "contact1.Distance(contact2) = " + << contact1.Distance(contact2) << std::endl; + igndbg << "this->resolution = " + << this->resolution << std::endl; igndbg << "steps2 = " << steps2 << std::endl; - igndbg << "contact1.Distance(contact4) = " << contact1.Distance(contact4) << std::endl; + igndbg << "contact1.Distance(contact4) = " + << contact1.Distance(contact4) << std::endl; igndbg << "this->resolution = " << this->resolution << std::endl; igndbg << "interpolatedVector: " << interpolatedVector << std::endl; - for (long unsigned int index1 = 0; index1 <= steps1; ++index1) + for (uint64_t index1 = 0; index1 <= steps1; ++index1) { tempVector = interpolatedVector; - for (long unsigned int index2 = 0; index2 < steps2; ++index2) + for (uint64_t index2 = 0; index2 < steps2; ++index2) { interpolatedVector += direction2; - igndbg << "interpolatedVector: " << interpolatedVector << std::endl; + igndbg << "interpolatedVector: " + << interpolatedVector << std::endl; this->interpolatedPosition.push_back(interpolatedVector); } if (index1 != steps1) { interpolatedVector = tempVector; interpolatedVector += direction1; - igndbg << "interpolatedVector: " << interpolatedVector << std::endl; - this->interpolatedPosition.push_back(interpolatedVector); - } + igndbg << "interpolatedVector: " + << interpolatedVector << std::endl; + this->interpolatedPosition.push_back(interpolatedVector); + } } - } + } } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::VisualizeSensorData(std::vector &positions) +void OpticalTactilePluginPrivate::VisualizeSensorData( + std::vector &positions) { // Delete previous shapes already in simulation this->positionMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); this->forceMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - this->node.Request("/marker",this->positionMarkerMsg); - this->node.Request("/marker",this->forceMarkerMsg); + this->node.Request("/marker", this->positionMarkerMsg); + this->node.Request("/marker", this->forceMarkerMsg); // Add the new ones this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - for (int index = 0; index < positions.size(); ++index) + for (uint64_t index = 0; index < positions.size(); ++index) { this->positionMarkerMsg.set_id(index); this->forceMarkerMsg.set_id(index); ignition::msgs::Set(this->positionMarkerMsg.mutable_pose(), - ignition::math::Pose3d(positions[index].X(), positions[index].Y(), - positions[index].Z(), 0, 0, 0)); + ignition::math::Pose3d(positions[index].X(), positions[index].Y(), + positions[index].Z(), 0, 0, 0)); ignition::msgs::Set(this->forceMarkerMsg.mutable_pose(), - ignition::math::Pose3d(positions[index].X(), positions[index].Y(), - positions[index].Z() + this->forceLenght, 0, 0, 0)); + ignition::math::Pose3d(positions[index].X(), positions[index].Y(), + positions[index].Z() + this->forceLenght, 0, 0, 0)); - this->node.Request("/marker",this->positionMarkerMsg); - this->node.Request("/marker",this->forceMarkerMsg); + this->node.Request("/marker", this->positionMarkerMsg); + this->node.Request("/marker", this->forceMarkerMsg); } } @@ -342,45 +354,72 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Create the marker message this->dataPtr->positionMarkerMsg.set_ns("positions"); - this->dataPtr->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->positionMarkerMsg.set_type(ignition::msgs::Marker::SPHERE); - this->dataPtr->positionMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->positionMarkerMsg.set_type( + ignition::msgs::Marker::SPHERE); + this->dataPtr->positionMarkerMsg.set_visibility( + ignition::msgs::Marker::GUI); this->dataPtr->forceMarkerMsg.set_ns("forces"); - this->dataPtr->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->forceMarkerMsg.set_type(ignition::msgs::Marker::CYLINDER); - this->dataPtr->forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); - - // Set material properties - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); - this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - this->dataPtr->positionMarkerMsg.mutable_lifetime()->set_sec(2); - this->dataPtr->positionMarkerMsg.mutable_lifetime()->set_nsec(0); - - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - this->dataPtr->forceMarkerMsg.mutable_lifetime()->set_sec(2); - this->dataPtr->forceMarkerMsg.mutable_lifetime()->set_nsec(0); + this->dataPtr->forceMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->forceMarkerMsg.set_type( + ignition::msgs::Marker::CYLINDER); + this->dataPtr->forceMarkerMsg.set_visibility( + ignition::msgs::Marker::GUI); + + // Set material properties + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()->set_sec(2); + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()->set_nsec(0); + + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + this->dataPtr-> + forceMarkerMsg.mutable_lifetime()->set_sec(2); + this->dataPtr-> + forceMarkerMsg.mutable_lifetime()->set_nsec(0); // Set scales ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->dataPtr->contactRadius, this->dataPtr->contactRadius, - this->dataPtr->contactRadius)); + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.05, 0.05, this->dataPtr->forceLenght)); + ignition::math::Vector3d(0.05, 0.05, this->dataPtr->forceLenght)); } ////////////////////////////////////////////////// @@ -397,8 +436,8 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, // Update new collision messages - // This is not an "else" because "initialized" can be set in the if block - // above + // This is not an "else" because "initialized" can be set + // in the if block above if (this->dataPtr->initialized) { // Fetch new collisions from simulation @@ -417,13 +456,12 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void OpticalTactilePlugin::PostUpdate( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("TouchPluginPrivate::PostUpdate"); this->dataPtr->Update(_info, _ecm); - } IGNITION_ADD_PLUGIN(OpticalTactilePlugin, @@ -432,4 +470,5 @@ IGNITION_ADD_PLUGIN(OpticalTactilePlugin, OpticalTactilePlugin::ISystemPreUpdate, OpticalTactilePlugin::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(OpticalTactilePlugin, "ignition::gazebo::systems::OpticalTactilePlugin") \ No newline at end of file +IGNITION_ADD_PLUGIN_ALIAS(OpticalTactilePlugin, + "ignition::gazebo::systems::OpticalTactilePlugin") diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 44232e9fda..b2dfac45aa 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -27,7 +27,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { @@ -40,9 +40,9 @@ namespace systems /// 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. + /// + /// Set this to true so the plugin works from the start and + /// doesn't need to be enabled. class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : public System, @@ -50,34 +50,33 @@ namespace systems public ISystemPreUpdate, public ISystemPostUpdate { - /// \brief Constructor - public: OpticalTactilePlugin(); - - /// \brief Destructor - public: ~OpticalTactilePlugin() override = default; + /// \brief Constructor + public: OpticalTactilePlugin(); - // Documentation inherited - public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; + /// \brief Destructor + public: ~OpticalTactilePlugin() override = default; - /// Documentation inherited - public: void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; - // Documentation inherited - public: void PostUpdate( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; - /// \brief Private data pointer - private: std::unique_ptr dataPtr; + // 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; }; } } } } -#endif \ No newline at end of file +#endif From 5b85a48dff959620e514b422aa58c696e7a1aebb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Fri, 12 Jun 2020 17:42:36 +0200 Subject: [PATCH 09/23] Update info after specified milliseconds 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 | 2 +- .../OpticalTactilePlugin.cc | 131 ++++++++---------- .../OpticalTactilePlugin.hh | 5 + 3 files changed, 67 insertions(+), 71 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index a70ba4e437..6e64987bc5 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -182,7 +182,7 @@ filename="libignition-gazebo-opticaltactileplugin-system.so" name="ignition::gazebo::systems::OpticalTactilePlugin"> true - 5 + 200 "Gaussian" diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 520bbe26ac..251f8c378a 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -54,12 +54,6 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \param[in] _value True to enable plugin. public: void Enable(const bool _value); - /// \brief Process contact sensor data - /// \param[in] _info Simulation update info - /// \param[in] _ecm Immutable reference to the EntityComponentManager - public: void Update(const UpdateInfo &_info, - const EntityComponentManager &_ecm); - /// \brief Filters out new collisions fetched not related to the sensors public: void FilterOutCollisions(const EntityComponentManager &_ecm, const std::vector &_entities); @@ -90,7 +84,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: std::vector collisionEntities; /// \brief Filtered collisions from the simulation that belong to - // one or more sensors. + /// one or more sensors. public: std::vector filteredCollisionEntities; /// \brief Wheter the plugin is enabled. @@ -119,6 +113,15 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Position interpolated from the Contact messages public: std::vector interpolatedPosition; + + /// \brief Update period in milliseconds + public: int64_t updatePeriod{100}; + + /// \brief Last time Update was called + public: std::chrono::steady_clock::duration lastUpdateTime{0}; + + /// \brief Allows the plugin to run + public: bool update{true}; }; ////////////////////////////////////////////////// @@ -166,17 +169,29 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Enable(const bool _value) { - igndbg << "OpticalTactilePluginPrivate::Enable" << std::endl; + // Just a placeholder method for the moment + _value; } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, - const EntityComponentManager &_ecm) +void OpticalTactilePlugin::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) { + // Nothing left to do if paused or not allowed to update + if (_info.paused || !this->dataPtr->update) + return; + IGN_PROFILE("TouchPluginPrivate::Update"); - // Print collision messages which have the sensor as one collision - for (const Entity colEntity : this->filteredCollisionEntities) + // Save the time for last time this function was called + this->dataPtr->lastUpdateTime = _info.simTime; + + // Step 1: Interpolate data + + // Delete old interpolated positions + this->dataPtr->interpolatedPosition.clear(); + + for (const Entity colEntity : this->dataPtr->filteredCollisionEntities) { auto* contacts = _ecm.Component(colEntity); @@ -185,14 +200,13 @@ void OpticalTactilePluginPrivate::Update(const UpdateInfo &_info, for (const auto &contact : contacts->Data().contact()) { // Interpolate data returned by the Contact message - this->InterpolateData(contact); - - // Visualize interpolated data - this->VisualizeSensorData(this->interpolatedPosition); + this->dataPtr->InterpolateData(contact); } - // ignition::common::Time::Sleep(ignition::common::Time(0.1)); } } + + // Step 2: Visualize data + this->dataPtr->VisualizeSensorData(this->dataPtr->interpolatedPosition); } ////////////////////////////////////////////////// @@ -210,11 +224,9 @@ void OpticalTactilePluginPrivate::FilterOutCollisions( for (Entity entity : _entities) { std::string name = scopedName(entity, _ecm); - igndbg << "scopedName: " << name << std::endl; + if (name.find(this->modelName) != std::string::npos) { - igndbg << "Filtered collision that belongs to a sensor" - << std::endl; this->filteredCollisionEntities.push_back(entity); } } @@ -224,13 +236,8 @@ void OpticalTactilePluginPrivate::FilterOutCollisions( void OpticalTactilePluginPrivate::InterpolateData( const ignition::msgs::Contact &contact) { - // Delete old positions - this->interpolatedPosition.clear(); - - // Interpolate new ones if (contact.position_size() == 4) { - igndbg << "Interpolate 4 position" << std::endl; ignition::math::Vector3d contact1 = ignition::msgs::Convert(contact.position(0)); ignition::math::Vector3d contact2 = @@ -256,44 +263,18 @@ void OpticalTactilePluginPrivate::InterpolateData( uint64_t steps2 = contact1.Distance(contact4) / this->resolution; - igndbg << "\n contact1: " << contact1 << "\n" - << "contact2: " << contact2 << "\n" - << "contact2: " << contact2 << "\n" - << "contact2: " << contact2 << "\n" - << "contact2: " << contact2 << "\n" - << "contact2: " << contact2 << "\n" - << "contact3: " << contact3 << "\n" - << "contact4: " << contact4 << "\n" - << "direction1: " << direction1 << "\n" - << "direction2: " << direction2 << "\n"; - - igndbg << "steps1 = " << steps1 << std::endl; - igndbg << "contact1.Distance(contact2) = " - << contact1.Distance(contact2) << std::endl; - igndbg << "this->resolution = " - << this->resolution << std::endl; - - igndbg << "steps2 = " << steps2 << std::endl; - igndbg << "contact1.Distance(contact4) = " - << contact1.Distance(contact4) << std::endl; - igndbg << "this->resolution = " << this->resolution << std::endl; - igndbg << "interpolatedVector: " << interpolatedVector << std::endl; for (uint64_t index1 = 0; index1 <= steps1; ++index1) { tempVector = interpolatedVector; for (uint64_t index2 = 0; index2 < steps2; ++index2) { interpolatedVector += direction2; - igndbg << "interpolatedVector: " - << interpolatedVector << std::endl; this->interpolatedPosition.push_back(interpolatedVector); } if (index1 != steps1) { interpolatedVector = tempVector; interpolatedVector += direction1; - igndbg << "interpolatedVector: " - << interpolatedVector << std::endl; this->interpolatedPosition.push_back(interpolatedVector); } } @@ -335,14 +316,13 @@ void OpticalTactilePluginPrivate::VisualizeSensorData( OpticalTactilePlugin::OpticalTactilePlugin() : System(), dataPtr(std::make_unique()) { - igndbg << "OpticalTactilePlugin Constructor" << std::endl; } ////////////////////////////////////////////////// void OpticalTactilePlugin::Configure(const Entity &_entity, const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) + EntityComponentManager &, + EventManager &) { igndbg << "OpticalTactilePlugin::Configure" << std::endl; this->dataPtr->sdfConfig = _sdf->Clone(); @@ -428,46 +408,57 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, { IGN_PROFILE("TouchPluginPrivate::PreUpdate"); + // Nothing left to do if paused + if (_info.paused) + return; + if (!this->dataPtr->initialized) { this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); this->dataPtr->initialized = true; } - // Update new collision messages - // This is not an "else" because "initialized" can be set // in the if block above if (this->dataPtr->initialized) { - // Fetch new collisions from simulation - std::vector newCollisions; - _ecm.EachNew( - [&](const Entity &_entity, const components::Collision *) -> bool - { - igndbg << "Fetched new collision" << std::endl; - newCollisions.push_back(_entity); - return true; - }); - - this->dataPtr->FilterOutCollisions(_ecm, newCollisions); + // Fetch new collisions from simulation + std::vector newCollisions; + _ecm.Each( + [&](const Entity &_entity, const components::Collision *) -> bool + { + newCollisions.push_back(_entity); + return true; + }); + this->dataPtr->FilterOutCollisions(_ecm, newCollisions); } } ////////////////////////////////////////////////// void OpticalTactilePlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const ignition::gazebo::EntityComponentManager &) { - IGN_PROFILE("TouchPluginPrivate::PostUpdate"); + IGN_PROFILE("TouchPlugin::PostUpdate"); + + // Nothing left to do if paused. + if (_info.paused) + return; + + // Check if info should be updated + this->dataPtr->update = true; + auto diff = std::chrono::duration_cast( + _info.simTime - this->dataPtr->lastUpdateTime); - this->dataPtr->Update(_info, _ecm); + if ((diff.count() >= 0) && (diff.count() < this->dataPtr->updatePeriod)) + this->dataPtr->update = false; } IGNITION_ADD_PLUGIN(OpticalTactilePlugin, ignition::gazebo::System, OpticalTactilePlugin::ISystemConfigure, OpticalTactilePlugin::ISystemPreUpdate, + OpticalTactilePlugin::ISystemUpdate, OpticalTactilePlugin::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(OpticalTactilePlugin, diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index b2dfac45aa..ede7e8c89d 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -48,6 +48,7 @@ namespace systems public System, public ISystemConfigure, public ISystemPreUpdate, + public ISystemUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -64,6 +65,10 @@ namespace systems /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// Documentation inherited + public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; // Documentation inherited From 93918ebce3e6f17ef7c76d7ad075bc71b31a7997 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Mon, 15 Jun 2020 09:44:11 +0200 Subject: [PATCH 10/23] Set marker lifetime for better performance MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 251f8c378a..2a16ca394d 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -285,15 +285,13 @@ void OpticalTactilePluginPrivate::InterpolateData( void OpticalTactilePluginPrivate::VisualizeSensorData( std::vector &positions) { - // Delete previous shapes already in simulation - this->positionMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - this->forceMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - this->node.Request("/marker", this->positionMarkerMsg); - this->node.Request("/marker", this->forceMarkerMsg); - - // Add the new ones + // Add new markers with specific lifetime this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->positionMarkerMsg.mutable_lifetime()->set_sec(0); + this->positionMarkerMsg.mutable_lifetime()->set_nsec(this->updatePeriod * 1000000); this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->forceMarkerMsg.mutable_lifetime()->set_sec(0); + this->forceMarkerMsg.mutable_lifetime()->set_nsec(this->updatePeriod * 1000000); for (uint64_t index = 0; index < positions.size(); ++index) { @@ -367,9 +365,11 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr-> positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); this->dataPtr-> - positionMarkerMsg.mutable_lifetime()->set_sec(2); + positionMarkerMsg.mutable_lifetime()-> + set_sec(0); this->dataPtr-> - positionMarkerMsg.mutable_lifetime()->set_nsec(0); + positionMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->updatePeriod * 1000000); this->dataPtr-> forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); @@ -388,9 +388,11 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr-> forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); this->dataPtr-> - forceMarkerMsg.mutable_lifetime()->set_sec(2); + forceMarkerMsg.mutable_lifetime()-> + set_sec(0); this->dataPtr-> - forceMarkerMsg.mutable_lifetime()->set_nsec(0); + forceMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->updatePeriod * 1000000); // Set scales ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), From d480c54b16a709671071c8f7d13e60f6c17c3ff0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 17 Jun 2020 11:03:56 +0200 Subject: [PATCH 11/23] Added Depth Camera 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 | 41 +++++++++++++++---- 1 file changed, 34 insertions(+), 7 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 6e64987bc5..a8bb05fad9 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -18,6 +18,11 @@ filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster"> + + ogre2 + @@ -29,7 +34,7 @@ docked - ogre + ogre2 scene 0.4 0.4 0.4 0.8 0.8 0.8 @@ -58,6 +63,8 @@ true /world/touch/control /world/touch/stats + /world/depth_camera_sensor/control + /world/depth_camera_sensor/stats @@ -83,7 +90,13 @@ true true /world/touch/stats + /world/depth_camera_sensor/stats + + + + docked + @@ -128,19 +141,19 @@ - 0 0 1.25 0 0 0 + 1.5 0 0.5 0 0 0 - 0.5 0.5 0.5 + 1 1 1 - 0.5 0.5 0.5 + 1 1 1 @@ -149,11 +162,11 @@ + true 0 0 0.5 0 0 0 - @@ -169,7 +182,22 @@ - + + 1 + depth_camera + + 1.05 + + 256 + 256 + RGB_INT8 + + + 0.1 + 10.0 + + + collision @@ -189,4 +217,3 @@ - From 3774efcd60ec3c9ee32083384cca59633ad9f96d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 17 Jun 2020 11:05:42 +0200 Subject: [PATCH 12/23] Added callback and unpacking to Depth Camera messages MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 146 +++++++++++++++++- 1 file changed, 140 insertions(+), 6 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 2a16ca394d..d16996e8cb 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -29,6 +29,7 @@ #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Sensor.hh" @@ -55,16 +56,40 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: void Enable(const bool _value); /// \brief Filters out new collisions fetched not related to the sensors + /// \param[in] _ecm Immutable reference to the EntityComponentManager + /// \param[in] _entities Collision entities to filter public: void FilterOutCollisions(const EntityComponentManager &_ecm, const std::vector &_entities); /// \brief Visualize the sensor's data using the Marker message + /// \param[in] positions Contact positions to visualize public: void VisualizeSensorData( std::vector &positions); /// \brief Interpolates contact data + /// \param[in] contactMsg Message containing the data to interpolate public: void InterpolateData(const ignition::msgs::Contact &contactMsg); + /// \brief Callback for the Depth Camera + /// \param[in] _msg Message from the subscribed topic + public: void DepthCameraCallback( + const ignition::msgs::PointCloudPacked &_msg); + + /// \brief Unpacks the point cloud retrieved by the Depth Camera + /// into XYZ and RGB data + /// \param[in] _msg Message containing the data to unpack + /// \param[in] _xyzBuffer Buffer with the unpacked XYZ data + /// \param[in] _rgbBuffer Buffer with the unpacked RGB data + public: void UnpackPointCloudMsg( + const ignition::msgs::PointCloudPacked &_msg, + float *_xyzBuffer, unsigned char *_rgbBuffer); + + /// \brief Computes the normal forces of the Optical Tactile sensor + /// \param[in] imageHeight Height of the image retrieved by the Depth Camera + /// \param[in] imageWidth Width of the image retrieved by the Depth Camera + public: void ComputeNormalForces(unsigned int imageHeight, + unsigned int imageWidth); + /// \brief Resolution of the sensor in mm. public: double resolution; @@ -74,9 +99,6 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Transport node to visualize data on Gazebo. public: transport::Node node; - /// \brief Publisher that publishes the sensors' contacts. - public: std::optionalsensorContactsPub; - /// \brief Collision entities that have been designated as contact sensors. public: std::vector sensorEntities; @@ -122,6 +144,12 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Allows the plugin to run public: bool update{true}; + + /// \brief Pointer to the XYZ data of the Depth Camera + float *pointsXYZBuffer = nullptr; + + /// \brief Pointer to the point cloud RGB data of the Depth Camera + unsigned char *pointsRGBBuffer = nullptr; }; ////////////////////////////////////////////////// @@ -288,10 +316,12 @@ void OpticalTactilePluginPrivate::VisualizeSensorData( // Add new markers with specific lifetime this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); this->positionMarkerMsg.mutable_lifetime()->set_sec(0); - this->positionMarkerMsg.mutable_lifetime()->set_nsec(this->updatePeriod * 1000000); + this->positionMarkerMsg.mutable_lifetime()->set_nsec( + this->updatePeriod * 1000000); this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); this->forceMarkerMsg.mutable_lifetime()->set_sec(0); - this->forceMarkerMsg.mutable_lifetime()->set_nsec(this->updatePeriod * 1000000); + this->forceMarkerMsg.mutable_lifetime()->set_nsec( + this->updatePeriod * 1000000); for (uint64_t index = 0; index < positions.size(); ++index) { @@ -322,10 +352,26 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, EntityComponentManager &, EventManager &) { - igndbg << "OpticalTactilePlugin::Configure" << std::endl; this->dataPtr->sdfConfig = _sdf->Clone(); this->dataPtr->model = Model(_entity); + // Configure subscriber for Depth Camera images + sdf::ElementPtr depthCameraSdf = + _sdf->GetParent()->GetElement("link")->GetElement("sensor"); + std::string topic = "/depth_camera/points"; + + if (depthCameraSdf->HasElement("topic")) + topic = "/" + depthCameraSdf->Get("topic") + "/points"; + + if (!this->dataPtr->node.Subscribe(topic, + &OpticalTactilePluginPrivate::DepthCameraCallback, + this->dataPtr.get())) + { + ignerr << "Error subscribing to topic " << "[" << topic << "]. " + " must not contain '/'" << std::endl; + return; + } + // Configure Marker messages for position and force of the contacts // Blue spheres for positions // Green cylinders for forces @@ -456,6 +502,94 @@ void OpticalTactilePlugin::PostUpdate( this->dataPtr->update = false; } +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::DepthCameraCallback( + const ignition::msgs::PointCloudPacked &_msg) +{ + unsigned int pointCloudSamples = _msg.width() * _msg.height(); + unsigned int pointCloudBufferSize = pointCloudSamples * 3; + if (!this->pointsXYZBuffer) + this->pointsXYZBuffer = new float[pointCloudBufferSize]; + if (!this->pointsRGBBuffer) + pointsRGBBuffer = new unsigned char[pointCloudBufferSize]; + + this->UnpackPointCloudMsg(_msg, this->pointsXYZBuffer, + this->pointsRGBBuffer); + + this->ComputeNormalForces(_msg.height(), _msg.width()); +} + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::UnpackPointCloudMsg( + const ignition::msgs::PointCloudPacked &_msg, + float *_xyzBuffer, unsigned char *_rgbBuffer) +{ + std::string msgBuffer = _msg.data(); + char *msgBufferIndex = msgBuffer.data(); + + for (uint32_t j = 0; j < _msg.height(); ++j) + { + for (uint32_t i = 0; i < _msg.width(); ++i) + { + int fieldIndex = 0; + int pointIndex = j*_msg.width()*3 + i*3; + + _xyzBuffer[pointIndex] = *reinterpret_cast( + msgBufferIndex + _msg.field(fieldIndex++).offset()); + _xyzBuffer[pointIndex + 1] = *reinterpret_cast( + msgBufferIndex + _msg.field(fieldIndex++).offset()); + _xyzBuffer[pointIndex + 2] = *reinterpret_cast( + msgBufferIndex + _msg.field(fieldIndex++).offset()); + + int fieldOffset = _msg.field(fieldIndex).offset(); + if (_msg.is_bigendian()) + { + _rgbBuffer[pointIndex] = *(msgBufferIndex + fieldOffset + 0); + _rgbBuffer[pointIndex + 1] = *(msgBufferIndex + fieldOffset + 1); + _rgbBuffer[pointIndex + 2] = *(msgBufferIndex + fieldOffset + 2); + } + else + { + _rgbBuffer[pointIndex] = *(msgBufferIndex + fieldOffset + 2); + _rgbBuffer[pointIndex + 1] = *(msgBufferIndex + fieldOffset + 1); + _rgbBuffer[pointIndex + 2] = *(msgBufferIndex + fieldOffset + 0); + } + msgBufferIndex += _msg.point_step(); + } + } +} + +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::ComputeNormalForces( + unsigned int imageHeight, + unsigned int imageWidth) +{ + for (unsigned int i = 0; i < imageHeight; ++i) + { + unsigned int step = i*imageWidth*3; + for (unsigned int j = 0; j < imageWidth; ++j) + { + // TODO(mcres): compute actual normal forces + + // Print distance to object + if ((i == imageHeight/2 && j == imageWidth/2) || + (i == 0 && j == 0) || + (i == 0 && j == (imageWidth-1)) || + (i == (imageHeight-1) && j == 0) || + (i == (imageHeight-1) && j == (imageWidth-1))) + { + float x = this->pointsXYZBuffer[step + j*3]; + float y = this->pointsXYZBuffer[step + j*3 + 1]; + float z = this->pointsXYZBuffer[step + j*3 + 2]; + + igndbg << "[DepthCamera] x = " << x << std::endl; + igndbg << "[DepthCamera] y = " << y << std::endl; + igndbg << "[DepthCamera] z = " << z << std::endl; + } + } + } +} + IGNITION_ADD_PLUGIN(OpticalTactilePlugin, ignition::gazebo::System, OpticalTactilePlugin::ISystemConfigure, From 2bab9c83ee11401cae84c6888c66c64721cece71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 25 Jun 2020 16:24:22 +0200 Subject: [PATCH 13/23] Compute and visualize normal forces 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 | 75 +++- .../OpticalTactilePlugin.cc | 365 +++++++++++------- 2 files changed, 287 insertions(+), 153 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index a8bb05fad9..1b6fe3ac07 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -141,19 +141,19 @@ - 1.5 0 0.5 0 0 0 + 0 3 3.5 0 0 0 - 1 1 1 + 3 2 3 - 1 1 1 + 3 2 3 @@ -166,19 +166,19 @@ - 0 0 0.5 0 0 0 + 0 0 3.5 0 0 0 - 1 1 1 + 0 1 1 - 1 1 1 + 0 1 1 @@ -190,11 +190,11 @@ 256 256 - RGB_INT8 + R_FLOAT32 0.1 - 10.0 + 5.0 @@ -205,15 +205,68 @@ - + true true - 200 - "Gaussian" + 30 + true + + 0 -3 3.5 0 0 0 + + + + + 1.5 + + + + + + + 1.5 + + + + 0 1 0 1 + 0 1 0 1 + + + + true + + + + 3 0 3.5 0 0 0 + + + + + 1.3 + 3 + + + + + + + 1.3 + 3 + + + + 0 1 0 1 + 0 1 0 1 + + + + true + + + diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index d16996e8cb..cf648907ba 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" @@ -61,37 +62,38 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: void FilterOutCollisions(const EntityComponentManager &_ecm, const std::vector &_entities); - /// \brief Visualize the sensor's data using the Marker message - /// \param[in] positions Contact positions to visualize - public: void VisualizeSensorData( - std::vector &positions); - /// \brief Interpolates contact data - /// \param[in] contactMsg Message containing the data to interpolate - public: void InterpolateData(const ignition::msgs::Contact &contactMsg); + /// \param[in] _contactMsg Message containing the data to interpolate + public: void InterpolateData(const ignition::msgs::Contact &_contactMsg); /// \brief Callback for the Depth Camera /// \param[in] _msg Message from the subscribed topic public: void DepthCameraCallback( - const ignition::msgs::PointCloudPacked &_msg); + const ignition::msgs::PointCloudPacked &_msg); /// \brief Unpacks the point cloud retrieved by the Depth Camera - /// into XYZ and RGB data + /// into XYZ data /// \param[in] _msg Message containing the data to unpack - /// \param[in] _xyzBuffer Buffer with the unpacked XYZ data - /// \param[in] _rgbBuffer Buffer with the unpacked RGB data public: void UnpackPointCloudMsg( - const ignition::msgs::PointCloudPacked &_msg, - float *_xyzBuffer, unsigned char *_rgbBuffer); + const ignition::msgs::PointCloudPacked &_msg); /// \brief Computes the normal forces of the Optical Tactile sensor - /// \param[in] imageHeight Height of the image retrieved by the Depth Camera - /// \param[in] imageWidth Width of the image retrieved by the Depth Camera - public: void ComputeNormalForces(unsigned int imageHeight, - unsigned int imageWidth); - - /// \brief Resolution of the sensor in mm. - public: double resolution; + /// \param[in] _msg Message returned by the Depth Camera + /// \param[in] _visualizeForces Whether to visualize the forces or not + /// + /// Implementation inspired by + /// https://stackoverflow.com/questions/ + /// 34644101/calculate-surface-normals-from-depth-image- + /// using-neighboring-pixels-cross-produc + public: void ComputeNormalForces( + const ignition::msgs::PointCloudPacked &_msg, + const bool _visualizeForces); + + /// \brief Resolution of the sensor in pixels to skip. + public: int resolution{100}; + + /// \brief Whether to visualize the normal forces. + public: int visualize_forces{false}; /// \brief Model interface. public: Model model{kNullEntity}; @@ -118,14 +120,14 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Copy of the sdf configuration used for this plugin. public: sdf::ElementPtr sdfConfig; - /// \brief Name of the model in which the sensor(s) is attached. + /// \brief Name of the model to which the sensor is attached. public: std::string modelName; /// \brief Message for visualizing contact positions public: ignition::msgs::Marker positionMarkerMsg; /// \brief Radius of the visualized contact sphere - public: double contactRadius{0.20}; + public: double contactRadius{0.10}; /// \brief Message for visualizing contact forces public: ignition::msgs::Marker forceMarkerMsg; @@ -136,20 +138,20 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Position interpolated from the Contact messages public: std::vector interpolatedPosition; - /// \brief Update period in milliseconds - public: int64_t updatePeriod{100}; + /// \brief Pose of the sensor model + public: ignition::math::Pose3f sensorWorldPose; - /// \brief Last time Update was called - public: std::chrono::steady_clock::duration lastUpdateTime{0}; + /// \brief Whether a new message has been returned by the Depth Camera + public: bool newCameraMsg{false}; - /// \brief Allows the plugin to run - public: bool update{true}; + /// \brief Depth Camera update rate + public: float cameraUpdateRate{1}; - /// \brief Pointer to the XYZ data of the Depth Camera - float *pointsXYZBuffer = nullptr; + /// \brief Message returned by the Depth Camera + public: ignition::msgs::PointCloudPacked cameraMsg; - /// \brief Pointer to the point cloud RGB data of the Depth Camera - unsigned char *pointsRGBBuffer = nullptr; + /// \brief 3D vector to store unpacked XYZ points + std::vector>> imageXYZ; }; ////////////////////////////////////////////////// @@ -166,8 +168,17 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, } else { - // resolution is specified in mm - this->resolution = _sdf->Get("resolution")/1000; + this->resolution = _sdf->Get("resolution"); + } + + if (!_sdf->HasElement("visualize_forces")) + { + ignerr << "Missing required parameter ." << std::endl; + return; + } + else + { + this->visualize_forces = _sdf->Get("visualize_forces"); } // Get all the sensors @@ -206,16 +217,11 @@ void OpticalTactilePlugin::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { // Nothing left to do if paused or not allowed to update - if (_info.paused || !this->dataPtr->update) + if (_info.paused) return; IGN_PROFILE("TouchPluginPrivate::Update"); - // Save the time for last time this function was called - this->dataPtr->lastUpdateTime = _info.simTime; - - // Step 1: Interpolate data - // Delete old interpolated positions this->dataPtr->interpolatedPosition.clear(); @@ -233,8 +239,15 @@ void OpticalTactilePlugin::Update(const UpdateInfo &_info, } } - // Step 2: Visualize data - this->dataPtr->VisualizeSensorData(this->dataPtr->interpolatedPosition); + // Process camera message if it's new + if (this->dataPtr->newCameraMsg) + { + this->dataPtr->UnpackPointCloudMsg(this->dataPtr->cameraMsg); + this->dataPtr->ComputeNormalForces(this->dataPtr->cameraMsg, + this->dataPtr->visualize_forces); + + this->dataPtr->newCameraMsg = false; + } } ////////////////////////////////////////////////// @@ -309,37 +322,6 @@ void OpticalTactilePluginPrivate::InterpolateData( } } -////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::VisualizeSensorData( - std::vector &positions) -{ - // Add new markers with specific lifetime - this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->positionMarkerMsg.mutable_lifetime()->set_sec(0); - this->positionMarkerMsg.mutable_lifetime()->set_nsec( - this->updatePeriod * 1000000); - this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->forceMarkerMsg.mutable_lifetime()->set_sec(0); - this->forceMarkerMsg.mutable_lifetime()->set_nsec( - this->updatePeriod * 1000000); - - for (uint64_t index = 0; index < positions.size(); ++index) - { - this->positionMarkerMsg.set_id(index); - this->forceMarkerMsg.set_id(index); - - ignition::msgs::Set(this->positionMarkerMsg.mutable_pose(), - ignition::math::Pose3d(positions[index].X(), positions[index].Y(), - positions[index].Z(), 0, 0, 0)); - ignition::msgs::Set(this->forceMarkerMsg.mutable_pose(), - ignition::math::Pose3d(positions[index].X(), positions[index].Y(), - positions[index].Z() + this->forceLenght, 0, 0, 0)); - - this->node.Request("/marker", this->positionMarkerMsg); - this->node.Request("/marker", this->forceMarkerMsg); - } -} - ////////////////////////////////////////////////// OpticalTactilePlugin::OpticalTactilePlugin() : System(), dataPtr(std::make_unique()) @@ -349,12 +331,19 @@ OpticalTactilePlugin::OpticalTactilePlugin() ////////////////////////////////////////////////// void OpticalTactilePlugin::Configure(const Entity &_entity, const std::shared_ptr &_sdf, - EntityComponentManager &, + EntityComponentManager &_ecm, EventManager &) { this->dataPtr->sdfConfig = _sdf->Clone(); this->dataPtr->model = Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "Touch plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + // Configure subscriber for Depth Camera images sdf::ElementPtr depthCameraSdf = _sdf->GetParent()->GetElement("link")->GetElement("sensor"); @@ -412,10 +401,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); this->dataPtr-> positionMarkerMsg.mutable_lifetime()-> - set_sec(0); - this->dataPtr-> - positionMarkerMsg.mutable_lifetime()-> - set_nsec(this->dataPtr->updatePeriod * 1000000); + set_sec(this->dataPtr->cameraUpdateRate); this->dataPtr-> forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); @@ -435,10 +421,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); this->dataPtr-> forceMarkerMsg.mutable_lifetime()-> - set_sec(0); - this->dataPtr-> - forceMarkerMsg.mutable_lifetime()-> - set_nsec(this->dataPtr->updatePeriod * 1000000); + set_sec(this->dataPtr->cameraUpdateRate); // Set scales ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), @@ -447,7 +430,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->contactRadius)); ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.05, 0.05, this->dataPtr->forceLenght)); + ignition::math::Vector3d(0.02, 0.02, this->dataPtr->forceLenght)); } ////////////////////////////////////////////////// @@ -479,6 +462,17 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, return true; }); this->dataPtr->FilterOutCollisions(_ecm, newCollisions); + + // Get the Depth Camera pose + ignition::math::Pose3d sensorPose = + _ecm.Component( + this->dataPtr->model.Entity())->Data(); + + // Depth Camera data is float, so convert Pose3d to Pose3f + this->dataPtr->sensorWorldPose = ignition::math::Pose3f( + sensorPose.Pos().X(), sensorPose.Pos().Y(), sensorPose.Pos().Z(), + sensorPose.Rot().W(), sensorPose.Rot().X(), sensorPose.Rot().Y(), + sensorPose.Rot().Z()); } } @@ -492,38 +486,32 @@ void OpticalTactilePlugin::PostUpdate( // Nothing left to do if paused. if (_info.paused) return; - - // Check if info should be updated - this->dataPtr->update = true; - auto diff = std::chrono::duration_cast( - _info.simTime - this->dataPtr->lastUpdateTime); - - if ((diff.count() >= 0) && (diff.count() < this->dataPtr->updatePeriod)) - this->dataPtr->update = false; } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::DepthCameraCallback( const ignition::msgs::PointCloudPacked &_msg) { - unsigned int pointCloudSamples = _msg.width() * _msg.height(); - unsigned int pointCloudBufferSize = pointCloudSamples * 3; - if (!this->pointsXYZBuffer) - this->pointsXYZBuffer = new float[pointCloudBufferSize]; - if (!this->pointsRGBBuffer) - pointsRGBBuffer = new unsigned char[pointCloudBufferSize]; - - this->UnpackPointCloudMsg(_msg, this->pointsXYZBuffer, - this->pointsRGBBuffer); + // This check avoids running the callback at t=0 and getting + // unexpected markers in the scene + if (!this->initialized) + return; - this->ComputeNormalForces(_msg.height(), _msg.width()); + this->cameraMsg = _msg; + this->newCameraMsg = true; } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::UnpackPointCloudMsg( - const ignition::msgs::PointCloudPacked &_msg, - float *_xyzBuffer, unsigned char *_rgbBuffer) + const ignition::msgs::PointCloudPacked &_msg) { + // 3D vector to store XYZ points + this->imageXYZ = std::vector>> + (_msg.width(), + std::vector>(_msg.height(), + std::vector(3))); + + // Unpack point cloud message std::string msgBuffer = _msg.data(); char *msgBufferIndex = msgBuffer.data(); @@ -532,28 +520,17 @@ void OpticalTactilePluginPrivate::UnpackPointCloudMsg( for (uint32_t i = 0; i < _msg.width(); ++i) { int fieldIndex = 0; - int pointIndex = j*_msg.width()*3 + i*3; - _xyzBuffer[pointIndex] = *reinterpret_cast( + // x coordinate in pixel (i,j) + this->imageXYZ[i][j][0] = *reinterpret_cast( msgBufferIndex + _msg.field(fieldIndex++).offset()); - _xyzBuffer[pointIndex + 1] = *reinterpret_cast( + // y coordinate in pixel (i,j) + this->imageXYZ[i][j][1] = *reinterpret_cast( msgBufferIndex + _msg.field(fieldIndex++).offset()); - _xyzBuffer[pointIndex + 2] = *reinterpret_cast( + // z coordinate in pixel (i,j) + this->imageXYZ[i][j][2] = *reinterpret_cast( msgBufferIndex + _msg.field(fieldIndex++).offset()); - int fieldOffset = _msg.field(fieldIndex).offset(); - if (_msg.is_bigendian()) - { - _rgbBuffer[pointIndex] = *(msgBufferIndex + fieldOffset + 0); - _rgbBuffer[pointIndex + 1] = *(msgBufferIndex + fieldOffset + 1); - _rgbBuffer[pointIndex + 2] = *(msgBufferIndex + fieldOffset + 2); - } - else - { - _rgbBuffer[pointIndex] = *(msgBufferIndex + fieldOffset + 2); - _rgbBuffer[pointIndex + 1] = *(msgBufferIndex + fieldOffset + 1); - _rgbBuffer[pointIndex + 2] = *(msgBufferIndex + fieldOffset + 0); - } msgBufferIndex += _msg.point_step(); } } @@ -561,32 +538,136 @@ void OpticalTactilePluginPrivate::UnpackPointCloudMsg( ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::ComputeNormalForces( - unsigned int imageHeight, - unsigned int imageWidth) + const ignition::msgs::PointCloudPacked &_msg, + const bool _visualizeForces) { - for (unsigned int i = 0; i < imageHeight; ++i) + // 2D vector to store normal force vectors + std::vector> normalForces(_msg.width(), + std::vector(_msg.height())); + + // Visualization is downsampled according to plugin parameter + // if is set to true + bool visualizeRow = true; + int rowCounter = 0; + + bool visualizeColumn = true; + int columnCounter = 0; + + int index = 1; + + + for (uint32_t j = 1; j < (_msg.height() - 1); ++j) { - unsigned int step = i*imageWidth*3; - for (unsigned int j = 0; j < imageWidth; ++j) + for (uint32_t i = 1; i < (_msg.width() - 1); ++i) { - // TODO(mcres): compute actual normal forces - - // Print distance to object - if ((i == imageHeight/2 && j == imageWidth/2) || - (i == 0 && j == 0) || - (i == 0 && j == (imageWidth-1)) || - (i == (imageHeight-1) && j == 0) || - (i == (imageHeight-1) && j == (imageWidth-1))) - { - float x = this->pointsXYZBuffer[step + j*3]; - float y = this->pointsXYZBuffer[step + j*3 + 1]; - float z = this->pointsXYZBuffer[step + j*3 + 2]; - - igndbg << "[DepthCamera] x = " << x << std::endl; - igndbg << "[DepthCamera] y = " << y << std::endl; - igndbg << "[DepthCamera] z = " << z << std::endl; - } + // Compute normal forces + double dxdi = + (this->imageXYZ[i+1][j][0] - + this->imageXYZ[i-1][j][0])/ + std::abs(this->imageXYZ[i+1][j][1] - + this->imageXYZ[i-1][j][1]); + + double dxdj = + (this->imageXYZ[i][j+1][0] + - this->imageXYZ[i][j-1][0])/ + std::abs(this->imageXYZ[i][j+1][2] + - this->imageXYZ[i][j-1][2]); + + ignition::math::Vector3f direction(-1, -dxdi, -dxdj); + + // todo(anyone) multiply vector by contact forces info + normalForces[i][j] = direction.Normalized(); + + if (!_visualizeForces) + continue; + + // Downsampling for visualization + if (visualizeRow && visualizeColumn) + { + // Add new markers with specific lifetime + this->positionMarkerMsg.set_id(index); + this->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->positionMarkerMsg.mutable_lifetime()->set_sec( + this->cameraUpdateRate); + this->positionMarkerMsg.mutable_lifetime()->set_nsec(0); + + this->forceMarkerMsg.set_id(index++); + this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->forceMarkerMsg.mutable_lifetime()->set_sec( + this->cameraUpdateRate); + this->forceMarkerMsg.mutable_lifetime()->set_nsec(0); + + // The pose of the markers must be published with reference + // to the simulation origin + ignition::math::Vector3f forceMarkerSensorPosition + (this->imageXYZ[i][j][0], this->imageXYZ[i][j][1], + this->imageXYZ[i][j][2]); + + ignition::math::Quaternionf forceMarkerSensorQuaternion; + forceMarkerSensorQuaternion.From2Axes( + ignition::math::Vector3f(0, 0, 1), normalForces[i][j]); + + // The position of the force marker is modified in order to place the + // end of the cylinder in the surface, not its middle point + ignition::math::Pose3f forceMarkerSensorPose( + forceMarkerSensorPosition + + normalForces[i][j] * (this->forceLenght/2), + forceMarkerSensorQuaternion); + + ignition::math::Pose3f forceMarkerWorldPose = + forceMarkerSensorPose + this->sensorWorldPose; + forceMarkerWorldPose.Correct(); + + ignition::math::Pose3f positionMarkerSensorPose( + forceMarkerSensorPosition, forceMarkerSensorQuaternion); + ignition::math::Pose3f positionMarkerWorldPose = + positionMarkerSensorPose + this->sensorWorldPose; + positionMarkerWorldPose.Correct(); + + // Set markers pose messages from previously computed poses + this->forceMarkerMsg.mutable_pose() + ->mutable_position()->set_x(forceMarkerWorldPose.Pos().X()); + this->forceMarkerMsg.mutable_pose() + ->mutable_position()->set_y(forceMarkerWorldPose.Pos().Y()); + this->forceMarkerMsg.mutable_pose() + ->mutable_position()->set_z(forceMarkerWorldPose.Pos().Z()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_x(forceMarkerWorldPose.Rot().X()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_y(forceMarkerWorldPose.Rot().Y()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_z(forceMarkerWorldPose.Rot().Z()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_w(forceMarkerWorldPose.Rot().W()); + + + this->positionMarkerMsg.mutable_pose() + ->mutable_position()->set_x(positionMarkerWorldPose.Pos().X()); + this->positionMarkerMsg.mutable_pose() + ->mutable_position()->set_y(positionMarkerWorldPose.Pos().Y()); + this->positionMarkerMsg.mutable_pose() + ->mutable_position()->set_z(positionMarkerWorldPose.Pos().Z()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_x(positionMarkerWorldPose.Rot().X()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_y(positionMarkerWorldPose.Rot().Y()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_z(positionMarkerWorldPose.Rot().Z()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_w(positionMarkerWorldPose.Rot().W()); + + this->node.Request("/marker", this->forceMarkerMsg); + this->node.Request("/marker", this->positionMarkerMsg); + } + visualizeColumn = (this->resolution == ++columnCounter); + if (visualizeColumn) + columnCounter = 0; } + visualizeColumn = true; + visualizeRow = (this->resolution == ++rowCounter); + if (visualizeRow) + rowCounter = 0; } } From badd572123488308439c4bb34d7e9819dda38a08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 9 Jul 2020 11:46:19 +0200 Subject: [PATCH 14/23] PR Feedback 1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../OpticalTactilePlugin.cc | 435 ++++++++++-------- .../OpticalTactilePlugin.hh | 25 +- 2 files changed, 259 insertions(+), 201 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index cf648907ba..c8fc639afa 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.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. @@ -50,7 +50,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \param[in] _sdf SDF element describing the Contact sensor /// \param[in] _ecm Immutable reference to the EntityComponentManager public: void Load(const EntityComponentManager &_ecm, - const sdf::ElementPtr &_sdf); + const std::shared_ptr &_sdf); /// \brief Actual function that enables the plugin. /// \param[in] _value True to enable plugin. @@ -60,7 +60,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \param[in] _ecm Immutable reference to the EntityComponentManager /// \param[in] _entities Collision entities to filter public: void FilterOutCollisions(const EntityComponentManager &_ecm, - const std::vector &_entities); + const std::vector &_entities); /// \brief Interpolates contact data /// \param[in] _contactMsg Message containing the data to interpolate @@ -69,13 +69,13 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Callback for the Depth Camera /// \param[in] _msg Message from the subscribed topic public: void DepthCameraCallback( - const ignition::msgs::PointCloudPacked &_msg); + const ignition::msgs::PointCloudPacked &_msg); /// \brief Unpacks the point cloud retrieved by the Depth Camera /// into XYZ data /// \param[in] _msg Message containing the data to unpack public: void UnpackPointCloudMsg( - const ignition::msgs::PointCloudPacked &_msg); + const ignition::msgs::PointCloudPacked &_msg); /// \brief Computes the normal forces of the Optical Tactile sensor /// \param[in] _msg Message returned by the Depth Camera @@ -86,14 +86,14 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// 34644101/calculate-surface-normals-from-depth-image- /// using-neighboring-pixels-cross-produc public: void ComputeNormalForces( - const ignition::msgs::PointCloudPacked &_msg, - const bool _visualizeForces); + const ignition::msgs::PointCloudPacked &_msg, + const bool _visualizeForces); /// \brief Resolution of the sensor in pixels to skip. public: int resolution{100}; /// \brief Whether to visualize the normal forces. - public: int visualize_forces{false}; + public: bool visualizeForces{false}; /// \brief Model interface. public: Model model{kNullEntity}; @@ -102,7 +102,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: transport::Node node; /// \brief Collision entities that have been designated as contact sensors. - public: std::vector sensorEntities; + public: ignition::gazebo::Entity contactSensorEntity; /// \brief Collision entities of the simulation. public: std::vector collisionEntities; @@ -111,14 +111,14 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// one or more sensors. public: std::vector filteredCollisionEntities; - /// \brief Wheter the plugin is enabled. + /// \brief Whether the plugin is enabled. public: bool enabled{true}; /// \brief Initialization flag. public: bool initialized{false}; /// \brief Copy of the sdf configuration used for this plugin. - public: sdf::ElementPtr sdfConfig; + public: std::shared_ptr sdfConfig{nullptr}; /// \brief Name of the model to which the sensor is attached. public: std::string modelName; @@ -127,13 +127,13 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: ignition::msgs::Marker positionMarkerMsg; /// \brief Radius of the visualized contact sphere - public: double contactRadius{0.10}; + public: double contactRadius{0.01}; /// \brief Message for visualizing contact forces public: ignition::msgs::Marker forceMarkerMsg; /// \brief Length of the visualized force cylinder - public: double forceLenght{0.20}; + public: double forceLength{0.03}; /// \brief Position interpolated from the Contact messages public: std::vector interpolatedPosition; @@ -152,104 +152,131 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief 3D vector to store unpacked XYZ points std::vector>> imageXYZ; + + /// \brief Mutex for variables mutated by the camera callback. + /// The variables are: newCameraMsg, cameraMsg. + public: std::mutex serviceMutex; }; ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, - const sdf::ElementPtr &_sdf) + const std::shared_ptr &_sdf) { igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; // Get sdf parameters if (!_sdf->HasElement("resolution")) { - ignerr << "Missing required parameter ." << std::endl; - return; + ignlog << "Missing required parameter , " + << "using default value" << std::endl; } else { - this->resolution = _sdf->Get("resolution"); + if (_sdf->Get("resolution") < 0) + { + ignwarn << "Parameter must be positive, " + << "using default value" << std::endl; + } + else + { + this->resolution = _sdf->Get("resolution"); + } } if (!_sdf->HasElement("visualize_forces")) { - ignerr << "Missing required parameter ." << std::endl; - return; + ignlog << "Missing required parameter , " + << "using default value" << std::endl; } else { - this->visualize_forces = _sdf->Get("visualize_forces"); + this->visualizeForces = _sdf->Get("visualize_forces"); } - // Get all the sensors + // todo(anyone) The plugin should be able to handle more than one link. In + // that case, one of the links would be the actual tactile sensor. + + // Check SDF structure + // Model should only have 1 link auto allLinks = _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); - - for (const Entity linkEntity : allLinks) + if (allLinks.size() != 1) { - auto linkCollisions = - _ecm.ChildrenByComponents(linkEntity, components::Collision()); - for (const Entity colEntity : linkCollisions) - { - if (_ecm.EntityHasComponentType(colEntity, - components::ContactSensorData::typeId)) - { - this->sensorEntities.push_back(colEntity); - - this->modelName = this->model.Name(_ecm); - - igndbg << "Sensor detected within model " - << this->modelName << std::endl; - } - } + ignerr << "Plugin must have 1 link" << std::endl; + return; } -} -////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool _value) -{ - // Just a placeholder method for the moment - _value; -} + // Link should have a collision component linked to a contact sensor + auto linkCollisions = + _ecm.ChildrenByComponents(allLinks[0], components::Collision()); + for (const Entity &colEntity : linkCollisions) + { + if (_ecm.EntityHasComponentType(colEntity, + components::ContactSensorData::typeId)) + { + this->contactSensorEntity = colEntity; -////////////////////////////////////////////////// -void OpticalTactilePlugin::Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ - // Nothing left to do if paused or not allowed to update - if (_info.paused) - return; + this->modelName = this->model.Name(_ecm); - IGN_PROFILE("TouchPluginPrivate::Update"); + igndbg << "Contactensor detected within model " + << this->modelName << std::endl; + } + } - // Delete old interpolated positions - this->dataPtr->interpolatedPosition.clear(); + // Link should have 1 Contact sensor and 1 Depth Camera sensor + auto sensorsInsideLink = + _ecm.ChildrenByComponents(allLinks[0], components::Sensor()); - for (const Entity colEntity : this->dataPtr->filteredCollisionEntities) + bool depthCameraInLink = false; + bool contactSensorInLink = false; + sdf::ElementPtr depthCameraSdf = nullptr; + for (const Entity &sensor : sensorsInsideLink) { - auto* contacts = - _ecm.Component(colEntity); - if (contacts) - { - for (const auto &contact : contacts->Data().contact()) - { - // Interpolate data returned by the Contact message - this->dataPtr->InterpolateData(contact); - } - } + if (_ecm.EntityHasComponentType(sensor, + components::DepthCamera::typeId)) + { + depthCameraInLink = true; + depthCameraSdf = + _ecm.Component(sensor)->Data().Element(); + } + + if (_ecm.EntityHasComponentType(sensor, + components::ContactSensor::typeId)) + { + contactSensorInLink = true; + } } - // Process camera message if it's new - if (this->dataPtr->newCameraMsg) + if (!depthCameraInLink || !contactSensorInLink || + sensorsInsideLink.size() != 2) { - this->dataPtr->UnpackPointCloudMsg(this->dataPtr->cameraMsg); - this->dataPtr->ComputeNormalForces(this->dataPtr->cameraMsg, - this->dataPtr->visualize_forces); + ignerr << "Link must have 1 Depth Camera sensor and " + << "1 Contact sensor" << std::endl; + return; + } + + // Configure subscriber for Depth Camera images + std::string topic = "/depth_camera/points"; + if (depthCameraSdf->HasElement("topic")) + topic = "/" + depthCameraSdf->Get("topic") + "/points"; - this->dataPtr->newCameraMsg = false; + if (!this->node.Subscribe(topic, + &OpticalTactilePluginPrivate::DepthCameraCallback, + this)) + { + ignerr << "Error subscribing to topic " << "[" << topic << "]. " + << " must not contain '/'" << std::endl; + return; } } +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::Enable(const bool _value) +{ + // Just a placeholder method for the moment + _value; +} + ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::FilterOutCollisions( const EntityComponentManager &_ecm, @@ -262,7 +289,7 @@ void OpticalTactilePluginPrivate::FilterOutCollisions( this->filteredCollisionEntities.clear(); // Get collisions from the sensor - for (Entity entity : _entities) + for (const Entity &entity : _entities) { std::string name = scopedName(entity, _ecm); @@ -277,6 +304,14 @@ void OpticalTactilePluginPrivate::FilterOutCollisions( void OpticalTactilePluginPrivate::InterpolateData( const ignition::msgs::Contact &contact) { + // The Optical Tactile Sensor is supposed to be box-shaped. + // Contacts returned by the Contact Sensor in this type of shapes should be + // 4 points in the corners of some of its faces, in this case the one + // corresponding to the 'sensing' face. + // Given that the number of contacts may change even when the objects are + // at rest, data is only interpolated when the number of contacts + // returned is 4. + if (contact.position_size() == 4) { ignition::math::Vector3d contact1 = @@ -334,30 +369,13 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, EntityComponentManager &_ecm, EventManager &) { - this->dataPtr->sdfConfig = _sdf->Clone(); + this->dataPtr->sdfConfig = _sdf; this->dataPtr->model = Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { ignerr << "Touch plugin should be attached to a model entity. " - << "Failed to initialize." << std::endl; - return; - } - - // Configure subscriber for Depth Camera images - sdf::ElementPtr depthCameraSdf = - _sdf->GetParent()->GetElement("link")->GetElement("sensor"); - std::string topic = "/depth_camera/points"; - - if (depthCameraSdf->HasElement("topic")) - topic = "/" + depthCameraSdf->Get("topic") + "/points"; - - if (!this->dataPtr->node.Subscribe(topic, - &OpticalTactilePluginPrivate::DepthCameraCallback, - this->dataPtr.get())) - { - ignerr << "Error subscribing to topic " << "[" << topic << "]. " - " must not contain '/'" << std::endl; + << "Failed to initialize." << std::endl; return; } @@ -430,7 +448,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->contactRadius)); ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.02, 0.02, this->dataPtr->forceLenght)); + ignition::math::Vector3d(0.003, 0.003, this->dataPtr->forceLength)); } ////////////////////////////////////////////////// @@ -445,6 +463,8 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, if (!this->dataPtr->initialized) { + // We call Load here instead of Configure because we can't be guaranteed + // that all entities have been created when Configure is called this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); this->dataPtr->initialized = true; } @@ -458,8 +478,8 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, _ecm.Each( [&](const Entity &_entity, const components::Collision *) -> bool { - newCollisions.push_back(_entity); - return true; + newCollisions.push_back(_entity); + return true; }); this->dataPtr->FilterOutCollisions(_ecm, newCollisions); @@ -479,13 +499,43 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void OpticalTactilePlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &) + const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("TouchPlugin::PostUpdate"); // Nothing left to do if paused. if (_info.paused) return; + + // Delete old interpolated positions + this->dataPtr->interpolatedPosition.clear(); + + for (const Entity &colEntity : this->dataPtr->filteredCollisionEntities) + { + auto* contacts = + _ecm.Component(colEntity); + if (contacts) + { + for (const auto &contact : contacts->Data().contact()) + { + // Interpolate data returned by the Contact message + this->dataPtr->InterpolateData(contact); + } + } + } + + // Process camera message if it's new + { + std::lock_guard lock(this->dataPtr->serviceMutex); + if (this->dataPtr->newCameraMsg) + { + this->dataPtr->UnpackPointCloudMsg(this->dataPtr->cameraMsg); + this->dataPtr->ComputeNormalForces(this->dataPtr->cameraMsg, + this->dataPtr->visualizeForces); + + this->dataPtr->newCameraMsg = false; + } + } } ////////////////////////////////////////////////// @@ -497,8 +547,11 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( if (!this->initialized) return; - this->cameraMsg = _msg; - this->newCameraMsg = true; + { + std::lock_guard lock(this->serviceMutex); + this->cameraMsg = _msg; + this->newCameraMsg = true; + } } ////////////////////////////////////////////////// @@ -515,9 +568,9 @@ void OpticalTactilePluginPrivate::UnpackPointCloudMsg( std::string msgBuffer = _msg.data(); char *msgBufferIndex = msgBuffer.data(); - for (uint32_t j = 0; j < _msg.height(); ++j) + for (uint64_t j = 0; j < _msg.height(); ++j) { - for (uint32_t i = 0; i < _msg.width(); ++i) + for (uint64_t i = 0; i < _msg.width(); ++i) { int fieldIndex = 0; @@ -553,12 +606,12 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( bool visualizeColumn = true; int columnCounter = 0; - int index = 1; + // Variable for setting the markers id through the iteration + int marker_id = 1; - - for (uint32_t j = 1; j < (_msg.height() - 1); ++j) + for (uint64_t j = 1; j < (_msg.height() - 1); ++j) { - for (uint32_t i = 1; i < (_msg.width() - 1); ++i) + for (uint64_t i = 1; i < (_msg.width() - 1); ++i) { // Compute normal forces double dxdi = @@ -568,106 +621,107 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( this->imageXYZ[i-1][j][1]); double dxdj = - (this->imageXYZ[i][j+1][0] - - this->imageXYZ[i][j-1][0])/ - std::abs(this->imageXYZ[i][j+1][2] - - this->imageXYZ[i][j-1][2]); + (this->imageXYZ[i][j+1][0] - + this->imageXYZ[i][j-1][0])/ + std::abs(this->imageXYZ[i][j+1][2] - + this->imageXYZ[i][j-1][2]); ignition::math::Vector3f direction(-1, -dxdi, -dxdj); // todo(anyone) multiply vector by contact forces info normalForces[i][j] = direction.Normalized(); + // todo(mcres) normal forces will be published even if visualization + // is turned off if (!_visualizeForces) continue; // Downsampling for visualization if (visualizeRow && visualizeColumn) - { - // Add new markers with specific lifetime - this->positionMarkerMsg.set_id(index); - this->positionMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->positionMarkerMsg.mutable_lifetime()->set_sec( - this->cameraUpdateRate); - this->positionMarkerMsg.mutable_lifetime()->set_nsec(0); - - this->forceMarkerMsg.set_id(index++); - this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->forceMarkerMsg.mutable_lifetime()->set_sec( - this->cameraUpdateRate); - this->forceMarkerMsg.mutable_lifetime()->set_nsec(0); - - // The pose of the markers must be published with reference - // to the simulation origin - ignition::math::Vector3f forceMarkerSensorPosition - (this->imageXYZ[i][j][0], this->imageXYZ[i][j][1], - this->imageXYZ[i][j][2]); - - ignition::math::Quaternionf forceMarkerSensorQuaternion; - forceMarkerSensorQuaternion.From2Axes( - ignition::math::Vector3f(0, 0, 1), normalForces[i][j]); - - // The position of the force marker is modified in order to place the - // end of the cylinder in the surface, not its middle point - ignition::math::Pose3f forceMarkerSensorPose( - forceMarkerSensorPosition + - normalForces[i][j] * (this->forceLenght/2), - forceMarkerSensorQuaternion); - - ignition::math::Pose3f forceMarkerWorldPose = - forceMarkerSensorPose + this->sensorWorldPose; - forceMarkerWorldPose.Correct(); - - ignition::math::Pose3f positionMarkerSensorPose( - forceMarkerSensorPosition, forceMarkerSensorQuaternion); - ignition::math::Pose3f positionMarkerWorldPose = - positionMarkerSensorPose + this->sensorWorldPose; - positionMarkerWorldPose.Correct(); - - // Set markers pose messages from previously computed poses - this->forceMarkerMsg.mutable_pose() - ->mutable_position()->set_x(forceMarkerWorldPose.Pos().X()); - this->forceMarkerMsg.mutable_pose() - ->mutable_position()->set_y(forceMarkerWorldPose.Pos().Y()); - this->forceMarkerMsg.mutable_pose() - ->mutable_position()->set_z(forceMarkerWorldPose.Pos().Z()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_x(forceMarkerWorldPose.Rot().X()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_y(forceMarkerWorldPose.Rot().Y()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_z(forceMarkerWorldPose.Rot().Z()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_w(forceMarkerWorldPose.Rot().W()); - - - this->positionMarkerMsg.mutable_pose() - ->mutable_position()->set_x(positionMarkerWorldPose.Pos().X()); - this->positionMarkerMsg.mutable_pose() - ->mutable_position()->set_y(positionMarkerWorldPose.Pos().Y()); - this->positionMarkerMsg.mutable_pose() - ->mutable_position()->set_z(positionMarkerWorldPose.Pos().Z()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_x(positionMarkerWorldPose.Rot().X()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_y(positionMarkerWorldPose.Rot().Y()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_z(positionMarkerWorldPose.Rot().Z()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_w(positionMarkerWorldPose.Rot().W()); - - this->node.Request("/marker", this->forceMarkerMsg); - this->node.Request("/marker", this->positionMarkerMsg); - } - visualizeColumn = (this->resolution == ++columnCounter); - if (visualizeColumn) - columnCounter = 0; + { + // Add new markers with specific lifetime + this->positionMarkerMsg.set_id(marker_id); + this->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->positionMarkerMsg.mutable_lifetime()->set_sec( + this->cameraUpdateRate); + this->positionMarkerMsg.mutable_lifetime()->set_nsec(0); + + this->forceMarkerMsg.set_id(marker_id++); + this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->forceMarkerMsg.mutable_lifetime()->set_sec( + this->cameraUpdateRate); + this->forceMarkerMsg.mutable_lifetime()->set_nsec(0); + + // The pose of the markers must be published with reference + // to the simulation origin + ignition::math::Vector3f forceMarkerSensorPosition + (this->imageXYZ[i][j][0], this->imageXYZ[i][j][1], + this->imageXYZ[i][j][2]); + + ignition::math::Quaternionf forceMarkerSensorQuaternion; + forceMarkerSensorQuaternion.From2Axes( + ignition::math::Vector3f(0, 0, 1), normalForces[i][j]); + + // The position of the force marker is modified in order to place the + // end of the cylinder in the surface, not its middle point + ignition::math::Pose3f forceMarkerSensorPose( + forceMarkerSensorPosition + + normalForces[i][j] * (this->forceLength/2), + forceMarkerSensorQuaternion); + + ignition::math::Pose3f forceMarkerWorldPose = + forceMarkerSensorPose + this->sensorWorldPose; + forceMarkerWorldPose.Correct(); + + ignition::math::Pose3f positionMarkerSensorPose( + forceMarkerSensorPosition, forceMarkerSensorQuaternion); + ignition::math::Pose3f positionMarkerWorldPose = + positionMarkerSensorPose + this->sensorWorldPose; + positionMarkerWorldPose.Correct(); + + // Set markers pose messages from previously computed poses + this->forceMarkerMsg.mutable_pose() + ->mutable_position()->set_x(forceMarkerWorldPose.Pos().X()); + this->forceMarkerMsg.mutable_pose() + ->mutable_position()->set_y(forceMarkerWorldPose.Pos().Y()); + this->forceMarkerMsg.mutable_pose() + ->mutable_position()->set_z(forceMarkerWorldPose.Pos().Z()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_x(forceMarkerWorldPose.Rot().X()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_y(forceMarkerWorldPose.Rot().Y()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_z(forceMarkerWorldPose.Rot().Z()); + this->forceMarkerMsg.mutable_pose() + ->mutable_orientation()->set_w(forceMarkerWorldPose.Rot().W()); + + this->positionMarkerMsg.mutable_pose() + ->mutable_position()->set_x(positionMarkerWorldPose.Pos().X()); + this->positionMarkerMsg.mutable_pose() + ->mutable_position()->set_y(positionMarkerWorldPose.Pos().Y()); + this->positionMarkerMsg.mutable_pose() + ->mutable_position()->set_z(positionMarkerWorldPose.Pos().Z()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_x(positionMarkerWorldPose.Rot().X()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_y(positionMarkerWorldPose.Rot().Y()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_z(positionMarkerWorldPose.Rot().Z()); + this->positionMarkerMsg.mutable_pose() + ->mutable_orientation()->set_w(positionMarkerWorldPose.Rot().W()); + + this->node.Request("/marker", this->forceMarkerMsg); + this->node.Request("/marker", this->positionMarkerMsg); + } + visualizeColumn = (this->resolution == ++columnCounter); + if (visualizeColumn) + columnCounter = 0; } visualizeColumn = true; visualizeRow = (this->resolution == ++rowCounter); - if (visualizeRow) - rowCounter = 0; + if (visualizeRow) + rowCounter = 0; } } @@ -675,7 +729,6 @@ IGNITION_ADD_PLUGIN(OpticalTactilePlugin, ignition::gazebo::System, OpticalTactilePlugin::ISystemConfigure, OpticalTactilePlugin::ISystemPreUpdate, - OpticalTactilePlugin::ISystemUpdate, OpticalTactilePlugin::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(OpticalTactilePlugin, diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index ede7e8c89d..ba0c2d31c7 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -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. @@ -36,19 +36,28 @@ namespace systems /// \brief Plugin that implements an optical tactile sensor /// - /// It requires that contact sensors be placed in at least one link on the - /// model on which this plugin is attached. + /// 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. + /// (todo) Set this to true so the plugin works from the start and + /// doesn't need to be enabled. This element is optional, and the + /// default value is true. + /// + /// Number of pixels to skip when visualizing forces. One + /// vector representing a normal force is computed for each of the camera + /// pixels. This element must be positive and it is optional. The default + /// value is 100. + /// + /// 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. class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : public System, public ISystemConfigure, public ISystemPreUpdate, - public ISystemUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -67,10 +76,6 @@ namespace systems public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) override; - /// Documentation inherited - public: void Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - // Documentation inherited public: void PostUpdate( const ignition::gazebo::UpdateInfo &_info, From 70774f4bd77e0c1359ea11e8f81ed29f31bcbab7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 9 Jul 2020 11:47:25 +0200 Subject: [PATCH 15/23] Update sdf example to a more realistic environment 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 | 145 +++++++----------- 1 file changed, 54 insertions(+), 91 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 1b6fe3ac07..ae3202d853 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -1,7 +1,7 @@ - + @@ -61,8 +61,8 @@ true true true - /world/touch/control - /world/touch/stats + /world/optical_tactile_plugin/control + /world/optical_tactile_plugin/stats /world/depth_camera_sensor/control /world/depth_camera_sensor/stats @@ -89,7 +89,7 @@ true true true - /world/touch/stats + /world/optical_tactile_plugin/stats /world/depth_camera_sensor/stats @@ -99,6 +99,19 @@ + + + + 0 + 0 + 263 + 50 + floating + false + #03a9f4 + + + true @@ -140,65 +153,40 @@ - - 0 3 3.5 0 0 0 - - - - - 3 2 3 - - - - - - - 3 2 3 - - - - 0 1 0 1 - 0 1 0 1 - - - - true - - - - 0 0 3.5 0 0 0 + + 0 0.1 0.8 0 0 -1.57 - 0 1 1 + 0.05 0.05 0.05 - 0 1 1 + 0.05 0.05 0.05 - + 1 depth_camera 1.05 - 256 - 256 + 640 + 480 R_FLOAT32 - 0.1 - 5.0 + 0.01 + 0.2 - + collision @@ -215,58 +203,33 @@ - - 0 -3 3.5 0 0 0 - - - - - 1.5 - - - - - - - 1.5 - - - - 0 1 0 1 - 0 1 0 1 - - - - true - - - - 3 0 3.5 0 0 0 - - - - - 1.3 - 3 - - - - - - - 1.3 - 3 - - - - 0 1 0 1 - 0 1 0 1 - - - - true - + + 0 0 0 0 0 0 + + 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 + + + + + 0 -0.7 0 0 0 3.14 + + 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 + + - From 1652081a763c74f39300b59f6e93a0ed3f173889 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 9 Jul 2020 16:03:38 +0200 Subject: [PATCH 16/23] Allow moving Depth Camera from model origin 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 | 9 +++++---- .../OpticalTactilePlugin.cc | 18 ++++++++++++++++-- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index ae3202d853..276cc0af9f 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -159,20 +159,21 @@ - 0.05 0.05 0.05 + 0.005 0.02 0.02 - 0.05 0.05 0.05 + 0.005 0.02 0.02 1 depth_camera + -0.05 0 0 0 0 0 1.05 @@ -181,8 +182,8 @@ R_FLOAT32 - 0.01 - 0.2 + 0.030 + 0.065 diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index c8fc639afa..e4e2c53caf 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -141,6 +141,9 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Pose of the sensor model public: ignition::math::Pose3f sensorWorldPose; + /// \brief Offset between Depth Camera pose and model pose + public: ignition::math::Pose3f depthCameraOffset; + /// \brief Whether a new message has been returned by the Depth Camera public: bool newCameraMsg{false}; @@ -255,6 +258,15 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, return; } + // Store depth camera offset from model + auto offset = depthCameraSdf->Get("pose"); + igndbg << "Camera offset: " << offset << std::endl; + // Depth Camera data is float, so convert Pose3d to Pose3f + this->depthCameraOffset = ignition::math::Pose3f( + offset.Pos().X(), offset.Pos().Y(), offset.Pos().Z(), + offset.Rot().W(), offset.Rot().X(), offset.Rot().Y(), + offset.Rot().Z()); + // Configure subscriber for Depth Camera images std::string topic = "/depth_camera/points"; if (depthCameraSdf->HasElement("topic")) @@ -671,13 +683,15 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( forceMarkerSensorQuaternion); ignition::math::Pose3f forceMarkerWorldPose = - forceMarkerSensorPose + this->sensorWorldPose; + (forceMarkerSensorPose + this->depthCameraOffset) + + this->sensorWorldPose; forceMarkerWorldPose.Correct(); ignition::math::Pose3f positionMarkerSensorPose( forceMarkerSensorPosition, forceMarkerSensorQuaternion); ignition::math::Pose3f positionMarkerWorldPose = - positionMarkerSensorPose + this->sensorWorldPose; + (positionMarkerSensorPose + this->depthCameraOffset) + + this->sensorWorldPose; positionMarkerWorldPose.Correct(); // Set markers pose messages from previously computed poses From 4bccc6cd3a6fe3a97217d7b2c0930d6189c49d6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 9 Jul 2020 16:50:09 +0200 Subject: [PATCH 17/23] Make markers dimensions available as parameters 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 | 42 +++++++++++++++++-- .../OpticalTactilePlugin.hh | 9 ++++ 3 files changed, 51 insertions(+), 3 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 276cc0af9f..24e3214260 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -201,6 +201,9 @@ true 30 true + 0.003 + 0.001 + 0.01 diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index e4e2c53caf..c73eebc920 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -127,13 +127,16 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: ignition::msgs::Marker positionMarkerMsg; /// \brief Radius of the visualized contact sphere - public: double contactRadius{0.01}; + public: double contactRadius{0.003}; /// \brief Message for visualizing contact forces public: ignition::msgs::Marker forceMarkerMsg; /// \brief Length of the visualized force cylinder - public: double forceLength{0.03}; + public: double forceLength{0.01}; + + /// \brief Radius of the visualized force cylinder + public: double forceRadius{0.001}; /// \brief Position interpolated from the Contact messages public: std::vector interpolatedPosition; @@ -395,6 +398,38 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Blue spheres for positions // Green cylinders for forces + // Get markers parameters + + if (!_sdf->HasElement("contact_radius")) + { + ignlog << "Missing required parameter , " + << "using default value" << std::endl; + } + else + { + this->dataPtr->contactRadius = _sdf->Get("contact_radius"); + } + + if (!_sdf->HasElement("force_radius")) + { + ignlog << "Missing required parameter , " + << "using default value" << std::endl; + } + else + { + this->dataPtr->forceRadius = _sdf->Get("force_radius"); + } + + if (!_sdf->HasElement("force_length")) + { + ignlog << "Missing required parameter , " + << "using default value" << std::endl; + } + else + { + this->dataPtr->forceLength = _sdf->Get("force_length"); + } + // Create the marker message this->dataPtr->positionMarkerMsg.set_ns("positions"); this->dataPtr->positionMarkerMsg.set_action( @@ -460,7 +495,8 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->contactRadius)); ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.003, 0.003, this->dataPtr->forceLength)); + ignition::math::Vector3d(this->dataPtr->forceRadius, + this->dataPtr->forceRadius, this->dataPtr->forceLength)); } ////////////////////////////////////////////////// diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index ba0c2d31c7..d1ca2dc897 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -53,6 +53,15 @@ namespace systems /// 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. + /// + /// Radius of the contacts visualized if + /// is set to true. This parameter is optional. + /// + /// Radius of the forces visualized if + /// is set to true. This parameter is optional. + /// + /// Length of the forces visualized if + /// is set to true. This parameter is optional. class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : public System, From 16732f6f7b7a1e36f743fed07458296bafa177c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Mon, 13 Jul 2020 16:17:35 +0200 Subject: [PATCH 18/23] Visualize sensor as a marker instead of and filter out normal forces outside sensor 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 | 28 ++-- .../OpticalTactilePlugin.cc | 146 +++++++++++++++++- .../OpticalTactilePlugin.hh | 13 +- 3 files changed, 170 insertions(+), 17 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 24e3214260..596d6f5c92 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -112,6 +112,16 @@ + + + + false + docked + + + + + true @@ -154,7 +164,7 @@ - 0 0.1 0.8 0 0 -1.57 + -0.005 -0.001 0.847 0 1.57 -1.57 @@ -163,19 +173,11 @@ - - - - 0.005 0.02 0.02 - - - 1 depth_camera -0.05 0 0 0 0 0 - 1.05 640 480 @@ -199,11 +201,13 @@ filename="libignition-gazebo-opticaltactileplugin-system.so" name="ignition::gazebo::systems::OpticalTactilePlugin"> true - 30 + 15 true - 0.003 - 0.001 + true + 0.001 + 0.0002 0.01 + 0.001 diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index c73eebc920..758b2c3229 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -144,6 +144,9 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Pose of the sensor model public: ignition::math::Pose3f sensorWorldPose; + /// \brief Pose of the sensor model in the previous iteration + public: ignition::math::Pose3f prevSensorWorldPose; + /// \brief Offset between Depth Camera pose and model pose public: ignition::math::Pose3f depthCameraOffset; @@ -162,6 +165,20 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Mutex for variables mutated by the camera callback. /// The variables are: newCameraMsg, cameraMsg. public: std::mutex serviceMutex; + + /// \brief If true, the plugin will draw a marker in the place of the + /// contact sensor so it's easy to know its position + public: bool visualizeSensor{false}; + + /// \brief Message for visualizing the sensor + public: ignition::msgs::Marker sensorMarkerMsg; + + /// \brief Size of the contact sensor + public: ignition::math::Vector3d sensorSize; + + /// \brief Extended sensing distance. The sensor will output data coming from + /// its volume plus this distance. + public: double extendedSensing{0.001}; }; ////////////////////////////////////////////////// @@ -199,6 +216,26 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, this->visualizeForces = _sdf->Get("visualize_forces"); } + if (!_sdf->HasElement("extended_sensing")) + { + ignlog << "Missing required parameter , " + << "using default value" << std::endl; + } + else + { + this->extendedSensing = _sdf->Get("extended_sensing"); + } + + if (!_sdf->HasElement("visualize_sensor")) + { + ignlog << "Missing required parameter , " + << "using default value" << std::endl; + } + else + { + this->visualizeSensor = _sdf->Get("visualize_sensor"); + } + // todo(anyone) The plugin should be able to handle more than one link. In // that case, one of the links would be the actual tactile sensor. @@ -261,9 +298,14 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, return; } - // Store depth camera offset from model + // Store depth camera update rate and offset from model auto offset = depthCameraSdf->Get("pose"); - igndbg << "Camera offset: " << offset << std::endl; + this->cameraUpdateRate = 1; + if (depthCameraSdf->HasElement("update_rate")) + { + this->cameraUpdateRate = depthCameraSdf->Get("update_rate"); + } + // Depth Camera data is float, so convert Pose3d to Pose3f this->depthCameraOffset = ignition::math::Pose3f( offset.Pos().X(), offset.Pos().Y(), offset.Pos().Z(), @@ -447,6 +489,15 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->forceMarkerMsg.set_visibility( ignition::msgs::Marker::GUI); + this->dataPtr->sensorMarkerMsg.set_ns("sensor"); + this->dataPtr->sensorMarkerMsg.set_id(1); + this->dataPtr->sensorMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->sensorMarkerMsg.set_type( + ignition::msgs::Marker::BOX); + this->dataPtr->sensorMarkerMsg.set_visibility( + ignition::msgs::Marker::GUI); + // Set material properties this->dataPtr-> positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); @@ -488,6 +539,27 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, forceMarkerMsg.mutable_lifetime()-> set_sec(this->dataPtr->cameraUpdateRate); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_ambient()->set_r(0.5); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_ambient()->set_g(0.5); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_ambient()->set_b(0.5); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_ambient()->set_a(0.75); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0.5); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0.5); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0.5); + this->dataPtr-> + sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_a(0.75); + this->dataPtr-> + sensorMarkerMsg.mutable_lifetime()->set_sec(0); + this->dataPtr-> + sensorMarkerMsg.mutable_lifetime()->set_nsec(0); + // Set scales ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), ignition::math::Vector3d(this->dataPtr->contactRadius, @@ -497,6 +569,18 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), ignition::math::Vector3d(this->dataPtr->forceRadius, this->dataPtr->forceRadius, this->dataPtr->forceLength)); + + // Get the size of the sensor from the SDF + + // If there's no specified inside , a default one + // is set + this->dataPtr->sensorSize = + _sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box")-> + Get("size"); + + ignition::msgs::Set(this->dataPtr->sensorMarkerMsg.mutable_scale(), + this->dataPtr->sensorSize); } ////////////////////////////////////////////////// @@ -584,6 +668,39 @@ void OpticalTactilePlugin::PostUpdate( this->dataPtr->newCameraMsg = false; } } + + // Publish sensor marker if required and sensor pose has changed + if (this->dataPtr->visualizeSensor && + (this->dataPtr->sensorWorldPose != this->dataPtr->prevSensorWorldPose)) + { + this->dataPtr->sensorMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->sensorMarkerMsg.set_id(1); + + this->dataPtr->sensorMarkerMsg.mutable_pose() + ->mutable_position()->set_x(this->dataPtr->sensorWorldPose.Pos().X()); + this->dataPtr->sensorMarkerMsg.mutable_pose() + ->mutable_position()->set_y(this->dataPtr->sensorWorldPose.Pos().Y()); + this->dataPtr->sensorMarkerMsg.mutable_pose() + ->mutable_position()->set_z(this->dataPtr->sensorWorldPose.Pos().Z()); + this->dataPtr->sensorMarkerMsg.mutable_pose()-> + mutable_orientation()->set_x( + this->dataPtr->sensorWorldPose.Rot().X()); + this->dataPtr->sensorMarkerMsg.mutable_pose()-> + mutable_orientation()->set_y( + this->dataPtr->sensorWorldPose.Rot().Y()); + this->dataPtr->sensorMarkerMsg.mutable_pose()-> + mutable_orientation()->set_z( + this->dataPtr->sensorWorldPose.Rot().Z()); + this->dataPtr->sensorMarkerMsg.mutable_pose()-> + mutable_orientation()->set_w( + this->dataPtr->sensorWorldPose.Rot().W()); + + this->dataPtr->node.Request("/marker", this->dataPtr->sensorMarkerMsg); + } + + // Store the pose of the sensor in the current iteration + this->dataPtr->prevSensorWorldPose = this->dataPtr->sensorWorldPose; } ////////////////////////////////////////////////// @@ -632,6 +749,31 @@ void OpticalTactilePluginPrivate::UnpackPointCloudMsg( this->imageXYZ[i][j][2] = *reinterpret_cast( msgBufferIndex + _msg.field(fieldIndex++).offset()); + // Filter out points outside of the contact sensor + + // We assume that the Depth Camera is only displaced in the -X direction + bool insideX = (this->imageXYZ[i][j][0] >= std::abs( + this->depthCameraOffset.X()) - this->extendedSensing) + && (this->imageXYZ[i][j][0] <= (std::abs(this->depthCameraOffset.X()) + + this->sensorSize.X() + this->extendedSensing)); + + bool insideY = (this->imageXYZ[i][j][1] <= this->sensorSize.Y() / 2 + + this->extendedSensing) + && (this->imageXYZ[i][j][1] >= - this->sensorSize.Y() / 2 - + this->extendedSensing); + + bool insideZ = (this->imageXYZ[i][j][2] <= this->sensorSize.Z() / 2 + + this->extendedSensing) + && (this->imageXYZ[i][j][2] >= - this->sensorSize.Z() / 2 - + this->extendedSensing); + + if (!insideX || !insideY || !insideZ) + { + this->imageXYZ[i][j][0] = ignition::math::INF_D; + this->imageXYZ[i][j][1] = ignition::math::INF_D; + this->imageXYZ[i][j][2] = ignition::math::INF_D; + } + msgBufferIndex += _msg.point_step(); } } diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index d1ca2dc897..e6c9dff800 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -54,14 +54,21 @@ namespace systems /// forces in the 3D world. This element is optional, and the /// default value is false. /// - /// Radius of the contacts visualized if + /// Radius in meters of the contacts visualized if /// is set to true. This parameter is optional. /// - /// Radius of the forces visualized if + /// Radius in meters of the forces visualized if /// is set to true. This parameter is optional. /// - /// Length of the forces visualized if + /// Length in meters of the forces visualized if /// is set to true. This parameter is optional. + /// + /// 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. class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : public System, From 2a45a84f83ba3c3087d45f6c7221e6caf399abe1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Fri, 31 Jul 2020 10:09:48 +0200 Subject: [PATCH 19/23] PR Feedback 2 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 +- .../optical_tactile_plugin/CMakeLists.txt | 1 + .../OpticalTactilePlugin.cc | 1151 +++++++---------- .../OpticalTactilePlugin.hh | 42 +- .../optical_tactile_plugin/Visualization.cc | 239 ++++ .../optical_tactile_plugin/Visualization.hh | 125 ++ 6 files changed, 883 insertions(+), 678 deletions(-) create mode 100644 src/systems/optical_tactile_plugin/Visualization.cc create mode 100644 src/systems/optical_tactile_plugin/Visualization.hh diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 596d6f5c92..e888d75092 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -201,7 +201,8 @@ filename="libignition-gazebo-opticaltactileplugin-system.so" name="ignition::gazebo::systems::OpticalTactilePlugin"> true - 15 + 15 + 1 true true 0.001 diff --git a/src/systems/optical_tactile_plugin/CMakeLists.txt b/src/systems/optical_tactile_plugin/CMakeLists.txt index 48420a8229..0899bfd2d1 100644 --- a/src/systems/optical_tactile_plugin/CMakeLists.txt +++ b/src/systems/optical_tactile_plugin/CMakeLists.txt @@ -1,6 +1,7 @@ gz_add_system(opticaltactileplugin SOURCES OpticalTactilePlugin.cc + Visualization.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) \ No newline at end of file diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 758b2c3229..b9f339d4b0 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -43,39 +43,42 @@ using namespace ignition; using namespace gazebo; using namespace systems; +using namespace optical_tactile_sensor; class ignition::gazebo::systems::OpticalTactilePluginPrivate { /// \brief Load the Contact sensor from an sdf element - /// \param[in] _sdf SDF element describing the Contact sensor /// \param[in] _ecm Immutable reference to the EntityComponentManager - public: void Load(const EntityComponentManager &_ecm, - const std::shared_ptr &_sdf); + 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); - /// \brief Filters out new collisions fetched not related to the sensors - /// \param[in] _ecm Immutable reference to the EntityComponentManager - /// \param[in] _entities Collision entities to filter - public: void FilterOutCollisions(const EntityComponentManager &_ecm, - const std::vector &_entities); - - /// \brief Interpolates contact data - /// \param[in] _contactMsg Message containing the data to interpolate - public: void InterpolateData(const ignition::msgs::Contact &_contactMsg); + /// \brief Interpolates contacts returned by the contact sensor + /// \param[in] _contact Message containing the data to interpolate + public: void InterpolateContactData(const ignition::msgs::Contact &_contact); /// \brief Callback for the Depth Camera /// \param[in] _msg Message from the subscribed topic public: void DepthCameraCallback( - const ignition::msgs::PointCloudPacked &_msg); - - /// \brief Unpacks the point cloud retrieved by the Depth Camera - /// into XYZ data - /// \param[in] _msg Message containing the data to unpack - public: void UnpackPointCloudMsg( - const ignition::msgs::PointCloudPacked &_msg); + const ignition::msgs::PointCloudPacked &_msg); + + /// \brief Map the (i,j) coordinates defined in the top-left corner of the + /// camera into its corresponding (X,Y,Z) measurement with respect to the + /// camera's origin + /// \param[in] _i Horizontal camera coordinate defined in the top-left corner + /// of the image, pointing rightwards + /// \param[in] _j Vertical camera coordinate defined in the top-left corner + /// of the image, pointing downwards + /// \returns The corresponding (X,Y,Z) point + public: ignition::math::Vector3f MapPointCloudData(const u_int32_t &_i, + const uint32_t &_j); + + /// \brief Check if a specific point returned by the Depth Camera is inside + /// the contact surface. + /// \param[in] _point Point returned by the Depth Camera + public: bool PointInsideSensor(ignition::math::Vector3f _point); /// \brief Computes the normal forces of the Optical Tactile sensor /// \param[in] _msg Message returned by the Depth Camera @@ -86,11 +89,14 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// 34644101/calculate-surface-normals-from-depth-image- /// using-neighboring-pixels-cross-produc public: void ComputeNormalForces( - const ignition::msgs::PointCloudPacked &_msg, - const bool _visualizeForces); + const ignition::msgs::PointCloudPacked &_msg, + const bool _visualizeForces); - /// \brief Resolution of the sensor in pixels to skip. - public: int resolution{100}; + /// \brief Resolution of the visualization in pixels to skip. + public: int visualizationResolution{30}; + + /// \brief Resolution of the interpolated contacts in mm. + public: double contactsResolution{1.0}; /// \brief Whether to visualize the normal forces. public: bool visualizeForces{false}; @@ -98,18 +104,15 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Model interface. public: Model model{kNullEntity}; - /// \brief Transport node to visualize data on Gazebo. + /// \brief Transport node public: transport::Node node; - /// \brief Collision entities that have been designated as contact sensors. - public: ignition::gazebo::Entity contactSensorEntity; - - /// \brief Collision entities of the simulation. - public: std::vector collisionEntities; + /// \brief Entity representing the contact sensor of the plugin + public: ignition::gazebo::Entity sensorCollisionEntity; - /// \brief Filtered collisions from the simulation that belong to - /// one or more sensors. - public: std::vector filteredCollisionEntities; + /// \brief Entity representing the collision of the object in contact with + /// the optical tactile sensor. + public: ignition::gazebo::Entity objectCollisionEntity; /// \brief Whether the plugin is enabled. public: bool enabled{true}; @@ -117,30 +120,18 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Initialization flag. public: bool initialized{false}; - /// \brief Copy of the sdf configuration used for this plugin. - public: std::shared_ptr sdfConfig{nullptr}; - /// \brief Name of the model to which the sensor is attached. public: std::string modelName; - /// \brief Message for visualizing contact positions - public: ignition::msgs::Marker positionMarkerMsg; - /// \brief Radius of the visualized contact sphere public: double contactRadius{0.003}; - /// \brief Message for visualizing contact forces - public: ignition::msgs::Marker forceMarkerMsg; - /// \brief Length of the visualized force cylinder public: double forceLength{0.01}; /// \brief Radius of the visualized force cylinder public: double forceRadius{0.001}; - /// \brief Position interpolated from the Contact messages - public: std::vector interpolatedPosition; - /// \brief Pose of the sensor model public: ignition::math::Pose3f sensorWorldPose; @@ -159,8 +150,8 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Message returned by the Depth Camera public: ignition::msgs::PointCloudPacked cameraMsg; - /// \brief 3D vector to store unpacked XYZ points - std::vector>> imageXYZ; + /// \brief Data returned by the Depth Camera after being preprocessed + public: char *msgBuffer; /// \brief Mutex for variables mutated by the camera callback. /// The variables are: newCameraMsg, cameraMsg. @@ -170,547 +161,472 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// contact sensor so it's easy to know its position public: bool visualizeSensor{false}; - /// \brief Message for visualizing the sensor - public: ignition::msgs::Marker sensorMarkerMsg; - /// \brief Size of the contact sensor public: ignition::math::Vector3d sensorSize; /// \brief Extended sensing distance. The sensor will output data coming from /// its volume plus this distance. public: double extendedSensing{0.001}; + + /// \brief Pointer to visualization object + public: std::unique_ptr visualizePtr; }; ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm, - const std::shared_ptr &_sdf) +OpticalTactilePlugin::OpticalTactilePlugin() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void OpticalTactilePlugin::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) { - igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "Touch plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + // Get the sdf parameters - // Get sdf parameters - if (!_sdf->HasElement("resolution")) + // + if (!_sdf->HasElement("enabled")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->enabled << std::endl; + } + else + { + this->dataPtr->enabled = _sdf->Get("enabled"); + } + + // + if (!_sdf->HasElement("visualization_resolution")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->visualizationResolution << std::endl; + } + else + { + if (_sdf->Get("visualization_resolution") < 0) { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; + ignwarn << "Parameter must be positive, " + << "setting to " << this->dataPtr->visualizationResolution << std::endl; } else { - if (_sdf->Get("resolution") < 0) - { - ignwarn << "Parameter must be positive, " - << "using default value" << std::endl; - } - else - { - this->resolution = _sdf->Get("resolution"); - } + this->dataPtr->visualizationResolution = + _sdf->Get("visualization_resolution"); } + } - if (!_sdf->HasElement("visualize_forces")) + // + if (!_sdf->HasElement("contacts_resolution")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->contactsResolution << std::endl; + } + else + { + if (_sdf->Get("contacts_resolution") < 0) { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; + ignwarn << "Parameter must be positive, " + << "setting to " << this->dataPtr->contactsResolution << std::endl; } else { - this->visualizeForces = _sdf->Get("visualize_forces"); + this->dataPtr->contactsResolution = + _sdf->Get("contacts_resolution"); } + } - if (!_sdf->HasElement("extended_sensing")) + // + if (!_sdf->HasElement("visualize_forces")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->visualizeForces << std::endl; + } + else + { + this->dataPtr->visualizeForces = _sdf->Get("visualize_forces"); + } + + // + if (!_sdf->HasElement("extended_sensing")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->extendedSensing << std::endl; + } + else + { + if (_sdf->Get("extended_sensing") < 0) { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; + ignwarn << "Parameter must be positive, " + << "setting to " << this->dataPtr->extendedSensing << std::endl; } else { - this->extendedSensing = _sdf->Get("extended_sensing"); + this->dataPtr->extendedSensing = _sdf->Get("extended_sensing"); } + } - if (!_sdf->HasElement("visualize_sensor")) + // + if (!_sdf->HasElement("visualize_sensor")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->visualizeSensor << std::endl; + } + else + { + this->dataPtr->visualizeSensor = _sdf->Get("visualize_sensor"); + } + + // + if (!_sdf->HasElement("contact_radius")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->contactRadius << std::endl; + } + else + { + if (_sdf->Get("contact_radius") < 0) { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; + ignwarn << "Parameter must be positive, " + << "setting to " << this->dataPtr->contactRadius << std::endl; } else { - this->visualizeSensor = _sdf->Get("visualize_sensor"); + this->dataPtr->contactRadius = _sdf->Get("contact_radius"); } + } - // todo(anyone) The plugin should be able to handle more than one link. In - // that case, one of the links would be the actual tactile sensor. - - // Check SDF structure - // Model should only have 1 link - auto allLinks = - _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); - if (allLinks.size() != 1) + // + if (!_sdf->HasElement("force_radius")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->forceRadius << std::endl; + } + else + { + if (_sdf->Get("force_radius") < 0) { - ignerr << "Plugin must have 1 link" << std::endl; - return; + ignwarn << "Parameter must be positive, " + << "setting to " << this->dataPtr->forceRadius << std::endl; } - - // Link should have a collision component linked to a contact sensor - auto linkCollisions = - _ecm.ChildrenByComponents(allLinks[0], components::Collision()); - for (const Entity &colEntity : linkCollisions) + else { - if (_ecm.EntityHasComponentType(colEntity, - components::ContactSensorData::typeId)) - { - this->contactSensorEntity = colEntity; - - this->modelName = this->model.Name(_ecm); - - igndbg << "Contactensor detected within model " - << this->modelName << std::endl; - } + this->dataPtr->forceRadius = _sdf->Get("force_radius"); } + } - // Link should have 1 Contact sensor and 1 Depth Camera sensor - auto sensorsInsideLink = - _ecm.ChildrenByComponents(allLinks[0], components::Sensor()); - - bool depthCameraInLink = false; - bool contactSensorInLink = false; - sdf::ElementPtr depthCameraSdf = nullptr; - for (const Entity &sensor : sensorsInsideLink) + // + if (!_sdf->HasElement("force_length")) + { + ignlog << "Missing parameter , " + << "setting to " << this->dataPtr->forceLength << std::endl; + } + else + { + if (_sdf->Get("force_length") < 0) { - if (_ecm.EntityHasComponentType(sensor, - components::DepthCamera::typeId)) - { - depthCameraInLink = true; - depthCameraSdf = - _ecm.Component(sensor)->Data().Element(); - } - - if (_ecm.EntityHasComponentType(sensor, - components::ContactSensor::typeId)) - { - contactSensorInLink = true; - } + ignwarn << "Parameter must be positive, " + << "setting to " << this->dataPtr->forceLength << std::endl; } - - if (!depthCameraInLink || !contactSensorInLink || - sensorsInsideLink.size() != 2) + else { - ignerr << "Link must have 1 Depth Camera sensor and " - << "1 Contact sensor" << std::endl; - return; + this->dataPtr->forceLength = _sdf->Get("force_length"); } + } + + // Get the size of the sensor from the SDF + // If there's no specified inside , a default one + // is set + this->dataPtr->sensorSize = + _sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box")-> + Get("size"); +} + +////////////////////////////////////////////////// +void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("TouchPluginPrivate::PreUpdate"); + + // Nothing left to do if paused + if (_info.paused) + return; - // Store depth camera update rate and offset from model - auto offset = depthCameraSdf->Get("pose"); - this->cameraUpdateRate = 1; - if (depthCameraSdf->HasElement("update_rate")) + if (!this->dataPtr->initialized) + { + // We call Load here instead of Configure because we can't be guaranteed + // that all entities have been created when Configure is called + this->dataPtr->Load(_ecm); + this->dataPtr->initialized = true; + } + + // This is not an "else" because "initialized" can be set + // in the if block above + if (this->dataPtr->initialized) + { + // Get the first object being touched by the sensor + // We assume there's only one object being touched + auto csd = _ecm.Component( + this->dataPtr->sensorCollisionEntity); + for (const auto &contact : csd->Data().contact()) { - this->cameraUpdateRate = depthCameraSdf->Get("update_rate"); + this->dataPtr->objectCollisionEntity = + contact.collision2().id(); + break; } + // Get the Depth Camera pose + ignition::math::Pose3d sensorPose = + _ecm.Component( + this->dataPtr->model.Entity())->Data(); + // Depth Camera data is float, so convert Pose3d to Pose3f - this->depthCameraOffset = ignition::math::Pose3f( - offset.Pos().X(), offset.Pos().Y(), offset.Pos().Z(), - offset.Rot().W(), offset.Rot().X(), offset.Rot().Y(), - offset.Rot().Z()); - - // Configure subscriber for Depth Camera images - std::string topic = "/depth_camera/points"; - if (depthCameraSdf->HasElement("topic")) - topic = "/" + depthCameraSdf->Get("topic") + "/points"; - - if (!this->node.Subscribe(topic, - &OpticalTactilePluginPrivate::DepthCameraCallback, - this)) - { - ignerr << "Error subscribing to topic " << "[" << topic << "]. " - << " must not contain '/'" << std::endl; - return; - } + this->dataPtr->sensorWorldPose = ignition::math::Pose3f( + sensorPose.Pos().X(), sensorPose.Pos().Y(), sensorPose.Pos().Z(), + sensorPose.Rot().W(), sensorPose.Rot().X(), sensorPose.Rot().Y(), + sensorPose.Rot().Z()); + } } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool _value) +void OpticalTactilePlugin::PostUpdate( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) { - // Just a placeholder method for the moment - _value; -} + IGN_PROFILE("TouchPlugin::PostUpdate"); -////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::FilterOutCollisions( - const EntityComponentManager &_ecm, - const std::vector &_entities) -{ - if (_entities.empty()) - return; + // Nothing left to do if paused. + if (_info.paused) + return; - // Clear vector that contains old data - this->filteredCollisionEntities.clear(); + auto* contacts = + _ecm.Component( + this->dataPtr->sensorCollisionEntity); - // Get collisions from the sensor - for (const Entity &entity : _entities) + if (contacts) + { + for (const auto &contact : contacts->Data().contact()) { - std::string name = scopedName(entity, _ecm); - - if (name.find(this->modelName) != std::string::npos) - { - this->filteredCollisionEntities.push_back(entity); - } + // Interpolate data returned by the Contact message + this->dataPtr->InterpolateContactData(contact); } -} + } -////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::InterpolateData( - const ignition::msgs::Contact &contact) -{ - // The Optical Tactile Sensor is supposed to be box-shaped. - // Contacts returned by the Contact Sensor in this type of shapes should be - // 4 points in the corners of some of its faces, in this case the one - // corresponding to the 'sensing' face. - // Given that the number of contacts may change even when the objects are - // at rest, data is only interpolated when the number of contacts - // returned is 4. - - if (contact.position_size() == 4) + // Process camera message if it's new + { + std::lock_guard lock(this->dataPtr->serviceMutex); + if (this->dataPtr->newCameraMsg) { - ignition::math::Vector3d contact1 = - ignition::msgs::Convert(contact.position(0)); - ignition::math::Vector3d contact2 = - ignition::msgs::Convert(contact.position(1)); - ignition::math::Vector3d contact3 = - ignition::msgs::Convert(contact.position(2)); - ignition::math::Vector3d contact4 = - ignition::msgs::Convert(contact.position(3)); - - ignition::math::Vector3d direction1 = - (contact2 - contact1).Normalized() * this->resolution; - ignition::math::Vector3d direction2 = - (contact4 - contact1).Normalized() * this->resolution; - - ignition::math::Vector3d interpolatedVector = contact1; - this->interpolatedPosition.push_back(contact1); - - // auxiliary Vector3d to iterate through contacts - ignition::math::Vector3d tempVector(0.0, 0.0, 0.0); - - uint64_t steps1 = - contact1.Distance(contact2) / this->resolution; - uint64_t steps2 = - contact1.Distance(contact4) / this->resolution; - - for (uint64_t index1 = 0; index1 <= steps1; ++index1) - { - tempVector = interpolatedVector; - for (uint64_t index2 = 0; index2 < steps2; ++index2) - { - interpolatedVector += direction2; - this->interpolatedPosition.push_back(interpolatedVector); - } - if (index1 != steps1) - { - interpolatedVector = tempVector; - interpolatedVector += direction1; - this->interpolatedPosition.push_back(interpolatedVector); - } - } + this->dataPtr->ComputeNormalForces(this->dataPtr->cameraMsg, + this->dataPtr->visualizeForces); + + this->dataPtr->newCameraMsg = false; } -} + } -////////////////////////////////////////////////// -OpticalTactilePlugin::OpticalTactilePlugin() - : System(), dataPtr(std::make_unique()) -{ + // Publish sensor marker if required and sensor pose has changed + if (this->dataPtr->visualizeSensor && + (this->dataPtr->sensorWorldPose != this->dataPtr->prevSensorWorldPose)) + { + this->dataPtr->visualizePtr->VisualizeSensor( + this->dataPtr->sensorWorldPose); + } + + // Store the pose of the sensor in the current iteration + this->dataPtr->prevSensorWorldPose = this->dataPtr->sensorWorldPose; } ////////////////////////////////////////////////// -void OpticalTactilePlugin::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &) +void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) { - this->dataPtr->sdfConfig = _sdf; - this->dataPtr->model = Model(_entity); - - if (!this->dataPtr->model.Valid(_ecm)) - { - ignerr << "Touch plugin should be attached to a model entity. " - << "Failed to initialize." << std::endl; - return; - } + igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; - // Configure Marker messages for position and force of the contacts - // Blue spheres for positions - // Green cylinders for forces + // todo(anyone) The plugin should be able to handle more than one link. In + // that case, one of the links would be the actual tactile sensor. - // Get markers parameters + // Check plugin structure + // Model should only have 1 link + auto allLinks = + _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); + if (allLinks.size() != 1) + { + ignerr << "Plugin must have exactly 1 link" << std::endl; + return; + } - if (!_sdf->HasElement("contact_radius")) - { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; - } - else + // Link should have a collision component linked to a contact sensor + auto linkCollisions = + _ecm.ChildrenByComponents(allLinks[0], components::Collision()); + for (const Entity &colEntity : linkCollisions) + { + if (_ecm.EntityHasComponentType(colEntity, + components::ContactSensorData::typeId)) { - this->dataPtr->contactRadius = _sdf->Get("contact_radius"); - } + this->sensorCollisionEntity = colEntity; - if (!_sdf->HasElement("force_radius")) - { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; - } - else - { - this->dataPtr->forceRadius = _sdf->Get("force_radius"); + this->modelName = this->model.Name(_ecm); + + igndbg << "Contact sensor detected within model " + << this->modelName << std::endl; } + } - if (!_sdf->HasElement("force_length")) + // Link should have exactly 1 Contact sensor and 1 Depth Camera sensor + auto sensorsInsideLink = + _ecm.ChildrenByComponents(allLinks[0], components::Sensor()); + + int depthCameraCounter = 0; + int contactSensorCounter = 0; + sdf::ElementPtr depthCameraSdf = nullptr; + for (const Entity &sensor : sensorsInsideLink) + { + if (_ecm.EntityHasComponentType(sensor, components::DepthCamera::typeId)) { - ignlog << "Missing required parameter , " - << "using default value" << std::endl; + depthCameraCounter += 1; + depthCameraSdf = + _ecm.Component(sensor)->Data().Element(); } - else + + if (_ecm.EntityHasComponentType(sensor, components::ContactSensor::typeId)) { - this->dataPtr->forceLength = _sdf->Get("force_length"); + contactSensorCounter += 1; } + } - // Create the marker message - this->dataPtr->positionMarkerMsg.set_ns("positions"); - this->dataPtr->positionMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->positionMarkerMsg.set_type( - ignition::msgs::Marker::SPHERE); - this->dataPtr->positionMarkerMsg.set_visibility( - ignition::msgs::Marker::GUI); - - this->dataPtr->forceMarkerMsg.set_ns("forces"); - this->dataPtr->forceMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->forceMarkerMsg.set_type( - ignition::msgs::Marker::CYLINDER); - this->dataPtr->forceMarkerMsg.set_visibility( - ignition::msgs::Marker::GUI); - - this->dataPtr->sensorMarkerMsg.set_ns("sensor"); - this->dataPtr->sensorMarkerMsg.set_id(1); - this->dataPtr->sensorMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->sensorMarkerMsg.set_type( - ignition::msgs::Marker::BOX); - this->dataPtr->sensorMarkerMsg.set_visibility( - ignition::msgs::Marker::GUI); - - // Set material properties - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - this->dataPtr-> - positionMarkerMsg.mutable_lifetime()-> - set_sec(this->dataPtr->cameraUpdateRate); - - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - this->dataPtr-> - forceMarkerMsg.mutable_lifetime()-> - set_sec(this->dataPtr->cameraUpdateRate); - - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_ambient()->set_r(0.5); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_ambient()->set_g(0.5); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_ambient()->set_b(0.5); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_ambient()->set_a(0.75); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0.5); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0.5); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0.5); - this->dataPtr-> - sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_a(0.75); - this->dataPtr-> - sensorMarkerMsg.mutable_lifetime()->set_sec(0); - this->dataPtr-> - sensorMarkerMsg.mutable_lifetime()->set_nsec(0); - - // Set scales - ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->dataPtr->contactRadius, - this->dataPtr->contactRadius, - this->dataPtr->contactRadius)); - - ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->dataPtr->forceRadius, - this->dataPtr->forceRadius, this->dataPtr->forceLength)); - - // Get the size of the sensor from the SDF - - // If there's no specified inside , a default one - // is set - this->dataPtr->sensorSize = - _sdf->GetParent()->GetElement("link")->GetElement("collision")-> - GetElement("geometry")->GetElement("box")-> - Get("size"); - - ignition::msgs::Set(this->dataPtr->sensorMarkerMsg.mutable_scale(), - this->dataPtr->sensorSize); -} + if ((depthCameraCounter != 1) || (contactSensorCounter != 1)) + { + ignerr << "Link must have 1 Depth Camera sensor and " + << "1 Contact sensor" << std::endl; + return; + } -////////////////////////////////////////////////// -void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ - IGN_PROFILE("TouchPluginPrivate::PreUpdate"); + // Store depth camera update rate and offset from model + auto offset = depthCameraSdf->Get("pose"); + if (!depthCameraSdf->HasElement("update_rate")) + { + ignerr << "Depth camera should have an value" << std::endl; + return; + } + this->cameraUpdateRate = depthCameraSdf->Get("update_rate"); - // Nothing left to do if paused - if (_info.paused) - return; + // Depth Camera data is float, so convert Pose3d to Pose3f + this->depthCameraOffset = ignition::math::Pose3f( + offset.Pos().X(), offset.Pos().Y(), offset.Pos().Z(), + offset.Rot().W(), offset.Rot().X(), offset.Rot().Y(), + offset.Rot().Z()); - if (!this->dataPtr->initialized) - { - // We call Load here instead of Configure because we can't be guaranteed - // that all entities have been created when Configure is called - this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); - this->dataPtr->initialized = true; - } + // Configure subscriber for Depth Camera images + if (!depthCameraSdf->HasElement("topic")) + { + ignerr << "Depth camera should have a unique value" << std::endl; + return; + } + std::string topic = + "/" + depthCameraSdf->Get("topic") + "/points"; + if (!this->node.Subscribe(topic, + &OpticalTactilePluginPrivate::DepthCameraCallback, this)) + { + ignerr << "Error subscribing to topic " << "[" << topic << "]. " + << " must not contain '/'" << std::endl; + return; + } - // This is not an "else" because "initialized" can be set - // in the if block above - if (this->dataPtr->initialized) - { - // Fetch new collisions from simulation - std::vector newCollisions; - _ecm.Each( - [&](const Entity &_entity, const components::Collision *) -> bool - { - newCollisions.push_back(_entity); - return true; - }); - this->dataPtr->FilterOutCollisions(_ecm, newCollisions); - - // Get the Depth Camera pose - ignition::math::Pose3d sensorPose = - _ecm.Component( - this->dataPtr->model.Entity())->Data(); - - // Depth Camera data is float, so convert Pose3d to Pose3f - this->dataPtr->sensorWorldPose = ignition::math::Pose3f( - sensorPose.Pos().X(), sensorPose.Pos().Y(), sensorPose.Pos().Z(), - sensorPose.Rot().W(), sensorPose.Rot().X(), sensorPose.Rot().Y(), - sensorPose.Rot().Z()); - } + // Instantiate the visualization class + this->visualizePtr = std::make_unique< + OpticalTactilePluginVisualization>(this->contactRadius, + this->forceRadius, this->forceLength, + this->cameraUpdateRate, this->depthCameraOffset, + this->visualizationResolution); + + // Create the marker messages + this->visualizePtr->CreateMarkersMsgs(this->modelName, this->sensorSize); } ////////////////////////////////////////////////// -void OpticalTactilePlugin::PostUpdate( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) +void OpticalTactilePluginPrivate::Enable(const bool _value) { - IGN_PROFILE("TouchPlugin::PostUpdate"); + // todo(mcres) Implement method + _value; +} - // Nothing left to do if paused. - if (_info.paused) - return; +////////////////////////////////////////////////// +void OpticalTactilePluginPrivate::InterpolateContactData( + const ignition::msgs::Contact &_contact) +{ + // The Optical Tactile Sensor is supposed to be box-shaped. + // Contacts returned by the Contact Sensor in this type of shapes should be + // 4 points in the corners of some of its faces, in this case the one + // corresponding to the 'sensing' face. + // Given that the number of contacts may change even when the objects are + // at rest, data is only interpolated when the number of contacts + // returned is 4. + + // todo(mcres) Handle a different number of contacts returned by the + // contact sensor + + if (_contact.position_size() == 4) + { + // todo(mcres) Interpolated contacts should be published in a message - // Delete old interpolated positions - this->dataPtr->interpolatedPosition.clear(); + ignition::math::Vector3d contact1 = + ignition::msgs::Convert(_contact.position(0)); + ignition::math::Vector3d contact2 = + ignition::msgs::Convert(_contact.position(1)); + ignition::math::Vector3d contact3 = + ignition::msgs::Convert(_contact.position(2)); + ignition::math::Vector3d contact4 = + ignition::msgs::Convert(_contact.position(3)); - for (const Entity &colEntity : this->dataPtr->filteredCollisionEntities) - { - auto* contacts = - _ecm.Component(colEntity); - if (contacts) - { - for (const auto &contact : contacts->Data().contact()) - { - // Interpolate data returned by the Contact message - this->dataPtr->InterpolateData(contact); - } - } - } + ignition::math::Vector3d direction1 = + (contact2 - contact1).Normalized() * (this->contactsResolution/1000.0); + ignition::math::Vector3d direction2 = + (contact4 - contact1).Normalized() * (this->contactsResolution/1000.0); - // Process camera message if it's new - { - std::lock_guard lock(this->dataPtr->serviceMutex); - if (this->dataPtr->newCameraMsg) - { - this->dataPtr->UnpackPointCloudMsg(this->dataPtr->cameraMsg); - this->dataPtr->ComputeNormalForces(this->dataPtr->cameraMsg, - this->dataPtr->visualizeForces); + ignition::math::Vector3d interpolatedVector = contact1; - this->dataPtr->newCameraMsg = false; - } - } + // auxiliary Vector3d to iterate through contacts + ignition::math::Vector3d tempVector(0.0, 0.0, 0.0); + + uint64_t steps1 = + contact1.Distance(contact2) / (this->contactsResolution/1000.0); + uint64_t steps2 = + contact1.Distance(contact4) / (this->contactsResolution/1000.0); - // Publish sensor marker if required and sensor pose has changed - if (this->dataPtr->visualizeSensor && - (this->dataPtr->sensorWorldPose != this->dataPtr->prevSensorWorldPose)) + for (uint64_t index1 = 0; index1 <= steps1; ++index1) { - this->dataPtr->sensorMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->sensorMarkerMsg.set_id(1); - - this->dataPtr->sensorMarkerMsg.mutable_pose() - ->mutable_position()->set_x(this->dataPtr->sensorWorldPose.Pos().X()); - this->dataPtr->sensorMarkerMsg.mutable_pose() - ->mutable_position()->set_y(this->dataPtr->sensorWorldPose.Pos().Y()); - this->dataPtr->sensorMarkerMsg.mutable_pose() - ->mutable_position()->set_z(this->dataPtr->sensorWorldPose.Pos().Z()); - this->dataPtr->sensorMarkerMsg.mutable_pose()-> - mutable_orientation()->set_x( - this->dataPtr->sensorWorldPose.Rot().X()); - this->dataPtr->sensorMarkerMsg.mutable_pose()-> - mutable_orientation()->set_y( - this->dataPtr->sensorWorldPose.Rot().Y()); - this->dataPtr->sensorMarkerMsg.mutable_pose()-> - mutable_orientation()->set_z( - this->dataPtr->sensorWorldPose.Rot().Z()); - this->dataPtr->sensorMarkerMsg.mutable_pose()-> - mutable_orientation()->set_w( - this->dataPtr->sensorWorldPose.Rot().W()); - - this->dataPtr->node.Request("/marker", this->dataPtr->sensorMarkerMsg); + tempVector = interpolatedVector; + for (uint64_t index2 = 0; index2 < steps2; ++index2) + { + interpolatedVector += direction2; + } + if (index1 != steps1) + { + interpolatedVector = tempVector; + interpolatedVector += direction1; + } } - - // Store the pose of the sensor in the current iteration - this->dataPtr->prevSensorWorldPose = this->dataPtr->sensorWorldPose; + } } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::DepthCameraCallback( - const ignition::msgs::PointCloudPacked &_msg) + const ignition::msgs::PointCloudPacked &_msg) { // This check avoids running the callback at t=0 and getting // unexpected markers in the scene if (!this->initialized) - return; + return; { std::lock_guard lock(this->serviceMutex); @@ -720,208 +636,123 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::UnpackPointCloudMsg( - const ignition::msgs::PointCloudPacked &_msg) +ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( + const u_int32_t &_i, const uint32_t &_j) { - // 3D vector to store XYZ points - this->imageXYZ = std::vector>> - (_msg.width(), - std::vector>(_msg.height(), - std::vector(3))); + // Initialize return variable + ignition::math::Vector3f measuredPoint; + + // Save original position of the pointer + char *initialMsgBuffer = this->msgBuffer; + + // 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->cameraMsg.row_step() + _i*this->cameraMsg.point_step(); + + this->msgBuffer += msgBufferIndex; + int fieldIndex = 0; + + // X coordinate + measuredPoint.X() = *reinterpret_cast( + this->msgBuffer + this->cameraMsg.field(fieldIndex++).offset()); + // Y coordinate + measuredPoint.Y() = *reinterpret_cast( + this->msgBuffer + this->cameraMsg.field(fieldIndex++).offset()); + // Z coordinate + measuredPoint.Z() = *reinterpret_cast( + this->msgBuffer + this->cameraMsg.field(fieldIndex).offset()); + + // Check if point is inside the sensor + bool pointInside = this->PointInsideSensor(measuredPoint); + if (!pointInside) + { + measuredPoint.X() = ignition::math::INF_F; + measuredPoint.Y() = ignition::math::INF_F; + measuredPoint.Z() = ignition::math::INF_F; + } - // Unpack point cloud message - std::string msgBuffer = _msg.data(); - char *msgBufferIndex = msgBuffer.data(); + // Restore msgBuffer to initial position + this->msgBuffer = initialMsgBuffer; - for (uint64_t j = 0; j < _msg.height(); ++j) - { - for (uint64_t i = 0; i < _msg.width(); ++i) - { - int fieldIndex = 0; - - // x coordinate in pixel (i,j) - this->imageXYZ[i][j][0] = *reinterpret_cast( - msgBufferIndex + _msg.field(fieldIndex++).offset()); - // y coordinate in pixel (i,j) - this->imageXYZ[i][j][1] = *reinterpret_cast( - msgBufferIndex + _msg.field(fieldIndex++).offset()); - // z coordinate in pixel (i,j) - this->imageXYZ[i][j][2] = *reinterpret_cast( - msgBufferIndex + _msg.field(fieldIndex++).offset()); - - // Filter out points outside of the contact sensor - - // We assume that the Depth Camera is only displaced in the -X direction - bool insideX = (this->imageXYZ[i][j][0] >= std::abs( - this->depthCameraOffset.X()) - this->extendedSensing) - && (this->imageXYZ[i][j][0] <= (std::abs(this->depthCameraOffset.X()) + - this->sensorSize.X() + this->extendedSensing)); - - bool insideY = (this->imageXYZ[i][j][1] <= this->sensorSize.Y() / 2 + - this->extendedSensing) - && (this->imageXYZ[i][j][1] >= - this->sensorSize.Y() / 2 - - this->extendedSensing); - - bool insideZ = (this->imageXYZ[i][j][2] <= this->sensorSize.Z() / 2 + - this->extendedSensing) - && (this->imageXYZ[i][j][2] >= - this->sensorSize.Z() / 2 - - this->extendedSensing); - - if (!insideX || !insideY || !insideZ) - { - this->imageXYZ[i][j][0] = ignition::math::INF_D; - this->imageXYZ[i][j][1] = ignition::math::INF_D; - this->imageXYZ[i][j][2] = ignition::math::INF_D; - } + return measuredPoint; +} - msgBufferIndex += _msg.point_step(); - } - } +////////////////////////////////////////////////// +bool OpticalTactilePluginPrivate::PointInsideSensor( + ignition::math::Vector3f _point) +{ + // We assume that the Depth Camera is only displaced in the -X direction + bool insideX = (_point.X() >= std::abs( + this->depthCameraOffset.X()) - this->extendedSensing) + && (_point.X() <= (std::abs(this->depthCameraOffset.X()) + + this->sensorSize.X() + this->extendedSensing)); + + bool insideY = (_point.Y() <= this->sensorSize.Y() / 2 + + this->extendedSensing) + && (_point.Y() >= - this->sensorSize.Y() / 2 - + this->extendedSensing); + + bool insideZ = (_point.Z() <= this->sensorSize.Z() / 2 + + this->extendedSensing) + && (_point.Z() >= - this->sensorSize.Z() / 2 - + this->extendedSensing); + + return (insideX && insideY && insideZ); } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::ComputeNormalForces( - const ignition::msgs::PointCloudPacked &_msg, - const bool _visualizeForces) + const ignition::msgs::PointCloudPacked &_msg, + const bool _visualizeForces) { - // 2D vector to store normal force vectors - std::vector> normalForces(_msg.width(), - std::vector(_msg.height())); - - // Visualization is downsampled according to plugin parameter - // if is set to true - bool visualizeRow = true; - int rowCounter = 0; + // Get data from the message + std::string rawMsgBuffer = _msg.data(); + this->msgBuffer = rawMsgBuffer.data(); - bool visualizeColumn = true; - int columnCounter = 0; + // Declare variables for storing the XYZ points + ignition::math::Vector3f p1, p2, p3, p4, markerPosition; - // Variable for setting the markers id through the iteration - int marker_id = 1; + // todo(anyone) Improve performance once the following issue is completed: + // https://github.com/ignitionrobotics/ign-math/issues/144 + // We don't get the image's edges because there are no adjacent points to + // compute the forces for (uint64_t j = 1; j < (_msg.height() - 1); ++j) { for (uint64_t i = 1; i < (_msg.width() - 1); ++i) { - // Compute normal forces - double dxdi = - (this->imageXYZ[i+1][j][0] - - this->imageXYZ[i-1][j][0])/ - std::abs(this->imageXYZ[i+1][j][1] - - this->imageXYZ[i-1][j][1]); - - double dxdj = - (this->imageXYZ[i][j+1][0] - - this->imageXYZ[i][j-1][0])/ - std::abs(this->imageXYZ[i][j+1][2] - - this->imageXYZ[i][j-1][2]); + // Get points for computing normal forces + p1 = this->MapPointCloudData(i + 1, j); + p2 = this->MapPointCloudData(i - 1, j); + p3 = this->MapPointCloudData(i, j + 1); + p4 = this->MapPointCloudData(i, j - 1); + + float dxdi = (p1.X() - p2.X()) / std::abs(p1.Y() - p2.Y()); + float dxdj = (p3.X() - p4.X()) / std::abs(p3.Z() - p4.Z()); ignition::math::Vector3f direction(-1, -dxdi, -dxdj); // todo(anyone) multiply vector by contact forces info - normalForces[i][j] = direction.Normalized(); + ignition::math::Vector3f normalForce = direction.Normalized(); - // todo(mcres) normal forces will be published even if visualization - // is turned off + // todo(mcres) Normal forces will be computed even if visualization + // is turned off. These forces should be published in the future. if (!_visualizeForces) - continue; + continue; - // Downsampling for visualization - if (visualizeRow && visualizeColumn) - { - // Add new markers with specific lifetime - this->positionMarkerMsg.set_id(marker_id); - this->positionMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->positionMarkerMsg.mutable_lifetime()->set_sec( - this->cameraUpdateRate); - this->positionMarkerMsg.mutable_lifetime()->set_nsec(0); - - this->forceMarkerMsg.set_id(marker_id++); - this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->forceMarkerMsg.mutable_lifetime()->set_sec( - this->cameraUpdateRate); - this->forceMarkerMsg.mutable_lifetime()->set_nsec(0); - - // The pose of the markers must be published with reference - // to the simulation origin - ignition::math::Vector3f forceMarkerSensorPosition - (this->imageXYZ[i][j][0], this->imageXYZ[i][j][1], - this->imageXYZ[i][j][2]); - - ignition::math::Quaternionf forceMarkerSensorQuaternion; - forceMarkerSensorQuaternion.From2Axes( - ignition::math::Vector3f(0, 0, 1), normalForces[i][j]); - - // The position of the force marker is modified in order to place the - // end of the cylinder in the surface, not its middle point - ignition::math::Pose3f forceMarkerSensorPose( - forceMarkerSensorPosition + - normalForces[i][j] * (this->forceLength/2), - forceMarkerSensorQuaternion); - - ignition::math::Pose3f forceMarkerWorldPose = - (forceMarkerSensorPose + this->depthCameraOffset) + - this->sensorWorldPose; - forceMarkerWorldPose.Correct(); - - ignition::math::Pose3f positionMarkerSensorPose( - forceMarkerSensorPosition, forceMarkerSensorQuaternion); - ignition::math::Pose3f positionMarkerWorldPose = - (positionMarkerSensorPose + this->depthCameraOffset) + - this->sensorWorldPose; - positionMarkerWorldPose.Correct(); - - // Set markers pose messages from previously computed poses - this->forceMarkerMsg.mutable_pose() - ->mutable_position()->set_x(forceMarkerWorldPose.Pos().X()); - this->forceMarkerMsg.mutable_pose() - ->mutable_position()->set_y(forceMarkerWorldPose.Pos().Y()); - this->forceMarkerMsg.mutable_pose() - ->mutable_position()->set_z(forceMarkerWorldPose.Pos().Z()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_x(forceMarkerWorldPose.Rot().X()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_y(forceMarkerWorldPose.Rot().Y()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_z(forceMarkerWorldPose.Rot().Z()); - this->forceMarkerMsg.mutable_pose() - ->mutable_orientation()->set_w(forceMarkerWorldPose.Rot().W()); - - this->positionMarkerMsg.mutable_pose() - ->mutable_position()->set_x(positionMarkerWorldPose.Pos().X()); - this->positionMarkerMsg.mutable_pose() - ->mutable_position()->set_y(positionMarkerWorldPose.Pos().Y()); - this->positionMarkerMsg.mutable_pose() - ->mutable_position()->set_z(positionMarkerWorldPose.Pos().Z()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_x(positionMarkerWorldPose.Rot().X()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_y(positionMarkerWorldPose.Rot().Y()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_z(positionMarkerWorldPose.Rot().Z()); - this->positionMarkerMsg.mutable_pose() - ->mutable_orientation()->set_w(positionMarkerWorldPose.Rot().W()); - - this->node.Request("/marker", this->forceMarkerMsg); - this->node.Request("/marker", this->positionMarkerMsg); - } - visualizeColumn = (this->resolution == ++columnCounter); - if (visualizeColumn) - columnCounter = 0; + markerPosition = this->MapPointCloudData(i, j); + this->visualizePtr->VisualizeNormalForce(i, j, markerPosition, + normalForce, this->sensorWorldPose); } - visualizeColumn = true; - visualizeRow = (this->resolution == ++rowCounter); - if (visualizeRow) - rowCounter = 0; } } IGNITION_ADD_PLUGIN(OpticalTactilePlugin, - ignition::gazebo::System, - OpticalTactilePlugin::ISystemConfigure, - OpticalTactilePlugin::ISystemPreUpdate, - OpticalTactilePlugin::ISystemPostUpdate) + ignition::gazebo::System, + OpticalTactilePlugin::ISystemConfigure, + OpticalTactilePlugin::ISystemPreUpdate, + OpticalTactilePlugin::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(OpticalTactilePlugin, - "ignition::gazebo::systems::OpticalTactilePlugin") + "ignition::gazebo::systems::OpticalTactilePlugin") diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index e6c9dff800..1793edcd7b 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -21,6 +21,7 @@ #include #include #include +#include "Visualization.hh" namespace ignition { @@ -45,23 +46,30 @@ namespace systems /// doesn't need to be enabled. This element is optional, and the /// default value is true. /// - /// Number of pixels to skip when visualizing forces. One - /// vector representing a normal force is computed for each of the camera - /// pixels. This element must be positive and it is optional. The default - /// value is 100. + /// Number of pixels to skip when visualizing + /// the forces. One vector representing a normal force is computed for each + /// of the camera pixels. This element must be positive and it is optional. + /// The default value is 30. + /// + /// Distance in mm to interpolate the contacts + /// returned by the contact sensor. This element must be positive and + /// it is optional. The default value is 1. /// /// 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. /// /// Radius in meters of the contacts visualized if - /// is set to true. This parameter is optional. + /// is set to true. This parameter is optional, and the + /// default value is 0.003. /// /// Radius in meters of the forces visualized if - /// is set to true. This parameter is optional. + /// is set to true. This parameter is optional, and the + /// default value is 0.001. /// /// Length in meters of the forces visualized if - /// is set to true. This parameter is optional. + /// 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 @@ -71,10 +79,10 @@ namespace systems /// is optional, and the default value is false. class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin : - public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: OpticalTactilePlugin(); @@ -84,18 +92,18 @@ namespace systems // Documentation inherited public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; + EntityComponentManager &_ecm) override; // Documentation inherited public: void PostUpdate( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc new file mode 100644 index 0000000000..53e4be53a8 --- /dev/null +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -0,0 +1,239 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include "Visualization.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +namespace systems +{ +namespace optical_tactile_sensor +{ + +////////////////////////////////////////////////// +OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( + double &_contactRadius, double &_forceRadius, double &_forceLength, + float &_cameraUpdateRate, ignition::math::Pose3f &_depthCameraOffset, + int &_visualizationResolution) : + contactRadius(_contactRadius), forceRadius(_forceRadius), + forceLength(_forceLength), cameraUpdateRate(_cameraUpdateRate), + depthCameraOffset(_depthCameraOffset), + visualizationResolution(_visualizationResolution) +{ +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::CreateMarkersMsgs( + std::string &_modelName, + ignition::math::Vector3d &_sensorSize) +{ + // Configure marker messages for position and force of the contacts, as well + // as the marker for visualizing the sensor + + // Blue spheres for positions + // Green cylinders for forces + // Grey transparent box for the sensor + + this->positionMarkerMsg.set_ns("positions_" + _modelName); + this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->positionMarkerMsg.set_type(ignition::msgs::Marker::SPHERE); + this->positionMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + this->forceMarkerMsg.set_ns("forces_" + _modelName); + this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->forceMarkerMsg.set_type(ignition::msgs::Marker::CYLINDER); + this->forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + this->sensorMarkerMsg.set_ns("sensor_" + _modelName); + this->sensorMarkerMsg.set_id(1); + this->sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->sensorMarkerMsg.set_type(ignition::msgs::Marker::BOX); + this->sensorMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + // Set material properties + this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); + this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); + this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); + this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); + this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + this->positionMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + + this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); + this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); + this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); + this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); + this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + this->forceMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + + this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_r(0.5); + this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_g(0.5); + this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_b(0.5); + this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_a(0.75); + this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0.5); + this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0.5); + this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0.5); + this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_a(0.75); + this->sensorMarkerMsg.mutable_lifetime()->set_sec(0); + this->sensorMarkerMsg.mutable_lifetime()->set_nsec(0); + + // Set scales + ignition::msgs::Set(this->positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(this->contactRadius, this->contactRadius, + this->contactRadius)); + + ignition::msgs::Set(this->forceMarkerMsg.mutable_scale(), + ignition::math::Vector3d(this->forceRadius, this->forceRadius, + this->forceLength)); + + ignition::msgs::Set(this->sensorMarkerMsg.mutable_scale(), _sensorSize); +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::VisualizeSensor( + ignition::math::Pose3f &_sensorPose) +{ + this->sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->sensorMarkerMsg.set_id(1); + + this->sensorMarkerMsg.mutable_pose()->mutable_position()->set_x( + _sensorPose.Pos().X()); + this->sensorMarkerMsg.mutable_pose()->mutable_position()->set_y( + _sensorPose.Pos().Y()); + this->sensorMarkerMsg.mutable_pose()->mutable_position()->set_z( + _sensorPose.Pos().Z()); + this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_x( + _sensorPose.Rot().X()); + this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_y( + _sensorPose.Rot().Y()); + this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_z( + _sensorPose.Rot().Z()); + this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_w( + _sensorPose.Rot().W()); + + this->node.Request("/marker", this->sensorMarkerMsg); +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::VisualizeNormalForce( + uint64_t &_i, uint64_t &_j, + ignition::math::Vector3f &_position, + ignition::math::Vector3f &_normalForce, + ignition::math::Pose3f &_sensorWorldPose) +{ + // Visualization is downsampled according to plugin parameter + // if is set to true + + // Check if _normalForce has to be visualized + bool visualizeForce = (_i % this->visualizationResolution == 0 && + _j % this->visualizationResolution == 0) || + (_i % this->visualizationResolution == 0 && _j == 1) || + (_i == 1 && _j % this->visualizationResolution == 0) || + (_i == 1 && _j == 1); + + if (!visualizeForce) + return; + + if (_i == 1 && _j == 1) + this->markerID = 1; + + // Add new markers with specific lifetime + this->positionMarkerMsg.set_id(this->markerID); + this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->positionMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + this->positionMarkerMsg.mutable_lifetime()->set_nsec(0); + + this->forceMarkerMsg.set_id(this->markerID++); + this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->forceMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + this->forceMarkerMsg.mutable_lifetime()->set_nsec(0); + + // The pose of the markers must be published with reference + // to the simulation origin + ignition::math::Vector3f forceMarkerSensorPosition( + _position.X(), _position.Y(), _position.Z()); + + ignition::math::Quaternionf forceMarkerSensorQuaternion; + forceMarkerSensorQuaternion.From2Axes( + ignition::math::Vector3f(0, 0, 1), _normalForce); + + // The position of the force marker is modified in order to place the + // end of the cylinder in the surface, not its middle point + ignition::math::Pose3f forceMarkerSensorPose(forceMarkerSensorPosition + + _normalForce * (this->forceLength/2), forceMarkerSensorQuaternion); + + ignition::math::Pose3f forceMarkerWorldPose = + (forceMarkerSensorPose + this->depthCameraOffset) + _sensorWorldPose; + forceMarkerWorldPose.Correct(); + + ignition::math::Pose3f positionMarkerSensorPose( + forceMarkerSensorPosition, forceMarkerSensorQuaternion); + ignition::math::Pose3f positionMarkerWorldPose = + (positionMarkerSensorPose + this->depthCameraOffset) + _sensorWorldPose; + positionMarkerWorldPose.Correct(); + + // Set markers pose messages from previously computed poses + this->forceMarkerMsg.mutable_pose()->mutable_position()->set_x( + forceMarkerWorldPose.Pos().X()); + this->forceMarkerMsg.mutable_pose()->mutable_position()->set_y( + forceMarkerWorldPose.Pos().Y()); + this->forceMarkerMsg.mutable_pose()->mutable_position()->set_z( + forceMarkerWorldPose.Pos().Z()); + this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_x( + forceMarkerWorldPose.Rot().X()); + this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_y( + forceMarkerWorldPose.Rot().Y()); + this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_z( + forceMarkerWorldPose.Rot().Z()); + this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_w( + forceMarkerWorldPose.Rot().W()); + + this->positionMarkerMsg.mutable_pose()->mutable_position()->set_x( + positionMarkerWorldPose.Pos().X()); + this->positionMarkerMsg.mutable_pose()->mutable_position()->set_y( + positionMarkerWorldPose.Pos().Y()); + this->positionMarkerMsg.mutable_pose()->mutable_position()->set_z( + positionMarkerWorldPose.Pos().Z()); + this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_x( + positionMarkerWorldPose.Rot().X()); + this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_y( + positionMarkerWorldPose.Rot().Y()); + this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_z( + positionMarkerWorldPose.Rot().Z()); + this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_w( + positionMarkerWorldPose.Rot().W()); + + this->node.Request("/marker", this->forceMarkerMsg); + this->node.Request("/marker", this->positionMarkerMsg); +} +} +} +} +} +} diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh new file mode 100644 index 0000000000..fa61de08b5 --- /dev/null +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -0,0 +1,125 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_OPTICAL_TACTILE_PLUGIN_VISUALIZATION_HH_ +#define IGNITION_GAZEBO_SYSTEMS_OPTICAL_TACTILE_PLUGIN_VISUALIZATION_HH_ + +#include +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ +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 + class OpticalTactilePluginVisualization + { + /// \brief Constructor + /// \param[in] _contactRadius Value of the contactRadius attribute + /// \param[in] _forceRadius Value of the forceRadius attribute + /// \param[in] _forceLength Value of the forceLength attribute + /// \param[in] _cameraUpdateRate Value of the cameraUpdateRate attribute + /// \param[in] _depthCameraOffset Value of the depthCameraOffset attribute + /// \param[in] _visualizationResolution Value of the + /// visualizationResolution attribute + public: OpticalTactilePluginVisualization(double &_contactRadius, + double &_forceRadius, double &_forceLength, float &_cameraUpdateRate, + ignition::math::Pose3f &_depthCameraOffset, + int &_visualizationResolution); + + /// \brief Destructor + public: ~OpticalTactilePluginVisualization(); + + /// \brief Initialize the messages needed to request the /marker service + /// \param[in] _modelName Name of the model + /// \param[in] _sensorSize Size of the contact sensor + public: void CreateMarkersMsgs(std::string &_modelName, + ignition::math::Vector3d &_sensorSize); + + /// \brief Visualize the collision geometry of the sensor. 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 VisualizeSensor(ignition::math::Pose3f &_sensorPose); + + /// \brief Visualize the normal forces computed for a (i,j) pixel of the + /// image returned by the depth camera + /// \param[in] _i First coordinate of the pixel + /// \param[in] _j Second coordinate of the pixel + /// \param[in] _position Position of the normal force referenced to the + /// depth camera's origin + /// \param[in] _normalForce Value of the normal force referenced to + /// the depth camera's origin + /// \param[in] _sensorWorldPose Pose of the plugin's model referenced to + /// the world's origin + public: void VisualizeNormalForce(uint64_t &_i, uint64_t &_j, + ignition::math::Vector3f &_position, + ignition::math::Vector3f &_normalForce, + ignition::math::Pose3f &_sensorWorldPose); + + /// \brief Message for visualizing the sensor + private: ignition::msgs::Marker sensorMarkerMsg; + + /// \brief Message for visualizing contacts positions + private: ignition::msgs::Marker positionMarkerMsg; + + /// \brief Message for visualizing contacts forces + private: ignition::msgs::Marker forceMarkerMsg; + + /// \brief Transport node to request the /marker service + private: ignition::transport::Node node; + + /// \brief Radius of the visualized contact sphere + private: double contactRadius; + + /// \brief Radius of the visualized force cylinder + private: double forceRadius; + + /// \brief Length of the visualized force cylinder + private: double forceLength; + + /// \brief Depth camera update rate + private: float cameraUpdateRate; + + /// \brief Offset between depth camera pose and model pose + private: ignition::math::Pose3f depthCameraOffset; + + /// \brief Resolution of the sensor in pixels to skip. + private: int visualizationResolution; + + /// \brief Variable for setting the markers id + private: int markerID; + }; +} +} +} +} +} + +#endif From e216bb2132d0a77ca77dc31a50435487273fbe09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 13 Aug 2020 11:00:40 +0200 Subject: [PATCH 20/23] PR Feedback 3 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 | 396 ++++++++---------- .../OpticalTactilePlugin.hh | 20 +- .../optical_tactile_plugin/Visualization.cc | 301 ++++++------- .../optical_tactile_plugin/Visualization.hh | 84 ++-- 5 files changed, 358 insertions(+), 446 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index e888d75092..d3532c0e06 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -202,11 +202,8 @@ name="ignition::gazebo::systems::OpticalTactilePlugin"> true 15 - 1 true true - 0.001 - 0.0002 0.01 0.001 diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index b9f339d4b0..58ca03e3cb 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -55,33 +55,29 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \param[in] _value True to enable plugin. public: void Enable(const bool _value); - /// \brief Interpolates contacts returned by the contact sensor - /// \param[in] _contact Message containing the data to interpolate - public: void InterpolateContactData(const ignition::msgs::Contact &_contact); - - /// \brief Callback for the Depth Camera + /// \brief Callback for the depth camera /// \param[in] _msg Message from the subscribed topic public: void DepthCameraCallback( const ignition::msgs::PointCloudPacked &_msg); /// \brief Map the (i,j) coordinates defined in the top-left corner of the /// camera into its corresponding (X,Y,Z) measurement with respect to the - /// camera's origin + /// camera's origin in the world frame /// \param[in] _i Horizontal camera coordinate defined in the top-left corner /// of the image, pointing rightwards /// \param[in] _j Vertical camera coordinate defined in the top-left corner /// of the image, pointing downwards /// \returns The corresponding (X,Y,Z) point - public: ignition::math::Vector3f MapPointCloudData(const u_int32_t &_i, - const uint32_t &_j); + public: ignition::math::Vector3f MapPointCloudData(const uint64_t &_i, + const uint64_t &_j); - /// \brief Check if a specific point returned by the Depth Camera is inside + /// \brief Check if a specific point from the depth camera is inside /// the contact surface. - /// \param[in] _point Point returned by the Depth Camera + /// \param[in] _point Point from the depth camera public: bool PointInsideSensor(ignition::math::Vector3f _point); /// \brief Computes the normal forces of the Optical Tactile sensor - /// \param[in] _msg Message returned by the Depth Camera + /// \param[in] _msg Message from the depth camera /// \param[in] _visualizeForces Whether to visualize the forces or not /// /// Implementation inspired by @@ -95,9 +91,6 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Resolution of the visualization in pixels to skip. public: int visualizationResolution{30}; - /// \brief Resolution of the interpolated contacts in mm. - public: double contactsResolution{1.0}; - /// \brief Whether to visualize the normal forces. public: bool visualizeForces{false}; @@ -123,34 +116,28 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Name of the model to which the sensor is attached. public: std::string modelName; - /// \brief Radius of the visualized contact sphere - public: double contactRadius{0.003}; - - /// \brief Length of the visualized force cylinder + /// \brief Length of the visualized forces public: double forceLength{0.01}; - /// \brief Radius of the visualized force cylinder - public: double forceRadius{0.001}; - /// \brief Pose of the sensor model public: ignition::math::Pose3f sensorWorldPose; /// \brief Pose of the sensor model in the previous iteration public: ignition::math::Pose3f prevSensorWorldPose; - /// \brief Offset between Depth Camera pose and model pose + /// \brief Offset between depth camera pose and model pose public: ignition::math::Pose3f depthCameraOffset; - /// \brief Whether a new message has been returned by the Depth Camera + /// \brief Whether a new message has been returned by the depth camera public: bool newCameraMsg{false}; - /// \brief Depth Camera update rate + /// \brief Depth camera update rate public: float cameraUpdateRate{1}; - /// \brief Message returned by the Depth Camera + /// \brief Message returned by the depth camera public: ignition::msgs::PointCloudPacked cameraMsg; - /// \brief Data returned by the Depth Camera after being preprocessed + /// \brief Preprocessed depth camera data public: char *msgBuffer; /// \brief Mutex for variables mutated by the camera callback. @@ -170,6 +157,12 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Pointer to visualization object public: std::unique_ptr visualizePtr; + + /// \brief Flag for checking the data type returned by the DepthCamera + public: bool checkDepthCameraData{true}; + + /// \brief Flag for allowing the plugin to output error/warning only once + public: bool initErrorPrinted{false}; }; ////////////////////////////////////////////////// @@ -195,7 +188,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, // Get the sdf parameters - // if (!_sdf->HasElement("enabled")) { ignlog << "Missing parameter , " @@ -206,7 +198,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->enabled = _sdf->Get("enabled"); } - // if (!_sdf->HasElement("visualization_resolution")) { ignlog << "Missing parameter , " @@ -226,27 +217,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, } } - // - if (!_sdf->HasElement("contacts_resolution")) - { - ignlog << "Missing parameter , " - << "setting to " << this->dataPtr->contactsResolution << std::endl; - } - else - { - if (_sdf->Get("contacts_resolution") < 0) - { - ignwarn << "Parameter must be positive, " - << "setting to " << this->dataPtr->contactsResolution << std::endl; - } - else - { - this->dataPtr->contactsResolution = - _sdf->Get("contacts_resolution"); - } - } - - // if (!_sdf->HasElement("visualize_forces")) { ignlog << "Missing parameter , " @@ -257,7 +227,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->visualizeForces = _sdf->Get("visualize_forces"); } - // if (!_sdf->HasElement("extended_sensing")) { ignlog << "Missing parameter , " @@ -276,7 +245,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, } } - // if (!_sdf->HasElement("visualize_sensor")) { ignlog << "Missing parameter , " @@ -287,45 +255,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->visualizeSensor = _sdf->Get("visualize_sensor"); } - // - if (!_sdf->HasElement("contact_radius")) - { - ignlog << "Missing parameter , " - << "setting to " << this->dataPtr->contactRadius << std::endl; - } - else - { - if (_sdf->Get("contact_radius") < 0) - { - ignwarn << "Parameter must be positive, " - << "setting to " << this->dataPtr->contactRadius << std::endl; - } - else - { - this->dataPtr->contactRadius = _sdf->Get("contact_radius"); - } - } - - // - if (!_sdf->HasElement("force_radius")) - { - ignlog << "Missing parameter , " - << "setting to " << this->dataPtr->forceRadius << std::endl; - } - else - { - if (_sdf->Get("force_radius") < 0) - { - ignwarn << "Parameter must be positive, " - << "setting to " << this->dataPtr->forceRadius << std::endl; - } - else - { - this->dataPtr->forceRadius = _sdf->Get("force_radius"); - } - } - - // if (!_sdf->HasElement("force_length")) { ignlog << "Missing parameter , " @@ -347,10 +276,28 @@ 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 = - _sdf->GetParent()->GetElement("link")->GetElement("collision")-> - GetElement("geometry")->GetElement("box")-> - Get("size"); + if (_sdf->GetParent() != nullptr) + { + if (_sdf->GetParent()->GetElement("link") != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision") + != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry") != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box") != nullptr) + { + this->dataPtr->sensorSize = + _sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box")-> + Get("size"); + } + } + } + } + } } ////////////////////////////////////////////////// @@ -368,7 +315,6 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, // We call Load here instead of Configure because we can't be guaranteed // that all entities have been created when Configure is called this->dataPtr->Load(_ecm); - this->dataPtr->initialized = true; } // This is not an "else" because "initialized" can be set @@ -386,12 +332,12 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, break; } - // Get the Depth Camera pose + // Get the model pose ignition::math::Pose3d sensorPose = _ecm.Component( this->dataPtr->model.Entity())->Data(); - // Depth Camera data is float, so convert Pose3d to Pose3f + // Depth camera data is float, so convert Pose3d to Pose3f this->dataPtr->sensorWorldPose = ignition::math::Pose3f( sensorPose.Pos().X(), sensorPose.Pos().Y(), sensorPose.Pos().Z(), sensorPose.Rot().W(), sensorPose.Rot().X(), sensorPose.Rot().Y(), @@ -402,26 +348,15 @@ 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"); - // Nothing left to do if paused. - if (_info.paused) + // Nothing left to do if paused or failed to initialize. + if (_info.paused || !this->dataPtr->initialized) return; - auto* contacts = - _ecm.Component( - this->dataPtr->sensorCollisionEntity); - - if (contacts) - { - for (const auto &contact : contacts->Data().contact()) - { - // Interpolate data returned by the Contact message - this->dataPtr->InterpolateContactData(contact); - } - } + // TODO(anyone) Get ContactSensor data and merge it with DepthCamera data // Process camera message if it's new { @@ -439,7 +374,7 @@ void OpticalTactilePlugin::PostUpdate( if (this->dataPtr->visualizeSensor && (this->dataPtr->sensorWorldPose != this->dataPtr->prevSensorWorldPose)) { - this->dataPtr->visualizePtr->VisualizeSensor( + this->dataPtr->visualizePtr->RequestSensorMarkerMsg( this->dataPtr->sensorWorldPose); } @@ -452,22 +387,32 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) { igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; - // todo(anyone) The plugin should be able to handle more than one link. In - // that case, one of the links would be the actual tactile sensor. - // Check plugin structure - // Model should only have 1 link auto allLinks = _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); - if (allLinks.size() != 1) + // Model should only have 1 link + // todo(anyone) The plugin should be able to handle more than one link. In + // that case, one of the links would be the actual tactile sensor. + if (allLinks.size() != 1 && !this->initErrorPrinted) { - ignerr << "Plugin must have exactly 1 link" << std::endl; + ignerr << "Plugin must have exactly 1 link (only printed once)" + << std::endl; + this->initErrorPrinted = true; return; } // Link should have a collision component linked to a contact sensor auto linkCollisions = _ecm.ChildrenByComponents(allLinks[0], components::Collision()); + + if (linkCollisions.size() == 0 && !this->initErrorPrinted) + { + ignerr << "Link must have at least 1 collision (only printed once)" + << std::endl; + this->initErrorPrinted = true; + return; + } + for (const Entity &colEntity : linkCollisions) { if (_ecm.EntityHasComponentType(colEntity, @@ -482,7 +427,7 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } } - // Link should have exactly 1 Contact sensor and 1 Depth Camera sensor + // Link should have exactly 1 ContactSensor and 1 DepthCamera sensor auto sensorsInsideLink = _ecm.ChildrenByComponents(allLinks[0], components::Sensor()); @@ -504,53 +449,68 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } } - if ((depthCameraCounter != 1) || (contactSensorCounter != 1)) + if (((depthCameraCounter != 1) || (contactSensorCounter != 1)) + && !this->initErrorPrinted) { - ignerr << "Link must have 1 Depth Camera sensor and " - << "1 Contact sensor" << std::endl; + ignerr << "Link must have exactly 1 depth camera sensor and " + << "1 contact sensor (only printed once)" << std::endl; + this->initErrorPrinted = true; return; } // Store depth camera update rate and offset from model auto offset = depthCameraSdf->Get("pose"); - if (!depthCameraSdf->HasElement("update_rate")) + if (!depthCameraSdf->HasElement("update_rate") && !this->initErrorPrinted) { - ignerr << "Depth camera should have an value" << std::endl; + ignerr << "Depth camera should have an value " + << "(only printed once)" << std::endl; + this->initErrorPrinted = true; return; } this->cameraUpdateRate = depthCameraSdf->Get("update_rate"); - // Depth Camera data is float, so convert Pose3d to Pose3f + // Depth camera data is float, so convert Pose3d to Pose3f this->depthCameraOffset = ignition::math::Pose3f( offset.Pos().X(), offset.Pos().Y(), offset.Pos().Z(), offset.Rot().W(), offset.Rot().X(), offset.Rot().Y(), offset.Rot().Z()); - // Configure subscriber for Depth Camera images + // Configure subscriber for depth camera images if (!depthCameraSdf->HasElement("topic")) { - ignerr << "Depth camera should have a unique value" << std::endl; - return; + ignwarn << "Depth camera publishing to __default__ topic. " + << "It's possible that two depth cameras are publishing into the same " + << "topic" << std::endl; + } + else + { + ignlog << "Depth camera publishing to " + << depthCameraSdf->Get("topic") << " topic" << std::endl; } + std::string topic = "/" + depthCameraSdf->Get("topic") + "/points"; if (!this->node.Subscribe(topic, - &OpticalTactilePluginPrivate::DepthCameraCallback, this)) + &OpticalTactilePluginPrivate::DepthCameraCallback, this) + && !this->initErrorPrinted) { ignerr << "Error subscribing to topic " << "[" << topic << "]. " - << " must not contain '/'" << std::endl; + << " must not contain '/' (only printing once)" << std::endl; + this->initErrorPrinted = true; return; } // Instantiate the visualization class this->visualizePtr = std::make_unique< - OpticalTactilePluginVisualization>(this->contactRadius, - this->forceRadius, this->forceLength, - this->cameraUpdateRate, this->depthCameraOffset, - this->visualizationResolution); - - // Create the marker messages - this->visualizePtr->CreateMarkersMsgs(this->modelName, this->sensorSize); + OpticalTactilePluginVisualization>( + this->modelName, + this->sensorSize, + this->forceLength, + this->cameraUpdateRate, + this->depthCameraOffset, + this->visualizationResolution); + + this->initialized = true; } ////////////////////////////////////////////////// @@ -560,65 +520,6 @@ void OpticalTactilePluginPrivate::Enable(const bool _value) _value; } -////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::InterpolateContactData( - const ignition::msgs::Contact &_contact) -{ - // The Optical Tactile Sensor is supposed to be box-shaped. - // Contacts returned by the Contact Sensor in this type of shapes should be - // 4 points in the corners of some of its faces, in this case the one - // corresponding to the 'sensing' face. - // Given that the number of contacts may change even when the objects are - // at rest, data is only interpolated when the number of contacts - // returned is 4. - - // todo(mcres) Handle a different number of contacts returned by the - // contact sensor - - if (_contact.position_size() == 4) - { - // todo(mcres) Interpolated contacts should be published in a message - - ignition::math::Vector3d contact1 = - ignition::msgs::Convert(_contact.position(0)); - ignition::math::Vector3d contact2 = - ignition::msgs::Convert(_contact.position(1)); - ignition::math::Vector3d contact3 = - ignition::msgs::Convert(_contact.position(2)); - ignition::math::Vector3d contact4 = - ignition::msgs::Convert(_contact.position(3)); - - ignition::math::Vector3d direction1 = - (contact2 - contact1).Normalized() * (this->contactsResolution/1000.0); - ignition::math::Vector3d direction2 = - (contact4 - contact1).Normalized() * (this->contactsResolution/1000.0); - - ignition::math::Vector3d interpolatedVector = contact1; - - // auxiliary Vector3d to iterate through contacts - ignition::math::Vector3d tempVector(0.0, 0.0, 0.0); - - uint64_t steps1 = - contact1.Distance(contact2) / (this->contactsResolution/1000.0); - uint64_t steps2 = - contact1.Distance(contact4) / (this->contactsResolution/1000.0); - - for (uint64_t index1 = 0; index1 <= steps1; ++index1) - { - tempVector = interpolatedVector; - for (uint64_t index2 = 0; index2 < steps2; ++index2) - { - interpolatedVector += direction2; - } - if (index1 != steps1) - { - interpolatedVector = tempVector; - interpolatedVector += direction1; - } - } - } -} - ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::DepthCameraCallback( const ignition::msgs::PointCloudPacked &_msg) @@ -628,6 +529,21 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( if (!this->initialized) return; + // Check whether DepthCamera returns FLOAT32 data + if (this->checkDepthCameraData) + { + int float32Type = 6; + for (auto const &field : _msg.field()) + { + if (field.datatype() != float32Type) + { + ignerr << "FLOAT32 is expected for a casting to float *" << std::endl; + return; + } + } + this->checkDepthCameraData = false; + } + { std::lock_guard lock(this->serviceMutex); this->cameraMsg = _msg; @@ -637,31 +553,35 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( ////////////////////////////////////////////////// ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( - const u_int32_t &_i, const uint32_t &_j) + const uint64_t &_i, const uint64_t &_j) { // Initialize return variable - ignition::math::Vector3f measuredPoint; + ignition::math::Vector3f measuredPoint(0, 0, 0); + + // Nothing left to do if failed to initialize. + if (!this->initialized) + return measuredPoint; // Save original position of the pointer - char *initialMsgBuffer = this->msgBuffer; + char *temporaryMsgBuffer = this->msgBuffer; // 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->cameraMsg.row_step() + _i*this->cameraMsg.point_step(); - this->msgBuffer += msgBufferIndex; + temporaryMsgBuffer += msgBufferIndex; int fieldIndex = 0; // X coordinate measuredPoint.X() = *reinterpret_cast( - this->msgBuffer + this->cameraMsg.field(fieldIndex++).offset()); + temporaryMsgBuffer + this->cameraMsg.field(fieldIndex++).offset()); // Y coordinate measuredPoint.Y() = *reinterpret_cast( - this->msgBuffer + this->cameraMsg.field(fieldIndex++).offset()); + temporaryMsgBuffer + this->cameraMsg.field(fieldIndex++).offset()); // Z coordinate measuredPoint.Z() = *reinterpret_cast( - this->msgBuffer + this->cameraMsg.field(fieldIndex).offset()); + temporaryMsgBuffer + this->cameraMsg.field(fieldIndex).offset()); // Check if point is inside the sensor bool pointInside = this->PointInsideSensor(measuredPoint); @@ -672,9 +592,6 @@ ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( measuredPoint.Z() = ignition::math::INF_F; } - // Restore msgBuffer to initial position - this->msgBuffer = initialMsgBuffer; - return measuredPoint; } @@ -682,21 +599,25 @@ ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( bool OpticalTactilePluginPrivate::PointInsideSensor( ignition::math::Vector3f _point) { - // We assume that the Depth Camera is only displaced in the -X direction - bool insideX = (_point.X() >= std::abs( - this->depthCameraOffset.X()) - this->extendedSensing) - && (_point.X() <= (std::abs(this->depthCameraOffset.X()) + - this->sensorSize.X() + this->extendedSensing)); - - bool insideY = (_point.Y() <= this->sensorSize.Y() / 2 + - this->extendedSensing) - && (_point.Y() >= - this->sensorSize.Y() / 2 - - this->extendedSensing); - - bool insideZ = (_point.Z() <= this->sensorSize.Z() / 2 + - this->extendedSensing) - && (_point.Z() >= - this->sensorSize.Z() / 2 - - this->extendedSensing); + // Nothing left to do if failed to initialize. + if (!this->initialized) + return false; + + // We assume that the depth camera is placed behind the contact surface, i.e. + // displaced in the -X direction with respect to the model's origin + bool insideX = + (_point.X() >= std::abs(this->depthCameraOffset.X()) - + this->extendedSensing) && + (_point.X() <= (std::abs(this->depthCameraOffset.X()) + + this->sensorSize.X() + this->extendedSensing)); + + bool insideY = + (_point.Y() <= this->sensorSize.Y() / 2 + this->extendedSensing) && + (_point.Y() >= - this->sensorSize.Y() / 2 - this->extendedSensing); + + bool insideZ = + (_point.Z() <= this->sensorSize.Z() / 2 + this->extendedSensing) && + (_point.Z() >= - this->sensorSize.Z() / 2 - this->extendedSensing); return (insideX && insideY && insideZ); } @@ -706,6 +627,10 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( const ignition::msgs::PointCloudPacked &_msg, const bool _visualizeForces) { + // Nothing left to do if failed to initialize. + if (!this->initialized) + return; + // Get data from the message std::string rawMsgBuffer = _msg.data(); this->msgBuffer = rawMsgBuffer.data(); @@ -713,14 +638,17 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Declare variables for storing the XYZ points ignition::math::Vector3f p1, p2, p3, p4, markerPosition; - // todo(anyone) Improve performance once the following issue is completed: - // https://github.com/ignitionrobotics/ign-math/issues/144 + // Marker messages representing the normal forces + ignition::msgs::Marker positionMarkerMsg; + ignition::msgs::Marker forceMarkerMsg; // We don't get the image's edges because there are no adjacent points to // compute the forces - for (uint64_t j = 1; j < (_msg.height() - 1); ++j) + for (uint64_t j = 1; j < (_msg.height() - 1); + j += this->visualizationResolution) { - for (uint64_t i = 1; i < (_msg.width() - 1); ++i) + for (uint64_t i = 1; i < (_msg.width() - 1); + i += this->visualizationResolution) { // Get points for computing normal forces p1 = this->MapPointCloudData(i + 1, j); @@ -734,18 +662,28 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( ignition::math::Vector3f direction(-1, -dxdi, -dxdj); // todo(anyone) multiply vector by contact forces info + + // todo(anyone) Replace with MatrixX and use vector multiplication instead + // of for-loops once the following issue is completed: + // https://github.com/ignitionrobotics/ign-math/issues/144 ignition::math::Vector3f normalForce = direction.Normalized(); - // todo(mcres) Normal forces will be computed even if visualization + // todo(mcres) Normal forces are computed even if visualization // is turned off. These forces should be published in the future. if (!_visualizeForces) continue; markerPosition = this->MapPointCloudData(i, j); - this->visualizePtr->VisualizeNormalForce(i, j, markerPosition, - normalForce, this->sensorWorldPose); + this->visualizePtr->AddNormalForceToMarkerMsgs(positionMarkerMsg, + forceMarkerMsg, markerPosition, normalForce, this->sensorWorldPose); } } + + if (_visualizeForces) + { + this->visualizePtr->RequestNormalForcesMarkerMsgs(positionMarkerMsg, + forceMarkerMsg); + } } IGNITION_ADD_PLUGIN(OpticalTactilePlugin, diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 1793edcd7b..fbefd9e05a 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -46,27 +46,15 @@ namespace systems /// doesn't need to be enabled. This element is optional, and the /// default value is true. /// - /// Number of pixels to skip when visualizing - /// the forces. One vector representing a normal force is computed for each - /// of the camera pixels. This element must be positive and it is optional. - /// The default value is 30. - /// - /// Distance in mm to interpolate the contacts - /// returned by the contact sensor. This element must be positive and - /// it is optional. The default value is 1. + /// 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. /// - /// Radius in meters of the contacts visualized if - /// is set to true. This parameter is optional, and the - /// default value is 0.003. - /// - /// Radius in meters of the forces visualized if - /// is set to true. This parameter is optional, and the - /// default value is 0.001. - /// /// Length in meters of the forces visualized if /// is set to true. This parameter is optional, and the /// default value is 0.01. diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 53e4be53a8..ef59688788 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -33,205 +33,176 @@ namespace optical_tactile_sensor ////////////////////////////////////////////////// OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( - double &_contactRadius, double &_forceRadius, double &_forceLength, - float &_cameraUpdateRate, ignition::math::Pose3f &_depthCameraOffset, + std::string &_modelName, + ignition::math::Vector3d &_sensorSize, + double &_forceLength, + float &_cameraUpdateRate, + ignition::math::Pose3f &_depthCameraOffset, int &_visualizationResolution) : - contactRadius(_contactRadius), forceRadius(_forceRadius), - forceLength(_forceLength), cameraUpdateRate(_cameraUpdateRate), + modelName(_modelName), + sensorSize(_sensorSize), + forceLength(_forceLength), + cameraUpdateRate(_cameraUpdateRate), depthCameraOffset(_depthCameraOffset), visualizationResolution(_visualizationResolution) { } ////////////////////////////////////////////////// -void OpticalTactilePluginVisualization::CreateMarkersMsgs( - std::string &_modelName, - ignition::math::Vector3d &_sensorSize) +void OpticalTactilePluginVisualization::InitializeSensorMarkerMsg( + ignition::msgs::Marker &_sensorMarkerMsg) { - // Configure marker messages for position and force of the contacts, as well - // as the marker for visualizing the sensor + // Initialize the marker for visualizing the sensor as a grey transparent box - // Blue spheres for positions - // Green cylinders for forces - // Grey transparent box for the sensor + _sensorMarkerMsg.set_ns("sensor_" + this->modelName); + _sensorMarkerMsg.set_id(1); + _sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + _sensorMarkerMsg.set_type(ignition::msgs::Marker::BOX); + _sensorMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + ignition::msgs::Set(_sensorMarkerMsg.mutable_scale(), this->sensorSize); - this->positionMarkerMsg.set_ns("positions_" + _modelName); - this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->positionMarkerMsg.set_type(ignition::msgs::Marker::SPHERE); - this->positionMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + // Set material properties + ignition::msgs::Set(_sensorMarkerMsg.mutable_material()-> + mutable_ambient(), math::Color(0.5, 0.5, 0.5, 0.75)); + ignition::msgs::Set(_sensorMarkerMsg.mutable_material()-> + mutable_diffuse(), math::Color(0.5, 0.5, 0.5, 0.75)); +} - this->forceMarkerMsg.set_ns("forces_" + _modelName); - this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->forceMarkerMsg.set_type(ignition::msgs::Marker::CYLINDER); - this->forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RequestSensorMarkerMsg( + ignition::math::Pose3f const &_sensorPose) +{ + ignition::msgs::Marker sensorMarkerMsg; - this->sensorMarkerMsg.set_ns("sensor_" + _modelName); - this->sensorMarkerMsg.set_id(1); - this->sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->sensorMarkerMsg.set_type(ignition::msgs::Marker::BOX); - this->sensorMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + this->InitializeSensorMarkerMsg(sensorMarkerMsg); - // Set material properties - this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); - this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); - this->positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); - this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); - this->positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - this->positionMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); - - this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); - this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); - this->forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); - this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); - this->forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - this->forceMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); - - this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_r(0.5); - this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_g(0.5); - this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_b(0.5); - this->sensorMarkerMsg.mutable_material()->mutable_ambient()->set_a(0.75); - this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0.5); - this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0.5); - this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0.5); - this->sensorMarkerMsg.mutable_material()->mutable_diffuse()->set_a(0.75); - this->sensorMarkerMsg.mutable_lifetime()->set_sec(0); - this->sensorMarkerMsg.mutable_lifetime()->set_nsec(0); - - // Set scales - ignition::msgs::Set(this->positionMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->contactRadius, this->contactRadius, - this->contactRadius)); - - ignition::msgs::Set(this->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->forceRadius, this->forceRadius, - this->forceLength)); - - ignition::msgs::Set(this->sensorMarkerMsg.mutable_scale(), _sensorSize); + 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())); + + this->node.Request("/marker", sensorMarkerMsg); } ////////////////////////////////////////////////// -void OpticalTactilePluginVisualization::VisualizeSensor( - ignition::math::Pose3f &_sensorPose) +void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg) { - this->sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->sensorMarkerMsg.set_id(1); - - this->sensorMarkerMsg.mutable_pose()->mutable_position()->set_x( - _sensorPose.Pos().X()); - this->sensorMarkerMsg.mutable_pose()->mutable_position()->set_y( - _sensorPose.Pos().Y()); - this->sensorMarkerMsg.mutable_pose()->mutable_position()->set_z( - _sensorPose.Pos().Z()); - this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_x( - _sensorPose.Rot().X()); - this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_y( - _sensorPose.Rot().Y()); - this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_z( - _sensorPose.Rot().Z()); - this->sensorMarkerMsg.mutable_pose()->mutable_orientation()->set_w( - _sensorPose.Rot().W()); - - this->node.Request("/marker", this->sensorMarkerMsg); + // Initialize marker messages for position and force of the contacts + + // Blue points for positions + // Green lines for forces + + _positionMarkerMsg.set_ns("positions_" + this->modelName); + _positionMarkerMsg.set_id(1); + _positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + _positionMarkerMsg.set_type(ignition::msgs::Marker::POINTS); + _positionMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + ignition::msgs::Set(_positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(1, 1, 1)); + + _forceMarkerMsg.set_ns("forces_" + this->modelName); + _forceMarkerMsg.set_id(1); + _forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + _forceMarkerMsg.set_type(ignition::msgs::Marker::LINE_LIST); + _forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + // Set material properties and lifetime + ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> + mutable_ambient(), math::Color(0, 0, 1, 1)); + ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> + mutable_diffuse(), math::Color(0, 0, 1, 1)); + _positionMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + + ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> + mutable_ambient(), math::Color(0, 1, 0, 1)); + ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> + mutable_diffuse(), math::Color(0, 1, 0, 1)); + _forceMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); } ////////////////////////////////////////////////// -void OpticalTactilePluginVisualization::VisualizeNormalForce( - uint64_t &_i, uint64_t &_j, +void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs( + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg, ignition::math::Vector3f &_position, ignition::math::Vector3f &_normalForce, ignition::math::Pose3f &_sensorWorldPose) { - // Visualization is downsampled according to plugin parameter - // if is set to true + // Check if messages have been initialized + if (!this->normalForcesMsgsAreInitialized) + { + this->InitializeNormalForcesMarkerMsgs(_positionMarkerMsg, + _forceMarkerMsg); + this->normalForcesMsgsAreInitialized = true; + } + + // We need to compute the two points that form a normal force (start and end + // points) with reference to the simulation origin + ignition::math::Vector3f normalForcePositionFromSensor( + _position.X(), _position.Y(), _position.Z()); + + ignition::math::Quaternionf normalForceOrientationFromSensor; + normalForceOrientationFromSensor.From2Axes( + ignition::math::Vector3f(0, 0, 1), _normalForce); - // Check if _normalForce has to be visualized - bool visualizeForce = (_i % this->visualizationResolution == 0 && - _j % this->visualizationResolution == 0) || - (_i % this->visualizationResolution == 0 && _j == 1) || - (_i == 1 && _j % this->visualizationResolution == 0) || - (_i == 1 && _j == 1); + ignition::math::Pose3f normalForcePoseFromSensor( + normalForcePositionFromSensor, normalForceOrientationFromSensor); - if (!visualizeForce) + ignition::math::Pose3f normalForcePoseFromWorld = + (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose; + normalForcePoseFromWorld.Correct(); + + // Get the first point of the normal force + ignition::math::Vector3f firstPointFromWorld = + normalForcePoseFromWorld.Pos(); + + // Move the normal force pose a distance of forceLength along the direction + // of _normalForce and get the second point + normalForcePoseFromSensor.Set(normalForcePositionFromSensor + + _normalForce * this->forceLength, normalForceOrientationFromSensor); + + normalForcePoseFromWorld = + (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose; + normalForcePoseFromWorld.Correct(); + + ignition::math::Vector3f secondPointFromWorld = + 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)) return; - if (_i == 1 && _j == 1) - this->markerID = 1; + // Add points to the messages - // Add new markers with specific lifetime - this->positionMarkerMsg.set_id(this->markerID); - this->positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->positionMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); - this->positionMarkerMsg.mutable_lifetime()->set_nsec(0); + ignition::msgs::Set(_positionMarkerMsg.add_point(), + ignition::math::Vector3d(firstPointFromWorld.X(), + firstPointFromWorld.Y(), firstPointFromWorld.Z())); - this->forceMarkerMsg.set_id(this->markerID++); - this->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->forceMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); - this->forceMarkerMsg.mutable_lifetime()->set_nsec(0); + ignition::msgs::Set(_forceMarkerMsg.add_point(), + ignition::math::Vector3d(firstPointFromWorld.X(), + firstPointFromWorld.Y(), firstPointFromWorld.Z())); - // The pose of the markers must be published with reference - // to the simulation origin - ignition::math::Vector3f forceMarkerSensorPosition( - _position.X(), _position.Y(), _position.Z()); + ignition::msgs::Set(_forceMarkerMsg.add_point(), + ignition::math::Vector3d(secondPointFromWorld.X(), + secondPointFromWorld.Y(), secondPointFromWorld.Z())); +} - ignition::math::Quaternionf forceMarkerSensorQuaternion; - forceMarkerSensorQuaternion.From2Axes( - ignition::math::Vector3f(0, 0, 1), _normalForce); +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RequestNormalForcesMarkerMsgs( + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg) +{ + this->node.Request("/marker", _forceMarkerMsg); + this->node.Request("/marker", _positionMarkerMsg); - // The position of the force marker is modified in order to place the - // end of the cylinder in the surface, not its middle point - ignition::math::Pose3f forceMarkerSensorPose(forceMarkerSensorPosition + - _normalForce * (this->forceLength/2), forceMarkerSensorQuaternion); - - ignition::math::Pose3f forceMarkerWorldPose = - (forceMarkerSensorPose + this->depthCameraOffset) + _sensorWorldPose; - forceMarkerWorldPose.Correct(); - - ignition::math::Pose3f positionMarkerSensorPose( - forceMarkerSensorPosition, forceMarkerSensorQuaternion); - ignition::math::Pose3f positionMarkerWorldPose = - (positionMarkerSensorPose + this->depthCameraOffset) + _sensorWorldPose; - positionMarkerWorldPose.Correct(); - - // Set markers pose messages from previously computed poses - this->forceMarkerMsg.mutable_pose()->mutable_position()->set_x( - forceMarkerWorldPose.Pos().X()); - this->forceMarkerMsg.mutable_pose()->mutable_position()->set_y( - forceMarkerWorldPose.Pos().Y()); - this->forceMarkerMsg.mutable_pose()->mutable_position()->set_z( - forceMarkerWorldPose.Pos().Z()); - this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_x( - forceMarkerWorldPose.Rot().X()); - this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_y( - forceMarkerWorldPose.Rot().Y()); - this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_z( - forceMarkerWorldPose.Rot().Z()); - this->forceMarkerMsg.mutable_pose()->mutable_orientation()->set_w( - forceMarkerWorldPose.Rot().W()); - - this->positionMarkerMsg.mutable_pose()->mutable_position()->set_x( - positionMarkerWorldPose.Pos().X()); - this->positionMarkerMsg.mutable_pose()->mutable_position()->set_y( - positionMarkerWorldPose.Pos().Y()); - this->positionMarkerMsg.mutable_pose()->mutable_position()->set_z( - positionMarkerWorldPose.Pos().Z()); - this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_x( - positionMarkerWorldPose.Rot().X()); - this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_y( - positionMarkerWorldPose.Rot().Y()); - this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_z( - positionMarkerWorldPose.Rot().Z()); - this->positionMarkerMsg.mutable_pose()->mutable_orientation()->set_w( - positionMarkerWorldPose.Rot().W()); - - this->node.Request("/marker", this->forceMarkerMsg); - this->node.Request("/marker", this->positionMarkerMsg); + // Let the messages be initialized again + this->normalForcesMsgsAreInitialized = false; } + } } } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index fa61de08b5..8c6df3144e 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -41,65 +41,83 @@ namespace optical_tactile_sensor class OpticalTactilePluginVisualization { /// \brief Constructor - /// \param[in] _contactRadius Value of the contactRadius attribute - /// \param[in] _forceRadius Value of the forceRadius attribute + /// \param[in] _modelName Name of the model to which the sensor is attached + /// \param[in] _sensorSize Size of the contact sensor /// \param[in] _forceLength Value of the forceLength attribute /// \param[in] _cameraUpdateRate Value of the cameraUpdateRate attribute /// \param[in] _depthCameraOffset Value of the depthCameraOffset attribute /// \param[in] _visualizationResolution Value of the /// visualizationResolution attribute - public: OpticalTactilePluginVisualization(double &_contactRadius, - double &_forceRadius, double &_forceLength, float &_cameraUpdateRate, + public: OpticalTactilePluginVisualization( + std::string &_modelName, + ignition::math::Vector3d &_sensorSize, + double &_forceLength, + float &_cameraUpdateRate, ignition::math::Pose3f &_depthCameraOffset, int &_visualizationResolution); /// \brief Destructor public: ~OpticalTactilePluginVisualization(); - /// \brief Initialize the messages needed to request the /marker service - /// \param[in] _modelName Name of the model - /// \param[in] _sensorSize Size of the contact sensor - public: void CreateMarkersMsgs(std::string &_modelName, - ignition::math::Vector3d &_sensorSize); + /// \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); - /// \brief Visualize the collision geometry of the sensor. This can be - /// helpful when debbuging, given that there shouldn't be a visual tag - /// in the plugin's model + /// \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 VisualizeSensor(ignition::math::Pose3f &_sensorPose); - - /// \brief Visualize the normal forces computed for a (i,j) pixel of the - /// image returned by the depth camera - /// \param[in] _i First coordinate of the pixel - /// \param[in] _j Second coordinate of the pixel + public: void RequestSensorMarkerMsg( + ignition::math::Pose3f const &_sensorPose); + + /// \brief Initialize the marker messages representing the normal forces + /// \param[out] _positionMarkerMsg Message for visualizing the contact + /// positions + /// \param[out] _forceMarkerMsg Message for visualizing the contact + /// normal forces + private: void InitializeNormalForcesMarkerMsgs( + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg); + + /// \brief Add a normal force to the marker messages representing the + /// normal forces computed + /// \param[out] _positionMarkerMsg Message for visualizing the contact + /// positions + /// \param[out] _forceMarkerMsg Message for visualizing the contact + /// normal forces /// \param[in] _position Position of the normal force referenced to the /// depth camera's origin /// \param[in] _normalForce Value of the normal force referenced to /// the depth camera's origin /// \param[in] _sensorWorldPose Pose of the plugin's model referenced to /// the world's origin - public: void VisualizeNormalForce(uint64_t &_i, uint64_t &_j, + public: void AddNormalForceToMarkerMsgs( + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg, ignition::math::Vector3f &_position, ignition::math::Vector3f &_normalForce, ignition::math::Pose3f &_sensorWorldPose); - /// \brief Message for visualizing the sensor - private: ignition::msgs::Marker sensorMarkerMsg; - - /// \brief Message for visualizing contacts positions - private: ignition::msgs::Marker positionMarkerMsg; - - /// \brief Message for visualizing contacts forces - private: ignition::msgs::Marker forceMarkerMsg; + /// \brief Request the "/marker" service for the marker messages + /// representing the normal forces + /// \param[in] _positionMarkerMsg Message for visualizing the contact + /// positions + /// \param[in] _forceMarkerMsg Message for visualizing the contact + /// normal forces + public: void RequestNormalForcesMarkerMsgs( + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg); /// \brief Transport node to request the /marker service private: ignition::transport::Node node; - /// \brief Radius of the visualized contact sphere - private: double contactRadius; + /// \brief Name of the model to which the sensor is attached + private: std::string modelName; - /// \brief Radius of the visualized force cylinder - private: double forceRadius; + /// \brief Size of the contact sensor + private: ignition::math::Vector3d sensorSize; /// \brief Length of the visualized force cylinder private: double forceLength; @@ -113,8 +131,8 @@ namespace optical_tactile_sensor /// \brief Resolution of the sensor in pixels to skip. private: int visualizationResolution; - /// \brief Variable for setting the markers id - private: int markerID; + /// \brief Whether the normal forces messages are initialized or not + private: bool normalForcesMsgsAreInitialized{false}; }; } } From b321a0044e4feab3e6a1406139691581b90ad892 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 20 Aug 2020 13:44:02 +0200 Subject: [PATCH 21/23] Minor fixes for sdf, sensor marker and profiler 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 | 20 ++++++------------- .../OpticalTactilePlugin.cc | 10 ++++++++-- .../optical_tactile_plugin/Visualization.cc | 4 ++-- 3 files changed, 16 insertions(+), 18 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index d3532c0e06..661a27b9b2 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 @@ -211,30 +211,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 - + -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 58ca03e3cb..74b728c95d 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -304,7 +304,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { - IGN_PROFILE("TouchPluginPrivate::PreUpdate"); + IGN_PROFILE("OpticalTactilePlugin::PreUpdate"); // Nothing left to do if paused if (_info.paused) @@ -350,7 +350,7 @@ void OpticalTactilePlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { - IGN_PROFILE("TouchPlugin::PostUpdate"); + IGN_PROFILE("OpticalTactilePlugin::PostUpdate"); // Nothing left to do if paused or failed to initialize. if (_info.paused || !this->dataPtr->initialized) @@ -555,6 +555,8 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( const uint64_t &_i, const uint64_t &_j) { + IGN_PROFILE("OpticalTactilePlugin::MapPointCloudData"); + // Initialize return variable ignition::math::Vector3f measuredPoint(0, 0, 0); @@ -599,6 +601,8 @@ ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( bool OpticalTactilePluginPrivate::PointInsideSensor( ignition::math::Vector3f _point) { + IGN_PROFILE("OpticalTactilePlugin::PointInsideSensor"); + // Nothing left to do if failed to initialize. if (!this->initialized) return false; @@ -627,6 +631,8 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( const ignition::msgs::PointCloudPacked &_msg, const bool _visualizeForces) { + IGN_PROFILE("OpticalTactilePlugin::ComputeNormalForces"); + // Nothing left to do if failed to initialize. if (!this->initialized) return; diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index ef59688788..322599568d 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -79,8 +79,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); } From 886f9042fccfa235a15676c1706873f7a3fe7d6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Tue, 25 Aug 2020 12:21:45 +0200 Subject: [PATCH 22/23] PR Feedback 4 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 | 2 +- .../OpticalTactilePlugin.cc | 207 +++++++++--------- .../optical_tactile_plugin/Visualization.cc | 6 +- .../optical_tactile_plugin/Visualization.hh | 3 - 4 files changed, 113 insertions(+), 105 deletions(-) diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 661a27b9b2..bf3d6f86f7 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -1,6 +1,6 @@ - + dataPtr->model.Valid(_ecm)) { - ignerr << "Touch plugin should be attached to a model entity. " + ignerr << "Optical tactile plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -190,7 +190,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("enabled")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->enabled << std::endl; } else @@ -200,7 +200,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("visualization_resolution")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->visualizationResolution << std::endl; } else @@ -219,7 +219,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("visualize_forces")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->visualizeForces << std::endl; } else @@ -229,7 +229,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("extended_sensing")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->extendedSensing << std::endl; } else @@ -247,7 +247,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("visualize_sensor")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->visualizeSensor << std::endl; } else @@ -257,7 +257,7 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, if (!_sdf->HasElement("force_length")) { - ignlog << "Missing parameter , " + igndbg << "Missing parameter , " << "setting to " << this->dataPtr->forceLength << std::endl; } else @@ -272,32 +272,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->forceLength = _sdf->Get("force_length"); } } - - // Get the size of the sensor from the SDF - // If there's no specified inside , a default one - // is set - if (_sdf->GetParent() != nullptr) - { - if (_sdf->GetParent()->GetElement("link") != nullptr) - { - if (_sdf->GetParent()->GetElement("link")->GetElement("collision") - != nullptr) - { - if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> - GetElement("geometry") != nullptr) - { - if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> - GetElement("geometry")->GetElement("box") != nullptr) - { - this->dataPtr->sensorSize = - _sdf->GetParent()->GetElement("link")->GetElement("collision")-> - GetElement("geometry")->GetElement("box")-> - Get("size"); - } - } - } - } - } } ////////////////////////////////////////////////// @@ -332,16 +306,20 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, break; } - // Get the model pose - ignition::math::Pose3d sensorPose = + // Get the tactile sensor pose, i.e. the model pose + ignition::math::Pose3d tactileSensorPose = _ecm.Component( this->dataPtr->model.Entity())->Data(); // Depth camera data is float, so convert Pose3d to Pose3f - this->dataPtr->sensorWorldPose = ignition::math::Pose3f( - sensorPose.Pos().X(), sensorPose.Pos().Y(), sensorPose.Pos().Z(), - sensorPose.Rot().W(), sensorPose.Rot().X(), sensorPose.Rot().Y(), - sensorPose.Rot().Z()); + this->dataPtr->tactileSensorWorldPose = ignition::math::Pose3f( + tactileSensorPose.Pos().X(), + tactileSensorPose.Pos().Y(), + tactileSensorPose.Pos().Z(), + tactileSensorPose.Rot().W(), + tactileSensorPose.Rot().X(), + tactileSensorPose.Rot().Y(), + tactileSensorPose.Rot().Z()); } } @@ -372,32 +350,35 @@ void OpticalTactilePlugin::PostUpdate( // Publish sensor marker if required and sensor pose has changed if (this->dataPtr->visualizeSensor && - (this->dataPtr->sensorWorldPose != this->dataPtr->prevSensorWorldPose)) + (this->dataPtr->tactileSensorWorldPose != + this->dataPtr->prevTactileSensorWorldPose)) { this->dataPtr->visualizePtr->RequestSensorMarkerMsg( - this->dataPtr->sensorWorldPose); + this->dataPtr->tactileSensorWorldPose); } // Store the pose of the sensor in the current iteration - this->dataPtr->prevSensorWorldPose = this->dataPtr->sensorWorldPose; + this->dataPtr->prevTactileSensorWorldPose = + this->dataPtr->tactileSensorWorldPose; } ////////////////////////////////////////////////// void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) { - igndbg << "Loading plugin [OpticalTactilePlugin]" << std::endl; - // Check plugin structure auto allLinks = _ecm.ChildrenByComponents(this->model.Entity(), components::Link()); // Model should only have 1 link // todo(anyone) The plugin should be able to handle more than one link. In // that case, one of the links would be the actual tactile sensor. - if (allLinks.size() != 1 && !this->initErrorPrinted) + if (allLinks.size() != 1) { - ignerr << "Plugin must have exactly 1 link (only printed once)" - << std::endl; - this->initErrorPrinted = true; + if (!this->initErrorPrinted) + { + ignerr << "Plugin must have exactly 1 link (only printed once)" + << std::endl; + this->initErrorPrinted = true; + } return; } @@ -405,11 +386,14 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) auto linkCollisions = _ecm.ChildrenByComponents(allLinks[0], components::Collision()); - if (linkCollisions.size() == 0 && !this->initErrorPrinted) + if (linkCollisions.size() == 0) { - ignerr << "Link must have at least 1 collision (only printed once)" - << std::endl; - this->initErrorPrinted = true; + if (!this->initErrorPrinted) + { + ignerr << "Link must have at least 1 collision (only printed once)" + << std::endl; + this->initErrorPrinted = true; + } return; } @@ -422,8 +406,21 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) this->modelName = this->model.Name(_ecm); - igndbg << "Contact sensor detected within model " - << this->modelName << std::endl; + // Get the size of the collision element + const auto geometry = + _ecm.Component(colEntity)->Data(); + const auto *box = geometry.BoxShape(); + if (box == nullptr) + { + if (!this->initErrorPrinted) + { + ignerr << "Contact sensor geometry must be a box" + << " (only printed once)" << std::endl; + this->initErrorPrinted = true; + } + return; + } + this->sensorSize = geometry.BoxShape()->Size(); } } @@ -433,14 +430,16 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) int depthCameraCounter = 0; int contactSensorCounter = 0; - sdf::ElementPtr depthCameraSdf = nullptr; + sdf::Sensor depthCameraSdf; + components::Pose depthCameraPose = components::Pose(); for (const Entity &sensor : sensorsInsideLink) { if (_ecm.EntityHasComponentType(sensor, components::DepthCamera::typeId)) { depthCameraCounter += 1; depthCameraSdf = - _ecm.Component(sensor)->Data().Element(); + _ecm.Component(sensor)->Data(); + depthCameraPose = *(_ecm.Component(sensor)); } if (_ecm.EntityHasComponentType(sensor, components::ContactSensor::typeId)) @@ -449,34 +448,42 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } } - if (((depthCameraCounter != 1) || (contactSensorCounter != 1)) - && !this->initErrorPrinted) + if (((depthCameraCounter != 1) || (contactSensorCounter != 1))) { - ignerr << "Link must have exactly 1 depth camera sensor and " - << "1 contact sensor (only printed once)" << std::endl; - this->initErrorPrinted = true; + if (!this->initErrorPrinted) + { + ignerr << "Link must have exactly 1 depth camera sensor and " + << "1 contact sensor (only printed once)" << std::endl; + this->initErrorPrinted = true; + } return; } - // Store depth camera update rate and offset from model - auto offset = depthCameraSdf->Get("pose"); - if (!depthCameraSdf->HasElement("update_rate") && !this->initErrorPrinted) + // Store depth camera update rate + if (!depthCameraSdf.Element()->HasElement("update_rate")) { - ignerr << "Depth camera should have an value " - << "(only printed once)" << std::endl; - this->initErrorPrinted = true; + if (!this->initErrorPrinted) + { + ignerr << "Depth camera should have an value " + << "(only printed once)" << std::endl; + this->initErrorPrinted = true; + } return; } - this->cameraUpdateRate = depthCameraSdf->Get("update_rate"); + this->cameraUpdateRate = depthCameraSdf.UpdateRate(); // Depth camera data is float, so convert Pose3d to Pose3f this->depthCameraOffset = ignition::math::Pose3f( - offset.Pos().X(), offset.Pos().Y(), offset.Pos().Z(), - offset.Rot().W(), offset.Rot().X(), offset.Rot().Y(), - offset.Rot().Z()); + depthCameraPose.Data().Pos().X(), + depthCameraPose.Data().Pos().Y(), + depthCameraPose.Data().Pos().Z(), + depthCameraPose.Data().Rot().W(), + depthCameraPose.Data().Rot().X(), + depthCameraPose.Data().Rot().Y(), + depthCameraPose.Data().Rot().Z()); // Configure subscriber for depth camera images - if (!depthCameraSdf->HasElement("topic")) + if (!depthCameraSdf.Element()->HasElement("topic")) { ignwarn << "Depth camera publishing to __default__ topic. " << "It's possible that two depth cameras are publishing into the same " @@ -484,19 +491,21 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } else { - ignlog << "Depth camera publishing to " - << depthCameraSdf->Get("topic") << " topic" << std::endl; + igndbg << "Depth camera publishing to " + << depthCameraSdf.Topic() << " topic" << std::endl; } std::string topic = - "/" + depthCameraSdf->Get("topic") + "/points"; + "/" + depthCameraSdf.Topic() + "/points"; if (!this->node.Subscribe(topic, - &OpticalTactilePluginPrivate::DepthCameraCallback, this) - && !this->initErrorPrinted) + &OpticalTactilePluginPrivate::DepthCameraCallback, this)) { - ignerr << "Error subscribing to topic " << "[" << topic << "]. " - << " must not contain '/' (only printing once)" << std::endl; - this->initErrorPrinted = true; + if (!this->initErrorPrinted) + { + ignerr << "Error subscribing to topic " << "[" << topic << "]. " + << " must not contain '/' (only printing once)" << std::endl; + this->initErrorPrinted = true; + } return; } @@ -553,7 +562,7 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( ////////////////////////////////////////////////// ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( - const uint64_t &_i, const uint64_t &_j) + const uint64_t &_i, const uint64_t &_j, const char *_msgBuffer) { IGN_PROFILE("OpticalTactilePlugin::MapPointCloudData"); @@ -565,7 +574,7 @@ ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( return measuredPoint; // Save original position of the pointer - char *temporaryMsgBuffer = this->msgBuffer; + const char *temporaryMsgBuffer = _msgBuffer; // Number of bytes from the beginning of the pointer (image coordinates at // 0,0) to the desired (i,j) position @@ -576,13 +585,13 @@ ignition::math::Vector3f OpticalTactilePluginPrivate::MapPointCloudData( int fieldIndex = 0; // X coordinate - measuredPoint.X() = *reinterpret_cast( + measuredPoint.X() = *reinterpret_cast( temporaryMsgBuffer + this->cameraMsg.field(fieldIndex++).offset()); // Y coordinate - measuredPoint.Y() = *reinterpret_cast( + measuredPoint.Y() = *reinterpret_cast( temporaryMsgBuffer + this->cameraMsg.field(fieldIndex++).offset()); // Z coordinate - measuredPoint.Z() = *reinterpret_cast( + measuredPoint.Z() = *reinterpret_cast( temporaryMsgBuffer + this->cameraMsg.field(fieldIndex).offset()); // Check if point is inside the sensor @@ -638,8 +647,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( return; // Get data from the message - std::string rawMsgBuffer = _msg.data(); - this->msgBuffer = rawMsgBuffer.data(); + const char *msgBuffer = _msg.data().data(); // Declare variables for storing the XYZ points ignition::math::Vector3f p1, p2, p3, p4, markerPosition; @@ -657,10 +665,10 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( i += this->visualizationResolution) { // Get points for computing normal forces - p1 = this->MapPointCloudData(i + 1, j); - p2 = this->MapPointCloudData(i - 1, j); - p3 = this->MapPointCloudData(i, j + 1); - p4 = this->MapPointCloudData(i, j - 1); + p1 = this->MapPointCloudData(i + 1, j, msgBuffer); + p2 = this->MapPointCloudData(i - 1, j, msgBuffer); + p3 = this->MapPointCloudData(i, j + 1, msgBuffer); + p4 = this->MapPointCloudData(i, j - 1, msgBuffer); float dxdi = (p1.X() - p2.X()) / std::abs(p1.Y() - p2.Y()); float dxdj = (p3.X() - p4.X()) / std::abs(p3.Z() - p4.Z()); @@ -679,9 +687,10 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( if (!_visualizeForces) continue; - markerPosition = this->MapPointCloudData(i, j); + markerPosition = this->MapPointCloudData(i, j, msgBuffer); this->visualizePtr->AddNormalForceToMarkerMsgs(positionMarkerMsg, - forceMarkerMsg, markerPosition, normalForce, this->sensorWorldPose); + forceMarkerMsg, markerPosition, normalForce, + this->tactileSensorWorldPose); } } diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 322599568d..0610f97479 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -114,13 +114,15 @@ void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( mutable_ambient(), math::Color(0, 0, 1, 1)); ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> mutable_diffuse(), math::Color(0, 0, 1, 1)); - _positionMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + _positionMarkerMsg.mutable_lifetime()->set_nsec( + this->cameraUpdateRate * 1000000000); ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> mutable_ambient(), math::Color(0, 1, 0, 1)); ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> mutable_diffuse(), math::Color(0, 1, 0, 1)); - _forceMarkerMsg.mutable_lifetime()->set_sec(this->cameraUpdateRate); + _forceMarkerMsg.mutable_lifetime()->set_sec( + this->cameraUpdateRate * 1000000000); } ////////////////////////////////////////////////// diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index 8c6df3144e..c1dbfb0b13 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -56,9 +56,6 @@ namespace optical_tactile_sensor ignition::math::Pose3f &_depthCameraOffset, int &_visualizationResolution); - /// \brief Destructor - public: ~OpticalTactilePluginVisualization(); - /// \brief Initialize the marker message representing the optical tactile /// sensor /// \param[out] _sensorMarkerMsg Message for visualizing the sensor From 16807b60c77246072b6fcfb0e9878b82db534ff3 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 5 Nov 2020 13:54:10 -0500 Subject: [PATCH 23/23] fix mac warning Signed-off-by: Mabel Zhang --- src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 0add2a98dd..1e0749772c 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -523,10 +523,9 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool _value) +void OpticalTactilePluginPrivate::Enable(const bool /*_value*/) { - // todo(mcres) Implement method - _value; + // todo(mcres) Implement method } //////////////////////////////////////////////////