void setFloor(pcl::visualization::PCLVisualizer *viewer, string svm_filename, float voxel_size, Eigen::Matrix3f rgb_intrinsics_matrix, float min_height, float max_height)
{
	callback_args cb_args;
	PointCloudT::Ptr clicked_points_3d(new PointCloudT);
	cb_args.clicked_points_3d = clicked_points_3d;
	cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
	viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);

	cout << "Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT";
	viewer->addText("Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT", 250, 300, 20, 1, 1, 1);
	viewer->addText("Nastepnie nacisnij klawisz Q", 250, 250, 20, 1, 1, 1);

	// Spin until 'Q' is pressed:
	viewer->spin();
	std::cout << "Gotowe." << std::endl;
	

	cloud_mutex.unlock();

	// Ground plane estimation:
	ground_coeffs.resize(4);
	std::vector<int> clicked_points_indices;

	for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
		clicked_points_indices.push_back(i);
		pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
		model_plane.computeModelCoefficients(clicked_points_indices, ground_coeffs);
		std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

		// Create classifier for people detection:
		pcl::people::PersonClassifier<pcl::RGB> person_classifier;
		person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

		// People detection app initialization:
		// people detection object
		people_detector.setVoxelSize(voxel_size);                        // set the voxel size
		people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
		people_detector.setClassifier(person_classifier);                // set person classifier
		//	  people_detector.setHeightLimits(min_height, max_height);         // set person height limits
			people_detector.setPersonClusterLimits(min_height, max_height, 0.1, 8.0); // set person height limits
		floor_tagged = true;
}

void LaserScan_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
  /* Using input from the front laser scanner check for two things:
  1. Whether there are not obstacles in front of the robot (close range)
  2. To which direction should the robot face to drive without hitting an obstacle (mid-range)
  */
  
  

  /*
  Number of range measurements = 512
  Angle of each sensor = about 0.35 degrees (0.00613592332229 radians)
  Field of view = about 180 degrees (3.141592741 radians)
  */

  // look for a clear corridor of 30 degrees and 1.5 meters ahead
  int numPixels = 512;
  float pixelAngle = 0.35; // in degrees


  if(numPixels!=msg->ranges.size())
    cout << "Warning: number of Hokuyo laser sensors different than usual (512)" << endl;


  // check that we don't run into any thing
  float safeDistance = 0.40; // 40cm
  float safetyAngle = 120; // 120 degrees
  int numClearPixelsPerSide = (safetyAngle/2)/pixelAngle;
  if(!canAdvance(msg->ranges,numClearPixelsPerSide,safeDistance)) {
    cout << "Stop, cannot advance" << endl;
	dir_value_mutex.lock();
	can_move = false;
	dir_value_mutex.unlock();
	} else {
		dir_value_mutex.lock();
		can_move = true;
		dir_value_mutex.unlock();
	}

  // find corridor direction
  int numCorridorPixels = 120; // 120*0.00613592332229*180/pi=42 degrees
  float clearDistance = 1.5; // 1.5 meters ahead
  float degreesToTurn = findClearCorridor(msg->ranges,numCorridorPixels,clearDistance,pixelAngle);

  // negative means to turn right
  dir_value_mutex.lock();
  direction = degreesToTurn;
  dir_value_mutex.unlock();
  cout << "Degrees to turn = " << degreesToTurn << endl;
}

	modbus_t *getContext() {
		update_mutex.lock();
		return ctx;
	}

