tf2_server_early_initial_params_nodelet.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <tf2_msgs/TFMessage.h>
#include <pluginlib/class_list_macros.h>

namespace tf2_server
{

class Tf2ServerEarlyInitialParamsNodelet : public nodelet::Nodelet
{
protected:
  void onInit() override
  {
    this->sub = this->getNodeHandle().subscribe("/tf", 10, &Tf2ServerEarlyInitialParamsNodelet::onMessage, this);
  }

  void onMessage(const tf2_msgs::TFMessageConstPtr& msg)
  {
    if (!this->pub)
    {
      this->pub = this->getNodeHandle().advertise<tf2_msgs::TFMessage>("/test", 10);
      ros::WallDuration(0.01).sleep();
    }
    this->pub.publish(msg);
  }

private:
  ros::Subscriber sub;
  ros::Publisher pub;
};

}

PLUGINLIB_EXPORT_CLASS(tf2_server::Tf2ServerEarlyInitialParamsNodelet, nodelet::Nodelet)