我正在通过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
答案 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调试级别,以了解您的节点是否正在获取数据