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

bool games_t::quit(int _pid, char* _pname) {
  if(_pid < 0) return false;
  if(_pid > MAX_PLAYER) return false;

  mtx_all_games.lock();    
  if(all_players_state[_pid] == PS_UNDEF) {
    mtx_all_games.unlock();
    write_score((char*)_pname, scores[_pid]);
    return true;
  }
  if(all_players_state[_pid] == PS_FREE || 
     all_players_state[_pid] == PS_WIN1 || 
     all_players_state[_pid] == PS_WIN2) {
    all_players_state[_pid] = PS_UNDEF;
    players.remove(_pid);
    mtx_all_games.unlock();
    write_score((char*)_pname, scores[_pid]);
    return true;
  }
  if(all_players_state[_pid] == PS_IN_GAME) {
    mtx_all_games.unlock();
    int ngid = get_game(_pid);
    if(ngid == -1) {
      printf("quit ERROR pid %d without gid\n", _pid);
      return true;
    }
    mtx_all_games.unlock();
    all_games.add_lost_to_score(ngid, _pid);
    all_games.app_lost(ngid, _pid);
    mtx_all_games.lock();
  }
  if(all_players_state[_pid] == PS_WAIT_GAME) {
    int ngid = get_game(_pid);
    if(ngid == -1) {
      printf("quit ERROR pid %d without gid\n", _pid);
      return true;
    }
    mtx_all_games.unlock();
    infos[ngid].player1_color = WHITE;
    infos[ngid].turn = FREE_SPACE;
    infos[ngid].player1_id = -1;
    sprintf(infos[ngid].player1_name, "%s", (char*)"");
    mtx_all_games.lock();
  }
  all_players_state[_pid] = PS_UNDEF;
  players.remove(_pid);
  mtx_all_games.unlock();
  write_score((char*)_pname, scores[_pid]);
  return 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;
}

void OnConnect(const boost::system::error_code &ec)
{
	if(ec)
	{
		global_stream_lock.lock();
		std::cout << "OnConnect Error: " << ec << ".\n";
		global_stream_lock.unlock();
	}
	else
	{
		global_stream_lock.lock();
		std::cout << "Connected!" << ".\n";
		global_stream_lock.unlock();
	}
}

