C++ mutex::try_lock方法代码示例

void f()
{
#if defined BOOST_THREAD_USES_CHRONO
  time_point t0 = Clock::now();
  BOOST_TEST(!m.try_lock());
  BOOST_TEST(!m.try_lock());
  BOOST_TEST(!m.try_lock());
  while (!m.try_lock())
    ;
  time_point t1 = Clock::now();
  m.unlock();
  ns d = t1 - t0 - ms(250);
  // This test is spurious as it depends on the time the thread system switches the threads
  BOOST_TEST(d < ns(50000000)+ms(1000)); // within 50ms
#else
  //time_point t0 = Clock::now();
  //BOOST_TEST(!m.try_lock());
  //BOOST_TEST(!m.try_lock());
  //BOOST_TEST(!m.try_lock());
  while (!m.try_lock())
    ;
  //time_point t1 = Clock::now();
  m.unlock();
  //ns d = t1 - t0 - ms(250);
  // This test is spurious as it depends on the time the thread system switches the threads
  //BOOST_TEST(d < ns(50000000)+ms(1000)); // within 50ms
#endif

}

void f()
{
  time_point t0 = Clock::now();
  BOOST_TEST(!m.try_lock());
  BOOST_TEST(!m.try_lock());
  BOOST_TEST(!m.try_lock());
  while (!m.try_lock())
    ;
  time_point t1 = Clock::now();
  m.unlock();
  ns d = t1 - t0 - ms(250);
  // This test is spurious as it depends on the time the thread system switches the threads
  BOOST_TEST(d < ns(50000000)+ms(1000)); // within 50ms
}

/* ---[ */
int
main(int argc, char** argv)
{
	if (argc > 1)
	{
		for (int i = 1; i < argc; i++)
		{
			if (std::string(argv[i]) == "-h")
			{
				printHelp(argc, argv);
				return (-1);
			}
		}
	}

	std::string device_id = "";
	pcl::console::parse_argument(argc, argv, "-dev", device_id);

	cld.reset(new pcl::visualization::PCLVisualizer(argc, argv, "OpenNI Viewer"));
	cld->setBackgroundColor(0, 0, 0);//背景を白色（255,255,255）に。
	cld->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "OpenNICloud");
	cld->addCoordinateSystem(1.0);
	cld->initCameraParameters();

	std::string mouseMsg3D("Mouse coordinates in PCL Visualizer");
	std::string keyMsg3D("Key event for PCL Visualizer");
	cld->registerMouseCallback(&mouse_callback, (void*)(&mouseMsg3D));
	cld->registerKeyboardCallback(&keyboard_callback, (void*)(&keyMsg3D));

	pcl::io::loadPCDFile("data\\robot\\raw_0.pcd", *g_cloud);

	bool cld_init = false;
	// Loop
	while (!cld->wasStopped())
	{
		// Render and process events in the two interactors
		cld->spinOnce();

		// Add the cloud
		if (g_cloud && cld_mutex.try_lock())
		{
			if (!cld_init)
			{
				cld->getRenderWindow()->SetSize(g_cloud->width, g_cloud->height);
				cld->getRenderWindow()->SetPosition(g_cloud->width, 0);
				cld_init = !cld_init;
			}

			if (!cld->updatePointCloud(g_cloud, "OpenNICloud"))
			{
				cld->addPointCloud(g_cloud, "OpenNICloud");
				cld->resetCameraViewpoint("OpenNICloud");
			}

			cld_mutex.unlock();
		}
		boost::this_thread::sleep(boost::posix_time::microseconds(100));
	}

}

 void
 depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
 {
   if (depth_image_mutex.try_lock ())
   {
     depth_image_ptr = depth_image;
     depth_image_mutex.unlock ();
     received_new_depth_data = true;
   }
 }

 void
 depth_image_cb (const openni_wrapper::DepthImage::Ptr& depth_image)
 {
   if (depth_image_mutex.try_lock ())
   {
     depth_image_ptr = depth_image;
     depth_image_mutex.unlock ();
     received_new_depth_data = true;
   }
 }

