Add functionalities for optical tactile plugin

examples/worlds/optical_tactile_sensor_plugin.sdf

@@ -196,16 +196,18 @@
         </sensor>
 
       </link>
-      <static>true</static>
+      <static>false</static>
       <plugin
         filename="libignition-gazebo-opticaltactileplugin-system.so"
         name="ignition::gazebo::systems::OpticalTactilePlugin">
         <enabled>true</enabled>
+        <namespace>optical_tactile_plugin</namespace>
         <visualization_resolution>15</visualization_resolution>
         <visualize_forces>true</visualize_forces>
         <visualize_sensor>true</visualize_sensor>
         <force_length>0.01</force_length>
         <extended_sensing>0.001</extended_sensing>
+        <visualize_contacts>true</visualize_contacts>
       </plugin>
     </model>
 

src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc

@@ -55,8 +55,8 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
   public: void Load(const EntityComponentManager &_ecm);
 
   /// \brief Actual function that enables the plugin.
-  /// \param[in] _value True to enable plugin.
-  public: void Enable(const bool _value);
+  /// \param[in] _enable Whether to enable the plugin or disable it.
+  public: void Enable(const ignition::msgs::Boolean &_req);
 
   /// \brief Callback for the depth camera
   /// \param[in] _msg Message from the subscribed topic
@@ -98,6 +98,9 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
   /// \brief Whether to visualize the normal forces.
   public: bool visualizeForces{false};
 
+  /// \brief Whether to visualize the contacts.
+  public: bool visualizeContacts{false};
+
   /// \brief Model interface.
   public: Model model{kNullEntity};
 
@@ -164,6 +167,12 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
 
   /// \brief Flag for allowing the plugin to output error/warning only once
   public: bool initErrorPrinted{false};
+
+  /// \brief Normal forces publisher
+  public: ignition::transport::Node::Publisher normalForcesPub;
+
+  /// \brief Namespace for transport topics
+  public: std::string ns{"optical_tactile_sensor"};
 };
 
 //////////////////////////////////////////////////
@@ -228,6 +237,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
     this->dataPtr->visualizeForces = _sdf->Get<bool>("visualize_forces");
   }
 
+  if (!_sdf->HasElement("visualize_contacts"))
+  {
+    igndbg << "Missing parameter <visualize_contacts>, "
+      << "setting to " << this->dataPtr->visualizeContacts << std::endl;
+  }
+  else
+  {
+    this->dataPtr->visualizeContacts = _sdf->Get<bool>("visualize_contacts");
+  }
+
   if (!_sdf->HasElement("extended_sensing"))
   {
     igndbg << "Missing parameter <extended_sensing>, "
@@ -273,6 +292,62 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
       this->dataPtr->forceLength = _sdf->Get<double>("force_length");
     }
   }
+
+  if (!_sdf->HasElement("namespace"))
+  {
+    ignlog << "Missing parameter <namespace>, "
+      << "setting to " << this->dataPtr->ns << std::endl;
+  }
+  else
+  {
+    this->dataPtr->ns = _sdf->Get<std::string>("namespace");
+  }
+
+  // Get the size of the sensor from the SDF
+  // If there's no <collision> specified inside <link>, a default one
+  // is set
+  this->dataPtr->sensorSize = ignition::math::Vector3d(0.005, 0.02, 0.02);
+  if (_sdf->GetParent() != nullptr)
+  {
+    if (_sdf->GetParent()->GetElement("link") != nullptr)
+    {
+      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<ignition::math::Vector3d>("size");
+          }
+        }
+      }
+    }
+  }
+
+  // Advertise topic for normal forces
+  std::string normalForcesTopic = "/" + this->dataPtr->ns + "/normal_forces";
+  this->dataPtr->normalForcesPub =
+    this->dataPtr->node.Advertise<ignition::msgs::Image>(normalForcesTopic);
+  if (!this->dataPtr->normalForcesPub)
+  {
+    ignerr << "Error advertising topic [" << normalForcesTopic << "]"
+      << std::endl;
+  }
+
+  // Advertise enabling service
+  std::string enableService = "/" + this->dataPtr->ns + "/enable";
+  if (!this->dataPtr->node.Advertise(enableService,
+      &OpticalTactilePluginPrivate::Enable, this->dataPtr.get()))
+  {
+    ignerr << "Error advertising service [" << enableService << "]"
+      << std::endl;
+  }
 }
 
 //////////////////////////////////////////////////
