C++ boost::mutex类代码示例

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
			{
//.........这里部分代码省略.........

// 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();
}

void WorkerThread(boost::shared_ptr<boost::asio::io_service> iosvc, int counter)
{
	global_stream_lock.lock();
	std::cout << "Thread " << counter << " Start.\n";
	global_stream_lock.unlock();

	try
	{
		iosvc->run();

		global_stream_lock.lock();
		std::cout << "Thread " << counter << " End.\n";
		global_stream_lock.unlock();
	}
	catch(std::exception & ex)
	{
		global_stream_lock.lock();
		std::cout << "Message: " << ex.what() << ".\n";
		global_stream_lock.unlock();
	}
}

// Check the state.  It should always be valid.
void MyInfo::checkState(robot_interaction::LockedRobotState &locked_state)
{
  robot_state::RobotStateConstPtr s = locked_state.getState();
  
  robot_state::RobotState cp1(*s);

  // take some time
  cnt_lock_.lock();
  cnt_lock_.unlock();
  cnt_lock_.lock();
  cnt_lock_.unlock();
  cnt_lock_.lock();
  cnt_lock_.unlock();

  // check mim_f == joint_f
  EXPECT_EQ(s->getVariablePositions()[MIM_F],
            s->getVariablePositions()[JOINT_F] * 1.5 + 0.1);

  robot_state::RobotState cp2(*s);

  EXPECT_NE(cp1.getVariablePositions(), cp2.getVariablePositions());
  EXPECT_NE(cp1.getVariablePositions(), s->getVariablePositions());

  int cnt = cp1.getVariableCount();
  for (int i = 0 ; i < cnt ; ++i)
  {
    EXPECT_EQ(cp1.getVariablePositions()[i], 
              cp2.getVariablePositions()[i]);
    EXPECT_EQ(cp1.getVariablePositions()[i], 
              s->getVariablePositions()[i]);
  }

  // check mim_f == joint_f
  EXPECT_EQ(s->getVariablePositions()[MIM_F],
            s->getVariablePositions()[JOINT_F] * 1.5 + 0.1);
}

void AutoTune::start()
{
    
//         vector<double> inputRates={ 0.630478,0.996342,0.729125,0.523573,0.276309,0.053071,0.00143688,0.431644};
//         double res;
//         EvaluateResult(inputRates, res);
    
#if 1
    this->workerSequence.resize(this->workerNum,NULL);
    vector<double> currResult(this->workerNum,-1.0);
    
    //construct parameter array.
    vector<Point3d> paramArray;
    
    for (double pCrit = pCritMin; pCrit <= pCritMax; pCrit+=critStep) {
        for (double nCrit = nCritMin; nCrit <= nCritMax; nCrit+=critStep) {
            for (double epsilon = epsilonMin; epsilon <= epsilonMax; epsilon+=epsilonStep) {
                Point3d newParam;
                newParam.x = pCrit;
                newParam.y = nCrit;
                newParam.z = epsilon;
                paramArray.push_back(newParam);
            }
        }
    }
    
    LOG(INFO)<<"paramArray size = "<<paramArray.size();
    
    while (!paramArray.empty()) {
        
        for (int i=0; i<workerNum; i++) {
            
            if(NULL == workerSequence[i])
            {
                if(paramArray.empty()) break;
                Point3d param = paramArray.back();
                paramArray.pop_back();
                this->workingParams[i] = param;
#if( defined(WIN32) && _MSC_VER<=1700)//before VS2012, the  variadic template is not fully supported.
                this->workerSequence[i] = new boost::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#else
				this->workerSequence[i] = new std::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#endif
            }else{
                workerSequence[i]->join();
                delete workerSequence[i];
                
                //write result
                myLock.lock();
                if(accuracies[i]>0.8)
                {
                    fstream fs("result.txt",ios::out|ios::app);
                    fs<<"Result = "<<this->accuracies[i]<<" parameters = "<<workingParams[i]<<" weights = ";
                    for (int j=0; j<dim; j++) {
                        fs<<this->weightResults[i][j]<<" ";
                    }
                    fs<<endl;
                    fs.close();
                }
                myLock.unlock();
                
                //create new worker.
                if(paramArray.empty()) break;
                
                Point3d param = paramArray.back();
                paramArray.pop_back();
                this->workingParams[i] = param;
#if( defined(WIN32) && _MSC_VER<=1700)//before VS2012, the variadic template is not fully supported.
				this->workerSequence[i] = new boost::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#else
                this->workerSequence[i] = new std::thread(&AutoTune::WorkWithParameters,this,i,param.x,param.y,param.z);
#endif                
            }
            
        }
        
        
    }
    
#endif
    
}