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:/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