中级教程: 连接ROS系统


回顾

我们的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. 如果您仔细阅读了代码,您会发现我们需要两个新功能:OnRosMsgQueueThread。让我们现在添加它们。

    /// \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的工具。玩得开心,模拟愉快!