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

您的可执行终端应显示它与凉亭的连接并显示请求消息。您应该看到凉亭终端显示一些消息并运行完成百分比统计,直到完成为止。


参考文献: