< >
Home » ROS2与Gazebo11入门教程 » ROS2与Gazebo11入门教程-接触式传感器（Contact Sensor）

ROS2与Gazebo11入门教程-接触式传感器（Contact Sensor）

说明：

  • 介绍如何创建接触式传感器并通过插件或消息获取接触式传感器数据的过程

简介

  • 接触式传感器会检测两个物体之间的碰撞，并报告与接触有关的力的位置

设置教程

  • 首先新建一个工作目录，命令为：
mkdir ~/gazebo_contact_tutorial; cd ~/gazebo_contact_tutorial
  • 然后创建一个SDF世界文件，该仿真世界中含有一个方盒，该方盒上含有一个接触式传感器。
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>
  • 接触式传感器被连接到方盒模型内的一个链接上。该传感器将会报告box_collision对象与仿真世界中任何其他对象之间的碰撞。

打印接触消息值

  • 使用Gazebo运行contact.world世界文件，命令为：
gazebo contact.world
  • 在另一个终端中，列举出Gazebo发布的所有话题，命令为：
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

  • 刚才插入的话题名为/gazebo/default/box/link/my_contact。名为my_contact的接触式传感器会在这个话题上发布消息。

  • 将接触式传感器发布的消息值打印输出到屏幕上，命令为：

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

  • 上面这条命令会将所有的接触消息转储输出到终端中。可以随时使用Ctrl + C组合键来停止消息在屏幕上的输出。

  • 如果不能正常地将消息输出到终端上，则可能需要在和标签之间添加下面这行代码，以便将消息输出到终端中。根据需要，可以将频率值“5”更改为不同的频率值。

<update_rate> 5 </update_rate>

接触式传感器插件

  • 也可以为接触式传感器创建插件。该插件可以获取碰撞数据，对碰撞数据进行处理，然后将该碰撞数据输出到任意目标位置（例如某个ROS话题上）。

  • 本节教程的这个小节需要您编译Gazebo插件。对于Gazebo 3.0版及更高版本，需要安装Gazebo开发包（名称类似于libgazebo * -dev的软件包）。请查阅安装教程以获取更多说明

  • 首先修改SDF文件contact.world。

gedit contact.world
  • 直接在sensor name = 'my_contact' type ='contact'下面添加一行：
<plugin name="my_plugin" filename="libcontact.so"/>

  • 这行代码会告知Gazebo加载libcontact.so传感器插件。下面将对该插件进行定义。

  • 为该插件创建一个头文件，名称为ContactPlugin.hh：

gedit 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
  • 创建一个名为http://ContactPlugin.cc的源代码文件：
gedit http://ContactPlugin.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";
}
}
}

代码说明

  • 函数Load中的下列代码通过_sensor参数获取指向接触式传感器的指针。然后进行测试以确保该指针有效，并创建与接触式传感器update事件的一个连接。最后一行代码保证该传感器已初始化和处于激活状态。
// 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);
  • 每当接触式传感器进行更新时，就会调用OnUpdate函数。在该函数中，打印输出了接触消息值。
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";
}
}
}

编译代码

  • 创建一个CMakeLists.txt文件，命令为：
cd ~/gazebo_contact_tutorial; gedit CMakeLists.txt
  • 将下面的代码复制到该文件中并保存该文件：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)

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

add_library(contact SHARED http://ContactPlugin.cc)
target_link_libraries(contact ${GAZEBO_LIBRARIES})
  • 然后新建一个build目录并对该插件进行编译，命令为：
mkdir build; cd build; cmake ../; make

运行代码

  • 进入到build目录中：
cd ~/gazebo_contact_tutorial/build
  • 首先修改您的LD_LIBRARY_PATH环境变量以便库加载器可以找到您的库（默认情况下库加载器只会在某些系统位置中查找库），然后运行gzserver，命令如下：
export LD_LIBRARY_PATH=~/gazebo_contact_tutorial/build:$LD_LIBRARY_PATH
gzserver ../contact.world

参考:

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与gazebo11入门教程