static void push_lock(void* c, const CLockLocation& locklocation, bool fTry)
{
    if (lockstack.get() == NULL)
        lockstack.reset(new LockStack);

    if (fDebug) printf("Locking: %s\n", locklocation.ToString().c_str());
    dd_mutex.lock();

    (*lockstack).push_back(std::make_pair(c, locklocation));

    if (!fTry) {
        BOOST_FOREACH(const PAIRTYPE(void*, CLockLocation)& i, (*lockstack)) {
            if (i.first == c) break;

            std::pair<void*, void*> p1 = std::make_pair(i.first, c);
            if (lockorders.count(p1))
                continue;
            lockorders[p1] = (*lockstack);

            std::pair<void*, void*> p2 = std::make_pair(c, i.first);
            if (lockorders.count(p2))
            {
                potential_deadlock_detected(p1, lockorders[p2], lockorders[p1]);
                break;
            }
        }
    }

 void
 reset()
 {
     m_mutex.lock();
     m_indent = 0;
     m_mutex.unlock();
 }

    void writeMessage(){
        while( working_ ){
            Message msg;
            queueMutex.lock();
            if( msgQueue.empty( ) ){
                queueMutex.unlock();
                wait_condition_.notify_one();

                continue;

            }
            msg = msgQueue.front();
            msgQueue.pop();
            queueMutex.unlock();
            IO_Sync *info;
            {
                boost::mutex::scoped_lock lock(map_mutex_);
                if(sync_map_.find( msg.name() ) == sync_map_.end() ){
                    std::string outFileName;
                    char *name = new char[ 8]();

                    strncpy( name,msg.name(),sizeof(msg.name() ) );
                   // memcpy( name, msg.name(), sizeof( msg.name() ) );
                    outFileName = BINARY_DIR"/output_" + boost::lexical_cast< std::string >( name ) + ".txt";
                    sync_map_[name] = new IO_Sync(outFileName);
                }
                info = sync_map_[msg.name()];
            }

            info->write(msg);
        }
    }

 void 
 cloud_cb (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud)
 {
   cld_mutex.lock ();
   g_cloud = cloud;
   cld_mutex.unlock ();
 }

void App::broadcastRobotState() {
  // Set up message
  drc::robot_state_t msg_out;
  msg_out.utime =  bot_timestamp_now();
  msg_out.pose.translation.x = 0;  msg_out.pose.translation.y = 0;  msg_out.pose.translation.z = 0;
  msg_out.pose.rotation.w = 1;  msg_out.pose.rotation.x = 0;  msg_out.pose.rotation.y = 0;  msg_out.pose.rotation.z = 0;
  int n_joints = 15;
  msg_out.joint_position.assign(n_joints , 0  );
  msg_out.joint_velocity.assign(n_joints , 0  );
  msg_out.joint_effort.assign(n_joints , 0  );
  msg_out.num_joints = n_joints;

  msg_out.joint_name = {"lwr_arm_0_joint", "lwr_arm_1_joint", "lwr_arm_2_joint", "lwr_arm_3_joint", "lwr_arm_4_joint", "lwr_arm_5_joint", "lwr_arm_6_joint", "sdh_knuckle_joint", "sdh_thumb_2_joint", "sdh_thumb_3_joint", "sdh_finger_12_joint", "sdh_finger_13_joint", "sdh_finger_22_joint", "sdh_finger_23_joint", "sdh_finger_21_joint"};

  // Add individual joint state messages
  int kuka_joints = 7; //kukaState_->joint_position.size();
  int sdh_joints = 8; //sdhState_->joint_position.size();

  mtx_.lock();
  for (int i = 0; i < kuka_joints; i++)  {
    msg_out.joint_position[ i ] = kukaState_.joint_position[ i ];
    msg_out.joint_velocity[ i ] = kukaState_.joint_velocity[ i ];
    msg_out.joint_effort[ i ] = kukaState_.joint_effort[ i ];
  }

  for (int j = 0; j < sdh_joints; j++)  {
    msg_out.joint_position[ j+7 ] = sdhState_.joint_position[ j ];
    msg_out.joint_velocity[ j+7 ] = sdhState_.joint_velocity[ j ];
    msg_out.joint_effort[ j+7 ] = sdhState_.joint_effort[ j ];
  }
  mtx_.unlock();

  lcm_->publish("EST_ROBOT_STATE", &msg_out);
}

int __stdcall on_sample(SampleStruct s) {
	if (!outlet_)
		return 0;

	float sample[num_channels] = {
		s.leftEye.gazeX,s.leftEye.gazeY,
		s.rightEye.gazeX,s.rightEye.gazeY,
		s.leftEye.diam,s.rightEye.diam,
		s.leftEye.eyePositionX,s.leftEye.eyePositionY,s.leftEye.eyePositionZ,
		s.rightEye.eyePositionX,s.rightEye.eyePositionY,s.rightEye.eyePositionZ,
		s.planeNumber};

	// calc sample age
	long long tracker_time = s.timestamp;
	iV_GetCurrentTimestamp(&tracker_time);
	double age = (tracker_time - s.timestamp)/1000000.0;
	
	// push into LSL
	outlet_->push_sample(sample,lsl::local_clock()-age);
	mtxx_.lock();
	x_pos = (s.leftEye.gazeX+s.rightEye.gazeX)/2.0;
	y_pos = (s.leftEye.gazeY+s.rightEye.gazeY)/2.0;
	mtxx_.unlock();
	return 1;
}

 int get_messages_expired ()
 {
     m_mutex.lock ();
     int expired = m_expired;
     m_mutex.unlock ();
     return expired;
 }

void cloud_cb_ (const PointCloudT::ConstPtr &callback_cloud, PointCloudT::Ptr& cloud, bool* new_cloud_available_flag)
{
    cloud_mutex.lock () ;    // for not overwriting the point cloud from another thread
    *cloud = *callback_cloud ;
    *new_cloud_available_flag = true ;
    cloud_mutex.unlock () ;
}

// spin, modifying the state to different values
void MyInfo::modifyThreadFunc(
    robot_interaction::LockedRobotState* locked_state,
    int* counter,
    double offset)
{
  bool go = true;
  while(go)
  {
    double val = offset;
    for (int loops = 0 ; loops < 100 ; ++loops)
    {
      val += 0.0001;

      locked_state->modifyState(boost::bind(&MyInfo::modifyFunc,
                                                 this,
                                                 _1,
                                                 val));
    }

    cnt_lock_.lock();
    go = !quit_;
    ++*counter;
    cnt_lock_.unlock();

    checkState(*locked_state);

    val += 0.000001;
  }
}

// spin, setting the state to different values
void MyInfo::setThreadFunc(
    robot_interaction::LockedRobotState* locked_state,
    int* counter,
    double offset)
{
  bool go = true;
  while(go)
  {
    double val = offset;
    for (int loops = 0 ; loops < 100 ; ++loops)
    {
      val += 0.0001;
      robot_state::RobotState cp1(*locked_state->getState());
      
      cp1.setVariablePosition(JOINT_A, val + 0.00001);
      cp1.setVariablePosition(JOINT_C, val + 0.00002);
      cp1.setVariablePosition(JOINT_F, val + 0.00003);

      locked_state->setState(cp1);
    }

    cnt_lock_.lock();
    go = !quit_;
    ++*counter;
    cnt_lock_.unlock();

    checkState(*locked_state);

    val += 0.000001;
  }
}

void CleanProtocolExclThread(std::string proto)
{
	while(true)
	{
		UINT status = CallProtoService(proto.c_str(), PS_GETSTATUS, 0, 0);
		if(status > ID_STATUS_OFFLINE)
			break;
		boost::this_thread::sleep(boost::posix_time::seconds(2));
	}

	std::list<HANDLE> contacts;
	for(HANDLE hContact = db_find_first(proto.c_str()); hContact; hContact = db_find_next(hContact, proto.c_str()))
		if(db_get_b(hContact, "CList", "NotOnList", 0) && db_get_b(hContact, pluginName, "Excluded", 0))
			contacts.push_back(hContact);

	boost::this_thread::sleep(boost::posix_time::seconds(5));
	clean_mutex.lock();
	std::list<HANDLE>::iterator end = contacts.end();
	for(std::list<HANDLE>::iterator i = contacts.begin(); i != end; ++i)
	{
		LogSpamToFile(*i, _T("Deleted"));
		HistoryLogFunc(*i, "Deleted");
		CallService(MS_DB_CONTACT_DELETE, (WPARAM)*i, 0);
	}
	clean_mutex.unlock();
}

int main(void)
{
	boost::shared_ptr<boost::asio::io_service> io_svc(
		new boost::asio::io_service
	);
	
	boost::shared_ptr<boost::asio::io_service::work> worker(
		new boost::asio::io_service::work(*io_svc)
	);

	global_stream_lock.lock();
	std::cout << "The program will exit once all work has finished.\n";
	global_stream_lock.unlock();

	boost::thread_group threads;
	for(int i=1; i<=2; i++)
		threads.create_thread(boost::bind(&WorkerThread, io_svc, i));

	io_svc->post(boost::bind(&ThrowAnException, io_svc, 1));
	io_svc->post(boost::bind(&ThrowAnException, io_svc, 2));
	io_svc->post(boost::bind(&ThrowAnException, io_svc, 3));
	io_svc->post(boost::bind(&ThrowAnException, io_svc, 4));
	io_svc->post(boost::bind(&ThrowAnException, io_svc, 5));

	threads.join_all();

	return 0;
}