Gazebo文件级
/usr/include/gazebo-9/gazebo
1. 主题订阅
Gazebo基于TCP/IP套接字通信,该套接字允许单独的程序与Gazebo进行交互。Gazebo使用Boost ASIO管理通信层,Google Protobufs用来进行消息传递和序列化库。
使用发布者-订阅者机制,通过TCP/IP套接字与Gazebo通信的最简单方法是链接Gazebo库,并使用库提供的功能。
运行Gazebo后,使用如下命令搜索正在使用的主题列表:
gz topic -l
库的安装:
# 注意,要换成对应的版本
sudo apt-get install libgazebo9-dev
示例代码:listener.cc
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo_client.hh>
#include <iostream>
/
// 回调函数
void cb(ConstWorldStatisticsPtr &_msg)
{
std::cout << _msg->DebugString();
}
/
int main(int _argc, char **_argv)
{
// Load gazebo
gazebo::client::setup(_argc, _argv);
// 创建用于通信的节点,该节点提供发布者和订阅者功能
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
// 订阅Gazebo发布的 world_stats topic
gazebo::transport::SubscriberPtr sub = node->Subscribe("~/world_stats", cb);
// 消息循环,此处为简单的循环等待
while (true)
gazebo::common::Time::MSleep(10);
// Make sure to shut everything down.
gazebo::client::shutdown();
}
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_executable(listener listener.cc)
target_link_libraries(listener ${GAZEBO_LIBRARIES} pthread)
2. 主题发布
publisher.cc
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <ignition/math/Pose3.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <iostream>
/
int main(int _argc, char **_argv)
{
// Load gazebo
gazebo::client::setup(_argc, _argv);
// Create our node for communication
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
// Publish to a Gazebo topic
gazebo::transport::PublisherPtr pub =
node->Advertise<gazebo::msgs::Pose>("~/pose_example");
// Wait for a subscriber to connect
pub->WaitForConnection();
// Publisher loop...replace with your own code.
while (true)
{
// Throttle Publication
gazebo::common::Time::MSleep(100);
// Generate a pose
ignition::math::Pose3d pose(1, 2, 3, 4, 5, 6);
// Convert to a pose message
gazebo::msgs::Pose msg;
gazebo::msgs::Set(&msg, pose);
pub->Publish(msg);
}
// Make sure to shut everything down.
gazebo::client::shutdown();
}
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_executable(publisher publisher.cc)
target_link_libraries(publisher ${GAZEBO_LIBRARIES})
3. 消息自定义
Gazebo提供的消息类型可参考API文档
消息内容的定义
Gazebo已经包含了一个消息库,位于:/usr/include/gazebo-9/gazebo/msgs/proto
,以vector2d.proto
为例
gedit ~/collision_map_creator_plugin/msgs/collision_map_request.proto
写入如下内容:
# 声明消息的名称空间
package collision_map_creator_msgs.msgs;
# 包含引用的消息库
import "vector2d.proto";
# 实际的消息构造
message CollisionMapRequest
{
# 每个枚举必须是一个唯一的数字。注意,必须使用完整的软件包名称来指定外部消息(在本例中为gazebo.msgs)。
required gazebo.msgs.Vector2d upperLeft = 1;
required gazebo.msgs.Vector2d upperRight = 2;
required gazebo.msgs.Vector2d lowerRight = 3;
required gazebo.msgs.Vector2d lowerLeft = 4;
required double height = 5;
required double resolution = 6;
# 消息可包含可选字段,如果未指定,则使用默认值
optional string filename = 7 [default = ""];
optional int32 threshold = 8 [default = 255];
}
消息的声明方法如下:
["optional"/"required"] [field type] [field name] = [enum];
自定义消息的CMakeLists
gedit ~/collision_map_creator_plugin/msgs/CMakeLists.txt
写入如下内容:
# 包含Protobuf包
find_package(Protobuf REQUIRED)
# 查找消息的安装位置
set(PROTOBUF_IMPORT_DIRS)
foreach(ITR ${GAZEBO_INCLUDE_DIRS})
if(ITR MATCHES ".*gazebo-[0-9.]+$")
set(PROTOBUF_IMPORT_DIRS "${ITR}/gazebo/msgs/proto")
endif()
endforeach()
# 创建列表msgs,该列表使用vector2d.proto消息,并依赖time.proto和header.proto
set(msgs
collision_map_request.proto
${PROTOBUF_IMPORT_DIRS}/vector2d.proto
${PROTOBUF_IMPORT_DIRS}/header.proto
${PROTOBUF_IMPORT_DIRS}/time.proto
)
# 构建必要的C++ 源文件
PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs})
# 链接
add_library(collision_map_creator_msgs SHARED ${PROTO_SRCS})
target_link_libraries(collision_map_creator_msgs ${PROTOBUF_LIBRARY})
自定义消息的使用
gedit ~/collision_map_creator_plugin/collision_map_creator.cc
写入如下代码:
// 必要的系统头文件
#include <iostream>
#include <math.h>
#include <boost/shared_ptr.hpp>
//用于编写png文件
#define png_infopp_NULL (png_infopp)NULL
#define int_p_NULL (int*)NULL
#define png_bytep_NULL (png_bytep)NULL
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <sdf/sdf.hh>
#include <ignition/math/Vector3.hh>
//必要的Gazebo头文件
#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "collision_map_request.pb.h"
namespace gazebo
{
//这是将被发送到我们的回调函数的对象
typedef const boost::shared_ptr<
const collision_map_creator_msgs::msgs::CollisionMapRequest>
CollisionMapRequestPtr;
//创建WorldPlugin的派生类,需要依赖NodePtr、PublisherPtr、SubcriberPtr和WorldPtr,因此要将他们添加为类成员
class CollisionMapCreator : public WorldPlugin
{
transport::NodePtr node;
transport::PublisherPtr imagePub;
transport::SubscriberPtr commandSubscriber;
physics::WorldPtr world;
//World插件的Load方法,用来设置节点、World、订阅者和发布者
public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
{
node = transport::NodePtr(new transport::Node());
world = _parent;
// Initialize the node with the world name
#if GAZEBO_MAJOR_VERSION >= 8
node->Init(world->Name());
#else
node->Init(world->GetName());
#endif
std::cout << "Subscribing to: " << "~/collision_map/command" << std::endl;
// 通过topic,回调函数和指向此插件对象的指针调用node-> Subscribe。 node-> Advertise,需要消息类型的模板参数,以及要发布的主题的参数。
commandSubscriber = node->Subscribe("~/collision_map/command",
&CollisionMapCreator::create, this);
imagePub = node->Advertise<msgs::Image>("~/collision_map/image");
}
// 回调函数,包含了先前定义的typedef,注意参数列表中是引用
public: void create(CollisionMapRequestPtr &msg)
{
std::cout << "Received message" << std::endl;
std::cout << "Creating collision map with corners at (" <<
msg->upperleft().x() << ", " << msg->upperleft().y() << "), (" <<
msg->upperright().x() << ", " << msg->upperright().y() << "), (" <<
msg->lowerright().x() << ", " << msg->lowerright().y() << "), (" <<
msg->lowerleft().x() << ", " << msg->lowerleft().y() <<
") with collision projected from z = " <<
msg->height() << "\nResolution = " << msg->resolution() << " m\n" <<
"Occupied spaces will be filled with: " << msg->threshold() <<
std::endl;
double dX_vertical = msg->upperleft().x() - msg->lowerleft().x();
double dY_vertical = msg->upperleft().y() - msg->lowerleft().y();
double mag_vertical =
sqrt(dX_vertical * dX_vertical + dY_vertical * dY_vertical);
dX_vertical = msg->resolution() * dX_vertical / mag_vertical;
dY_vertical = msg->resolution() * dY_vertical / mag_vertical;
double dX_horizontal = msg->upperright().x() - msg->upperleft().x();
double dY_horizontal = msg->upperright().y() - msg->upperleft().y();
double mag_horizontal =
sqrt(dX_horizontal * dX_horizontal + dY_horizontal * dY_horizontal);
dX_horizontal = msg->resolution() * dX_horizontal / mag_horizontal;
dY_horizontal = msg->resolution() * dY_horizontal / mag_horizontal;
int count_vertical = mag_vertical / msg->resolution();
int count_horizontal = mag_horizontal / msg->resolution();
if (count_vertical == 0 || count_horizontal == 0)
{
std::cout << "Image has a zero dimensions, check coordinates"
<< std::endl;
return;
}
double x,y;
boost::gil::gray8_pixel_t fill(255-msg->threshold());
boost::gil::gray8_pixel_t blank(255);
boost::gil::gray8_image_t image(count_horizontal, count_vertical);
double dist;
std::string entityName;
ignition::math::Vector3d start, end;
start.Z(msg->height());
end.Z(0.001);
//调用使用的物理引擎进行单光线跟踪
#if GAZEBO_MAJOR_VERSION >= 8
gazebo::physics::PhysicsEnginePtr engine = world->Physics();
#else
gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
#endif
engine->InitForThread();
gazebo::physics::RayShapePtr ray =
boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
engine->CreateShape("ray", gazebo::physics::CollisionPtr()));
std::cout << "Rasterizing model and checking collisions" << std::endl;
boost::gil::fill_pixels(image._view, blank);
// 在栅格上栅格化,以确定哪里有对象哪里没对象
for (int i = 0; i < count_vertical; ++i)
{
std::cout << "Percent complete: " << i * 100.0 / count_vertical
<< std::endl;
x = i * dX_vertical + msg->lowerleft().x();
y = i * dY_vertical + msg->lowerleft().y();
for (int j = 0; j < count_horizontal; ++j)
{
x += dX_horizontal;
y += dY_horizontal;
start.X(x);
end.X(x);
start.Y(y);
end.Y(y);
ray->SetPoints(start, end);
ray->GetIntersection(dist, entityName);
if (!entityName.empty())
{
image._view(i,j) = fill;
}
}
}
std::cout << "Completed calculations, writing to image" << std::endl;
if (!msg->filename().empty())
{
boost::gil::gray8_view_t view = image._view;
boost::gil::png_write_view(msg->filename(), view);
}
}
};
// 在模拟器中注册这个插件
GZ_REGISTER_WORLD_PLUGIN(CollisionMapCreator)
}
gedit ~/collision_map_creator_plugin/request_publisher.cc
#include <iostream>
#include <math.h>
#include <deque>
#include <sdf/sdf.hh>
#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/msgs/msgs.hh"
// 自定义的消息头文件
#include "collision_map_request.pb.h"
#include "vector2d.pb.h"
using namespace std;
//该函数将由(x1,y1)(x2,y2)(x3,y3)(x4,y4)定义的坐标字符串解析为Vector2d消息的双端队列
bool createVectorArray(const char * vectorString,
deque<gazebo::msgs::Vector2d*> corners)
{
//初始化双端队列迭代器
deque<gazebo::msgs::Vector2d*>::iterator it;
string cornersStr = vectorString;
size_t opening=0;
size_t closing=0;
//遍历双端队列
for (it = corners.begin(); it != corners.end(); ++it)
{
opening = cornersStr.find('(', closing);
closing = cornersStr.find(')', opening);
if (opening == string::npos || closing == string::npos)
{
std::cout << "Poorly formed string: " << cornersStr << std::endl;
std::cout << "( found at: " << opening << " ) found at: " << closing << std::endl;
return false;
}
string oneCornerStr = cornersStr.substr(opening + 1, closing - opening - 1);
size_t commaLoc = oneCornerStr.find(",");
string x = oneCornerStr.substr(0,commaLoc);
string y = oneCornerStr.substr(commaLoc + 1, oneCornerStr.length() - commaLoc);
//通过找到每个x和y的字符串转换为float来设置X和Y
(*it)->set_x(atof(x.c_str()));
(*it)->set_y(atof(y.c_str()));
}
return true;
}
int main(int argc, char * argv[])
{
//只有至少由4个参数时才继续执行
if (argc > 4)
{
//创建CollisionMapRequest消息,需要在发送前填写此消息
collision_map_creator_msgs::msgs::CollisionMapRequest request;
deque<gazebo::msgs::Vector2d*> corners;
//如果消息是简单的(double、int32、字符串),则可通过其关联方法进行设置;如果字段本身就是消息类型,则必修通过其方法访问。Vector2d本身就是一条单独的消息,故必须从可变函数调用中获取指针
corners.push_back(request.mutable_upperleft());
corners.push_back(request.mutable_upperright());
corners.push_back(request.mutable_lowerright());
corners.push_back(request.mutable_lowerleft());
// 将Vector2d指针的双端队列和第一个argv字符串传递给 createVectorArray。如果失败,则输入的格式可能不正确。在这种情况下,退出程序
if (!createVectorArray(argv[1],corners))
{
return -1;
}
//对于简单字段,我们可以通过调用其set_field方法直接设置其值
request.set_height(atof(argv[2]));
request.set_resolution(atof(argv[3]));
request.set_filename(argv[4]);
//对于我们的可选阈值字段,我们设置为如果在命令参数中指定了阈值字段
if (argc == 6)
{
request.set_threshold(atoi(argv[5]));
}
//这是主要方法的重要组成部分。这将初始化Gazebo消息传递系统。对于插件,Gazebo已经解决了这个问题,但是对于我们自己的可执行文件,我们必须自己完成。还要注意,我们没有使用Gazebo名称空间,因此必须明确。
gazebo::transport::init();
gazebo::transport::run();
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init("default");
std::cout << "Request: " <<
" UL.x: " << request.upperleft().x() <<
" UL.y: " << request.upperleft().y() <<
" UR.x: " << request.upperright().x() <<
" UR.y: " << request.upperright().y() <<
" LR.x: " << request.lowerright().x() <<
" LR.y: " << request.lowerright().y() <<
" LL.x: " << request.lowerleft().x() <<
" LL.y: " << request.lowerleft().y() <<
" Height: " << request.height() <<
" Resolution: " << request.resolution() <<
" Filename: " << request.filename() <<
" Threshold: " << request.threshold() << std::endl;
//这将初始化主题上的请求发布者,~/collision_map/command
gazebo::transport::PublisherPtr imagePub =
node->Advertise<collision_map_creator_msgs::msgs::CollisionMapRequest>(
"~/collision_map/command");
//要发布我们的消息,我们必须首先等待发布者连接到Master,然后我们可以直接发布我们的消息。
imagePub->WaitForConnection();
imagePub->Publish(request);
//退出之前,程序必须调用以将其全部拆除 transport::fini()
gazebo::transport::fini();
return 0;
}
return -1;
}
CMakeLists.txt
# 2.8.8 required to use PROTOBUF_IMPORT_DIRS
cmake_minimum_required(VERSION 2.8.8 FATAL_ERROR)
# 包含必需的Boost库
FIND_PACKAGE( Boost 1.40 COMPONENTS system REQUIRED )
set (CMAKE_CXX_FLAGS "-g -Wall -std=c++11")
find_package(gazebo REQUIRED)
# 包含引用头文件目录
include_directories(
${GAZEBO_INCLUDE_DIRS}
${SDF_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}/msgs
)
link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs)
add_subdirectory(msgs)
# 将可执行文件添加request_publisher到makefile。我们必须将它与collision_map_creator_msgs,GAZEBO_LIBRARIES和 链接起来Boost_LIBRARIES。该add_dependencies指令告诉CMake首先构建 collision_map_creator_msgs。
add_executable(request_publisher request_publisher.cc)
target_link_libraries(request_publisher collision_map_creator_msgs ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDF_LIBRARIES})
add_dependencies(request_publisher collision_map_creator_msgs)
#这将构建我们的插件,并且与您的标准WorldPlugin CMakeLists.txt非常相似,除了我们必须将其链接到并将其 collision_map_creator_msgs也添加为依赖项。
add_library(collision_map_creator SHARED collision_map_creator.cc )
target_link_libraries(collision_map_creator collision_map_creator_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES})
add_dependencies(collision_map_creator collision_map_creator_msgs)
构建和配置
安装依赖包protobuf-compiler
sudo apt-get install protobuf-compiler
构建:
cd ~/collision_map_creator_plugin
mkdir build
cd build
cmake ../
make
将编译好的插件复制到插件目录
sudo cp libcollision_map_creator.so /usr/lib/gazebo-<YOUR-GAZEBO_VERSION>/plugins/
测试用的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>
<include>
<uri>model://willowgarage</uri>
</include>
<plugin filename="libcollision_map_creator.so" name="collision_map_creator"/>
</world>
</sdf>
运行:
gazebo ~/collision_map_creator_plugin/map_creator.world
在单独的终端中,运行可执行文件,并告诉其构建分辨率为1cm的20m x 20m地图:
~/collision_map_creator_plugin/build/request_publisher "(-10,10)(10,10)(10,-10)(-10,-10)" 10 0.01 ~/map.png
您的可执行终端应显示它与凉亭的连接并显示请求消息。您应该看到凉亭终端显示一些消息并运行完成百分比统计,直到完成为止。
参考文献:
评论(0)
您还未登录,请登录后发表或查看评论