文章目录
- 0. 准备
- 1. 修改SDF
- 2. 编写插件
- 3. 执行
- 4. 插件配置
-
-
-
- 修改插件,读取自定义SDF参数
- 创建API
- 测试消息传递API
-
-
0. 准备
sudo apt-get install libgazebo9-dev
- 修改SDF在相应的<model>标签下添加如下代码:
<plugin name="velodyne_control" filename="libvelodyne_plugin.so"/>
或者在模型编辑器中的Plugin下Add也可以!
- 编写插件
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
- 执行
mkdir build && cd build cmake .. make
在build目录下执行:
gazebo --verbose ../velodyne.world
-
插件配置
修改插件,读取自定义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()); }
完整代码如下:
测试消息传递API新建消息发布者val.cc#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
#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
参考文献:http://gazebosim.org/tutorials?cat=guided_i&tut=guided_i5
评论(0)
您还未登录,请登录后发表或查看评论