Hello, I just have problem subscribing to a topic in gazebo plugin published by a ros node.
I have followed this [link text](http://gazebosim.org/tutorials?tut=guided_i6)
Here's my plugin
namespace gazebo
{
class GraspTest : public ModelPlugin
{
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
common::Time::Sleep(common::Time::MSleep(10000));
this->model = _parent;
this->joint =_parent->GetJoint("nut_joint");
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "gazebo_client",
ros::init_options::NoSigintHandler);
}
this->rosNode.reset(new ros::NodeHandle("gazebo_client"));
this->rosNode->setCallbackQueue(&(this->rosQueue));
this->rosSub = this->rosNode->subscribe("part_pose", 1000, &GraspTest::OnRosMsg,this);
std::cout<<"num of publishers"<rosSub.getNumPublishers()<rosQueueThread =
std::thread(std::bind(&GraspTest::QueueThread, this));
}
public: void OnRosMsg(const geometry_msgs::PoseConstPtr &_msg)
{
//......
}
private: void QueueThread()
{
while (this->rosNode->ok())
{
this->rosQueue.callAvailable();
}
}
private: physics::ModelPtr model;
private: event::ConnectionPtr updateConnection;
private: gazebo::physics::JointPtr joint;
private: std::unique_ptr rosNode;
private: ros::Subscriber rosSub;
private: ros::CallbackQueue rosQueue;
private: std::thread rosQueueThread;
};
GZ_REGISTER_MODEL_PLUGIN(GraspTest);
}
Here's my publisher
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise("part_pose", 1000);
ros::Rate loop_rate(10);
ros::WallDuration sleep_time(15.0);
int i=0;
while (ros::ok())
{ if(i==20)break;
i++;
geometry_msgs::Pose msg;
msg.position.x=1.5;
msg.position.y=0.0;
msg.position.z=1.35;
msg.orientation.x=0.0;
msg.orientation.y=0.0;
msg.orientation.z=0.0;
msg.orientation.w=0.0;
chatter_pub.publish(msg);
ROS_INFO("there is %d subscriber(s)!!!!!!!!!!!",chatter_pub.getNumSubscribers());
ros::spinOnce();
ROS_INFO("message sent :)");
sleep_time.sleep();
loop_rate.sleep();
}
return 0;
}
And I just add the plugin into my urdf and launch the publisher node and spawn_model node by a launch file.
However, the result of getNumPublishers in the plugin and the result of getNumSubscribers in the publisher node are both 0 always...
Can anyone tell what's wrong ?
Thx a lot !!!
↧