@@ -282,7 +357,7 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info,
   IGN_PROFILE("OpticalTactilePlugin::PreUpdate");
 
   // Nothing left to do if paused
-  if (_info.paused)
+  if (_info.paused || !this->dataPtr->enabled)
     return;
 
   if (!this->dataPtr->initialized)
@@ -327,16 +402,28 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info,
 //////////////////////////////////////////////////
 void OpticalTactilePlugin::PostUpdate(
   const ignition::gazebo::UpdateInfo &_info,
-  const ignition::gazebo::EntityComponentManager &/*_ecm*/)
+  const ignition::gazebo::EntityComponentManager &_ecm)
 {
   IGN_PROFILE("OpticalTactilePlugin::PostUpdate");
 
   // Nothing left to do if paused or failed to initialize.
-  if (_info.paused || !this->dataPtr->initialized)
+  if (_info.paused || !this->dataPtr->initialized || !this->dataPtr->enabled)
     return;
 
   // TODO(anyone) Get ContactSensor data and merge it with DepthCamera data
 
+  if (this->dataPtr->visualizeContacts)
+  {
+    auto* contacts =
+      _ecm.Component<components::ContactSensorData>(
+        this->dataPtr->sensorCollisionEntity);
+
+    if (contacts)
+    {
+      this->dataPtr->visualizePtr->RequestContactsMarkerMsg(contacts);
+    }
+  }
+
   // Process camera message if it's new
   {
     std::lock_guard<std::mutex> lock(this->dataPtr->serviceMutex);
@@ -524,9 +611,17 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
 }
 
 //////////////////////////////////////////////////
-void OpticalTactilePluginPrivate::Enable(const bool /*_value*/)
+void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req)
 {
-  // todo(mcres) Implement method
+  {
+    std::lock_guard<std::mutex> lock(this->serviceMutex);
+    this->enabled = _req.data();
+  }
+
+  if (!_req.data())
+  {
+    this->visualizePtr->RemoveNormalForcesAndContactsMarkers();
+  }
 }
 
 //////////////////////////////////////////////////
