virtual void onInit() {
ROS_INFO("Image Sink Nodelet 'onInit()' done.");
// Get the NodeHandle
private_nh = getPrivateNodeHandle();
ROS_INFO(">>> LOOOOOOL %s", "LOL");
if (private_nh.getParam("depthlookup_image_topic", depth_topic))
{
//NODELET_INFO(">>> Input depth image topic: " << depth_topic);
} else {
NODELET_ERROR("!Failed to get depth image topic parameter!");
exit(EXIT_FAILURE);
}
if (private_nh.getParam("depthlookup_info_topic", depth_info))
{
//NODELET_INFO(">>> Input depth camera info topic: " << depth_info);
} else {
NODELET_ERROR("!Failed to get depth camera info topic parameter!");
exit(EXIT_FAILURE);
}
if (private_nh.getParam("depthlookup_out_topic", out_topic))
{
//NODELET_INFO(">>> Output Topic: " << out_topic);
} else {
NODELET_ERROR("!Failed to get output topic parameter!");
exit(EXIT_FAILURE);
}
if (private_nh.getParam("depthlookup_in_topic", in_topic))
{
//NODELET_INFO(">>> Input Topic: " << in_topic);
} else {
NODELET_ERROR("!Failed to get input topic parameter!");
exit(EXIT_FAILURE);
}
message_filters::Subscriber<Image> image_sub(private_nh, depth_topic.c_str(), 5);
message_filters::Subscriber<CameraInfo> info_sub(private_nh, depth_info.c_str(), 5);
people_pub = private_nh.advertise<clf_perception_vision_msgs::ExtendedPeople>(out_topic.c_str(), 1);
TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 5);
sync.registerCallback(boost::bind(&DepthLookup::depth_callback, this ,_1, _2));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_listener");
ros::NodeHandle nh;
//image_transport::ImageTransport it(nh);
//image_transport::Subscriber sub = it.subscribe("/camera/rgb/image_color", 1, imageCallback);
message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_policy;
message_filters::Synchronizer<sync_policy> sync(sync_policy(10), image_sub, depth_sub);
sync.registerCallback(boost::bind(&imageCallback,_1,_2));
pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
ros::spin();
}