void callbackClusteredClouds(const clustered_clouds_msgs::ClusteredCloudsConstPtr& msg)
{
#if PUBLISH_TRANSFORM
  static tf::TransformBroadcaster tf_br;
#endif

  g_marker_array.markers.clear();
  g_marker_id = 0;

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<tf::Vector3> hand_positions, arm_directions;
  mutex_config.lock();
  for(size_t i = 0; i < msg->clouds.size(); i++)
  {
    bool cloud_with_rgb_data = false;
    std::string field_list = pcl::getFieldsList (msg->clouds[i]);    
    if(field_list.rfind("rgb") != std::string::npos)
    {
      cloud_with_rgb_data = true;
    }



    if(cloud_with_rgb_data)
    {
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr hand_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
      //pcl::PointCloud<pcl::PointXYZRGB>::Ptr finger_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
      tf::Vector3 hand_position, arm_position, arm_direction;
      bool found = checkCloud<pcl::PointXYZRGB>(msg->clouds[i],
                                                hand_cloud,
                                                //finger_cloud,
                                                msg->header.frame_id,
                                                hand_position,
                                                arm_position,
                                                arm_direction);
      if(found)
      {
        hand_positions.push_back(hand_position);
        arm_directions.push_back(arm_direction);
        *cloud_out += *hand_cloud;
      }
    }
    //else
    //{
      //checkCloud<pcl::PointXYZ>(msg->clouds[i], msg->header.frame_id);
    //}
  }
  mutex_config.unlock();



  if(hand_positions.size() > g_hand_trackers.size())
  {
    std::vector<tf::Vector3> hand_positions_tmp = hand_positions;
    cv::Mat result;
    for (size_t i = 0; i < g_hand_trackers.size(); i++)
    {
      result = g_hand_trackers[i].first.lastResult();
      tf::Vector3 point1(result.at<float>(0), result.at<float>(1), result.at<float>(2));
      int index = closestPoint(point1, hand_positions_tmp);
      //remove
      hand_positions_tmp.erase(hand_positions_tmp.begin() + index);
    }

    //add new tracker
    for(size_t i = 0; i < hand_positions_tmp.size(); i++)
    {
      KalmanFilter3d tracker;
      cv::Point3f point(hand_positions_tmp[i].x(),
                        hand_positions_tmp[i].y(),
                        hand_positions_tmp[i].z());
      tracker.initialize(1.0 / 30.0, point, 1e-2, 1e-1, 1e-1);
      TrackerWithHistory t;
      t.first = tracker;
      g_hand_trackers.push_back(t);
    }
  }

  cv::Mat result;
  std::vector<tf::Vector3> results;
  interaction_msgs::ArmsPtr arms_msg(new interaction_msgs::Arms);
  arms_msg->arms.resize(g_hand_trackers.size());
  for (size_t i = 0; i < g_hand_trackers.size(); i++)
  {
    g_hand_trackers[i].first.predict(result);
    results.push_back(tf::Vector3(result.at<float>(0), result.at<float>(1), result.at<float>(2)));
    arms_msg->arms[i].hand.rotation.w = 1;
  }

  int index;
  cv::Point3f measurement;
  tf::Quaternion q_hand;
  Eigen::Quaternionf q;
  tf::Quaternion q_rotate;
  q_rotate.setEuler(0, 0, M_PI);
  int last_index = -1;
  for(size_t i = 0; i < hand_positions.size(); i++)
  {
    index = closestPoint(hand_positions[i], results);
    if(last_index == index)
//.........这里部分代码省略.........

void threadHandler(char driveLtr)
{
    /* Lock the mutex so we don't try and access the database at the same time */
    gMutex.lock();

    bool authorized = false;

    /* Lock the USB drive */
    UsbOps ops;
    ops.lockUSB(driveLtr);

    /* Query authorized devices */
    Sql sql;
    if (!sql.dbConnect((char*)Paths::getDatabasePath().c_str(), false))
    {
        ErrorLog::logErrorToFile("*CRITICAL*", "Unable to open authorized drives database!");
        ops.ejectUSB();
        gMutex.unlock();
        return;
    }
    std::vector<authedDrive> drvs;
    sql.queryAuthedDrives(&drvs);

    /* Get the serial key of the device */
    UsbKey usbKey;
    UsbKeyhdr hdr;

    ops.unlockUSB();
    usbKey.getUsbKeyHdr(&hdr, driveLtr);
    ops.lockUSB(driveLtr);

    /* Check if the serial exists in the database */
    for (std::vector<authedDrive>::iterator it = drvs.begin(); it != drvs.end(); it++)
    {
        std::cout << it->serial.c_str() << " " << hdr.serialkey.c_str() << std::endl;
        if (it->serial.compare(hdr.serialkey) == 0)
        {
            authorized = true;
            break;
        }
    }

    ops.unlockUSB();

    /* Log media insertion event */

    AccessLog *log = new AccessLog();
    log->createLogStruct(&log->logUSBStruct, driveLtr, (char*)hdr.serialkey.c_str());
    log->logUSBStruct.accepted = authorized;


    /* Get config settings, check if remote SQL is enabled */
    ConfigParser configParser((char*)Paths::getConfigPath().c_str());
    if (configParser.getValue("SQLenabled") == "1")
    {
        /* SQL enabled = true */
        log->logUsbDrive(log->logUSBStruct, true);
    }
    else
        log->logUsbDrive(log->logUSBStruct, false);

    /* If not authorized, eject it! */
    if (!authorized)
    {
        ops.lockUSB(driveLtr);
        ops.ejectUSB();
    }

    /* Check for viruses here... */

    gMutex.unlock();
    delete log;
}

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);
  if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
    cout << "Setting support size to "<<support_size<<"m.\n";
  if (pcl::console::parse (argc, argv, "-i", min_interest_value) >= 0)
    cout << "Setting minimum interest value to "<<min_interest_value<<".\n";
  if (pcl::console::parse (argc, argv, "-t", max_no_of_threads) >= 0)
    cout << "Setting maximum number of threads to "<<max_no_of_threads<<".\n";
  
  pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image");
  
  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.addCoordinateSystem (1.0f);
  viewer.setBackgroundColor (1, 1, 1);
  
  viewer.initCameraParameters ();
  viewer.camera_.pos[0] = 0.0;
  viewer.camera_.pos[1] = -0.3;
  viewer.camera_.pos[2] = -2.0;
  viewer.camera_.focal[0] = 0.0;
  viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
  viewer.camera_.focal[2] = 1.0;
  viewer.camera_.view[0] = 0.0;
  viewer.camera_.view[1] = -1.0;
  viewer.camera_.view[2] = 0.0;
  viewer.updateCamera ();
  
  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 boost::shared_ptr<openni_wrapper::DepthImage>&) > 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";
  
  boost::shared_ptr<pcl::RangeImagePlanar> range_image_planar_ptr (new pcl::RangeImagePlanar);
  pcl::RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
  
  pcl::RangeImageBorderExtractor range_image_border_extractor;
  pcl::NarfKeypoint narf_keypoint_detector;
  narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
  narf_keypoint_detector.getParameters ().support_size = support_size;
  narf_keypoint_detector.getParameters ().max_no_of_threads = max_no_of_threads;
  narf_keypoint_detector.getParameters ().min_interest_value = min_interest_value;
  //narf_keypoint_detector.getParameters ().calculate_sparse_interest_image = false;
  //narf_keypoint_detector.getParameloadters ().add_points_on_straight_edges = true;
  
  pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>& keypoints_cloud = *keypoints_cloud_ptr;
  
  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
    range_image_widget.spinOnce ();  // process GUI 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;
      
      //unsigned long time_stamp = depth_image_ptr->getTimeStamp ();
      //int frame_id = depth_image_ptr->getFrameID ();
      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;