/* ---[ */
int
main (int argc, char** argv)
{
  srand (time (0));

  if (argc > 1)
  {
    for (int i = 1; i < argc; i++)
    {
      if (std::string (argv[i]) == "-h")
      {
        printHelp (argc, argv);
        return (-1);
      }
    }
  }

  p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "OpenNI Viewer"));

  std::string device_id = "";
  pcl::console::parse_argument (argc, argv, "-dev", device_id);

  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);

  EventHelper h;
  boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &h, _1);
  boost::signals2::connection c1 = interface->registerCallback (f);

  interface->start ();
  while (!p->wasStopped ())
  {
    p->spinOnce ();
    if (new_cloud && mutex_.try_lock ())
    {
      new_cloud = false;
      if (g_cloud)
      {
        FPS_CALC ("drawing");
        pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler (g_cloud);
        if (!p->updatePointCloud (g_cloud, handler, "OpenNICloud"))
        {
          p->addPointCloud (g_cloud, handler, "OpenNICloud");
          p->resetCameraViewpoint ("OpenNICloud");
        }
      }
      mutex_.unlock ();
    }
    else
      boost::this_thread::sleep (boost::posix_time::microseconds (1000));
  }

  interface->stop ();
}

  void 
  cloud_cb (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud)
  {
    uint64_t timestamp;
#ifdef USE_ROS
    timestamp = cloud->header.stamp.toNSec() / 1000; //Microseconds
#else
    timestamp = cloud->header.stamp;
#endif //USE_ROS
    if (timestamp > 0)
      PCL_INFO ("Acquired cloud with timestamp of %lu\n", timestamp);
    if (mutex_.try_lock ())
    {
      cloud_ = cloud;
      mutex_.unlock ();
    }
  }

void hpFrequentThread(
    long start,
    long stop,
    std::vector<Conjunction> const& conjunctions,
    std::map<Rule, OrderedCover> const& basicCovers,
    OrderedCover const& trueCover,
    double const treshold,
    std::vector<Conjunction>& result,
    boost::mutex& mutex
    )
{
    std::vector<Conjunction> _result;
    _result.reserve(stop-start);
    auto start_i = conjunctions.begin();
    auto stop_i  = conjunctions.begin();
    std::advance(start_i, start);
    std::advance(stop_i, stop);
    long counter = 0;
    for (auto i=start_i ; i!=stop_i ; ++i) {
        OrderedCover _ordc(conjunctionCover(*i, basicCovers));
        //auto xxx = _ordc.metrics(trueCover);
        //printf("%d %d %d %d\n", xxx.tp(), xxx.fp(), xxx.tn(), xxx.fn());
        //printf("%lf\n", _ordc.metrics(trueCover).fprate());
        auto m = _ordc.metrics(trueCover);
        // also require precision to be good enough as fprate is
        // not reasonable metric anyway
        if (m.fprate() <= treshold && m.precision() > 0) {
            _result.push_back(*i);
            counter += 1;
        }
        // try copying intermediate results
        if (counter % 256 == 0) {
            if (mutex.try_lock()) {
                std::copy(_result.begin(), _result.end(),
                          std::back_inserter(result));
                mutex.unlock();
                _result.clear();
            }
        }
    }
    // copy to master _result vector
    mutex.lock();
    std::copy(_result.begin(), _result.end(), std::back_inserter(result));
    mutex.unlock();
}

void runThread(int thread_id, boost::function<S()> threadFn, S& returnValue)
{
  /* Uncomment line to delay first thread, useful to unearth errors/debug */
  // if(thread_id == 0) { sleep(1); }
  returnValue = threadFn();

  if( mutex_done.try_lock() ) {
    if(global_flag_done == false) 
    {
      {
        boost::lock_guard<boost::mutex> lock(mutex_main_wait);
        global_winner = thread_id;
        global_flag_done = true;
      }
      condition_var_main_wait.notify_all(); // we want main thread to quit
    }
    mutex_done.unlock();
  }
}

