ROS2与Gazebo11入门教程-代码自检

说明：

• 介绍代码自检实用程序

概述

• Gazebo 8中引入了新的代码自检实用程序。这项新的服务使客户端可以接收具有某些请求变量值的更新消息。代码自检服务可用于调试Gazebo、插件甚至独立应用程序中的内部变量的状态

注册项（Registering items）

• 使用代码自检服务时涉及两个步骤：注册和订阅。注册阶段会将某个特定变量注册到自检服务中。通过注册变量，可以使该变量可以进行自检。请注意，注册变量并不会触发任何更新消息的发布，也不会导致系统中的相关开销。

• 自检管理器是提供注册变量（称为“项”）功能的实体。GzServer有一个正在运行的自检管理器实例，而且已经预先注册了一些项，这些项使我们可以自检仿真时间或者模型和链接的位置、速度和加速度，以及其他项。

• 通过查看Gazebo源代码中的Util/IntrospectionManager类，可以了解有关自检管理器及其API的更多信息

订阅接收注册项的更新消息

• 一旦注册了所有潜在的可自检项，客户端就需要通知自检服务它对一个或多个项感兴趣。因此，客户端需要用自检管理器ID和感兴趣项的列表创建一个过滤器。

• 这个操作会为自检管理器与客户端之间的通信创建一个专用通道。 该通道包含在过滤器中指定的变量值的消息。如果有一个或多个变量未注册，则客户端不会接收这些变量。

示例：对插件进行自检

• 在本示例中，我们将会创建一个非常简单的仿真世界插件，该插件在每次仿真世界更新时都会对一个整数变量进行自增。这个插件有趣的部分是我们将会让计数器成为可自检的。我们将会编写一个名为watcher的可执行文件，该可执行文件将会显示仿真时间和自定义计数器的值

• 编译插件和watcher可执行文件

• 创建一个用于存储本教程中所有文件的新目录，命令为：

mkdir ~/gazebo_introspection_tutorial

cd ~/gazebo_introspection_tutorial

• 下载该插件和watcher程序的代码以及CMakeLists.txt文件：

wget http://github.com/osrf/gazebo_tutorials/raw/master/introspection/files/introspectable_plugin.cc
wget http://github.com/osrf/gazebo_tutorials/raw/master/introspection/files/watcher.cc
wget https://github.com/osrf/gazebo_tutorials/raw/master/introspection/files/CMakeLists.txt

• 现在来编译该代码：

mkdir build && cd build

cmake ..

make

• 这样就应该有了一个libintrospectable_plugin.so插件和一个watcher可执行文件，从而为测试做好了准备。

运行代码

• 下载一个仿真世界文件，该世界文件会加载上面这个仿真世界插件，命令为：

cd ~/gazebo_introspection_tutorial

wget https://github.com/osrf/gazebo_tutorials/raw/master/introspection/files/empty.world

• 启动Gazebo，命令为：

cd ~/gazebo_introspection_tutorial/build

GAZEBO_PLUGIN_PATH=`pwd` gazebo --verbose ../empty.world

• 请注意，这里用build目录的路径对GAZEBO_PLUGIN_PATH环境变量进行了设置，以帮助Gazebo查找到该插件。Gazebo准备就绪后，在一个新的终端中执行以下命令：

cd ~/gazebo_introspection_tutorial/build

./watcher

• 这样就应该可以看到类似于下面的输出：

param {

name: "data://world/default?p=time/sim_time"

value {

type: TIME

time_value {

sec: 12

nsec: 616000000

}

}

}



param {

name: "data://my_plugin/counter"

value {

type: INT32

int_value: 12617

}

}



...

• 如您所见，watcher可执行文件正在连续打印输出仿真时间和计数器的值

代码逐步说明

• 首先来看一下http://introspectable_plugin.cc源代码文件：

#include <functional>

#include <gazebo/common/common.hh>

#include <gazebo/physics/physics.hh>

#include <gazebo/util/IntrospectionManager.hh>

#include <sdf/sdf.hh>



namespace gazebo

{

class ModelPush : public WorldPlugin

{

public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)

{

// Listen to the update event. This event is broadcast every

// simulation iteration.

this->updateConnection = event::Events::ConnectWorldUpdateBegin(

std::bind(&ModelPush::OnUpdate, this));



// Introspection callback.

auto fCounterValue = [this]()

{

return this->counter;

};



// Register the counter element.

gazebo::util::IntrospectionManager::Instance()->Register<int>(

"data://my_plugin/counter", fCounterValue);

}



// Called by the world update start event.

public: void OnUpdate()

{

++this->counter;

}



// Pointer to the update event connection.

private: event::ConnectionPtr updateConnection;



// A counter for testing the introspection capabilites.

private: int counter = 0;

};



// Register this plugin with the simulator.

GZ_REGISTER_WORLD_PLUGIN(ModelPush)

}

• 在Load（）函数中，仿真世界更新事件被连接到OnUpdate（）函数上。Load（）函数中的其余代码将计数器注册到自检管理器中。 可以看到如何获取管理器的实例并调用Register（）函数。必须指定注册项的类型（本例中为int）、注册项的字符串表示形式（data：// my_plugin/counter）和一个回调函数。在本示例中，回调函数是一个lambda函数。

• 自检管理器会将此回调函数与data：//my_plugin/counter关联在一起。本质上，该字符串是管理器中该注册项的名称。回调函数会让管理器检索该注册项的下一个值。因此，如果有任何客户端对这个值感兴趣，则该注册项每次更新时，管理器都会调用该回调函数。 在该回调函数中，直接返回成员变量counter的值，但是您可以在该函数中随意编写所需的任何代码。

• 下面来研究一下watcher程序的源代码：

// Use the introspection service for finding the "sim_time" and "counter"

// items.

gazebo::util::IntrospectionClient client;



// Wait for the managers to come online.

std::set<std::string> managerIds = client.WaitForManagers(

std::chrono::seconds(2));

• 该可执行文件负责订阅一个特定的可自检注册项集合。我们创建了IntrospectionClient类来帮助自检服务的所有客户端。如您所见，这里实例化了一个IntrospectionClient类的对象，然后，就开始等待自检管理器上线。

// Pick up the first manager.

std::string managerId = *managerIds.begin();

• 从理论上讲，可以有多个自检管理器同时在运行，尽管在Gazebo中只会有一个自检管理器。这里正是基于这个假设条件进行操作的，因此会保存检测到的第一个自检管理器的ID。

// sim_time is a pre-registered item with the following URI format:

// data://world/<world_name>?p=<variable_type>/<variable_name>

std::string simTime = "data://world/default?p=time/sim_time";

std::string counter = "data://my_plugin/counter";



// Check if "sim_time" is registered.

if (!client.IsRegistered(managerId, simTime))

{

std::cerr << "The sim_time item is not registered on the manager.\n";

return -1;

}



// Check if "counter" is registered.

if (!client.IsRegistered(managerId, counter))

{

std::cerr << "The counter item is not registered on the manager.\n";

return -1;

}



// The variables to watch are registered with the manager

• 这个代码块会执行完整性检查，以确保两个注册项都在自检管理器中注册过了。

// Create a filter for watching the items.

std::string filterId, topic;

if (!client.NewFilter(managerId, {simTime, counter}, filterId, topic))

return -1;



// Let's subscribe to the topic for receiving updates.

ignition::transport::Node node;

node.Subscribe(topic, cb);



// zZZZ.

ignition::transport::waitForShutdown();

• 这段代码通知了自检管理器我们对一组注册项（simTime和counter）感兴趣。filterId和topic是输出变量。在这个函数后面，管理器会在话题topic下使用我们的自定义更新创建一个通信通道。 filterId是过滤器的唯一标识符，以防将来要更新或删除该过滤器。

• 最后，实例化了ignition :: transport :: Node节点，并使用该节点来订阅我们最近创建的话题。请注意，这里将cb回调函数作为参数传递给了该节点对象的订阅函数。这个回调函数将会定期执行以获得请求的值。

参考:

• 资料来源