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, "save_scan");
ros::NodeHandle n;
ros::NodeHandle pn("~");
pn.param<std::string>("folder", folder, std::string(getenv("HOME")) + std::string("/.ros/mapping_refinement"));
boost::filesystem::path dir(folder);
if (!boost::filesystem::exists(dir) && !boost::filesystem::create_directory(dir)) {
ROS_ERROR("Failed to create directory %s", folder.c_str());
return 0;
}
ROS_INFO("Saving scans in %s", folder.c_str());
std::string camera;
pn.param<std::string>("camera", camera, std::string("head_xtion"));
std::string pcd_input = std::string("/") + camera + std::string("/depth_registered/points");
std::string info_input = std::string("/") + camera + std::string("/rgb/camera_info");
listener = new tf::TransformListener();
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Subscriber<sensor_msgs::PointCloud2> pcd_sub(n, pcd_input, 1);
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub(n, info_input, 1);
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), pcd_sub, info_sub);
sync.registerCallback(&callback);
// register a service for saving a scan
save = false;
num = 0;
ros::ServiceServer service = n.advertiseService("save_scan_service", &srv_callback);
ros::spin();
return 0;
}
