0. 準備
sudo apt-get install libgazebo9-dev
1. 修改SDF
在相應的<model>
標籤下添加如下代碼:
<plugin name="velodyne_control" filename="libvelodyne_plugin.so"/>
或者在模型編輯器中的Plugin下Add也可以!
2. 編寫插件
gedit 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)
{
// 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);
// Set the joint's target velocity. This target velocity is just
// for demonstration purposes.
this->model->GetJointController()->SetVelocityTarget(
this->joint->GetScopedName(), 10.0);
}
/// \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
3. 執行
mkdir build && cd build
cmake ..
make
在build
目錄下執行:
gazebo --verbose ../velodyne.world
4. 插件配置
修改插件,讀取自定義SDF參數
在velodyne.world中相應標籤下寫入:
<plugin name="velodyne_control" filename="libvelodyne_plugin.so">
<velocity>25</velocity>
</plugin>
然後在插件Load
函數中讀取此值,修改Load
末尾,使用sdf::ElementPtr
讀取<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);
編譯並運行:
cd ~/velodyne_plugin/build
cmake ../
make
gazebo --verbose ../velodyne.world
調整<velocity>
SDF值,然後重新啓動仿真以查看效果。
創建API
法1:消息傳遞和函數
消息傳遞依賴於gazebo的傳輸機制,並且涉及創建一個命名主題,發佈者可以在該主題上發送雙精度值(double)。插件會收到這些消息,並適當設置速度。消息傳遞對於進程間通信很方便。
法2:創建一個新的公共函數來調整參數
新插件將從我們當前的插件繼承,子插件將由Gazebo而不是我們當前的插件實例化,並將通過調用我們的函數來控制速度。將gazebo連接到ROS時最常使用這種方法。
① 設置速度的函數
/// \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 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());
}
完整代碼如下:
#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
測試消息傳遞API
新建消息發佈者val.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
}
CMakeLists.txt
末尾添加如下代碼:
# 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 1
參考文獻: