spl.cpp

00001 
00002 /***************************************************************************
00003  *  spl.cpp - Fawkes SPL refbox repeater
00004  *
00005  *  Created: Tue Jul 08 13:50:06 2008
00006  *  Copyright  2008  Tim Niemueller [www.niemueller.de]
00007  *             2009  Tobias Kellner
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 #include "spl.h"
00025 #include "state_handler.h"
00026 #include <core/exception.h>
00027 #include <netcomm/socket/datagram.h>
00028 #include <utils/logging/logger.h>
00029 
00030 #ifdef USE_SPL_GC6
00031 #  include <interfaces/SoccerPenaltyInterface.h>
00032 #endif
00033 
00034 #include <cstring>
00035 #include <cstdio>
00036 #include <unistd.h>
00037 #include <cerrno>
00038 // it it was defined, Exception::errno() could not be called...
00039 #ifdef errno
00040 #  undef errno
00041 #endif
00042 using namespace fawkes;
00043 
00044 #ifdef USE_SPL_GC6
00045 static const uint32_t  SPL_STRUCT_VERSION = 6;
00046 #else
00047 static const uint32_t  SPL_STRUCT_VERSION = 7;
00048 #endif
00049 
00050 static const uint8_t   SPL_STATE_INITIAL  = 0;
00051 static const uint8_t   SPL_STATE_READY    = 1;
00052 static const uint8_t   SPL_STATE_SET      = 2;
00053 static const uint8_t   SPL_STATE_PLAYING  = 3;
00054 static const uint8_t   SPL_STATE_FINISHED = 4;
00055 
00056 static const uint8_t   SPL_STATE2_NORMAL       = 0;
00057 static const uint8_t   SPL_STATE2_PENALTYSHOOT = 1;
00058 
00059 static const uint8_t   SPL_PENALTY_NONE               =  0;
00060 #ifdef USE_SPL_GC6
00061 static const uint8_t   SPL_PENALTY_BALL_HOLDING       =  1;
00062 static const uint8_t   SPL_PENALTY_GOALIE_PUSHING     =  2;
00063 static const uint8_t   SPL_PENALTY_PLAYER_PUSHING     =  3;
00064 static const uint8_t   SPL_PENALTY_ILLEGAL_DEFENDER   =  4;
00065 static const uint8_t   SPL_PENALTY_ILLEGAL_DEFENSE    =  5;
00066 static const uint8_t   SPL_PENALTY_OBSTRUCTION        =  6;
00067 static const uint8_t   SPL_PENALTY_REQ_FOR_PICKUP     =  7;
00068 static const uint8_t   SPL_PENALTY_LEAVING            =  8;
00069 static const uint8_t   SPL_PENALTY_DAMAGE             =  9;
00070 static const uint8_t   SPL_PENALTY_MANUAL             = 10;
00071 #else
00072 static const uint8_t   SPL_PENALTY_BALL_HOLDING       =  1;
00073 static const uint8_t   SPL_PENALTY_PLAYER_PUSHING     =  2;
00074 static const uint8_t   SPL_PENALTY_OBSTRUCTION        =  3;
00075 static const uint8_t   SPL_PENALTY_INACTIVE_PLAYER    =  4;
00076 static const uint8_t   SPL_PENALTY_ILLEGAL_DEFENDER   =  5;
00077 static const uint8_t   SPL_PENALTY_LEAVING_THE_FIELD  =  6;
00078 static const uint8_t   SPL_PENALTY_PLAYING_WITH_HANDS =  7;
00079 static const uint8_t   SPL_PENALTY_REQ_FOR_PICKUP     =  8;
00080 static const uint8_t   SPL_PENALTY_MANUAL             = 15;
00081 #endif
00082 
00083 // team numbers
00084 static const uint8_t   SPL_TEAM_BLUE                  =  0;
00085 static const uint8_t   SPL_TEAM_RED                   =  1;
00086 
00087 static const uint8_t   SPL_GOAL_BLUE                  =  0;
00088 static const uint8_t   SPL_GOAL_YELLOW                =  1;
00089 
00090 static const char    SPL_GAMECONTROL_HEADER[SPL_HEADER_SIZE] = {'R','G','m','e'};
00091 
00092 
00093 /** @class SplRefBoxProcessor "processor/spl.h"
00094  * SPL league refbox repeater.
00095  * This class will listen to SPL refbox commands and derive matching
00096  * game states from the communication stream and send this via the world info.
00097  * @author Tim Niemueller
00098  */
00099 
00100 /** Constructor.
00101  * @param logger Logger
00102  * @param broadcast_port Broadcast port
00103  * @param team_number our team number
00104  * @param player_number individual player number
00105  */
00106 SplRefBoxProcessor::SplRefBoxProcessor(fawkes::Logger *logger,
00107                                        unsigned short int broadcast_port,
00108                                        unsigned int team_number,
00109                                        unsigned int player_number)
00110 {
00111   __player_number = player_number;
00112   __team_number = team_number;
00113   __logger = logger;
00114   __quit = false;
00115   __s = new DatagramSocket(0.0000000001);
00116   __s->bind(broadcast_port);
00117 
00118   __penalty = SPL_PENALTY_NONE;
00119 }
00120 
00121 
00122 /** Destructor. */
00123 SplRefBoxProcessor::~SplRefBoxProcessor()
00124 {
00125   __s->close();
00126   delete __s;
00127 }
00128 
00129 
00130 /** Process received struct. */
00131 void
00132 SplRefBoxProcessor::process_struct(spl_gamecontrol_t *msg)
00133 {
00134   fawkes::worldinfo_gamestate_team_t our_team;
00135   //fawkes::worldinfo_gamestate_goalcolor_t our_goal;
00136 
00137   int team_index;
00138   if (msg->teams[0].team_number == __team_number) team_index = 0;
00139   else if (msg->teams[1].team_number == __team_number) team_index = 1;
00140   else return; //Message doesn't concern us
00141 
00142   switch (msg->teams[team_index].team_color) {
00143     case SPL_TEAM_BLUE:
00144       our_team = TEAM_CYAN;
00145       break;
00146     case SPL_TEAM_RED:
00147       our_team = TEAM_MAGENTA;
00148       break;
00149     default:
00150       printf("Ignoring faulty packet\n");
00151       return;
00152   }
00153 
00154   _rsh->set_score(msg->teams[team_index].score, msg->teams[(team_index == 1 ? 0 : 1)].score);
00155   _rsh->set_team_goal(our_team, (our_team == TEAM_CYAN ? GOAL_BLUE : GOAL_YELLOW)); //blue team defends blue goal
00156 
00157   for (unsigned int pl_num = 0; pl_num < SPL_MAX_NUM_PLAYERS; ++pl_num)
00158   {
00159     if ((pl_num + 1) == __player_number)
00160     {
00161       if ((msg->teams[team_index].players[pl_num].penalty != __penalty) ||
00162           (msg->teams[team_index].players[pl_num].penalty != PENALTY_NONE))
00163       {
00164         __penalty = msg->teams[team_index].players[pl_num].penalty;
00165 
00166 #ifdef USE_SPL_GC6
00167         // convert GC6 codes to new GC7 codes, "closest match"
00168         switch (__penalty) {
00169         case SPL_PENALTY_BALL_HOLDING:
00170           __penalty = SoccerPenaltyInterface::SPL_PENALTY_BALL_HOLDING; break;
00171         case SPL_PENALTY_GOALIE_PUSHING:
00172         case SPL_PENALTY_PLAYER_PUSHING:
00173           __penalty = SoccerPenaltyInterface::SPL_PENALTY_PLAYER_PUSHING; break;
00174         case SPL_PENALTY_ILLEGAL_DEFENDER:
00175         case SPL_PENALTY_ILLEGAL_DEFENSE:
00176           __penalty = SoccerPenaltyInterface::SPL_PENALTY_ILLEGAL_DEFENDER; break;
00177         case SPL_PENALTY_OBSTRUCTION:
00178           __penalty = SoccerPenaltyInterface::SPL_PENALTY_OBSTRUCTION; break;
00179         case SPL_PENALTY_REQ_FOR_PICKUP:
00180           __penalty = SoccerPenaltyInterface::SPL_PENALTY_REQ_FOR_PICKUP; break;
00181         case SPL_PENALTY_LEAVING:
00182           __penalty = SoccerPenaltyInterface::SPL_PENALTY_LEAVING_THE_FIELD; break;
00183         case SPL_PENALTY_DAMAGE:
00184         case SPL_PENALTY_MANUAL:
00185           __penalty = SoccerPenaltyInterface::SPL_PENALTY_MANUAL; break;
00186         default:
00187           __penalty = SoccerPenaltyInterface::SPL_PENALTY_NONE; break;
00188         }
00189 #endif
00190 
00191         _rsh->add_penalty(__penalty,
00192                           msg->teams[team_index].players[pl_num].secs_till_unpenalized);
00193       }
00194       break;
00195     }
00196   }
00197 
00198   switch (msg->state) {
00199   case SPL_STATE_INITIAL:
00200     _rsh->set_gamestate(GS_SPL_INITIAL, TEAM_BOTH);
00201     break;
00202   case SPL_STATE_READY:
00203     _rsh->set_gamestate(GS_SPL_READY, TEAM_BOTH);
00204     break;
00205   case SPL_STATE_SET:
00206     _rsh->set_gamestate(GS_SPL_SET, TEAM_BOTH);
00207     break;
00208   case SPL_STATE_PLAYING:
00209     _rsh->set_gamestate(GS_SPL_PLAY, TEAM_BOTH);
00210     break;
00211   case SPL_STATE_FINISHED:
00212     _rsh->set_gamestate(GS_SPL_FINISHED, TEAM_BOTH);
00213     break;
00214   default:
00215     _rsh->set_gamestate(GS_SPL_FINISHED, TEAM_BOTH);
00216     break;
00217   }
00218 
00219   _rsh->set_half((msg->first_half == 1) ? HALF_FIRST : HALF_SECOND,
00220                  msg->kick_off_team == team_index);
00221 }
00222 
00223 
00224 void
00225 SplRefBoxProcessor::refbox_process()
00226 {
00227   try {
00228     spl_gamecontrol_t ctrlmsg;
00229     size_t bytes_read = __s->recv((void *)&ctrlmsg, sizeof(ctrlmsg));
00230     if ( bytes_read == sizeof(ctrlmsg) ) {
00231       if ((strncmp(ctrlmsg.header, SPL_GAMECONTROL_HEADER, SPL_HEADER_SIZE) == 0) &&
00232           (ctrlmsg.version == SPL_STRUCT_VERSION) ) {
00233         process_struct(&ctrlmsg);
00234       }
00235     }
00236   } catch (fawkes::Exception &e) {
00237     if ( e.errno() != EAGAIN ) {
00238       __logger->log_warn("SplRefBoxProcessor", "Receiving failed, exception follows");
00239       __logger->log_warn("SplRefBoxProcessor", e);
00240     } // else just no data available this time
00241   }
00242 }
00243 
00244 bool
00245 SplRefBoxProcessor::check_connection()
00246 {
00247   return true;
00248 }
00249 
00250 
00251 /** Run.
00252  * Reads messages from the network, processes them and calls the refbox state sender.
00253  */
00254 void
00255 SplRefBoxProcessor::run()
00256 {
00257   spl_gamecontrol_t ctrlmsg;
00258   while ( ! __quit ) {
00259     size_t bytes_read = __s->recv((void *)&ctrlmsg, sizeof(ctrlmsg));
00260     if ( bytes_read == sizeof(ctrlmsg) ) {
00261       if ( (strncmp(ctrlmsg.header, SPL_GAMECONTROL_HEADER, SPL_HEADER_SIZE) == 0) &&
00262            (ctrlmsg.version == SPL_STRUCT_VERSION) ) {
00263         process_struct(&ctrlmsg);
00264         _rsh->handle_refbox_state();
00265       } else {
00266         printf("Received illegal package\n");
00267       }
00268     }
00269   }
00270 }

Generated on Tue Feb 22 13:32:31 2011 for Fawkes API by  doxygen 1.4.7