Plugins Example

BACK: Plugins

Application

I’ll comment the code more later on

This tutorial requires a Velodyne Lidar model which you can download here

In the gazebo_plugin_tutorial folder, go ahead and extract the velodyne_hdl32 folder there and make a new file: velodyne_plugin.cc:

#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.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)
    {
      // Just output a message for now
      std::cerr << "\nThe velodyne plugin is attach to model[" <<
        _model->GetName() << "]\n";

    }
    /// \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

Also add this to your CMakeLists.txt:

add_library(velodyne_plugin SHARED velodyne_plugin.cc)
target_link_libraries(velodyne_plugin ${GAZEBO_libraries})

Compile and don’t forget export $ LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:~/gazebo_plugin_tutorial/build Run with $ gazebo --verbose velodyne.world

You should see in your terminal the printed statement! Now let’s make it spinaroo: In the Load() function, add

if (_model->GetJointCount() == 0) { //http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/classgazebo_1_1physics_1_1Model.html
  std::cerr << "ERROR: No joints available.\n";
  return;
}
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);

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

As well as some private members to the class (after Load()):

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

Recompile and run. The Lidar should be spinning now.

Changing Values

However, the model spins at a hard-coded value of 10.0. One way that we can change this is by adding an argument to the xml (.world) file.

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

In the Load() function, change some things around:

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

Recompile and try again. It should be running faster (or to whatever you set the parameter to).

DYNAMIC values

While being able to edit the XML file is convenient, we still want to be able to change the velocity from a different program. To do this, we want to create an API that is similar to the whole publish/subscribe system of ROS. (This is also how you interface to ROS).

Create a public function that can set the Velocity value (in 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);
}

Gazebo uses a similar publish/subscribe system as ROS. Add a node and subscriber to the plugin.

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

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

Instantiate these two at the end of the Load() function.

// Create the node
this->node = transport::NodePtr(new transport::Node());
this->node->Init(this->model->GetWorld()->Name());

// Create a topic name
std::string topicName = "~/" + this->model->GetName() + "/cmd_vel";

// Subscribe to the topic, and register a callback
this->sub = this->node->Subscribe(topicName,
   &VelodynePlugin::OnMsg, this);

Create the callback function when a message is recieved.

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

Add the appropriate headers

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

Create another program, velocity.cc that publishes a Vector to the plugin.

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

#include <gazebo/gazebo_client.hh> //only if version is > 6
#include <iostream>

int main(int argc, char** argv) {
  gazebo::client::setup(argc, argv);

  //Create node for communication
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  std::string topicName = "~/my_velodyne/cmd_vel";
  gazebo::transport::PublisherPtr pub =
    node->Advertise<gazebo::msgs::Vector3d>(topicName);

  std::cout << "Waiting for connection to" << topicName << "...\n";
  pub->WaitForConnection();
  std::cout << "Connection made!\n";

  gazebo::msgs::Vector3d msg;

  gazebo::msgs::Set(&msg, ignition::math::Vector3d(std::atof(argv[1]), 0, 0));
  std::cout << "Attempting to publish...\n";
  pub->Publish(msg);
  std::cout << "Published!\n";
  gazebo::client::shutdown();
}

Edit the CMakeLists.txt

add_executable(velocity velocity.cc)
target_link_libraries(velocity ${GAZEBO_LIBRARIES})

Compile and Run $ ./vel 2