#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include "../include/fuse_point_multi_camera/fuse_point_multi_camera.h"
using namespace cv;
using namespace std;
using namespace sensor_msgs;
using namespace Eigen;
namespace point_cloud_color {
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudXYZRGB;
ros::Publisher pcl_pub;
image_transport::Publisher pub;
using namespace cv;
class Color_Nodelet : public nodelet::Nodelet {
// private:
ros::NodeHandle nh_;
image_transport::ImageTransport it;//(nh_);
message_filters::Subscriber<PointCloud2> pc_sub;
message_filters::Subscriber<CameraInfo> cinfo_sub;
message_filters::Subscriber<Image> image_sub;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, CameraInfo, Image> SyncPolicy;
typedef message_filters::Synchronizer<SyncPolicy> sync;//(SyncPolicy(10), pc_sub, cinfo_sub, image_sub);
boost::shared_ptr<sync> sync_;
public:
Color_Nodelet()
:it(nh_){
;
}
~Color_Nodelet(){};
void callback(const PointCloud2::ConstPtr& pcl_msg, const CameraInfoConstPtr& cinfo_msg, const ImageConstPtr& image_msg){
ROS_INFO("Nodelet ................................!");
std::cout<<"received image!!!"<<std::endl;
ROS_INFO("Projecting poincloud to the image %ld", pcl_msg->header.stamp.toNSec()-image_msg->header.stamp.toNSec());
}
void onInit() {
NODELET_DEBUG("Initializing nodelet...");
ROS_INFO("Nodelet is Ok for test!!");
pc_sub.subscribe(nh_, "/velodyne/velodyne_points", 1);
cinfo_sub.subscribe(nh_, "/camera_17082034/camera_info", 1);
image_sub.subscribe(nh_, "/camera_17082034/image_raw", 1);
sync_.reset(new sync( SyncPolicy(10), pc_sub, cinfo_sub, image_sub));
sync_->registerCallback(boost::bind(&Color_Nodelet::callback,this, _1, _2, _3));
}
};
}
PLUGINLIB_EXPORT_CLASS(point_cloud_color::Color_Nodelet, nodelet::Nodelet)