void start(const Mode mode)
{
this->mode = mode;
running = true;
std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";
// std::cout << "test1..." << std::endl;
image_transport::TransportHints hints("raw");
subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);
// std::cout << "test2..." << std::endl;
syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
spinner.start();
std::cout << "waiting for kinect2_bridge..." << std::endl;
std::chrono::milliseconds duration(1);
while(!updateImage || !updateCloud)
{
if(!ros::ok())
{
return;
}
std::this_thread::sleep_for(duration);
}
std::cout << "kinect2_bridge is running.." << std::endl;
cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>());
cloud->height = color.rows;
cloud->width = color.cols;
cloud->is_dense = false;
cloud->points.resize(cloud->height * cloud->width);
createLookup(this->color.cols, this->color.rows);
//
// // Create the filtering object
// // pcl::PassThrough<pcl::PointXYZRGBA> pass;
// // pass.setInputCloud (cloud);
// // pass.setFilterFieldName ("z");
// // pass.setFilterLimits (0.0, 1.0);
// // //pass.setFilterLimitsNegative (true);
// // pass.filter (*cloud);
cloudViewer();
}
bool loadRTTPlugin(RTT::TaskContext* c){
log(Info)<<"Initializing ROS node"<<endlog();
if(!ros::isInitialized()){
int argc =__os_main_argc();
char ** argv = __os_main_argv();
ros::init(argc,argv,"rtt",ros::init_options::AnonymousName);
if(ros::master::check())
ros::start();
else{
log(Warning)<<"'roscore' is not running: no ROS functions will be available."<<endlog();
ros::shutdown();
return true;
}
}
static ros::AsyncSpinner spinner(1); // Use 1 threads
spinner.start();
log(Info)<<"ROS node spinner started"<<endlog();
loadROSService();
return true;
}
void startRecord()
{
std::cout << "Controls:" << std::endl
<< " [ESC, q] - Exit" << std::endl
<< " [SPACE, s] - Save current frame" << std::endl
<< " [l] - decreas min and max value for IR value rage" << std::endl
<< " [h] - increas min and max value for IR value rage" << std::endl
<< " [1] - decreas min value for IR value rage" << std::endl
<< " [2] - increas min value for IR value rage" << std::endl
<< " [3] - decreas max value for IR value rage" << std::endl
<< " [4] - increas max value for IR value rage" << std::endl;
image_transport::TransportHints hints("compressed");
image_transport::TransportHints hintsIr("compressed");
image_transport::TransportHints hintsDepth("compressedDepth");
subImageColor = new image_transport::SubscriberFilter(it, topicColor, 4, hints);
subImageIr = new image_transport::SubscriberFilter(it, topicIr, 4, hintsIr);
subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, 4, hintsDepth);
sync = new message_filters::Synchronizer<ColorIrDepthSyncPolicy>(ColorIrDepthSyncPolicy(4), *subImageColor, *subImageIr, *subImageDepth);
sync->registerCallback(boost::bind(&Recorder::callback, this, _1, _2, _3));
spinner.start();
}