示例#1
0
  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();
  }