//.........这里部分代码省略.........

static Result execute_testcase (struct InfoForExecutor &ie)
{
	pthread_t tid = pthread_self ();
	LoggerPtr logger = Logger :: getLogger (HARNESS_LOGGER_NAME);

    string tid_str = iTos ((uint64_t)tid);
    string worker_tag = LOGGER_TAG_WORKER;
	if (ie.selftesting == false)
		worker_tag = worker_tag + '[' + tid_str + ']';
	else
		worker_tag = worker_tag + "[]";

	LogString saved_context;
    LOGGER_PUSH_NDCTAG (worker_tag);

	int retValue;
	Result result = RESULT_SYSTEM_EXCEPTION;
	string result_str = "FAILED";
	long int sTime = 0, eTime = 0;
	string failureReason("Test case execution failed, Check log file.");

	scidbtestharness::executors::ExecutorFactory f;
	scidbtestharness :: executors :: Executor *caseexecutor = 0;
	try
	{
		prepare_filepaths (ie, true);

		string actualrfile_backup = ie.actual_rfile + ".bak";
		string difffile_backup = ie.diff_file + ".bak";
		string logfile_backup = ie.log_file + ".bak";
		if (ie.keepPreviousRun)
		{
			/* rename all the files with extra extension .bak */
			if (Is_regular (ie.actual_rfile))
			{
				bfs::remove (actualrfile_backup);	
				bfs::rename (ie.actual_rfile, actualrfile_backup);	
			}
			if (Is_regular (ie.diff_file))
			{
				bfs::remove (difffile_backup);	
				bfs::rename (ie.diff_file, difffile_backup);	
			}
			if (Is_regular (ie.log_file))
			{
				bfs::remove (logfile_backup);	
				bfs::rename (ie.log_file, logfile_backup);	
			}
		}
		else
		{
			/* remove actual files as well as backup files */
			bfs::remove (ie.actual_rfile);
			bfs::remove (ie.diff_file);
			bfs::remove (ie.log_file);
			bfs::remove (actualrfile_backup);	
			bfs::remove (difffile_backup);	
			bfs::remove (logfile_backup);	
		}

		if ((caseexecutor = f.getExecutor (g_executor_type)) == NULL)
		{
			throw ConfigError (FILE_LINE_FUNCTION, ERR_CONFIG_INVALID_EXECUTOR_TYPE);
		}
		sTime = time (0);
		ie.logger_name = tid_str;

		complete_es_mutex.lock();   /* lock */
		g_test_count++;
		ie.test_sequence_number = g_test_count;
		ie.tid = tid;
		ie.testID = converttoid (ie.rootDir, ie.tcfile);
		complete_es_mutex.unlock(); /* unlock */

		/* test case execution by the executor.
		 * Now all the exceptions from 'defaultexecutor' are being locally handled by it and
		 * only return code SUCCESS/FAILURE is only being returned. Hence resolving the issue of crash
		 * during pthread_mutex_destroy() at the end of harness execution. */
		retValue = caseexecutor->execute (ie);
		eTime = time (0);
		delete caseexecutor;
		caseexecutor = 0;

		LOG4CXX_DEBUG (logger, "Executor returned : " << (retValue == SUCCESS ? "SUCCESS" : "FAILURE"));
		/* lock */
		complete_es_mutex.lock();

		if (retValue == SUCCESS)
		{
			if (ie.record)                 // PASS
			{
				bfs::remove (ie.expected_rfile);	
				bfs::rename (ie.actual_rfile, ie.expected_rfile);	
				result = RESULT_RECORDED;
				result_str = "RECORDED";
				failureReason = "";
				complete_es.testcasesPassed++;
			}
			else
			{
//.........这里部分代码省略.........

	void lock()
	{
		mutex.lock();
	}

namespace pei
{
    static boost::mutex sMutex;
    static bool isInitialized(false);

    //////////////////////////////////////////////////////////////////////////
    //

    TtFontAsset::TtFontAsset( const char* name, size_t size )
        : FontAsset( name, size )
        , m_FontName(name)
        , m_FontSize(size)
    {
        sMutex.lock();
        if ( !isInitialized ) {
            pei::TtFontFactory().Init();
            isInitialized = true;
        }
        sMutex.unlock();
    }

    TtFontAsset::TtFontAsset( const TtFontAsset& fa )
        : FontAsset(fa.m_FontName.c_str(), fa.m_FontSize)
        , m_FontName(fa.m_FontName)
        , m_FontSize(fa.m_FontSize)
        , m_TtFont(fa.m_TtFont)
    {
    }

    TtFontAsset::~TtFontAsset()
    {
    }

    bool TtFontAsset::CanHandle()
    {
        m_TtFont = pei::TtFontFactory().CreateTtFont( m_FontName.c_str(), m_FontSize );
        return m_TtFont != NULL;
    }

    bool TtFontAsset::Open()
    {
        if ( m_TtFont == NULL ) {
            m_TtFont = pei::TtFontFactory().CreateTtFont( m_FontName.c_str(), m_FontSize );
        }
        return m_TtFont != NULL;
    }

    void TtFontAsset::Close()
    {
        m_TtFont = TtFontPtr();
    }

    int TtFontAsset::GetMemoryUsage()
    {
        return 0;
    }

    std::string TtFontAsset::GetFormatString() const
    {
        return ".ttf";
    }

    const char* TtFontAsset::GetResourceDirectory() const
    {
        return "";
    }

    SurfacePtr TtFontAsset::RenderText( const char *text, const pei::Color& color /*= pei::Color(255,255,255)*/ )
    {
#ifdef _HAS_SDL_TTF_
        if ( m_TtFont ) {
            // this is a hack! We swap r & g and correct the color format manually
            // SDL_ttf renders ARGB, but we need ABGR (or LE RGBA)
            SDL_Color c = { color.b, color.g, color.r, color.a };
            SDL_Surface * s = TTF_RenderText_Blended( (TTF_Font*)m_TtFont->GetFontHandle(), text, c );
            // swap r&g in the format description
            s->format->Rmask = 0x000000ff; s->format->Rshift = 0;
            s->format->Bmask = 0x00ff0000; s->format->Bshift = 16;
            return SurfacePtr( new pei::SDL::SurfaceWrapper(s) );
        }
#endif
        return SurfacePtr();
    }

    void TtFontAsset::RenderText( SurfacePtr dest, int& x, int& y, const char* text, const pei::Color& color /*= pei::XColor(255,255,255)*/ )
    {
        //     assert ( pFont );
        if (!m_TtFont || dest.get() == NULL ) {
            return;
        }
#ifdef _HAS_SDL_TTF_
        SDL_Surface *text_surface = NULL;
        // this is a hack! We swap r & g and correct the color format manually
        // SDL_ttf renders ARGB, but we need ABGR (or LE RGBA) to match RGBA texture format - performace reasons!
        SDL_Color c = { color.b, color.g, color.r, color.a };
        if ( (text_surface = TTF_RenderText_Blended( (TTF_Font*)m_TtFont->GetFontHandle(), text, c )) != NULL )
        {
            // swap r&g in the format description
            text_surface->format->Rmask = 0x000000ff; text_surface->format->Rshift = 0;
            text_surface->format->Bmask = 0x00ff0000; text_surface->format->Bshift = 16;
//.........这里部分代码省略.........

	void unlock()
	{
		mutex.unlock();
	}

void callbackConfig(pcl_hand_arm_detector::PclHandArmDetectorConfig &config, uint32_t level)
{
  mutex_config.lock();
  g_config = config;
  mutex_config.unlock();
}

// Used for keeping track on things that use multithreading
void reportReady2()
{
    mutex_lock.lock();
    threads_ready2++;
    mutex_lock.unlock();
}

int main (int argc, char** argv)
{
  if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
        return print_help();

  // Algorithm parameters:
  std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
  float min_confidence = -1.5;
  float min_height = 1.3;
  float max_height = 2.3;
  float voxel_size = 0.06;
  Eigen::Matrix3f rgb_intrinsics_matrix;
  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics

  // Read if some parameters are passed from command line:
  pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
  pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
  pcl::console::parse_argument (argc, argv, "--min_h", min_height);
  pcl::console::parse_argument (argc, argv, "--max_h", max_height);

  // Read Kinect live stream:
  PointCloudT::Ptr cloud (new PointCloudT);
  bool new_cloud_available_flag = false;
  pcl::Grabber* interface = new pcl::OpenNIGrabber();
  boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
      boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag);
  interface->registerCallback (f);
  interface->start ();

  // Wait for the first frame:
  while(!new_cloud_available_flag) 
    boost::this_thread::sleep(boost::posix_time::milliseconds(1));
  new_cloud_available_flag = false;

  cloud_mutex.lock ();    // for not overwriting the point cloud

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:
  viewer.spin();
  std::cout << "done." << std::endl;
  
  cloud_mutex.unlock ();    

  // Ground plane estimation:
  Eigen::VectorXf ground_coeffs;
  ground_coeffs.resize(4);
  std::vector<int> clicked_points_indices;
  for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
    clicked_points_indices.push_back(i);
  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

  // Initialize new viewer:
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Create classifier for people detection:  
  pcl::people::PersonClassifier<pcl::RGB> person_classifier;
  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:
  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
  people_detector.setClassifier(person_classifier);                // set person classifier
  people_detector.setHeightLimits(min_height, max_height);         // set person classifier
//  people_detector.setSensorPortraitOrientation(true);             // set sensor orientation to vertical

  // For timing:
  static unsigned count = 0;
  static double last = pcl::getTime ();

  // Main loop:
  while (!viewer.wasStopped())
  {
    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

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

	void releaseContext() {
		update_mutex.unlock();
	}

void free_adjlst(struct adjlst<edge_t> * adjl) {
    free_mutex.lock();
    free(adjl->edges);
    free(adjl);
    free_mutex.unlock();
}

// spin until all counters reach at least max
void MyInfo::waitThreadFunc(
            robot_interaction::LockedRobotState* locked_state,
            int** counters,
            int max)
{
  bool go = true;
  while(go)
  {
    go = false;
    cnt_lock_.lock();
    for (int i = 0 ; counters[i] ; ++i)
    {
      if (counters[i][0] < max)
        go = true;
    }
    cnt_lock_.unlock();

    checkState(*locked_state);
    checkState(*locked_state);
  }
  cnt_lock_.lock();
  quit_ = true;
  cnt_lock_.unlock();
}

void TimerHandler(
	const boost::system::error_code & error,
	boost::shared_ptr< boost::asio::deadline_timer > timer,
	boost::shared_ptr< boost::asio::io_service::strand > strand
)
{
	if( error )
	{
		global_stream_lock.lock();
		std::cout << "[" << boost::this_thread::get_id()
			<< "] Error: " << error << std::endl;
		global_stream_lock.unlock();
	}
	else
	{
		std::cout << "[" << boost::this_thread::get_id()
			<< "] TimerHandler " << std::endl;

		timer->expires_from_now( boost::posix_time::seconds( 1 ) );
		timer->async_wait(
			strand->wrap( boost::bind( &TimerHandler, _1, timer, strand ) )
		);
	}
}

void cmd_vel_received(const geometry_msgs::Twist::ConstPtr& cmd_vel)
{
    //roomba->drive(cmd_vel->linear.x, cmd_vel->angular.z);
    double linear_speed = cmd_vel->linear.x; // m/s
    double angular_speed = cmd_vel->angular.z; // rad/s
    ROS_INFO("Velocity received: %.2f %.2f", linear_speed, angular_speed);

    //int left_speed_mm_s = (int)((linear_speed-ROBOT_AXLE_LENGTH*angular_speed/2)*1e3);		// Left wheel velocity in mm/s
    //int right_speed_mm_s = (int)((linear_speed+ROBOT_AXLE_LENGTH*angular_speed/2)*1e3);	// Right wheel velocity in mm/s

    orcp2::ORCP2 orcp(drive_serial);

    drive_serial_write.lock();

    if( linear_speed <  std::numeric_limits<double>::epsilon() &&
            linear_speed > -std::numeric_limits<double>::epsilon() ) {
        // zero linear speed - turn in place
        uint16_t speed = speed_koef * angular_speed * wheel_track  * gear_reduction / 2.0;
        orcp.drive_4wd(speed, -speed, speed, -speed);
    }
    else if( angular_speed <  std::numeric_limits<double>::epsilon() &&
             angular_speed > -std::numeric_limits<double>::epsilon() ) {
        // zero angular speed - pure forward/backward motion
        orcp.motorsWrite(speed_koef * linear_speed * wheel_track  * gear_reduction / 2.0);
    }
    else {
        // Rotation about a point in space
        //$TODO
        uint16_t left = speed_koef * linear_speed - angular_speed * wheel_track  * gear_reduction / 2.0;
        uint16_t right = speed_koef * linear_speed + angular_speed * wheel_track  * gear_reduction / 2.0;

        orcp.drive_4wd(left, right, left, right);
    }

    drive_serial_write.unlock();
}

void ServiceImpl::startSimControlTask()
{
	assert(serviceThread == NULL);

	//get module handles
	initModuleHandles();
	
	//get cmd arguments
	s_vpi_vlog_info vlog_info;
	vpi_get_vlog_info(&vlog_info);
	parseOptions(vlog_info.argc, vlog_info.argv);

	simMutex.lock();

	//start server thread
	serviceThread = new boost::thread(boost::bind(&ServiceImpl::serverThreadProc, this));
}

void WorkerThread( boost::shared_ptr< boost::asio::io_service > io_service )
{
	global_stream_lock.lock();
	std::cout << "[" << boost::this_thread::get_id()
		<< "] Thread Start" << std::endl;
	global_stream_lock.unlock();

	while( true )
	{
		try
		{
			boost::system::error_code ec;
			io_service->run( ec );
			if( ec )
			{
				global_stream_lock.lock();
				std::cout << "[" << boost::this_thread::get_id()
					<< "] Error: " << ec << std::endl;
				global_stream_lock.unlock();
			}
			break;
		}
		catch( std::exception & ex )
		{
			global_stream_lock.lock();
			std::cout << "[" << boost::this_thread::get_id()
				<< "] Exception: " << ex.what() << std::endl;
			global_stream_lock.unlock();
		}
	}

	global_stream_lock.lock();
	std::cout << "[" << boost::this_thread::get_id()
		<< "] Thread Finish" << std::endl;
	global_stream_lock.unlock();
}