为什么Gazebo不会启动ROS?

时间:2016-07-13 18:34:58

标签: c++ simulation ros

我正在通过Gazebo教程将Gazebo传感器连接到ROS并传递消息。 http://gazebosim.org/tutorials?cat=guided_i&tut=guided_i6

该程序构建一个Gazebo ModelPlugin对象,并从该对象中初始化ROS。然后,它创建一个ROS节点,订户,队列和标准线程来运行ROS队列。这个程序使用Gazebo的传输对象,但是当我尝试添加ROS传输对象时(如前所述),该程序不起作用。我的问题源于roscore节点(包括rosmaster)没有初始化的事实。

我的传感器插件代码如下。 ROS整合从第70行开始:

#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/subscribe_options.h>
#include <thread>
#include <std_msgs/Float32.h>

namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class VelodynePlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: VelodynePlugin() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
            gzwarn << "HERE";
      if (_model->GetJointCount() == 0) {
        std::cerr <<
          "Invalid joint count, Velodyne plugin not loaded\n";
      }

      // Store the model pointer for convenience.
      this->model = _model;

      // Get the first joint. We are making an assumption about the
      // model having one joint that is the rotational joint.
      this->joint = _model->GetJoints()[0];

      // Setup a P-controller with a gain of 0.1.
      this->pid=common::PID(0.1,0,0);

      // Apply the P-controller to the joint.
      this->model->GetJointController()->SetVelocityPID(
          this->joint->GetScopedName(), this->pid);

      // Default to zero velocity
      double velocity=0;

      // Check that the velocity element exists, then read the value
      if (_sdf->HasElement("velocity"))
        velocity=_sdf->Get<double>("velocity");

      this->SetVelocity(velocity);

      // Create the node
      this->node = transport::NodePtr(new transport::Node());
      this->node->Init(this->model->GetWorld()->GetName());

      // Create a topic name
      std::string topicName = "~/" + this->model->GetName() +
        "/vel_cmd";

      // Subscribe to the topic, and register a callback.
      this->sub = this->node->Subscribe(topicName,
          &VelodynePlugin::OnMsg, this);

            // Initialize ros, if it has not already been initialized.
            if (!ros::isInitialized()) {
                std::cout << "initializing ros" << std::endl;
                int argc = 0;
                char **argv=NULL;
                ros::init(argc,argv,"gazebo_client",
                    ros::init_options::NoSigintHandler);
            } else { std::cout << "NOT initializing ros" << std::endl; }

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

    /// \brief Set the velocity of the Velodyne
    /// \param[in] _vel New target velocity
    public: void SetVelocity(const double &_vel) {
      // Set the joint's target velocity.
      this->model->GetJointController()->SetVelocityTarget(
          this->joint->GetScopedName(), _vel);
    }

    /// \brief Handle incoming message
    /// \param[in] _msg Repurpose a vector3 message. This function will
    /// only use the x component.
    private: void OnMsg(ConstVector3dPtr &_msg) {
      this->SetVelocity(_msg->x());
    }

    /// \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 = .01;
      while (this->rosNode->ok()) {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }


    /// \brief Pointer to the model;
    private: physics::ModelPtr model;

    /// \brief Control surfaces joints.
    private: physics::JointPtr joint;

    /// \brief Velocity PID for the propeller.
    private: common::PID pid;

    /// \brief A node used for transport
    private: transport::NodePtr node;

    /// \brief A subscriber to a named topic.
    private: transport::SubscriberPtr sub;

    /// \brief A node used 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 that keeps running the rosQueue
    private: std::thread rosQueueThread;

  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}
#endif

让我知道如何更具体地说明这个问题。第一个问题肯定是ROS没有初始化。我可以说这是因为在rostopic list输出的另一个终端中的呼叫:

ERROR: Unable to communicate with master

4 个答案:

答案 0 :(得分:3)

我觉得roscore没有运行。您必须先手动运行roscore,然后才能启动任何ROS节点。

您可以将roscore想象为所有节点连接的服务器,并管理这些节点之间的通信。它不会自动启动,因此您必须先启动roscore作为第一步,然后才能使用任何ROS节点。

例外情况是,如果您使用的是启动文件。如果roslaunch尚未运行,roscore确实会自动启动。{/ p>

答案 1 :(得分:0)

在启动Gazebo服务器之前,通过运行roscore解决了该问题。我必须接受该教程的确遵循凉亭优先管线。尽管如此,我认为在ROS和凉亭之间架起桥梁是一种过时的方法。您应该检查gazebo-ros-pkgs中的代码,以找到更简单的方法。

Gazebo插件是一个运行时组件,在技术上为共享库,它通过SDF / URDF文件附加到特定对象或直接附加到World实例。可以通过命令gzserver <world_file>模拟世界实例。如果您将凉亭与ROS 结合使用(我强烈建议您在某个时候使用ROS),那么您需要的是 gazebo-ros 节点。再次在上面的链接中,有几个示例。

在ROS Universe中,所有进程都表示为ROS节点,并且Gazebo仿真器没有不同。当您使用gazebo_ros程序包(单线建立ROS-Gazebo连接)时,Gazebo仿真器被初始化为带有指定世界文件的/gazebo节点,并且所有附加的Gazebo插件都在其中充当组件节点。第一个问题在这里。您不应该在Gazebo插件中调用ros::init(),因为它已经有一个初始化的节点。第二个问题是您的假设,因此ros::init()调用不会启动roscore节点。它仅以可执行文件可以与同一主机下的其他ROS节点通信的方式来初始化。因此,您只需要一个ros::NodeHandle

我不知道为什么OSRF没有更新教程。也许,他们正在严格尝试在凉亭网站上提供凉亭优先方面,并将ROS视为次要组成部分。但是,我发现这很成问题,特别是当人们想使用ROS作为主要成分时。由于roslaunch仅启动ROS节点,这使gzserver <world_file>命令无法正常工作,因此本教程将无效,甚至与主流用法矛盾。

答案 2 :(得分:0)

据我了解,凉亭不是ROS的一部分,所以如果它不以roscore开始,那是有道理的。

如果要在此节点启动时同时启动roscore,请尝试使用roslaunch命令而不是rosrun。

答案 3 :(得分:-1)

问题确实是roscore,但之后您可以查看以rosnode info nodename开头的节点来检查子版和发布商状态,或者roswtf检查错误。< / p>

您可以使用rqt logger级别(最简单的方法)检查​​实时调试日志设置ros调试级别,以了解您的节点是否正在获取数据