@@ -535,7 +630,7 @@ void OpticalTactilePluginPrivate::DepthCameraCallback(
 {
   // This check avoids running the callback at t=0 and getting
   // unexpected markers in the scene
-  if (!this->initialized)
+  if (!this->initialized || !this->enabled)
     return;
 
   // Check whether DepthCamera returns FLOAT32 data
@@ -652,6 +747,17 @@ void OpticalTactilePluginPrivate::ComputeNormalForces(
   // Declare variables for storing the XYZ points
   ignition::math::Vector3f p1, p2, p3, p4, markerPosition;
 
+  // Message for publishing normal forces
+  ignition::msgs::Image normalsMsg;
+  normalsMsg.set_width(_msg.width());
+  normalsMsg.set_height(_msg.height());
+  normalsMsg.set_step(3 * sizeof(float) * _msg.width());
+  normalsMsg.set_pixel_format_type(ignition::msgs::PixelFormatType::R_FLOAT32);
+
+  uint32_t bufferSize = 3 * sizeof(float) * _msg.width() * _msg.height();
+  auto *normalForcesBuffer = new char[bufferSize];
+  uint32_t bufferIndex;
+
   // Marker messages representing the normal forces
   ignition::msgs::Marker positionMarkerMsg;
   ignition::msgs::Marker forceMarkerMsg;
@@ -682,8 +788,14 @@ void OpticalTactilePluginPrivate::ComputeNormalForces(
       // https://github.com/ignitionrobotics/ign-math/issues/144
       ignition::math::Vector3f normalForce = direction.Normalized();
 
-      // todo(mcres) Normal forces are computed even if visualization
-      // is turned off. These forces should be published in the future.
+      // Add force to buffer
+      // Forces buffer is composed of XYZ coordinates, while _msg buffer is
+      // made up of XYZRGB values
+      bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2);
+      normalForcesBuffer[bufferIndex] = normalForce.X();
+      normalForcesBuffer[bufferIndex + sizeof(float)] = normalForce.Y();
+      normalForcesBuffer[bufferIndex + 2 * sizeof(float)] = normalForce.Z();
+
       if (!_visualizeForces)
         continue;
 
@@ -694,6 +806,11 @@ void OpticalTactilePluginPrivate::ComputeNormalForces(
     }
   }
 
+  // Publish message
+  normalsMsg.set_data(normalForcesBuffer,
+    3 * sizeof(float) * _msg.width() * _msg.height());
+  this->normalForcesPub.Publish(normalsMsg);
+
   if (_visualizeForces)
   {
     this->visualizePtr->RequestNormalForcesMarkerMsgs(positionMarkerMsg,

src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh

@@ -32,73 +32,90 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
 {
 namespace systems
 {
-    // Forward declaration
-    class OpticalTactilePluginPrivate;
+  // Forward declaration
+  class OpticalTactilePluginPrivate;
 
-    /// \brief Plugin that implements an optical tactile sensor
-    ///
-    /// It requires that contact sensor and depth camera be placed in at least
-    /// one link on the model on which this plugin is attached.
-    ///
-    /// Parameters:
-    ///
-    /// <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.
-    ///
-    /// <visualization_resolution> 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
-    ///
-    /// <visualize_forces> 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.
-    ///
-    /// <force_length> Length in meters of the forces visualized if
-    /// <visualize_forces> is set to true. This parameter is optional, and the
-    /// default value is 0.01.
-    ///
-    /// <extended_sensing> 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.
-    ///
-    /// <visualize_sensor> Whether to visualize the sensor or not. This element
-    /// is optional, and the default value is false.
+  /// \brief Plugin that implements an optical tactile sensor
+  ///
+  /// It requires that contact sensor and depth camera be placed in at least
+  /// one link on the model on which this plugin is attached.
+  ///
+  /// Parameters:
+  ///
+  /// <enabled> Set this to true so the plugin works from the start and
+  ///           doesn't need to be enabled. This element is optional, and the
+  ///           default value is true.
+  ///
+  /// <namespace> Namespace for transport topics/services. If there are more
+  ///             than one optical tactile plugins, their namespaces should
+  ///             be different.
+  ///             This element is optional, and the default value is
+  ///             "optical_tactile_sensor".
+  ///             /<namespace>/enable : Service used to enable and disable the
+  ///                                   plugin.
+  ///             /<namespace>/normal_forces : Topic where a message is
+  ///                                          published each time the
+  ///                                          normal forces are computed
+  ///
+  /// <visualization_resolution> 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.
+  ///
+  /// <visualize_forces> 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.
+  ///
+  /// <force_length> Length in meters of the forces visualized if
+  ///                <visualize_forces> is set to true. This parameter is
+  ///                optional, and the default value is 0.01.
+  ///
+  /// <extended_sensing> 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.
+  ///
+  /// <visualize_sensor> Whether to visualize the sensor or not. This element
+  ///                    is optional, and the default value is false.
+  ///
+  /// <visualize_contacts> Whether to visualize the contacts from the contact
+  ///                      sensor or not. This element is optional, and the
+  ///                      default value is false.
 
-    class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin :
-      public System,
-      public ISystemConfigure,
-      public ISystemPreUpdate,
-      public ISystemPostUpdate
-    {
-      /// \brief Constructor
-      public: OpticalTactilePlugin();
+  class IGNITION_GAZEBO_VISIBLE OpticalTactilePlugin :
+    public System,
+    public ISystemConfigure,
+    public ISystemPreUpdate,
+    public ISystemPostUpdate
+  {
+    /// \brief Constructor
+    public: OpticalTactilePlugin();
 
-      /// \brief Destructor
-      public: ~OpticalTactilePlugin() override = default;
+    /// \brief Destructor
+    public: ~OpticalTactilePlugin() override = default;
 
-      // Documentation inherited
-      public: void Configure(const Entity &_entity,
-        const std::shared_ptr<const sdf::Element> &_sdf,
-        EntityComponentManager &_ecm,
-        EventManager &_eventMgr) override;
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) override;
 
-      /// Documentation inherited
-      public: void PreUpdate(const UpdateInfo &_info,
-        EntityComponentManager &_ecm) override;
+    /// Documentation inherited
+    public: void PreUpdate(const UpdateInfo &_info,
+                           EntityComponentManager &_ecm) override;
 
-      // Documentation inherited
-      public: void PostUpdate(
+    // Documentation inherited
+    public: void PostUpdate(
         const ignition::gazebo::UpdateInfo &_info,
         const ignition::gazebo::EntityComponentManager &_ecm) override;
 
-      /// \brief Private data pointer
-      private: std::unique_ptr<OpticalTactilePluginPrivate> dataPtr;
-    };
-}
-}
-}
-}
+    /// \brief Private data pointer
+    private: std::unique_ptr<OpticalTactilePluginPrivate> dataPtr;
+  };
+}  // namespace systems
+}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+}  // namespace gazebo
+}  // namespace ignition
 
 #endif