官方教程:http://gazebosim.org/tutorials?tut=plugins_world&cat=write_plugin
本系列关注的 是如何在仿真的过程中加入已经生成好的模型,就是仿真实际情况下物体出现的随机性。仿真无人机在实际过程中遇到 突然出现的障碍物的反应和路径规划等。
《一》模型的插入方法
模型有三种方法在plugin中插入到仿真环境:
其一:利用gazebo环境下的模型,这一要保证The filename must be in the GAZEBO_MODEL_PATH environment variable(官网:The first method uses a World method to load a model based on a file in the resource path defined by the GAZEBO_MODEL_PATH environment variable.)
_parent->InsertModelFile("model://box");
其二:将模型定义成字符串类型,通过函数的调用(官网:The second method uses a World method to load a model based on string data.
):
sdf::SDF sphereSDF;
sphereSDF.SetFromString(
"<sdf version ='1.4'>\
<model name ='sphere'>\
<pose>1 0 0 0 0 0</pose>\
<link name ='link'>\
<pose>0 0 .5 0 0 0</pose>\
<collision name ='collision'>\
<geometry>\
<sphere><radius>0.5</radius></sphere>\
</geometry>\
</collision>\
<visual name ='visual'>\
<geometry>\
<sphere><radius>0.5</radius></sphere>\
</geometry>\
</visual>\
</link>\
</model>\
</sdf>");
// Demonstrate using a custom model name.
sdf::ElementPtr model = sphereSDF.Root()->GetElement("model");
model->GetAttribute("name")->SetFromString("unique_sphere");
_parent->InsertModelSDF(sphereSDF);
其三:通过消息传送机制(官网:The third method uses the message passing mechanism to insert a model. This method is most useful for stand alone applications that communicate with Gazebo over a network connection.)
{
// Create a new transport node
transport::NodePtr node(new transport::Node());
// Initialize the node with the world name
node->Init(_parent->Name());
// Create a publisher on the ~/factory topic
transport::PublisherPtr factoryPub =
node->Advertise<msgs::Factory>("~/factory");
// Create the message
msgs::Factory msg;
// Model file to load
msg.set_sdf_filename("model://cylinder");
// Pose to initialize the model to
msgs::Set(msg.mutable_pose(),
ignition::math::Pose3d(
ignition::math::Vector3d(1, -2, 0),
ignition::math::Quaterniond(0, 0, 0)));
// Send the message
factoryPub->Publish(msg);
}
《二》创建调用的模型
两个文件:sdf模型文件和config模型描述文件,两个文件放在一个文件夹。文件夹的名字就是程序中调用的名字。sdf文件和config文件的命名:model.sdf和model.config
cd box
gedit model.sdf
<?xml version='1.0'?>
<sdf version ='1.6'>
<model name ='box'>
<pose>1 2 0 0 0 0</pose>
<link name ='link'>
<pose>0 0 .5 0 0 0</pose>
<collision name ='collision'>
<geometry>
<box><size>1 1 1</size></box>
</geometry>
</collision>
<visual name ='visual'>
<geometry>
<box><size>1 1 1</size></box>
</geometry>
</visual>
</link>
</model>
</sdf>
gedit model.config
<?xml version='1.0'?>
<model>
<name>box</name>
<version>1.0</version>
<sdf >model.sdf</sdf>
<author>
<name>me</name>
<email>somebody@somewhere.com</email>
</author>
<description>
A simple Box.
</description>
</model>
《三》plugin插入world文件
由于这个是world的plugin,所以我们需要 插进world标签
gedit factory.world
<?xml version="1.0"?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<plugin name="factory" filename="libfactory.so"/>
</world>
</sdf>
《四》运行
gazebo factory.world
附:官网的例程过程代码。
mkdir ~/gazebo_plugin_tutorial
cd ~/gazebo_plugin_tutorial
gedit factory.cc``
#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
namespace gazebo
{
class Factory : public WorldPlugin
{
public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
{
// Option 1: Insert model from file via function call.
// The filename must be in the GAZEBO_MODEL_PATH environment variable.
_parent->InsertModelFile("model://box");
// Option 2: Insert model from string via function call.
// Insert a sphere model from string
sdf::SDF sphereSDF;
sphereSDF.SetFromString(
"<sdf version ='1.4'>\
<model name ='sphere'>\
<pose>1 0 0 0 0 0</pose>\
<link name ='link'>\
<pose>0 0 .5 0 0 0</pose>\
<collision name ='collision'>\
<geometry>\
<sphere><radius>0.5</radius></sphere>\
</geometry>\
</collision>\
<visual name ='visual'>\
<geometry>\
<sphere><radius>0.5</radius></sphere>\
</geometry>\
</visual>\
</link>\
</model>\
</sdf>");
// Demonstrate using a custom model name.
sdf::ElementPtr model = sphereSDF.Root()->GetElement("model");
model->GetAttribute("name")->SetFromString("unique_sphere");
_parent->InsertModelSDF(sphereSDF);
// Option 3: Insert model from file via message passing.
{
// Create a new transport node
transport::NodePtr node(new transport::Node());
// Initialize the node with the world name
node->Init(_parent->Name());
// Create a publisher on the ~/factory topic
transport::PublisherPtr factoryPub =
node->Advertise<msgs::Factory>("~/factory");
// Create the message
msgs::Factory msg;
// Model file to load
msg.set_sdf_filename("model://cylinder");
// Pose to initialize the model to
msgs::Set(msg.mutable_pose(),
ignition::math::Pose3d(
ignition::math::Vector3d(1, -2, 0),
ignition::math::Quaterniond(0, 0, 0)));
// Send the message
factoryPub->Publish(msg);
}
}
};
// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(Factory)
}
gedit ~/gazebo_plugin_tutorial/CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(factory SHARED factory.cc)
target_link_libraries(factory
${GAZEBO_LIBRARIES}
)
mkdir ~/gazebo_plugin_tutorial/build
cd ~/gazebo_plugin_tutorial/build
cmake ../
make
cd box
gedit model.sdf
<?xml version='1.0'?>
<sdf version ='1.6'>
<model name ='box'>
<pose>1 2 0 0 0 0</pose>
<link name ='link'>
<pose>0 0 .5 0 0 0</pose>
<collision name ='collision'>
<geometry>
<box><size>1 1 1</size></box>
</geometry>
</collision>
<visual name ='visual'>
<geometry>
<box><size>1 1 1</size></box>
</geometry>
</visual>
</link>
</model>
</sdf>
gedit model.config
<?xml version='1.0'?>
<model>
<name>box</name>
<version>1.0</version>
<sdf >model.sdf</sdf>
<author>
<name>me</name>
<email>somebody@somewhere.com</email>
</author>
<description>
A simple Box.
</description>
</model>
cd ~/gazebo_plugin_tutorial/models/cylinder
gedit model.sdf
<?xml version='1.0'?>
<sdf version ='1.6'>
<model name ='cylinder'>
<pose>1 2 0 0 0 0</pose>
<link name ='link'>
<pose>0 0 .5 0 0 0</pose>
<collision name ='collision'>
<geometry>
<cylinder><radius>0.5</radius><length>1</length></cylinder>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<cylinder><radius>0.5</radius><length>1</length></cylinder>
</geometry>
</visual>
</link>
</model>
</sdf>
gedit model.config
<?xml version='1.0'?>
<model>
<name>cylinder</name>
<version>1.0</version>
<sdf>model.sdf</sdf>
<author>
<name>me</name>
<email>somebody@somewhere.com</email>
</author>
<description>
A simple cylinder.
</description>
</model>
export GAZEBO_MODEL_PATH=$HOME/gazebo_plugin_tutorial/models:$GAZEBO_MODEL_PATH
export GAZEBO_PLUGIN_PATH=$HOME/gazebo_plugin_tutorial/build:$GAZEBO_PLUGIN_PATH
cd ~/gazebo_plugin_tutorial
gedit factory.world
<?xml version="1.0"?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<plugin name="factory" filename="libfactory.so"/>
</world>
</sdf>
gazebo ~/gazebo_plugin_tutorial/factory.world
评论(1)
您还未登录,请登录后发表或查看评论