【Gazebo入门教程】第七讲 Gazebo与ROS的通信（附如何开源模型到线上数据库）

前言：在前两篇博客中，我们首先了解了控制器插件的具体使用方法和配置流程，采用多个实例分别了解了模型插件、世界插件、系统插件以及传感器插件等的具体使用方法，本节博客将重点介绍如何将我们在Gazebo中的构造的仿真组件及模型与ROS链接在一起，最终完成真实世界和仿真世界之间的切换，并在文章末尾，我们给出了上传自己的模型到线上数据库的具体方法，敬请期待。

一、Gazebo与ROS的连接

1. 基本介绍

• 目的：将Gazebo与ROS连接在一起，完成真实世界与模拟世界之间的切换，将设定好的机器人模型与ROS系统完成通信，最终通过ROS系统插件完成对于Gazebo仿真的控制；
• 配置流程：本节内容将分为以下五个部分，头文件、成员变量、函数、编译、测试；

• 注意事项：

1. 流程的第一步实际上就是修改我们当前的插件内容，在其中添加ROS插件，其方式类似于在上一教程中添加Gazebo插件的方式。
2. 确保您已获取ROS源： source /opt/ros//setup.bash
3. 最终效果：照常加载Gazebo插件，它将在ROS主题上监听传入的浮动数据类型的消息（float）。然后，这些消息将用于设置Velodyne的转速

2. 具体流程

\qquad ① 添加头文件：

#include 
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"

\qquad ② 添加成员变量：

/// \brief A node use for ROS transport
private: std::unique_ptr<ros::NodeHandle> rosNode;

/// \brief A ROS subscriber
private: ros::Subscriber rosSub;

/// \brief A ROS callbackqueue that helps process messages
private: ros::CallbackQueue rosQueue;

/// \brief A thread the keeps running the rosQueue
private: std::thread rosQueueThread;

\qquad ③ 修改函数：

• Load函数结尾添加如下代码：

// Initialize ros, if it has not already bee initialized.
if (!ros::isInitialized())
{
  int argc = 0;
  char **argv = NULL;
  ros::init(argc, argv, "gazebo_client",
      ros::init_options::NoSigintHandler);
}

// Create our ROS node. This acts in a similar manner to
// the Gazebo node
this->rosNode.reset(new ros::NodeHandle("gazebo_client"));

// Create a named topic, and subscribe to it.
ros::SubscribeOptions so =
  ros::SubscribeOptions::create<std_msgs::Float32>(
      "/" + this->model->GetName() + "/vel_cmd",
      1,
      boost::bind(&VelodynePlugin::OnRosMsg, this, _1),
      ros::VoidPtr(), &this->rosQueue);
this->rosSub = this->rosNode->subscribe(so);

// Spin up the queue helper thread.
this->rosQueueThread =
  std::thread(std::bind(&VelodynePlugin::QueueThread, this));

• 新函数OnRosMsg和QueueThread定义：

/// \brief Handle an incoming message from ROS
/// \param[in] _msg A float value that is used to set the velocity
/// of the Velodyne.
public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
{
  this->SetVelocity(_msg->data);
}

/// \brief ROS helper function that processes messages
private: void QueueThread()
{
  static const double timeout = 0.01;
  while (this->rosNode->ok())
  {
    this->rosQueue.callAvailable(ros::WallDuration(timeout));
  }
}

\qquad ④ 编译：

• 修改Cmake文件：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

########### Add ############
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(${roscpp_INCLUDE_DIRS})
include_directories(${std_msgs_INCLUDE_DIRS})
############################

# Find Gazebo
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

# Build our plugin
add_library(velodyne_plugin SHARED velodyne_plugin.cc)

########### Add ############
target_link_libraries(velodyne_plugin ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES})
############################

# Build the stand-alone test program
add_executable(vel vel.cc)

if (${gazebo_VERSION_MAJOR} LESS 6)
  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()

• 编译：

cmake ../
make

\qquad ⑤ 测试：

• 启动roscore：

source /opt/ros/<DISTRO>/setup.bash
roscore

• 新终端启动Gazebo：

cd ~/velodyne_plugin/build
source /opt/ros/<DISTRO>/setup.bash
gazebo ../velodyne.world

• 使用rostopic发送速度信息：

source /opt/ros/<DISTRO>/setup.bash
rostopic pub /my_velodyne/vel_cmd std_msgs/Float32 1.0

二、上传模型

• 目的：贡献机器人模型/仿真世界文件到Gazebo的线上数据库，开源帮助他人的同时也在提升自己；

• 上传流程：

1. 访问链接github/gazebp来fork````其中的gazebo_models数据库，并clone```复制模型数据库到本地：

git clone https://github.com/your_user_name/gazebo_models.git

2. 查看克隆的仓库下，确保没有你的模型，然后从~/.gazebo/models目录复制模型到克隆仓库：

cp -r ~/.gazebo/models/velodyne-hdl32 ~/gazebo_models

3. 创建新的分支，方便pull-request的过程：

cd ~/gazebo_models
git checkout -b velodyne_tutorial_do_not_merge

4. 对模型进行add，commit，push:

git add velodyne*
git commit -m "Add a Velodyne HDL-32"
git push -u origin velodyne_tutorial_do_not_merge

5. 创建一个pull-request，返回主仓库：

\qquad 1）在浏览器打开之前fork的仓库https://github.com/your_user_name/gazebo_models.git
\qquad 2）去Pull requests选项里创建New pull request
\qquad 3）输入标题和描述后点击Creat Pull Request按钮

• 注意事项：

1. 入门教程中的模型都来自于官方教程，非原创，不会上传到仓库中；
2. 登录Github，建议科学上网；
3. 将请求合并到gazebo_models主仓库之前，需要获得两次批准，所以如果有评论，请尽快回复；

总结

• 内容分析：本篇博客主要介绍了如何将Gazebo下的仿真组件与模型和ROS系统间完成链接与通信，完成真实世界和仿真世界之间的切换，并在文章末尾给出了上传自己的模型到线上数据库的具体方法，下篇博客将会重点介绍日志与回放功能，敬请期待。

• 注意：本文参考了Gazebo官方网站以及古月居中的Gazebo有关教程、知乎Bruce的教程等，主要目的是方便自行查询知识，巩固学习经验，无任何商业用途。