csdn同步更新，主頁(yè)

我們已經(jīng)對(duì)轉(zhuǎn)速進(jìn)行了硬編碼，但是每次修改參數(shù)就需要重新編譯，不能實(shí)現(xiàn)動(dòng)態(tài)地調(diào)節(jié)插件。因此，在本節(jié)中，我們將修改插件以讀取自定義SDF參數(shù)，該參數(shù)是Velodyne的目標(biāo)速度。

首先添加一個(gè)新標(biāo)簽<plugin>。新標(biāo)簽可以是任何東西，只要它是有效的XML。我們的插件將可以訪問(wèn)Load函數(shù)中的值。首先打開(kāi)world文件編輯

一、準(zhǔn)備工作

gedit ~/velodyne_plugin/velodyne.world

添加<plugin>,里面包含一個(gè)<velocity>標(biāo)簽。

<plugin name="velodyne_control" filename="libvelodyne_plugin.so">
  <velocity>25</velocity>
</plugin>

現(xiàn)在，讓我們?cè)诓寮?code>Load函數(shù)中讀取此值。

gedit ~/velodyne_plugin/velodyne_plugin.cc

修改Load函數(shù)的末尾，使用 sdf::ElementPtr參數(shù)來(lái)讀取<velocity>。

// Default to zero velocity
double velocity = 0;

// Check that the velocity element exists, then read the value
if (_sdf->HasElement("velocity"))
  velocity = _sdf->Get<double>("velocity");

// Set the joint's target velocity. This target velocity is just
// for demonstration purposes.
this->model->GetJointController()->SetVelocityTarget(
    this->joint->GetScopedName(), velocity);

重新編譯運(yùn)行看下效果：

cd ~/velodyne_plugin/build
cmake ../
make
gazebo --verbose ../velodyne.world

二、創(chuàng)建一個(gè)API

通過(guò)SDF調(diào)整目標(biāo)速度非常方便，但支持動(dòng)態(tài)調(diào)整會(huì)更好。此更改將需要添加其他程序可以用來(lái)更改速度值的API。

我們可以使用兩種API類(lèi)型：

• 消息傳遞和函數(shù)。消息傳遞依賴(lài)于gazebo的傳輸機(jī)制，并且涉及創(chuàng)建一個(gè)命名主題，發(fā)布者可以在該主題上發(fā)送雙精度值（double）。插件會(huì)收到這些消息，并適當(dāng)設(shè)置速度。消息傳遞對(duì)于進(jìn)程間通信很方便。

• 函數(shù)方法將創(chuàng)建一個(gè)新的公共函數(shù)來(lái)調(diào)整速度。為此，新插件將從我們當(dāng)前的插件繼承。子插件將由Gazebo而不是我們當(dāng)前的插件實(shí)例化，并將通過(guò)調(diào)用我們的函數(shù)來(lái)控制速度。將gazebo連接到ROS時(shí)最常使用這種方法。

由于我們插件的簡(jiǎn)單性，可以輕松同時(shí)實(shí)現(xiàn)兩者。

1、打開(kāi)velodyne_plugin.cc文件，添加以下函數(shù)：

/// \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、現(xiàn)在，我們將設(shè)置傳遞消息的基礎(chǔ)結(jié)構(gòu)。先將node和 subscriber 添加到插件。

/// \brief A node used for transport
private: transport::NodePtr node;

/// \brief A subscriber to a named topic.
private: transport::SubscriberPtr sub;

在Load函數(shù)末尾，實(shí)例化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);

創(chuàng)建處理傳入消息的回調(diào)函數(shù)：

/// \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());
}

向插件添加兩個(gè)必要的頭文件。

#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

3、現(xiàn)在，該插件可以動(dòng)態(tài)更改目標(biāo)速度了。完整的插件應(yīng)如下所示。

#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

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<double>("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

三、測(cè)試消息傳遞API

1、val.cc

在您的工作空間中創(chuàng)建一個(gè)新的源文件。

gedit ~/velodyne_plugin/vel.cc

添加以下代碼：

#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

// 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 <gazebo/gazebo.hh>
#else
#include <gazebo/gazebo_client.hh>
#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<gazebo::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文件

在工作區(qū)中的CMakeLists.txt文件中添加幾行，以生成可執(zhí)行的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()

編譯運(yùn)行之后，在新的終端中，進(jìn)入文件所在目錄并運(yùn)行vel命令。確保設(shè)置數(shù)值，該數(shù)值被解釋為目標(biāo)速度值。

cd ~/velodyne_plugin/build
./vel 2

，，，