gazebo plugin subscribe to ros topic problems

asked 2018-03-04 10:12:20 -0600

wy.z gravatar image

Hello, I just have problem subscribing to a topic in gazebo plugin published by a ros node. I have followed this link text 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"<<this->rosSub.getNumPublishers()<<std::endl; 

    this->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<ros::NodeHandle> 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<geometry_msgs::Pose>("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 !!!

edit retag flag offensive close merge delete