Gazebo | Ignition | Community
Ask Your Question
0

Contact sensor plugin not working

asked 2020-10-13 05:50:44 -0500

sana gravatar image

Hello, I am trying to add a contact sensor to my robotic arm tool to be able to detect its collision with external parts. I tried to follow the tutorial proposed here . When I run my gazebo project, I find the desired gazebo topic but It does not display the collisions.

I added the contact sensor to my .urdf as follows:

  <gazebo reference="tool0">
        <sensor name='my_contact' type='contact'>
         <update_rate>10</update_rate>
        <plugin name="my_plugin" filename="libcontact.so"/>
    <contact>
                <collision>tool_collision</collision>
        </contact>
        </sensor>
  </gazebo>

and my plugin is as follows:

ContactPlugin.hh

#ifndef _GAZEBO_CONTACT_PLUGIN_HH_
#define _GAZEBO_CONTACT_PLUGIN_HH_

#include <string>

#include <gazebo/gazebo.hh>
#include <gazebo/sensors/sensors.hh>

namespace gazebo
{
  /// \brief An example plugin for a contact sensor.
  class ContactPlugin : public SensorPlugin
  {
    /// \brief Constructor.
    public: ContactPlugin();

    /// \brief Destructor.
    public: virtual ~ContactPlugin();

    /// \brief Load the sensor plugin.
    /// \param[in] _sensor Pointer to the sensor that loaded this plugin.
    /// \param[in] _sdf SDF element that describes the plugin.
    public: virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);

    /// \brief Callback that receives the contact sensor's update signal.
    private: virtual void OnUpdate();

    /// \brief Pointer to the contact sensor
    private: sensors::ContactSensorPtr parentSensor;

    /// \brief Connection that maintains a link between the contact sensor's
    /// updated signal and the OnUpdate callback.
    private: event::ConnectionPtr updateConnection;
  };
}
#endif

and

PluginContact.cc

#include "ContactPlugin.hh"
using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(ContactPlugin)

/////////////////////////////////////////////////
ContactPlugin::ContactPlugin() : SensorPlugin()
{
}

/////////////////////////////////////////////////
ContactPlugin::~ContactPlugin()
{
}

/////////////////////////////////////////////////
void ContactPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/)
{
  // Get the parent sensor.
  this->parentSensor =
    std::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);

  // Make sure the parent sensor is valid.
  if (!this->parentSensor)
  {
    gzerr << "ContactPlugin requires a ContactSensor.\n";
    return;
  }

  // Connect to the sensor update event.
  this->updateConnection = this->parentSensor->ConnectUpdated(
      std::bind(&ContactPlugin::OnUpdate, this));

  // Make sure the parent sensor is active.
  this->parentSensor->SetActive(true);
}

/////////////////////////////////////////////////
void ContactPlugin::OnUpdate()
{
  // Get all the contacts.
  msgs::Contacts contacts;
  contacts = this->parentSensor->Contacts();
  for (unsigned int i = 0; i < contacts.contact_size(); ++i)
  {
    std::cout << "Collision between[" << contacts.contact(i).collision1()
              << "] and [" << contacts.contact(i).collision2() << "]\n";

    for (unsigned int j = 0; j < contacts.contact(i).position_size(); ++j)
    {
      std::cout << j << "  Position:"
                << contacts.contact(i).position(j).x() << " "
                << contacts.contact(i).position(j).y() << " "
                << contacts.contact(i).position(j).z() << "\n";
      std::cout << "   Normal:"
                << contacts.contact(i).normal(j).x() << " "
                << contacts.contact(i).normal(j).y() << " "
                << contacts.contact(i).normal(j).z() << "\n";
      std::cout << "   Depth:" << contacts.contact(i).depth(j) << "\n";
    }
  }
}

when I reand the target link contact through the command

gz topic -e /gazebo/default/robot/tool0/my_contact/contacts

All what I obtain is info about simulation time. Here an example

time {
  sec: 332
  nsec: 996000000
}

Can you please tell what can be the problem???? I am so thankful

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-10-13 06:28:58 -0500

nlamprian gravatar image

The name of your collision is probably wrong. Run

gz sdf -p robot.urdf

locate the collision, and identify its name (as it is defined in the final sdf description).

edit flag offensive delete link more

Comments

Thank you! I changed my collision name in urdf with respect to the one mentioned in sdf. It works!

sana gravatar imagesana ( 2020-10-16 02:15:21 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2020-10-13 05:50:44 -0500

Seen: 68 times

Last updated: Oct 13 '20