Skip to content

Commit

Permalink
Add functionalities for optical tactile plugin (#431)
Browse files Browse the repository at this point in the history 
Signed-off-by: Martiño Crespo <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
Co-authored-by: Mabel Zhang <[email protected]>
  • Loading branch information
@mcres @chapulina @mabelzhang
3 people authored May 3, 2021
1 parent 1326174 commit 064b3ec
Show file tree
Hide file tree
Showing 8 changed files with 676 additions and 136 deletions.
26 changes: 21 additions & 5 deletions examples/worlds/optical_tactile_sensor_plugin.sdf
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,17 @@
<?xml version="1.0" ?>
<!--
Demo of an optical tactile plugin.
Move the tactile sensor to touch different objects and see the output.
Disable the sensor with:
ign service -s /optical_tactile_plugin/enable \
--reqtype ignition.msgs.Boolean \
--reptype ignition.msgs.Empty \
--timeout 2000 --req 'data: false'
-->

<sdf version="1.7">
<world name="optical_tactile_plugin">
Expand Down Expand Up @@ -38,7 +51,8 @@
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
<camera_pose>0.35 0.23 0.94 0 0.05 -2.53</camera_pose>

</plugin>

<!-- World control -->
Expand Down Expand Up @@ -196,14 +210,16 @@
</sensor>

</link>
<static>true</static>
<static>false</static>
<plugin
filename="libignition-gazebo-opticaltactileplugin-system.so"
name="ignition::gazebo::systems::OpticalTactilePlugin">
<enabled>true</enabled>
<visualization_resolution>15</visualization_resolution>
<visualize_forces>true</visualize_forces>
<namespace>optical_tactile_plugin</namespace>
<visualize_sensor>true</visualize_sensor>
<visualize_contacts>true</visualize_contacts>
<visualize_forces>true</visualize_forces>
<visualization_resolution>15</visualization_resolution>
<force_length>0.01</force_length>
<extended_sensing>0.001</extended_sensing>
</plugin>
Expand All @@ -223,7 +239,7 @@
<pose>0 -0.7 0 0 0 3.14</pose>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue</uri>
</include>

<include>
<pose>-1.5 0 0 0 0 0</pose>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine</uri>
Expand Down
156 changes: 144 additions & 12 deletions src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/

#include <algorithm>
#include <atomic>
#include <optional>
#include <string>
#include <vector>
Expand Down Expand Up @@ -55,8 +56,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] _req 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
Expand Down Expand Up @@ -98,6 +99,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};

Expand All @@ -112,7 +116,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
public: ignition::gazebo::Entity objectCollisionEntity;

/// \brief Whether the plugin is enabled.
public: bool enabled{true};
public: std::atomic<bool> enabled{true};

/// \brief Initialization flag.
public: bool initialized{false};
Expand Down Expand Up @@ -150,7 +154,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
public: bool visualizeSensor{false};

/// \brief Size of the contact sensor
public: ignition::math::Vector3d sensorSize;
public: ignition::math::Vector3d sensorSize{0.005, 0.02, 0.02};

/// \brief Extended sensing distance. The sensor will output data coming from
/// its volume plus this distance.
Expand All @@ -164,6 +168,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"};
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -228,6 +238,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>, "
Expand Down Expand Up @@ -273,6 +293,73 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
this->dataPtr->forceLength = _sdf->Get<double>("force_length");
}
}

if (!_sdf->HasElement("namespace"))
{
igndbg << "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
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");
igndbg << "Setting sensor size to box collision size: ["
<< this->dataPtr->sensorSize << "]" << std::endl;
}
}
}
}
}

// 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;
}
else
{
ignmsg << "Topic publishing normal forces [" << 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;
}
else
{
ignmsg << "Service to enable tactile sensor [" << enableService << "]"
<< std::endl;
}
}

//////////////////////////////////////////////////
Expand All @@ -282,7 +369,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)
Expand Down Expand Up @@ -327,15 +414,26 @@ 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 (nullptr != contacts)
{
this->dataPtr->visualizePtr->RequestContactsMarkerMsg(contacts);
}
}

// Process camera message if it's new
{
Expand Down Expand Up @@ -524,9 +622,20 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
}

//////////////////////////////////////////////////
void OpticalTactilePluginPrivate::Enable(const bool /*_value*/)
void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req)
{
// todo(mcres) Implement method
if (_req.data() != this->enabled)
{
ignmsg << "Enabling optical tactile sensor with namespace [" << this->ns
<< "]: " << _req.data() << std::endl;
}

this->enabled = _req.data();

if (!_req.data())
{
this->visualizePtr->RemoveNormalForcesAndContactsMarkers();
}
}

//////////////////////////////////////////////////
Expand All @@ -535,7 +644,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
Expand Down Expand Up @@ -652,6 +761,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();
std::shared_ptr<char> normalForcesBuffer(new char[bufferSize]);
uint32_t bufferIndex;

// Marker messages representing the normal forces
ignition::msgs::Marker positionMarkerMsg;
ignition::msgs::Marker forceMarkerMsg;
Expand Down Expand Up @@ -682,8 +802,15 @@ 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.get()[bufferIndex] = normalForce.X();
normalForcesBuffer.get()[bufferIndex + sizeof(float)] = normalForce.Y();
normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] =
normalForce.Z();

if (!_visualizeForces)
continue;

Expand All @@ -694,6 +821,11 @@ void OpticalTactilePluginPrivate::ComputeNormalForces(
}
}

// Publish message
normalsMsg.set_data(normalForcesBuffer.get(),
3 * sizeof(float) * _msg.width() * _msg.height());
this->normalForcesPub.Publish(normalsMsg);

if (_visualizeForces)
{
this->visualizePtr->RequestNormalForcesMarkerMsgs(positionMarkerMsg,
Expand Down
Loading

0 comments on commit 064b3ec

Please sign in to comment.