field.c

在LINUX下运行的仿真机器人服务器源代码

  double lcmt = 0;
  int c_simt = 1,
    c_sent = 1,
    //c_rect = 1,
    c_sbt = 1,
    c_svt = 1,
    c_synch = 1,
    q_simt = ServerParam::instance().lcm_st / ServerParam::instance().sim_st,
    q_sent = ServerParam::instance().lcm_st / ServerParam::instance().send_st * 4,
    //q_rect = ServerParam::instance().lcm_st / ServerParam::instance().recv_st,
    q_sbt = ServerParam::instance().lcm_st / ServerParam::instance().sb_step,
    q_svt = ServerParam::instance().lcm_st / ServerParam::instance().sv_st,
    q_synch = ServerParam::instance().lcm_st / ServerParam::instance().sim_st;

  itv.it_interval.tv_sec = 0 ;
  itv.it_interval.tv_usec = 50* 1000 ;
  itv.it_value.tv_sec = 0 ;
  itv.it_value.tv_usec = 50* 1000 ;
  
  alarm_action.sa_flags = 0; 
  //alarm_action.sa_flags &= (~SA_RESETHAND) ;
  sigaction(SIGALRM, &alarm_action, NULL) ;
  setitimer(ITIMER_REAL, &itv, NULL) ;

  //reset the done flags
  for (int i=0; i<MAX_PLAYER*2; i++)
    player[i]->done_received = FALSE;
  for (int i = 0; i < 2; i++)
    olcoach[i]->done_received = FALSE;
  coach->done_received = FALSE;

  while (!shutdown) {
    lcmt += TIMEDELTA;

   
    if (lcmt >= ServerParam::instance().sim_st * c_simt) {
      if ( text_log_open () 
	   && ServerParam::instance().logTimes () )
        {
          tp_old = tp_new;  
          gettimeofday(&tp_new, NULL);
          write_times(tp_new, tp_old);
        }
      reset_player_comm_flag() ;
      step() ;
      if (q_simt <= c_simt)
        c_simt = 1;
      else
        c_simt++;
    }
    
    if (lcmt >= ServerParam::instance().sb_step * c_sbt) {
      int i ;
      for (i = 0 ; i < MAX_PLAYER * 2; i++)
        if (player[i]->version >= 5.0)
          {
            player[i]->sense_body() ;
            if( ( player[i]->team->side == LEFT 
                  && ServerParam::instance().fullstate_l ) 
                || ( player[i]->team->side == RIGHT 
                     && ServerParam::instance().fullstate_r ) ) 
              { 
                if( player[i]->connected() ) 
                  player[i]->send_fullstate_information(); 
              }
          }
      std::for_each( M_listeners.begin(), M_listeners.end(),
                     Listener::NewCycle() );

      c_sbt = (int) (floor (lcmt / ServerParam::instance().sb_step ) );
      if (q_sbt <= c_sbt)
        c_sbt = 1;
      else
        c_sbt++;
    } 

    if (lcmt >= (ServerParam::instance().send_st / 4) * c_sent) {
      udp_send_message() ;
      c_sent = (int) (floor (lcmt / (ServerParam::instance().send_st / 4)) );
      if (q_sent <= c_sent)
        c_sent = 1;
      else
        c_sent++;
    }
				
    if (lcmt >= ServerParam::instance().sv_st * c_svt) {
      int i ;
      if (coach->assignedp && coach->eye)
        coach->send_visual_info() ;
      for (i = 0; i < 2; i++)
        if (olcoach[i]->assignedp && olcoach[i]->eye)
          olcoach[i]->send_visual_info() ;
      c_svt = (int) (floor (lcmt / ServerParam::instance().sv_st) );
      if (q_svt <= c_svt)
        c_svt = 1;
      else
        c_svt++;
    }
    
    //we do a c_synch-1 because of the offset
    if (lcmt >= ServerParam::instance().sim_st * (c_synch-1) + ServerParam::instance().synch_offset) {
      if (time <= 0) {
	//still waiting for players to connect, so let's run a little more slowly
        usleep(50 * 1000);
      }

      //figure out who we are going to wait for
      bool waitPlayer[MAX_PLAYER*2];
      bool waitCoach[2];
      bool waitTrainer;
      for (int i=0; i<MAX_PLAYER*2; i++)
        waitPlayer[i] = !(player[i]->alive == DISABLE);
      for (int i = 0; i < 2; i++)
        waitCoach[i] = olcoach[i]->eye;
      waitTrainer = coach->eye;

      //tell the clients they should start thinking
      for (int i=0; i<MAX_PLAYER*2; i++)
        if (waitPlayer[i])
          player[i]->send(think_command);
      for (int i = 0; i < 2; i++)
        if (waitCoach[i])
          olcoach[i]->send(think_command);
      if (waitTrainer)
        coach->send(think_command);

      //wait for confirmations from the clients

      //reset alarm count; we use this in case a player doesn't respond
      alrms_received = 0;
      
      enum {
        DS_FALSE = 0,
        DS_TRUE = 1,
        DS_TRUE_BUT_INCOMPLETE = 2
      } done;
      int num_sleeps = 0;
      struct timeval tv;
      gettimeofday( &tv, NULL );
      // cerr << tv.tv_sec << " " << tv.tv_usec << ": do " << time << endl;
      do {
	done = DS_TRUE;
	num_sleeps++;

	usleep(ServerParam::instance().synch_micro_sleep);

	udp_recv_message() ;
	udp_recv_from_online_coach() ;
	if (ServerParam::instance().CoachMode || ServerParam::instance().CwRMode)
	  udp_recv_from_coach();

	for (int i=0; i<MAX_PLAYER*2; i++) {
	  if (waitPlayer[i] && !player[i]->done_received && player[i]->alive != DISABLE) {
	    done = DS_FALSE;
	    break;
	  }
	}
	for (int i = 0; i < 2; i++) {
	  if (waitCoach[i] && !olcoach[i]->done_received && olcoach[i]->assignedp) {
	    done = DS_FALSE;
	    break;
	  }
	}
	if (waitTrainer && !coach->done_received)
	  done = DS_FALSE;

	if (alrms_received > max_alrms_wait) {
	  done = DS_TRUE_BUT_INCOMPLETE;
	  if (time > 0) {
	    cycles_missed++;
	    std::cerr << "Someone missed a cycle at " << time << std::endl;
	  }
	  if (cycles_missed > max_cycles_missed) {
	    std::cerr << "Waiting too long for clients! Exiting" << std::endl;
	    shutdown = true;
	  }
	}
      } while (done == DS_FALSE);
      gettimeofday( &tv, NULL );
     // cerr << tv.tv_sec << " " << tv.tv_usec << ": do_end " << time << endl;
      
      //reset the done flags
      for (int i=0; i<MAX_PLAYER*2; i++)
	player[i]->done_received = FALSE;
      for (int i = 0; i < 2; i++)
	olcoach[i]->done_received = FALSE;
      coach->done_received = FALSE;

      if (done != DS_TRUE_BUT_INCOMPLETE)
	cycles_missed = 0;
      
      if ( text_log_open () 
	   && ServerParam::instance().logTimes () ) 
	{
    text_log_stream () << time 
                       << "\tNum sleeps called: " 
                       << num_sleeps << std::endl;
 	}

      // because of the strange offset nature here,
	    // we let the lcmt advancement below handle the resetting to 1 
      c_synch++;
    }
    
    
    if (lcmt >= ServerParam::instance().lcm_st) {
      lcmt = 0;
      c_synch = 1;
    }
    
  }
  closeLogs ();
}
*/

