回顾

我们的Velodyne传感器具有完整的功能，但与ROS等机器人中间件没有任何联系。将Gazebo与ROS结合使用的好处之一是，可以轻松地在真实世界和模拟世界之间进行切换。为了做到这一点，我们需要让我们的传感器与ROS生态系统完美配合。

添加ROS传输

我们将以与在上一教程中添加Gazebo传输机制的方式类似的方式，修改当前的插件以包括ROS传输机制。

我们假设您的系统上当前已安装ROS。

1. 将头文件添加到该文件。velodyne_plugin.cc

   #include <thread>
   #include "ros/ros.h"
   #include "ros/callback_queue.h"
   #include "ros/subscribe_options.h"
   #include "std_msgs/Float32.h"

2. 向插件添加一些成员变量。

   /// \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;

3. 在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));

4. 如果您仔细阅读了代码，您会发现我们需要两个新功能：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));
     }
   }

5. 最后需要照顾的是cmake构建。

   1. 打开。CMakeLists.txt

   2. 修改文件的顶部，如下所示。

      cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
      
      find_package(roscpp REQUIRED)
      find_package(std_msgs REQUIRED)
      include_directories(${roscpp_INCLUDE_DIRS})
      include_directories(${std_msgs_INCLUDE_DIRS})

   3. 修改插件的目标链接库。

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

   4. 您现在应该看起来像这样。CMakeLists.txt

      cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
      
      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)
      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()

6. 确保您已获取ROS：

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

7. 重新编译插件。

   cd ~/velodyne_plugin/build
   cmake ../
   make

从ROS控制Velodyne

现在，我们可以照常加载Gazebo插件，它将在ROS主题上侦听传入的浮动消息。然后，这些消息将用于设置Velodyne的转速。

1. 开始 roscore

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

2. 在新的终端中，启动Gazebo

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

3. 在新的终端中，用于rostopic发送速度信息。

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

4. 更改以上命令的最后一个数字以设置不同的速度。

结论

恭喜，您现在拥有构建自定义模型，共享模型并生成公共API的工具。玩得开心，模拟愉快！