Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add functionalities for optical tactile plugin #1

Closed
wants to merge 7 commits into from
Closed

Add functionalities for optical tactile plugin #1

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
f3dc183
Fix world example and add new parameter
mcres Aug 19, 2020
a9402fd
Add enable service and normal forces topic
mcres Aug 19, 2020
17c5b87
Add test
mcres Aug 19, 2020
ee79d64
Visualize contacts
mcres Aug 21, 2020
b7e7056
Fix test
mcres Aug 21, 2020
90c29b5
Fix conflicts
mcres Aug 21, 2020
2db11bc
PR Feedback
mcres Aug 25, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 9 additions & 15 deletions examples/worlds/optical_tactile_sensor_plugin.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,7 @@
</image>
<clip>
<near>0.030</near>
<far>0.065</far>
<far>10.0</far>
</clip>
</camera>
</sensor>
Expand All @@ -196,45 +196,39 @@
</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>

<include>
<pose>0 0 0 0 0 0</pose>
<uri>
https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SurgicalTrolleyMed
</uri>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SurgicalTrolleyMed</uri>
</include>

<include>
<pose>0 0 0.7 0 0 0</pose>
<uri>
https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke
</uri>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Coke</uri>
</include>

<include>
<pose>0 -0.7 0 0 0 3.14</pose>
<uri>
https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue
</uri>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue</uri>
</include>

<include>
<pose>-1.5 0 0 0 0 1.57</pose>
<uri>
https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine
</uri>
<pose>-1.5 0 0 0 0 0</pose>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine</uri>
</include>

</world>
Expand Down
118 changes: 107 additions & 11 deletions src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,8 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
public: void Load(const EntityComponentManager &_ecm);

/// \brief Actual function that enables the plugin.
/// \param[in] _value True to enable plugin.
public: void Enable(const bool _value);
/// \param[in] _enable Whether to enable the plugin or disable it.
public: void Enable(const bool _enable);

/// \brief Callback for the depth camera
/// \param[in] _msg Message from the subscribed topic
Expand Down Expand Up @@ -94,6 +94,9 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate
/// \brief Whether to visualize the normal forces.
public: bool visualizeForces{false};

/// \brief Whether to visualize the contacts.
public: bool visualizeContacts{false};

/// \brief Model interface.
public: Model model{kNullEntity};

Expand Down Expand Up @@ -163,6 +166,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 @@ -227,6 +236,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
this->dataPtr->visualizeForces = _sdf->Get<bool>("visualize_forces");
}

if (!_sdf->HasElement("visualize_contacts"))
{
ignlog << "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"))
{
ignlog << "Missing parameter <extended_sensing>, "
Expand Down Expand Up @@ -273,6 +292,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
}
}

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
Expand All @@ -298,6 +327,32 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
}
}
}

// Advertise topics for normal forces
std::string normalForcesTopic = "/" + this->dataPtr->ns + "/normal_forces";
this->dataPtr->normalForcesPub =
this->dataPtr->node.Advertise<ignition::msgs::Image>(normalForcesTopic);
if (!this->dataPtr->normalForcesPub)
{
ignerr << "Error advertising topic [" << normalForcesTopic << "]"
<< std::endl;
return;
}

// Advertise enabling service
std::string enableService{"/" + this->dataPtr->ns + "/enable"};
std::function<void(const msgs::Boolean &)> enableCb =
[this](const msgs::Boolean &_req)
{
this->dataPtr->Enable(_req.data());
};

if (!this->dataPtr->node.Advertise(enableService, enableCb))
{
ignerr << "Error advertising service [" << enableService << "]"
<< std::endl;
return;
}
}

//////////////////////////////////////////////////
Expand All @@ -307,7 +362,7 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info,
IGN_PROFILE("TouchPluginPrivate::PreUpdate");

// Nothing left to do if paused
if (_info.paused)
if (_info.paused || !this->dataPtr->enabled)
return;

if (!this->dataPtr->initialized)
Expand Down Expand Up @@ -348,16 +403,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("TouchPlugin::PostUpdate");

// Nothing left to do if paused or failed to initialize.
if (_info.paused || !this->dataPtr->initialized)
if (_info.paused || !this->dataPtr->initialized || !this->dataPtr->enabled)
return;

// TODO(anyone) Get ContactSensor data and merge it with DepthCamera data

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);
Expand Down Expand Up @@ -514,10 +581,17 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
}

//////////////////////////////////////////////////
void OpticalTactilePluginPrivate::Enable(const bool _value)
void OpticalTactilePluginPrivate::Enable(const bool _enable)
{
// todo(mcres) Implement method
_value;
{
std::lock_guard<std::mutex> lock(this->serviceMutex);
this->enabled = _enable;
}

if (!_enable)
{
this->visualizePtr->RemoveNormalForcesAndContactsMarkers();
}
}

//////////////////////////////////////////////////
Expand All @@ -526,7 +600,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 @@ -638,6 +712,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;
Expand Down Expand Up @@ -668,8 +753,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;

Expand All @@ -679,6 +770,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,
Expand Down
15 changes: 14 additions & 1 deletion src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,10 +42,20 @@ namespace systems
///
/// Parameters:
///
/// <enabled> (todo) Set this to true so the plugin works from the start and
/// <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.
Expand All @@ -65,6 +75,9 @@ namespace systems
///
/// <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,
Expand Down
Loading