void Stadium::assign(void)
{
	field.assign(this) ;
}

/* pfr 06/07/200 added short name support */
MPObject* Stadium::NewObject( const PObject::obj_type& object_type,
			      const MPObjectType t,
			      const Name name,
			      const Name short_name,
			      const std::string& sub_name,
			      const PVector pos,
			      const Value size,
			      const Value angle,
			      const Value decay,
			      const Value rand,
			      const Value weight,
			      const Value speed,
			      // th 6.3.00
			      const Value accel)
                              //
{
	MPObject* obj = motable.NewObject( object_type, name, short_name, sub_name) ;

	votable.AssignObject(obj) ;
	obj->stadium = this ;
	obj->weather = &weather ;
	obj->weight = weight ;
	obj->max_speed = speed ;
	// th 6.3.00
	obj->max_accel = accel ;
	//
	obj->Set(t,pos,size,angle,PVector(0.0,0.0),PVector(0.0,0.0), decay,rand) ;
	return obj ;
}

void Stadium::assignBall(void)
{
  ball = NewObject( PObject::OT_BALL,
		    MPO_Ball,
		    BALL_NAME,
		    BALL_NAME_SHORT,
		    "",
		    PVector(0.0,0.0),
		    ServerParam::instance().bsize,		/* ball size */
		    0.0,		/* ball angle */
		    ServerParam::instance().bdecay,		/* ball decay */
		    ServerParam::instance().brand,		/* ball rand */ 
		    ServerParam::instance().bweight,	/* ball weight */ 
		    ServerParam::instance().bspeed_max,	/* ball speed max */
		    // th 6.3.00
		    ServerParam::instance().baccel_max      /* ball acceleration max */
		    //
		    );
}

void Stadium::initPlayer(void)
{
	int i ;
	Player *p_l ;
	Player *p_r ;

	team_l = new Team(this, LEFT) ;
	team_r = new Team(this, RIGHT) ;

	for (i = 0; i < MAX_PLAYER; i++) {
		p_l = new Player(team_l, this, i+1) ;
		p_r = new Player(team_r, this, i+1) ;
		team_l->player[i] = p_l ;
		team_r->player[i] = p_r ;
		player[i] = p_l ;
		player[i+MAX_PLAYER] = p_r ;
		dest[i] = i ;
		dest[i+MAX_PLAYER] = i+MAX_PLAYER ;
	}
}