////////////////////////////////////////////////////////////////////////////////
// high recall
////////////////////////////////////////////////////////////////////////////////
void hrFrequentThread(
    long start,
    long stop,
    std::vector<Conjunction> const& conjunctions,
    std::map<Rule, OrderedCover> const& basicCovers,
    OrderedCover const& trueCover,
    double const treshold,
    std::vector<Conjunction>& result,
    boost::mutex& mutex
    )
{
    std::vector<Conjunction> _result;
    _result.reserve(stop-start);
    auto start_i = conjunctions.begin();
    auto stop_i  = conjunctions.begin();
    std::advance(start_i, start);
    std::advance(stop_i, stop);
    long counter = 0;
    for (auto i=start_i ; i!=stop_i ; ++i) {
        OrderedCover _ordc(conjunctionCover(*i, basicCovers));
        if (_ordc.metrics(trueCover).recall() >= treshold) {
            _result.push_back(*i);
            counter += 1;
        }
        // try copying intermediate results
        if (counter % 256 == 0) {
            if (mutex.try_lock()) {
                std::copy(_result.begin(), _result.end(),
                          std::back_inserter(result));
                mutex.unlock();
                _result.clear();
            }
        }
    }
    // copy to master _result vector
    mutex.lock();
    std::copy(_result.begin(), _result.end(), std::back_inserter(result));
    mutex.unlock();
}

void mainLoopPlain(pcl::visualization::PCLVisualizer *viewer, PointCloudT::Ptr cloud, bool &new_cloud_available_flag)
{
	while (true)
	{
		if (new_cloud_available_flag && cloud_mutex.try_lock())    // if a new cloud is available
		{
			new_cloud_available_flag = false;

			// Draw cloud and people bounding boxes in the viewer:
			viewer->removeAllPointClouds();
			viewer->removeAllShapes();

			pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
			viewer->addPointCloud<PointT>(cloud, rgb, "input_cloud");

			viewer->spinOnce();

			cloud_mutex.unlock();
		}
	}

}

    void
    run ()
    {
      pcl::Grabber* interface = new pcl::OpenNIGrabber ();

      boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&OpenNIOrganizedMultiPlaneSegmentation::cloud_cb_, this, _1);

      //make a viewer
      pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
      viewer = cloudViewer (init_cloud_ptr);
      boost::signals2::connection c = interface->registerCallback (f);

      interface->start ();

      unsigned char red [6] = {255,   0,   0, 255, 255,   0};
      unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
      unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};

      pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
      ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
      ne.setMaxDepthChangeFactor (0.03f);
      ne.setNormalSmoothingSize (20.0f);

      pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
      mps.setMinInliers (10000);
      mps.setAngularThreshold (0.017453 * 2.0); //3 degrees
      mps.setDistanceThreshold (0.02); //2cm

      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
      size_t prev_models_size = 0;
      char name[1024];

      while (!viewer->wasStopped ())
      {
        viewer->spinOnce (100);

        if (prev_cloud && cloud_mutex.try_lock ())
        {
          regions.clear ();
          pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
          double normal_start = pcl::getTime ();
          ne.setInputCloud (prev_cloud);
          ne.compute (*normal_cloud);
          double normal_end = pcl::getTime ();
          std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;

          double plane_extract_start = pcl::getTime ();
          mps.setInputNormals (normal_cloud);
          mps.setInputCloud (prev_cloud);
          mps.segmentAndRefine (regions);
          double plane_extract_end = pcl::getTime ();
          std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
          std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;

          pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);

          if (!viewer->updatePointCloud<PointT> (prev_cloud, "cloud"))
            viewer->addPointCloud<PointT> (prev_cloud, "cloud");

          removePreviousDataFromScreen (prev_models_size);
          //Draw Visualization
          for (size_t i = 0; i < regions.size (); i++)
          {
            Eigen::Vector3f centroid = regions[i].getCentroid ();
            Eigen::Vector4f model = regions[i].getCoefficients ();
            pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
            pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
                                               centroid[1] + (0.5f * model[1]),
                                               centroid[2] + (0.5f * model[2]));
            sprintf (name, "normal_%lu", i);
            viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);

            contour->points = regions[i].getContour ();
            sprintf (name, "plane_%02zu", i);
            pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
            viewer->addPointCloud (contour, color, name);
            viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
          }
          prev_models_size = regions.size ();
          cloud_mutex.unlock ();
        }
      }

      interface->stop ();
    }

