Contact Sensor

Introduction

이번 튜토리얼에서는 contact sensor를 만드는 과정과 plugin 또는 message를 통해 contact data를 얻는 방법에 대해 demonstrate 합니다. Contact sensor는 두 대상 사이에 충돌을 감지하고, 힘과 관련된 contact의 위치를 report한다.

Setup Tutorial

work 폴더를 만들면서 시작한다.

mkdir ~/gazebo_contact_tutorial; cd ~/gazebo_contact_tutorial

다음은, contact 센서를 갖는 박스를 포함한 SDF world 파일을 만든다.

gedit contact.world

아래의 코드를 복사하여, contact.world에 붙여넣기 한다.

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>

     <include>
      <uri>model://sun</uri>
    </include>

    <model name="box">
      <link name="link">
        <pose>0 0 0.5 0 0 0</pose>

        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </visual>

        <sensor name='my_contact' type='contact'>
          <contact>
            <collision>box_collision</collision>
          </contact>
        </sensor>
      </link>
    </model>
  </world>
</sdf>

contact sensor는 box model안에 있는 link에 속해있다. 센서는 box_collision object와 world의 다른 object 사이에 일어나는 충돌을 report 할 것이다.

Print Contact Values

Gazebo를 이용해 contact.world 실행한다:

gazebo contact.world

다른 분리된 terminalctrl-alt-t을 열고, 아래 명령어를 이용해 Gazebo로부터 published된 topic list를 열어보자.

gz topic -l

결과는 다음과 같을 것이다:

/gazebo/default/pose/info
/gazebo/default/gui
/gazebo/default/log/status
/gazebo/default/response
/gazebo/default/world_stats
/gazebo/default/selection
/gazebo/default/model/info
/gazebo/default/light
/gazebo/default/physics/contacts
/gazebo/default/visual
/gazebo/default/request
/gazebo/default/joint
/gazebo/default/sensor
/gazebo/default/box/link/my_contact
/gazebo/default/box/link/my_contact/contacts
/gazebo/world/modify
/gazebo/default/diagnostics
/gazebo/default/factory
/gazebo/default/model/modify
/gazebo/default/scene
/gazebo/default/physics
/gazebo/default/world_control
/gazebo/server/control

우리가 관심있는 topic인 /gazebo/default/box/link/my_contact로 부르자. my_contact는 contact sensor가 이 topic에 publish 하는것이다.

아래 명령어를 이용해, contact sensor의 값을 스크린에 프린트하자:

gz topic -e /gazebo/default/box/link/my_contact

위 command는 모든 contact를 terminal로 옴길 것이다. 또한, 당신은 ctrl-c를 이용해 멈출수도 있다.

NOTE: 만약 동작하지 않으면, terminal에서 결과를 얻기 위해 </contact> and </sensor> tags 사이에 아래 term을 추가해라

<update_rate> 5 </update_rate>

Contact Sensor Plugin

It is also possible to create a plugin for the contact sensor. This plugin can get the collision data, manipulate it, and output it to an arbitrary destination (for example a ROS topic).

• Note This section of the tutorial requires you to compile a Gazebo plugin. For Gazebo version 3.0 and above, you will need to have the Gazebo dev packages installed (something like libgazebo*-dev). Check the installation tutorials for further instructions.

Start by modifying the contact.world SDF file. Add the following line directly below <sensor name='my_contact' type='contact'>:

gedit contact.world

<plugin name="my_plugin" filename="libcontact.so"/>

This line tells Gazebo to load the libcontact.so sensor plugin. Which we will now define.

Create a header file for the plugin, call it ContactPlugin.hh:

gedit ContactPlugin.hh

and paste in the following content:

#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

Create a source file called ContactPlugin.cc:

gedit ContactPlugin.cc

and paste in the following content:

#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";
    }
  }
}

The Code Explained

The following code from the Load function gets pointer to the contact sensor through the _sensor parameter. We then test to make sure the pointer is valid, and create a connection to the contact sensor's updated event. The last line guarantees that the sensor is initialized.

  // 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);

The OnUpdate function is called whenever the contact sensor is updated. In this function we print out the contact values.

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";
    }
  }
}

Compiling the code

Create a CMakeLists.txt file:

cd ~/gazebo_contact_tutorial; gedit CMakeLists.txt

Copy in the following code and save the file:

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

add_library(contact SHARED ContactPlugin.cc)
target_link_libraries(contact ${GAZEBO_libraries})

Next, create a build directory and make the plugin:

mkdir build; cd build; cmake ../; make

Running the code

Enter the build directory

cd ~/gazebo_contact_tutorial/build

Run gzserver, first modifying your LD_LIBRARY_PATH so that the library loader can find your library (by default it will only look in certain system locations):

export LD_LIBRARY_PATH=~/gazebo_contact_tutorial/build:$LD_LIBRARY_PATH
gzserver ../contact.world