csdn同步更新，主頁

在之前的文章中，我們構(gòu)建的Velodyne傳感器已經(jīng)具有完整的功能，但這僅局限與仿真而已，與真實的機器人系統(tǒng)很難產(chǎn)生實際聯(lián)系。因此，我們需要將gazebo與ros連接在一起，這樣做的好處之一是，可以輕松地在真實世界和模擬世界之間進(jìn)行切換，也就是說，你在仿真環(huán)境中實現(xiàn)的功能，在某些現(xiàn)實情況下，也能夠輕易實現(xiàn)。為了做到這一點，我們需要讓我們的傳感器與ROS生態(tài)系統(tǒng)完美配合。

添加ROS插件

修改我們當(dāng)前的插件內(nèi)容，在其中添加ROS插件，其方式類似于在上一教程中添加Gazebo插件的方式。假設(shè)您的系統(tǒng)上當(dāng)前已安裝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函數(shù)末尾，添加以下內(nèi)容。

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

在以上代碼中，定義了兩個新函數(shù)：OnRosMsg和QueueThread，具體實現(xiàn)如下，將其添加到相應(yīng)位置：

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

4、最后需要修改的是cmake構(gòu)建。

• 打開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})

• 修改插件的目標(biāo)鏈接庫。

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

現(xiàn)在完整的CMakeLists.txt應(yīng)該是這樣的。

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()

5、確保您已獲取ROS源：

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

6、重新編譯插件。

cd ~/velodyne_plugin/build
cmake ../
make

在ROS中控制Velodyne

現(xiàn)在，我們可以照常加載Gazebo插件，它將在ROS主題上監(jiān)聽傳入的浮動數(shù)據(jù)類型的消息（float）。然后，這些消息將用于設(shè)置Velodyne的轉(zhuǎn)速。

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發(fā)送速度信息。

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

更改以上命令的最后一個數(shù)字即可以設(shè)置不同的速度。

，，，