Gazebo使用筆記（7） —— 主題訂閱、發佈及自定義消息

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

您的可執行終端應顯示它與涼亭的連接並顯示請求消息。您應該看到涼亭終端顯示一些消息並運行完成百分比統計，直到完成爲止。

---

參考文獻：

• http://gazebosim.org/tutorials?tut=topics_subscribed&cat=transport
• http://gazebosim.org/tutorials?tut=custom_messages&cat=transport