插件配置

一、准备工作

二、创建一个API

三、测试消息传递API

1、val.cc

编辑cmake文件

我们已经对转速进行了硬编码，但是每次修改参数就需要重新编译，不能实现动态地调节插件。因此，在本节中，我们将修改插件以读取自定义SDF参数，该参数是Velodyne的目标速度。

首先添加一个新标签。新标签可以是任何东西，只要它是有效的XML。我们的插件将可以访问Load函数中的值。首先打开world文件编辑

一、准备工作

gedit ~/velodyne_plugin/velodyne.world

添加,里面包含一个标签。

25

现在，让我们在插件的Load函数中读取此值。

gedit ~/velodyne_plugin/velodyne_plugin.cc

修改Load函数的末尾，使用 sdf::ElementPtr参数来读取。

// Default to zero velocity

double velocity = 0;

// Check that the velocity element exists, then read the value

if (_sdf->HasElement("velocity"))

velocity = _sdf->Get("velocity");

// Set the joint's target velocity. This target velocity is just

// for demonstration purposes.

this->model->GetJointController()->SetVelocityTarget(

this->joint->GetScopedName(), velocity);

重新编译运行看下效果：

cd ~/velodyne_plugin/build

cmake ../

make

gazebo --verbose ../velodyne.world

二、创建一个API

通过SDF调整目标速度非常方便，但支持动态调整会更好。此更改将需要添加其他程序可以用来更改速度值的API。

我们可以使用两种API类型：

消息传递和函数。消息传递依赖于gazebo的传输机制，并且涉及创建一个命名主题，发布者可以在该主题上发送双精度值(double)。插件会收到这些消息，并适当设置速度。消息传递对于进程间通信很方便。

函数方法将创建一个新的公共函数来调整速度。为此，新插件将从我们当前的插件继承。子插件将由Gazebo而不是我们当前的插件实例化，并将通过调用我们的函数来控制速度。将gazebo连接到ROS时最常使用这种方法。

由于我们插件的简单性，可以轻松同时实现两者。

1、打开velodyne_plugin.cc文件，添加以下函数：

/// \brief Set the velocity of the Velodyne

/// \param[in] _vel New target velocity

public: void SetVelocity(const double &_vel)

{

// Set the joint's target velocity.

this->model->GetJointController()->SetVelocityTarget(

this->joint->GetScopedName(), _vel);

}

2、现在，我们将设置传递消息的基础结构。先将node和 subscriber 添加到插件。

/// \brief A node used for transport

private: transport::NodePtr node;

/// \brief A subscriber to a named topic.

private: transport::SubscriberPtr sub;

在Load函数末尾，实例化node和 subscriber。

// Create the node

this->node = transport::NodePtr(new transport::Node());

#if GAZEBO_MAJOR_VERSION < 8

this->node->Init(this->model->GetWorld()->GetName());

#else

this->node->Init(this->model->GetWorld()->Name());

#endif

// Create a topic name

std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";

// Subscribe to the topic, and register a callback

this->sub = this->node->Subscribe(topicName,

&VelodynePlugin::OnMsg, this);

创建处理传入消息的回调函数：

/// \brief Handle incoming message

/// \param[in] _msg Repurpose a vector3 message. This function will

/// only use the x component.

private: void OnMsg(ConstVector3dPtr &_msg)

{

this->SetVelocity(_msg->x());

}

向插件添加两个必要的头文件。

#include

#include

3、现在，该插件可以动态更改目标速度了。完整的插件应如下所示。

#ifndef _VELODYNE_PLUGIN_HH_

#define _VELODYNE_PLUGIN_HH_

#include

#include

#include

#include

namespace gazebo

{

/// \brief A plugin to control a Velodyne sensor.

class VelodynePlugin : public ModelPlugin

{

/// \brief Constructor

public: VelodynePlugin() {}

/// \brief The load function is called by Gazebo when the plugin is

/// inserted into simulation

/// \param[in] _model A pointer to the model that this plugin is

/// attached to.

/// \param[in] _sdf A pointer to the plugin's SDF element.

public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)

{

// Safety check

if (_model->GetJointCount() == 0)

{

std::cerr << "Invalid joint count, Velodyne plugin not loaded\n";

return;

}

// Store the model pointer for convenience.

this->model = _model;

// Get the first joint. We are making an assumption about the model

// having one joint that is the rotational joint.

this->joint = _model->GetJoints()[0];

// Setup a P-controller, with a gain of 0.1.

this->pid = common::PID(0.1, 0, 0);

// Apply the P-controller to the joint.

this->model->GetJointController()->SetVelocityPID(

this->joint->GetScopedName(), this->pid);

// Default to zero velocity

double velocity = 0;

// Check that the velocity element exists, then read the value

if (_sdf->HasElement("velocity"))

velocity = _sdf->Get("velocity");

this->SetVelocity(velocity);

// Create the node

this->node = transport::NodePtr(new transport::Node());

#if GAZEBO_MAJOR_VERSION < 8

this->node->Init(this->model->GetWorld()->GetName());

#else

this->node->Init(this->model->GetWorld()->Name());

#endif

// Create a topic name

std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";

// Subscribe to the topic, and register a callback

this->sub = this->node->Subscribe(topicName,

&VelodynePlugin::OnMsg, this);

}

/// \brief Set the velocity of the Velodyne

/// \param[in] _vel New target velocity

public: void SetVelocity(const double &_vel)

{

// Set the joint's target velocity.

this->model->GetJointController()->SetVelocityTarget(

this->joint->GetScopedName(), _vel);

}

/// \brief Handle incoming message

/// \param[in] _msg Repurpose a vector3 message. This function will

/// only use the x component.

private: void OnMsg(ConstVector3dPtr &_msg)

{

this->SetVelocity(_msg->x());

}

/// \brief A node used for transport

private: transport::NodePtr node;

/// \brief A subscriber to a named topic.

private: transport::SubscriberPtr sub;

/// \brief Pointer to the model.

private: physics::ModelPtr model;

/// \brief Pointer to the joint.

private: physics::JointPtr joint;

/// \brief A PID controller for the joint.

private: common::PID pid;

};

// Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.

GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)

}

#endif

三、测试消息传递API

1、val.cc

在您的工作空间中创建一个新的源文件。

gedit ~/velodyne_plugin/vel.cc

添加以下代码：

#include

#include

#include

// Gazebo's API has changed between major releases. These changes are

// accounted for with #if..#endif blocks in this file.

#if GAZEBO_MAJOR_VERSION < 6

#include

#else

#include

#endif

/

int main(int _argc, char **_argv)

{

// Load gazebo as a client

#if GAZEBO_MAJOR_VERSION < 6

gazebo::setupClient(_argc, _argv);

#else

gazebo::client::setup(_argc, _argv);

#endif

// Create our node for communication

gazebo::transport::NodePtr node(new gazebo::transport::Node());

node->Init();

// Publish to the velodyne topic

gazebo::transport::PublisherPtr pub =

node->Advertise<:msgs::vector3d>("~/my_velodyne/vel_cmd");

// Wait for a subscriber to connect to this publisher

pub->WaitForConnection();

// Create a a vector3 message

gazebo::msgs::Vector3d msg;

// Set the velocity in the x-component

#if GAZEBO_MAJOR_VERSION < 6

gazebo::msgs::Set(&msg, gazebo::math::Vector3(std::atof(_argv[1]), 0, 0));

#else

gazebo::msgs::Set(&msg, ignition::math::Vector3d(std::atof(_argv[1]), 0, 0));

#endif

// Send the message

pub->Publish(msg);

// Make sure to shut everything down.

#if GAZEBO_MAJOR_VERSION < 6

gazebo::shutdown();

#else

gazebo::client::shutdown();

#endif

}

编辑cmake文件

在工作区中的CMakeLists.txt文件中添加几行，以生成可执行的vel程序。

# Build the stand-alone test program

add_executable(vel vel.cc)

if (${gazebo_VERSION_MAJOR} LESS 6)

# These two

include(FindBoost)

find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)

target_link_libraries(vel ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})

else()

target_link_libraries(vel ${GAZEBO_LIBRARIES})

endif()

编译运行之后，在新的终端中，进入文件所在目录并运行vel命令。确保设置数值，该数值被解释为目标速度值。

cd ~/velodyne_plugin/build

./vel 2