void Stadium::initCoach(Stadium *stad)
{
	coach = new Coach(stad) ;
	coach->eye = FALSE ;
}

void Stadium::initOnlineCoach(Stadium *stad)
{
	OnlineCoach *p_olcoach ;

	p_olcoach = new OnlineCoach(stad) ;
	team_l->olcoach = p_olcoach ;
	olcoach[0] = p_olcoach ;
	olcoach[0]->eye = FALSE ;
	olcoach[0]->freeform_messages_allowed = ServerParam::instance().say_cnt_max;
	p_olcoach = new OnlineCoach(stad) ;
	team_r->olcoach = p_olcoach ;
	olcoach[1] = p_olcoach ;
	olcoach[1]->eye = FALSE ;
	olcoach[1]->freeform_messages_allowed = ServerParam::instance().say_cnt_max;
}

Player* Stadium::newPlayer( const Name teamname, Value version, int goalie_flag,
							const rcss::net::Addr& addr )
{
	Team *tm ;

	if (team_l->name == NULLCHAR) {
		tm = team_l ;
		tm->name = teamname ;
		tm->enable = TRUE ;
		//renameLogs ();
	}
	else if (!strcmp(team_l->name, teamname))
		tm = team_l ;
	else if (team_r->name == NULLCHAR) {
		tm = team_r ;
		tm->name = teamname ;
		tm->enable = TRUE ;
		//renameLogs (); 
	}
	else if (!strcmp(team_r->name, teamname))
		tm = team_r ;
	else {
		if (ServerParam::instance().verbose)
			std::cerr << "Warning:Too many teams. [teamname = '"
					<< teamname << "']" << std::endl ;
		return NULL ; 
	}

	Player *p = tm->newPlayer(version, goalie_flag) ;

	if (p == NULL)
		return (Player*)NULL;

  
	if( !p->connect( addr ) )
	{
	    p->disable();
	    return NULL;      
	}
  
  p->setEnforceDedicatedPort( version >= 8.0 );
  M_remote_players.push_back( p );

	motable.AssignObject(p) ;
	votable.AssignObject(p) ;

	return p ;
}

Player* Stadium::newClient( const rcss::net::Addr& addr, const char* init_message)
{
	char buffer[128], *teamname ;
	const char *ptr = init_message ;
	int goalie_flag = FALSE ;
	float version = 3.0 ;
	unsigned int count = 0 ;

  if (strncmp(ptr, "(init ", 6)) {
		if (ServerParam::instance().verbose)
			std::cerr << "Warning:Illegal initialize message." << std::endl 
           << "   message = " << init_message << std::endl ;
		return NULL ;
	}
  ptr += 6 ;
  
  while (*ptr != (char)NULL) {
    if (*ptr == '(') {
      if (!strncmp(ptr, "(version ", 9)) { /* noda */
        ptr += 9 ;
        break ;
			}
      else if (!strncmp(ptr, "(goalie)", 8)) {
        ptr += 8 ;
        goalie_flag = TRUE ;
        break ;
			}
			else { 
				if (ServerParam::instance().verbose)
					std::cerr << "Warning:Illegal team name." << std::endl 
               << "   message = " << init_message << std::endl ;
				return NULL ;
      }
    }
    else if (*ptr == ')' ) {
			buffer[count] = NULLCHAR ;
			sscanf(buffer, "%[-_a-zA-Z0-9]", buffer) ;
			if(count != strlen(buffer)) {
				if (ServerParam::instance().verbose)
					std::cerr << "Warning:illegal_team_char."
               << std::endl ;
			 	return NULL ;
			}
			teamname = strdup(buffer) ;
			return newPlayer(teamname, (Value)version, goalie_flag, addr );
		}
    else {
      buffer[count++] = *ptr++ ;
			if (count > 32) {
				if (ServerParam::instance().verbose)
					std::cerr << "Warning:Too long team name." << std::endl
               << "   message = " << init_message << std::endl ;
				return NULL ;
			}
		}
  }
	
	if (isspace(buffer[count-1]))
		count-- ;
	buffer[count] = NULLCHAR ;
	sscanf(buffer, "%[-_a-zA-Z0-9]", buffer) ;
	if(count != strlen(buffer)) {
		if (ServerParam::instance().verbose)
			std::cerr << "Warning:illegal_team_char." << std::endl ;
	 	return NULL ;
	}
	teamname = strdup(buffer) ;

  while (*ptr != (char)NULL) {
    if (isspace(*ptr))
      ptr++ ;
    else
      break ;
	}

	if (goalie_flag) {
		if (!strncmp(ptr, "(version ", 9))
			ptr += 9 ;
		else { 
			if (ServerParam::instance().verbose)