void startRecording(string PCDfilepath, string TRJfilepath, pcl::visualization::PCLVisualizer *viewer, PointCloudT::Ptr &cloud, float dist, bool& new_cloud_available_flag)
{
//	Writer::startWriting();
	
	while (flag3)
	{
		if (new_cloud_available_flag && cloud_mutex.try_lock())    // if a new cloud is available
		{

			new_cloud_available_flag = false;
			// Perform people detection on the new cloud:
			std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
			people_detector.setInputCloud(cloud);
			people_detector.setGround(ground_coeffs);                    // set floor coefficients
			people_detector.compute(clusters);                           // perform people detection
			ground_coeffs = people_detector.getGround();                 // get updated floor coefficients


			// Draw cloud and people bounding boxes in the viewer:
			viewer->removeAllPointClouds();
			viewer->removeAllShapes();

			pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
			viewer->addPointCloud<PointT>(cloud, rgb, "input_cloud");

			unsigned int k = 0;

			centroids_curr.clear();

			for (std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
			{
				if (it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold
				{
					Eigen::Vector3f centroid_coords; //vector containing persons centroid coordinates

					centroid_coords = it->getCenter(); // calculate centroid coordinates

					int person_on_screen; // person number displayed on screen

					person_on_screen = if_same_person(centroids_prev, centroid_coords, dist); // check if current person existed in prev frame

					// add coordinates to vector containing people from current frame

					float x = centroid_coords[0]; // extract x coordinate of centroid
					float y = centroid_coords[1]; // extract y coordinate of centroid
					float z = centroid_coords[2]; // extract z coorfinate of centroid

					PointT pp;
					pp.getVector3fMap() = it->getCenter();

					Eigen::Vector4f centroid_coords_person;
					centroid_coords_person[0] = x;
					centroid_coords_person[1] = y;
					centroid_coords_person[2] = z;
					centroid_coords_person[3] = (float)person_on_screen;
					centroids_curr.push_back(centroid_coords_person);

					// draw theoretical person bounding box in the PCL viewer:
					it->drawTBoundingBox(*viewer, k); // draw persons bounding box

					cout << "tesst";
					//creating text to display near person box
					string tekst = "person ";

					string Result;

					ostringstream convert;

					convert << person_on_screen;

					Result = convert.str();

					tekst = tekst + Result;

					viewer->addText3D(tekst, pp, 0.08); // display text
					k++;

					cout << "-------------";
				}
			}

			if (k == 0)
			{
				empty_in_row++;
				cout << "Empty in a row: " << empty_in_row;
			}
			else empty_in_row = 0;

			if (empty_in_row == 3) {
				cout << "Czyszcze wektor przechowujacy dane o postaciach";
				centroids_prev.clear();
			}

			if (k > 0)centroids_prev = centroids_curr;

			std::cout << k << " people found" << std::endl;
			viewer->spinOnce();

//			Writer::write(cloud, clusters);

//.........这里部分代码省略.........

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
    cout << "Using device id \""<<device_id<<"\".\n";
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = pcl::deg2rad (angular_resolution);
  
  pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image");
  
  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.addCoordinateSystem (1.0f, "global");
  viewer.setBackgroundColor (1, 1, 1);
  
  // Set the viewing pose so that the openni cloud is visible
  viewer.initCameraParameters ();
   viewer.setCameraPosition (0.0, -0.3, -2.0,
                            0.0, -0.3, 1.0,
                            0.0, -1.0, 0.0);
  
  openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
  if (driver.getNumberDevices () > 0)
  {
    for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
    {
      cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
           << ", product: " << driver.getProductName (deviceIdx) << ", connected: "
           << (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
           << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
    }
  }
  else
  {
    cout << "\nNo devices connected.\n\n";
    return 1;
  }
  
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  EventHelper event_helper;
  
  boost::function<void (const openni_wrapper::DepthImage::Ptr&) > f_depth_image =
    boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
  boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
  
  cout << "Starting grabber\n";
  interface->start ();
  cout << "Done\n";
  
  pcl::RangeImagePlanar::Ptr range_image_planar_ptr (new pcl::RangeImagePlanar);
  pcl::RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
  
  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();  // process 3D Viewer events
    range_image_widget.spinOnce ();  // process Image Viewer events
    pcl_sleep (0.01);
    
    bool got_new_range_image = false;
    if (received_new_depth_data && depth_image_mutex.try_lock ())
    {
      received_new_depth_data = false;

      int frame_id = depth_image_ptr->getFrameID ();
      cout << "Visualizing frame "<<frame_id<<"\n";
      const unsigned short* depth_map = depth_image_ptr->getDepthMetaData ().Data ();
      int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
      float center_x = width/2, center_y = height/2;
      float focal_length_x = depth_image_ptr->getFocalLength (), focal_length_y = focal_length_x;
      float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
      float desired_angular_resolution = angular_resolution;
      range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
                                        focal_length_x, focal_length_y, desired_angular_resolution);
      depth_image_mutex.unlock ();
      got_new_range_image = !range_image_planar.points.empty ();
    }
    
    if (!got_new_range_image)
      continue;
    
    // Show range image in the image widget
    range_image_widget.showRangeImage (range_image_planar, 0.5f, 10.0f);
    
    // Show range image in the 3D viewer
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud
      (range_image_planar_ptr, 0, 0, 0);
    if (!viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image"))
      viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
  }

  interface->stop ();
}

void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
    if (!we_have_a_map)
    {
        ROS_INFO("SnapMapICP waiting for map to be published");
        return;
    }

    ros::Time scan_in_time = scan_in->header.stamp;
    ros::Time time_received = ros::Time::now();

    if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) )
    {
        ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec());
        return;
    }


    //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_);
    if (!scan_callback_mutex.try_lock())
        return;

    ros::Duration scan_age = ros::Time::now() - scan_in_time;

    //check if we want to accept this scan, if its older than 1 sec, drop it
    if (!use_sim_time)
        if (scan_age.toSec() > AGE_THRESHOLD)
        {
            //ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD);
            ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() );
            scan_callback_mutex.unlock();

            return;
        }

    count_sc_++;
    //ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_);

    //if (count_sc_ > 10)
    //if (count_sc_ > 10)
    {
        count_sc_ = 0;

        tf::StampedTransform base_at_laser;
        if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time))
        {
            ROS_WARN("Did not get base pose at laser scan time");
            scan_callback_mutex.unlock();

            return;
        }


        sensor_msgs::PointCloud cloud;
        sensor_msgs::PointCloud cloudInMap;

        projector_->projectLaser(*scan_in,cloud);

        we_have_a_scan = false;
        bool gotTransform = false;

        if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05)))
        {
            scan_callback_mutex.unlock();
            ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED");
            return;
        }

        if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05)))
        {
            scan_callback_mutex.unlock();
            ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED");
            return;
        }


        while (!gotTransform && (ros::ok()))
        {
            try
            {
                gotTransform = true;
                listener_->transformPointCloud ("/map",cloud,cloudInMap);
            }
            catch (...)
            {
                gotTransform = false;
                ROS_WARN("DIDNT GET TRANSFORM IN A");
            }
        }

        for (size_t k =0; k < cloudInMap.points.size(); k++)
        {
            cloudInMap.points[k].z = 0;
        }


        gotTransform = false;
        tf::StampedTransform oldPose;
        while (!gotTransform && (ros::ok()))
        {
//.........这里部分代码省略.........