Why ROS node cannot accept TF frames published from rosbag ?

asked 2017-02-12 20:19:14 -0500

Megacephalo gravatar image

updated 2017-02-12 20:23:36 -0500

Hi, all !!

I am implementing a member function to convert a laser scan topic and turn it into PointCloud2 one. I am using laser_geometry library to do the job, the output cloud is directly fed into the next member function. However, every time I display the output point cloud onto Rviz, the cloud would actually be published at the /map frame instead of the corresponding /laser_link. I am suspecting that the node is not receiving any TF frames published from the robot_state_publisher, but what could be the reason?

The converted PointCloud2 is published from the /map frame but should have being published on /laser_link frame. The translation and rotation are correct. The converted point cloud is located on the top left corresponding to where the map frame is; the original laser scan is published at down right,

rqt_graph to reveal the node graph between the rosbag and the node. Note, the rosbag does publish <code>tf</code> and <code>tf_static</code>.

The summarized code is as follows:

class ndt_matching_node {
  private:
    ros::NodeHandle nh_ ;

    ros::Subscriber points_sub ;
    ros::Publisher scan_cloud_pub ;

    tf::TransformListener listener_ ;

  public:
    ndt_matching_node () ;
    ~ndt_matching_node () ;    
    void points_callback (const sensor_msgs::PointCloud2::ConstPtr& input) ;
    void points_callback_swap(const sensor_msgs::LaserScan::ConstPtr& scanIn);
} ;

ndt_matching_node::ndt_matching_node() {
      points_sub = nh_.subscribe(in_scan, 1, &ndt_matching_node::points_callback_swap, this);
      scan_cloud_pub = nh_.advertise<sensor_msgs::PointCloud2> ("scan_cloud", 1) ;
}

void 
ndt_matching_node::points_callback_swap(const sensor_msgs::LaserScan::ConstPtr& scanIn) {
    laser_geometry::LaserProjection projector_;
    sensor_msgs::PointCloud2 cloud;

    try {
    projector_.transformLaserScanToPointCloud(scanIn->header.frame_id,*scanIn, cloud,listener_);
    }
    catch (tf::TransformException& e) {
    std::cout << e . what() << std::endl ;
    return ;
    }

    const sensor_msgs::PointCloud2ConstPtr cloud_ptr = 
    boost::make_shared<sensor_msgs::PointCloud2>(cloud);
    ndt_matching_node::points_callback(cloud_ptr);

  scan_cloud_pub . publish (cloud) ;
}

void 
ndt_matching_node::points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)  {
  // Do some further processing...
}
edit retag flag offensive close merge delete