spl.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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
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
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
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
00123 SplRefBoxProcessor::~SplRefBoxProcessor()
00124 {
00125 __s->close();
00126 delete __s;
00127 }
00128
00129
00130
00131 void
00132 SplRefBoxProcessor::process_struct(spl_gamecontrol_t *msg)
00133 {
00134 fawkes::worldinfo_gamestate_team_t our_team;
00135
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;
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));
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
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 }
00241 }
00242 }
00243
00244 bool
00245 SplRefBoxProcessor::check_connection()
00246 {
00247 return true;
00248 }
00249
00250
00251
00252
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 }