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 <core/exceptions/system.h>
00025 #include <core/exceptions/software.h>
00026
00027 #include <netcomm/worldinfo/transceiver.h>
00028 #include <netcomm/worldinfo/messages.h>
00029 #include <netcomm/worldinfo/encrypt.h>
00030 #include <netcomm/worldinfo/decrypt.h>
00031
00032 #include <netcomm/socket/datagram_broadcast.h>
00033 #include <netcomm/socket/datagram_multicast.h>
00034 #include <netcomm/utils/resolver.h>
00035
00036 #include <utils/logging/liblogger.h>
00037
00038 #include <arpa/inet.h>
00039 #include <netinet/in.h>
00040 #include <cstdlib>
00041 #include <cstring>
00042
00043 namespace fawkes {
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 WorldInfoException::WorldInfoException(const char *msg)
00054 : Exception(msg)
00055 {
00056 }
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101 WorldInfoTransceiver::WorldInfoTransceiver(SocketType socket_type,
00102 const char *addr, unsigned short port,
00103 const char *key, const char *iv,
00104 NetworkNameResolver *resolver) :
00105 pose_changed( false ),
00106 vel_changed( false ),
00107 rel_ball_changed( false ),
00108 rel_ball_vel_changed( false ),
00109 glob_ball_changed( false ),
00110 glob_ball_vel_changed( false ),
00111 gamestate_changed( false )
00112 {
00113 try {
00114 switch (socket_type) {
00115 case MULTICAST: {
00116 MulticastDatagramSocket* ms = new MulticastDatagramSocket(addr, port);
00117 ms->bind();
00118 s = ms;
00119 break;
00120 }
00121 case BROADCAST: {
00122 BroadcastDatagramSocket* bs = new BroadcastDatagramSocket(addr, port);
00123 bs->bind();
00124 s = bs;
00125 break;
00126 }
00127 }
00128 set_loop(false);
00129 } catch (SocketException &e) {
00130 e.append("WorldInfoTransceiver cannot instantiate socket for %s:%u", addr, port);
00131 throw;
00132 }
00133
00134 in_buffer = malloc(WORLDINFO_MTU);
00135 out_buffer = malloc(WORLDINFO_MTU);
00136 if (! in_buffer || ! out_buffer) {
00137 throw OutOfMemoryException();
00138 }
00139
00140 fatmsg_enabled = false;
00141 fatmsg_bufsize = 0;
00142 fatmsg_buf = NULL;
00143 fatmsg_header = NULL;
00144 fatmsg_msgheader = NULL;
00145 fatmsg = NULL;
00146
00147 __key = strdup(key);
00148 __iv = strdup(iv);
00149
00150 encryptor = new WorldInfoMessageEncryptor((const unsigned char *)__key, (const unsigned char *)__iv);
00151 decryptor = new WorldInfoMessageDecryptor((const unsigned char *)__key, (const unsigned char *)__iv);
00152
00153
00154 encryptor->set_plain_buffer(out_buffer, WORLDINFO_MTU);
00155
00156 crypt_buffer_size = encryptor->recommended_crypt_buffer_size();
00157 crypted_out_buffer = malloc(crypt_buffer_size);
00158 crypted_in_buffer = malloc(crypt_buffer_size);
00159
00160 if (! crypted_in_buffer || ! crypted_out_buffer) {
00161 throw OutOfMemoryException();
00162 }
00163
00164 encryptor->set_crypt_buffer(crypted_out_buffer, crypt_buffer_size);
00165
00166 decryptor->set_plain_buffer(in_buffer, WORLDINFO_MTU);
00167
00168 if ( resolver == NULL ) {
00169 this->resolver = new NetworkNameResolver();
00170 resolver_delete = true;
00171 } else {
00172 this->resolver = resolver;
00173 resolver_delete = false;
00174 }
00175
00176 out_seq = 0;
00177 }
00178
00179
00180
00181 WorldInfoTransceiver::~WorldInfoTransceiver()
00182 {
00183 set_fatmsg_enabled(false);
00184 free(out_buffer);
00185 free(in_buffer);
00186 free(crypted_out_buffer);
00187 free(crypted_in_buffer);
00188 free(__key);
00189 free(__iv);
00190 delete s;
00191 delete encryptor;
00192 delete decryptor;
00193 if ( resolver_delete ) {
00194 delete resolver;
00195 }
00196 }
00197
00198
00199
00200
00201
00202
00203
00204
00205 void
00206 WorldInfoTransceiver::set_loop(bool loop)
00207 {
00208 MulticastDatagramSocket* ms = dynamic_cast<MulticastDatagramSocket*>(s);
00209 if (s) {
00210 ms->set_loop( loop );
00211 }
00212 this->loop = loop;
00213 }
00214
00215
00216
00217
00218
00219
00220
00221
00222 void
00223 WorldInfoTransceiver::set_fatmsg_enabled(bool fatmsg_enabled)
00224 {
00225 if ( this->fatmsg_enabled && ! fatmsg_enabled ) {
00226
00227 free(fatmsg_buf);
00228 fatmsg_buf = NULL;
00229 fatmsg_msgheader = NULL;
00230 fatmsg_header = NULL;
00231 fatmsg = NULL;
00232 fatmsg_bufsize = 0;
00233 } else if (! this->fatmsg_enabled && fatmsg_enabled ) {
00234
00235 fatmsg_bufsize = sizeof(worldinfo_header_t) + sizeof(worldinfo_message_header_t)
00236 + sizeof(worldinfo_fat_message_t);
00237 fatmsg_buf = calloc(1, fatmsg_bufsize);
00238 fatmsg_header = (worldinfo_header_t *)fatmsg_buf;
00239 fatmsg_msgheader = (worldinfo_message_header_t *)((char *)fatmsg_buf + sizeof(worldinfo_header_t));
00240 fatmsg = (worldinfo_fat_message_t *)((char *)fatmsg_buf + sizeof(worldinfo_header_t)
00241 + sizeof(worldinfo_message_header_t));
00242 }
00243
00244 this->fatmsg_enabled = fatmsg_enabled;
00245 }
00246
00247
00248
00249
00250
00251
00252
00253 void
00254 WorldInfoTransceiver::add_handler(WorldInfoHandler *h)
00255 {
00256 handlers.lock();
00257 handlers.push_back(h);
00258 handlers.sort();
00259 handlers.unique();
00260 handlers.unlock();
00261 }
00262
00263
00264
00265
00266
00267
00268
00269
00270 void
00271 WorldInfoTransceiver::rem_handler(WorldInfoHandler *h)
00272 {
00273 handlers.remove_locked(h);
00274 }
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285 void
00286 WorldInfoTransceiver::flush_sequence_numbers(unsigned int sec)
00287 {
00288 time_t limit = time(NULL) - sec;
00289
00290 std::map<uint32_t, time_t>::iterator lrtit2;
00291 lrtit = last_received_time.begin();
00292 while (lrtit != last_received_time.end()) {
00293 if ( (*lrtit).second < limit ) {
00294 sequence_numbers.erase((*lrtit).first);
00295 lrtit2 = lrtit;
00296 ++lrtit;
00297 last_received_time.erase(lrtit2);
00298 } else {
00299 ++lrtit;
00300 }
00301 }
00302 }
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322 void
00323 WorldInfoTransceiver::set_pose(float x, float y, float theta, float *covariance)
00324 {
00325 pose_x = x;
00326 pose_y = y;
00327 pose_theta = theta;
00328 pose_covariance = covariance;
00329 pose_changed = true;
00330 }
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345 void
00346 WorldInfoTransceiver::set_velocity(float vel_x, float vel_y, float vel_theta, float *covariance)
00347 {
00348 this->vel_x = vel_x;
00349 this->vel_y = vel_y;
00350 this->vel_theta = vel_theta;
00351 this->vel_covariance = covariance;
00352 this->vel_changed = true;
00353 }
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371 void
00372 WorldInfoTransceiver::set_rel_ball_pos(float dist, float bearing, float slope, float *covariance)
00373 {
00374 rel_ball_dist = dist;
00375 rel_ball_bearing = bearing;
00376 rel_ball_slope = slope;
00377 rel_ball_covariance = covariance;
00378 rel_ball_changed = true;
00379 }
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396 void
00397 WorldInfoTransceiver::set_glob_ball_pos(float x, float y, float z, float *covariance)
00398 {
00399 glob_ball_x = x;
00400 glob_ball_y = y;
00401 glob_ball_z = z;
00402 glob_ball_covariance = covariance;
00403 glob_ball_changed = true;
00404 }
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419 void
00420 WorldInfoTransceiver::set_rel_ball_visible(bool visible, int visibility_history)
00421 {
00422 rel_ball_visible = visible;
00423 rel_ball_visibility_history = visibility_history;
00424 rel_ball_changed = true;
00425 }
00426
00427
00428
00429
00430
00431
00432
00433 void
00434 WorldInfoTransceiver::set_glob_ball_visible(bool visible, int visibility_history)
00435 {
00436 glob_ball_visible = visible;
00437 glob_ball_visibility_history = visibility_history;
00438 glob_ball_changed = true;
00439 }
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453 void
00454 WorldInfoTransceiver::set_rel_ball_velocity(float vel_x, float vel_y, float vel_z,
00455 float *covariance)
00456 {
00457 rel_ball_vel_x = vel_x;
00458 rel_ball_vel_y = vel_y;
00459 rel_ball_vel_z = vel_z;
00460 rel_ball_vel_covariance = covariance;
00461 rel_ball_vel_changed = true;
00462 }
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476 void
00477 WorldInfoTransceiver::set_glob_ball_velocity(float vel_x, float vel_y, float vel_z,
00478 float *covariance)
00479 {
00480 glob_ball_vel_x = vel_x;
00481 glob_ball_vel_y = vel_y;
00482 glob_ball_vel_z = vel_z;
00483 glob_ball_vel_covariance = covariance;
00484 glob_ball_vel_changed = true;
00485 }
00486
00487
00488
00489
00490
00491
00492 void
00493 WorldInfoTransceiver::set_gamestate(int gamestate,
00494 worldinfo_gamestate_team_t state_team)
00495 {
00496 if ((gamestate < 0) || (gamestate >= 16)) {
00497 throw OutOfBoundsException("Illegal gamestate", gamestate, 0, 15);
00498 }
00499 gamestate_msg.game_state = gamestate;
00500 gamestate_msg.state_team = state_team;
00501 gamestate_changed = true;
00502 }
00503
00504
00505
00506
00507
00508
00509 void
00510 WorldInfoTransceiver::set_score(unsigned int score_cyan, unsigned int score_magenta)
00511 {
00512 gamestate_msg.score_cyan = score_cyan;
00513 gamestate_msg.score_magenta = score_magenta;
00514 gamestate_changed = true;
00515 }
00516
00517
00518
00519
00520
00521
00522
00523 void
00524 WorldInfoTransceiver::add_penalty(unsigned int player, unsigned int penalty,
00525 unsigned int seconds_remaining)
00526 {
00527 worldinfo_penalty_message_t pm;
00528 pm.reserved = 0;
00529 pm.player = player;
00530 pm.penalty = penalty;
00531 pm.seconds_remaining = seconds_remaining;
00532 penalties[player] = pm;
00533 }
00534
00535
00536
00537
00538
00539 void
00540 WorldInfoTransceiver::set_team_goal(worldinfo_gamestate_team_t our_team,
00541 worldinfo_gamestate_goalcolor_t goal_color)
00542 {
00543 gamestate_msg.our_team = our_team;
00544 gamestate_msg.our_goal_color = goal_color;
00545 gamestate_changed = true;
00546 }
00547
00548
00549
00550
00551
00552 void
00553 WorldInfoTransceiver::set_half(worldinfo_gamestate_half_t half)
00554 {
00555 gamestate_msg.half = half;
00556 gamestate_changed = true;
00557 }
00558
00559
00560
00561
00562
00563
00564 void
00565 WorldInfoTransceiver::clear_opponents()
00566 {
00567 opponents.clear();
00568 }
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587 void
00588 WorldInfoTransceiver::add_opponent(unsigned int uid,
00589 float distance, float bearing, float *covariance)
00590 {
00591 opponent_t o = { uid, distance, bearing, covariance };
00592 opponents.push_back(o);
00593 }
00594
00595
00596
00597
00598
00599
00600
00601
00602 void
00603 WorldInfoTransceiver::add_disappeared_opponent(unsigned int uid)
00604 {
00605 disappeared_opponents.push_back(uid);
00606 }
00607
00608
00609
00610
00611
00612
00613
00614
00615 void
00616 WorldInfoTransceiver::append_outbound(uint16_t msg_type,
00617 void *msg, uint16_t msg_size)
00618 {
00619 worldinfo_message_header_t mh;
00620
00621 if ( (outbound_bytes + sizeof(mh) + msg_size ) > WORLDINFO_MTU ) {
00622 throw OutOfMemoryException();
00623 }
00624
00625
00626 mh.type = htons(msg_type);
00627 mh.size = htons(msg_size);
00628 memcpy(outbound_buffer, &mh, sizeof(mh));
00629
00630 outbound_bytes += sizeof(mh);
00631 outbound_buffer += sizeof(mh);
00632
00633
00634 memcpy(outbound_buffer, msg, msg_size);
00635 outbound_bytes += msg_size;
00636 outbound_buffer += msg_size;
00637 ++outbound_num_msgs;
00638 }
00639
00640
00641
00642
00643 void
00644 WorldInfoTransceiver::reset_outbound()
00645 {
00646 worldinfo_header_t *header = (worldinfo_header_t *)out_buffer;
00647 header->beef = htons(0xBEEF);
00648 header->version = WORLDINFO_VERSION;
00649
00650 if ( fatmsg_enabled ) {
00651 memset(fatmsg_buf, 0, fatmsg_bufsize);
00652 fatmsg_header->beef = htons(0xBEEF);
00653 fatmsg_header->version = WORLDINFO_VERSION;
00654 fatmsg_msgheader->type = htons(WORLDINFO_MSGTYPE_FAT_WORLDINFO);
00655 fatmsg_msgheader->size = htons(sizeof(worldinfo_fat_message_t));
00656 }
00657
00658 outbound_buffer = (unsigned char *)out_buffer + sizeof(worldinfo_header_t);
00659 outbound_bytes = sizeof(worldinfo_header_t);
00660 outbound_num_msgs = 0;
00661 }
00662
00663
00664
00665
00666
00667
00668 void
00669 WorldInfoTransceiver::send()
00670 {
00671 worldinfo_header_t *header = (worldinfo_header_t *)out_buffer;
00672
00673 reset_outbound();
00674
00675 if ( pose_changed ) {
00676 worldinfo_pose_message_t pm;
00677 pm.x = pose_x;
00678 pm.y = pose_y;
00679 pm.theta = pose_theta;
00680 memcpy(&(pm.covariance), pose_covariance, sizeof(pm.covariance));
00681 pose_changed = false;
00682
00683 append_outbound(WORLDINFO_MSGTYPE_POSE, &pm, sizeof(pm));
00684
00685 if ( fatmsg_enabled ) {
00686
00687 memcpy(&(fatmsg->pose), &pm, sizeof(pm));
00688 fatmsg->valid_pose = 1;
00689 }
00690 } else {
00691 if ( fatmsg_enabled ) {
00692 fatmsg->valid_pose = 0;
00693 }
00694 }
00695
00696 if ( vel_changed ) {
00697 worldinfo_velocity_message_t vm;
00698 vm.vel_x = vel_x;
00699 vm.vel_y = vel_y;
00700 vm.vel_theta = vel_theta;
00701 memcpy(&(vm.covariance), vel_covariance, sizeof(vm.covariance));
00702 vel_changed = false;
00703
00704 append_outbound(WORLDINFO_MSGTYPE_VELO, &vm, sizeof(vm));
00705
00706 if ( fatmsg_enabled ) {
00707
00708 memcpy(&(fatmsg->velo), &vm, sizeof(vm));
00709 fatmsg->valid_velo = 1;
00710 }
00711 } else {
00712 if ( fatmsg_enabled ) {
00713 fatmsg->valid_velo = 0;
00714 }
00715 }
00716
00717 if ( rel_ball_changed ) {
00718 worldinfo_relballpos_message_t bm;
00719 bm.dist = rel_ball_dist;
00720 bm.bearing = rel_ball_bearing;
00721 bm.slope = rel_ball_slope;
00722 bm.history = rel_ball_visibility_history;
00723 bm.visible = rel_ball_visible ? -1 : 0;
00724 memcpy(&(bm.covariance), rel_ball_covariance, sizeof(bm.covariance));
00725
00726 rel_ball_changed = false;
00727
00728 append_outbound(WORLDINFO_MSGTYPE_GLOBBALL, &bm, sizeof(bm));
00729
00730 if ( fatmsg_enabled ) {
00731
00732 memcpy(&(fatmsg->relball_pos), &bm, sizeof(bm));
00733 fatmsg->valid_relball_pos = 1;
00734 }
00735 } else {
00736 if ( fatmsg_enabled ) {
00737 fatmsg->valid_relball_pos = 0;
00738 }
00739 }
00740
00741 if ( glob_ball_changed ) {
00742 worldinfo_globballpos_message_t bm;
00743 bm.x = glob_ball_x;
00744 bm.y = glob_ball_y;
00745 bm.z = glob_ball_z;
00746 bm.history = glob_ball_visibility_history;
00747 bm.visible = glob_ball_visible ? -1 : 0;
00748 memcpy(&(bm.covariance), glob_ball_covariance, sizeof(bm.covariance));
00749
00750 glob_ball_changed = false;
00751
00752 append_outbound(WORLDINFO_MSGTYPE_GLOBBALL, &bm, sizeof(bm));
00753 }
00754
00755 if ( gamestate_changed ) {
00756 append_outbound(WORLDINFO_MSGTYPE_GAMESTATE,
00757 &gamestate_msg, sizeof(worldinfo_gamestate_message_t));
00758 gamestate_changed = false;
00759 }
00760
00761 if ( rel_ball_vel_changed ) {
00762 worldinfo_relballvelo_message_t rbvm;
00763 rbvm.vel_x = rel_ball_vel_x;
00764 rbvm.vel_y = rel_ball_vel_y;
00765 rbvm.vel_z = rel_ball_vel_z;
00766 memcpy(&(rbvm.covariance), rel_ball_vel_covariance, sizeof(rbvm.covariance));
00767 rel_ball_vel_changed = false;
00768
00769 append_outbound(WORLDINFO_MSGTYPE_RELBALLVELO, &rbvm, sizeof(rbvm));
00770
00771 if ( fatmsg_enabled ) {
00772
00773 memcpy(&(fatmsg->relball_velo), &rbvm, sizeof(rbvm));
00774 fatmsg->valid_relball_velo = 1;
00775 }
00776 } else {
00777 if ( fatmsg_enabled ) {
00778 fatmsg->valid_relball_velo = 0;
00779 }
00780 }
00781
00782 if ( glob_ball_vel_changed ) {
00783 worldinfo_globballvelo_message_t rbvm;
00784 rbvm.vel_x = glob_ball_vel_x;
00785 rbvm.vel_y = glob_ball_vel_y;
00786 rbvm.vel_z = glob_ball_vel_z;
00787 memcpy(&(rbvm.covariance), glob_ball_vel_covariance, sizeof(rbvm.covariance));
00788 glob_ball_vel_changed = false;
00789
00790 append_outbound(WORLDINFO_MSGTYPE_GLOBBALLVELO, &rbvm, sizeof(rbvm));
00791 }
00792
00793
00794 for (penit = penalties.begin(); penit != penalties.end(); ++penit) {
00795 append_outbound(WORLDINFO_MSGTYPE_PENALTY,
00796 &(penit->second), sizeof(worldinfo_penalty_message_t));
00797 }
00798 penalties.clear();
00799
00800
00801 unsigned int num_opponents = 0;
00802 for ( oppit = opponents.begin(); oppit != opponents.end(); ++oppit) {
00803 worldinfo_opppose_message_t opm;
00804 opm.uid = (*oppit).uid;
00805 opm.dist = (*oppit).distance;
00806 opm.bearing = (*oppit).bearing;
00807 memcpy(&(opm.covariance), (*oppit).covariance, sizeof(opm.covariance));
00808
00809 append_outbound(WORLDINFO_MSGTYPE_OPP_POSE, &opm, sizeof(opm));
00810
00811 if ( fatmsg_enabled ) {
00812 if ( num_opponents < WORLDINFO_FATMSG_NUMOPPS ) {
00813
00814 memcpy(&(fatmsg->opponents[num_opponents]), &opm, sizeof(opm));
00815 ++num_opponents;
00816 fatmsg->num_opponents = num_opponents;
00817 }
00818 }
00819 }
00820 opponents.clear();
00821
00822 for ( doppit = disappeared_opponents.begin(); doppit != disappeared_opponents.end(); ++doppit) {
00823 worldinfo_oppdisappeared_message_t opdm;
00824 opdm.uid = *doppit;
00825
00826 append_outbound(WORLDINFO_MSGTYPE_OPP_DISAPP, &opdm, sizeof(opdm));
00827 }
00828 disappeared_opponents.clear();
00829
00830 if ( outbound_num_msgs > 0 ) {
00831
00832 header->seq = htonl(out_seq++);
00833
00834 encryptor->set_plain_buffer(out_buffer, outbound_bytes);
00835 crypted_out_bytes = encryptor->encrypt();
00836
00837 s->send(crypted_out_buffer, crypted_out_bytes);
00838
00839 if ( fatmsg_enabled ) {
00840
00841 fatmsg_header->seq = htonl(out_seq++);
00842
00843 encryptor->set_plain_buffer(fatmsg_buf, fatmsg_bufsize);
00844 crypted_out_bytes = encryptor->encrypt();
00845
00846 s->send(crypted_out_buffer, crypted_out_bytes);
00847 }
00848 }
00849
00850 }
00851
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876 void
00877 WorldInfoTransceiver::recv(bool block, unsigned int max_num_msgs)
00878 {
00879 if ( ! block ) {
00880 if ( ! s->available() ) {
00881 return;
00882 }
00883 }
00884
00885 handlers.lock();
00886
00887 unsigned int num_msgs = (max_num_msgs == 0 ? 0 : 1);
00888 do {
00889 struct sockaddr_in from;
00890 socklen_t addr_len = sizeof(from);
00891 size_t bytes = crypt_buffer_size;
00892
00893 if ( max_num_msgs != 0 ) ++num_msgs;
00894
00895 bytes = s->recv(crypted_in_buffer, bytes, (struct sockaddr *)&from, &addr_len);
00896
00897
00898 if (!loop) {
00899 struct in_addr localhost;
00900 ::inet_aton("127.0.0.1", &localhost);
00901 if (from.sin_addr.s_addr == localhost.s_addr) {
00902 continue;
00903 }
00904 }
00905
00906
00907 decryptor->set_crypt_buffer(crypted_in_buffer, bytes);
00908 try {
00909 inbound_bytes = decryptor->decrypt();
00910 } catch (MessageDecryptionException &e) {
00911 LibLogger::log_warn("WorldInfoTransceiver", "Message decryption failed, ignoring");
00912 LibLogger::log_warn("WorldInfoTransceiver", e);
00913 continue;
00914 }
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926 worldinfo_header_t *header = (worldinfo_header_t *)in_buffer;
00927 if ( ntohs(header->beef) != 0xBEEF ) {
00928
00929 LibLogger::log_warn("WorldInfoTransceiver", "Invalid message received (no 0xBEEF), ignoring");
00930 continue;
00931 }
00932
00933 if ( header->version != WORLDINFO_VERSION ) {
00934 LibLogger::log_warn("WorldInfoTransceiver", "Unsupported version of world info data received, ignoring");
00935 continue;
00936 }
00937
00938
00939
00940 unsigned int cseq = ntohl(header->seq);
00941 if ( sequence_numbers.find(from.sin_addr.s_addr) != sequence_numbers.end() ) {
00942 if ( cseq <= sequence_numbers[from.sin_addr.s_addr] ) {
00943
00944 LibLogger::log_warn("WorldInfoTransceiver", "Received packet twice, ignoring");
00945 continue;
00946 }
00947 }
00948 sequence_numbers[from.sin_addr.s_addr] = cseq;
00949 last_received_time[from.sin_addr.s_addr] = time(NULL);
00950
00951 inbound_bytes -= sizeof(worldinfo_header_t);
00952 inbound_buffer = (unsigned char *)in_buffer + sizeof(worldinfo_header_t);
00953
00954 std::string hostname_s;
00955 if ( ! resolver->resolve_address((struct sockaddr *)&from, sizeof(from), hostname_s) ) {
00956 hostname_s = "unknown";
00957 }
00958 const char *hostname = hostname_s.c_str();
00959
00960
00961 while ( inbound_bytes > 0 ) {
00962 worldinfo_message_header_t *msgh = (worldinfo_message_header_t *)inbound_buffer;
00963 inbound_bytes -= sizeof(worldinfo_message_header_t);
00964 inbound_buffer += sizeof(worldinfo_message_header_t);
00965 uint16_t msg_type = ntohs(msgh->type);
00966 uint16_t msg_size = ntohs(msgh->size);
00967
00968
00969 if ( inbound_bytes < msg_size ) {
00970 LibLogger::log_warn("WorldInfoTransceiver", "Truncated packet received or protocol "
00971 "error, ignoring rest of packet (got %zu bytes, but expected "
00972 "%zu bytes)", inbound_bytes, msg_size);
00973 break;
00974 }
00975 switch ( msg_type ) {
00976 case WORLDINFO_MSGTYPE_POSE:
00977 if ( msg_size == sizeof(worldinfo_pose_message_t) ) {
00978 worldinfo_pose_message_t *pose_msg = (worldinfo_pose_message_t *)inbound_buffer;
00979 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
00980 (*hit)->pose_rcvd(hostname,
00981 pose_msg->x, pose_msg->y, pose_msg->theta,
00982 pose_msg->covariance);
00983 }
00984 } else {
00985 LibLogger::log_warn("WorldInfoTransceiver", "Invalid pose message received "
00986 "(got %zu bytes but expected %zu bytes), ignoring",
00987 msg_size, sizeof(worldinfo_pose_message_t));
00988 }
00989 break;
00990
00991 case WORLDINFO_MSGTYPE_VELO:
00992 if ( msg_size == sizeof(worldinfo_velocity_message_t) ) {
00993 worldinfo_velocity_message_t *velo_msg = (worldinfo_velocity_message_t *)inbound_buffer;
00994 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
00995 (*hit)->velocity_rcvd(hostname,
00996 velo_msg->vel_x, velo_msg->vel_y, velo_msg->vel_theta,
00997 velo_msg->covariance);
00998 }
00999 } else {
01000 LibLogger::log_warn("WorldInfoTransceiver", "Invalid velocity message received "
01001 "(got %zu bytes but expected %zu bytes), ignoring",
01002 msg_size, sizeof(worldinfo_velocity_message_t));
01003 }
01004 break;
01005
01006 case WORLDINFO_MSGTYPE_RELBALL:
01007 if ( msg_size == sizeof(worldinfo_relballpos_message_t) ) {
01008 worldinfo_relballpos_message_t *ball_msg = (worldinfo_relballpos_message_t *)inbound_buffer;
01009 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01010 (*hit)->ball_pos_rcvd(hostname,
01011 (ball_msg->visible == -1), ball_msg->history,
01012 ball_msg->dist, ball_msg->bearing, ball_msg->slope,
01013 ball_msg->covariance);
01014 }
01015 } else {
01016 LibLogger::log_warn("WorldInfoTransceiver", "Invalid relative ball pos message received "
01017 "(got %zu bytes but expected %zu bytes), ignoring",
01018 msg_size, sizeof(worldinfo_relballpos_message_t));
01019 }
01020 break;
01021
01022 case WORLDINFO_MSGTYPE_GLOBBALL:
01023 if ( msg_size == sizeof(worldinfo_globballpos_message_t) ) {
01024 worldinfo_globballpos_message_t *ball_msg = (worldinfo_globballpos_message_t *)inbound_buffer;
01025 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01026 (*hit)->global_ball_pos_rcvd(hostname,
01027 (ball_msg->visible == -1), ball_msg->history,
01028 ball_msg->x, ball_msg->y, ball_msg->z,
01029 ball_msg->covariance);
01030 }
01031 } else {
01032 LibLogger::log_warn("WorldInfoTransceiver", "Invalid global ball pos message received "
01033 "(got %zu bytes but expected %zu bytes), ignoring",
01034 msg_size, sizeof(worldinfo_globballpos_message_t));
01035 }
01036 break;
01037
01038 case WORLDINFO_MSGTYPE_RELBALLVELO:
01039 if ( msg_size == sizeof(worldinfo_relballvelo_message_t) ) {
01040 worldinfo_relballvelo_message_t *bvel_msg = (worldinfo_relballvelo_message_t *)inbound_buffer;
01041 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01042 (*hit)->ball_velocity_rcvd(hostname,
01043 bvel_msg->vel_x, bvel_msg->vel_y, bvel_msg->vel_z,
01044 bvel_msg->covariance);
01045 }
01046 } else {
01047 LibLogger::log_warn("WorldInfoTransceiver", "Invalid relative ball velocity message received "
01048 "(got %zu bytes but expected %zu bytes), ignoring",
01049 msg_size, sizeof(worldinfo_relballvelo_message_t));
01050 }
01051 break;
01052
01053 case WORLDINFO_MSGTYPE_GLOBBALLVELO:
01054 if ( msg_size == sizeof(worldinfo_globballvelo_message_t) ) {
01055 worldinfo_globballvelo_message_t *bvel_msg = (worldinfo_globballvelo_message_t *)inbound_buffer;
01056 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01057 (*hit)->global_ball_velocity_rcvd(hostname,
01058 bvel_msg->vel_x, bvel_msg->vel_y, bvel_msg->vel_z,
01059 bvel_msg->covariance);
01060 }
01061 } else {
01062 LibLogger::log_warn("WorldInfoTransceiver", "Invalid global ball velocity message received "
01063 "(got %zu bytes but expected %zu bytes), ignoring",
01064 msg_size, sizeof(worldinfo_globballvelo_message_t));
01065 }
01066 break;
01067
01068 case WORLDINFO_MSGTYPE_OPP_POSE:
01069 if ( msg_size == sizeof(worldinfo_opppose_message_t) ) {
01070 worldinfo_opppose_message_t *oppp_msg = (worldinfo_opppose_message_t *)inbound_buffer;
01071 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01072 (*hit)->opponent_pose_rcvd(hostname,
01073 oppp_msg->uid, oppp_msg->dist, oppp_msg->bearing,
01074 oppp_msg->covariance);
01075 }
01076 } else {
01077 LibLogger::log_warn("WorldInfoTransceiver", "Invalid opponent pose message received "
01078 "(got %zu bytes but expected %zu bytes), ignoring",
01079 msg_size, sizeof(worldinfo_opppose_message_t));
01080 }
01081 break;
01082
01083 case WORLDINFO_MSGTYPE_OPP_DISAPP:
01084 if ( msg_size == sizeof(worldinfo_oppdisappeared_message_t) ) {
01085 worldinfo_oppdisappeared_message_t *oppd_msg = (worldinfo_oppdisappeared_message_t *)inbound_buffer;
01086 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01087 (*hit)->opponent_disapp_rcvd(hostname, oppd_msg->uid);
01088 }
01089 } else {
01090 LibLogger::log_warn("WorldInfoTransceiver", "Invalid opponent disappeared message received "
01091 "(got %zu bytes but expected %zu bytes), ignoring",
01092 msg_size, sizeof(worldinfo_oppdisappeared_message_t));
01093 }
01094 break;
01095
01096 case WORLDINFO_MSGTYPE_GAMESTATE:
01097 if ( msg_size == sizeof(worldinfo_gamestate_message_t) ) {
01098 worldinfo_gamestate_message_t *gs_msg = (worldinfo_gamestate_message_t *)inbound_buffer;
01099 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01100 (*hit)->gamestate_rcvd(hostname,
01101 gs_msg->game_state,
01102 (worldinfo_gamestate_team_t)gs_msg->state_team,
01103 gs_msg->score_cyan, gs_msg->score_magenta,
01104 (worldinfo_gamestate_team_t)gs_msg->our_team,
01105 (worldinfo_gamestate_goalcolor_t)gs_msg->our_goal_color,
01106 (worldinfo_gamestate_half_t)gs_msg->half);
01107 }
01108 } else {
01109 LibLogger::log_warn("WorldInfoTransceiver", "Invalid gamestate message received "
01110 "(got %zu bytes but expected %zu bytes), ignoring",
01111 msg_size, sizeof(worldinfo_gamestate_message_t));
01112 }
01113 break;
01114
01115 case WORLDINFO_MSGTYPE_PENALTY:
01116 if ( msg_size == sizeof(worldinfo_penalty_message_t) ) {
01117 worldinfo_penalty_message_t *p_msg = (worldinfo_penalty_message_t *)inbound_buffer;
01118 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01119 (*hit)->penalty_rcvd(hostname,
01120 p_msg->player, p_msg->penalty, p_msg->seconds_remaining);
01121 }
01122
01123 } else {
01124 LibLogger::log_warn("WorldInfoTransceiver", "Invalid penalty message received "
01125 "(got %zu bytes but expected %zu bytes), ignoring",
01126 msg_size, sizeof(worldinfo_penalty_message_t));
01127 }
01128 break;
01129
01130 case WORLDINFO_MSGTYPE_FAT_WORLDINFO:
01131 if ( msg_size == sizeof(worldinfo_fat_message_t) ) {
01132 worldinfo_fat_message_t *fat_msg = (worldinfo_fat_message_t *)inbound_buffer;
01133 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
01134 if ( fat_msg->valid_pose ) {
01135 (*hit)->pose_rcvd(hostname,
01136 fat_msg->pose.x, fat_msg->pose.y, fat_msg->pose.theta,
01137 fat_msg->pose.covariance);
01138 }
01139
01140 if ( fat_msg->valid_velo ) {
01141 (*hit)->velocity_rcvd(hostname,
01142 fat_msg->velo.vel_x, fat_msg->velo.vel_y,
01143 fat_msg->velo.vel_theta, fat_msg->velo.covariance);
01144 }
01145 if ( fat_msg->valid_relball_pos ) {
01146 (*hit)->ball_pos_rcvd(hostname,
01147 (fat_msg->relball_pos.visible == -1),
01148 fat_msg->relball_pos.history,
01149 fat_msg->relball_pos.dist, fat_msg->relball_pos.bearing,
01150 fat_msg->relball_pos.slope, fat_msg->relball_pos.covariance);
01151 }
01152 if ( fat_msg->valid_relball_velo ) {
01153 (*hit)->ball_velocity_rcvd(hostname,
01154 fat_msg->relball_velo.vel_x,
01155 fat_msg->relball_velo.vel_y,
01156 fat_msg->relball_velo.vel_z,
01157 fat_msg->relball_velo.covariance);
01158 }
01159
01160 if ( fat_msg->num_opponents > WORLDINFO_FATMSG_NUMOPPS ) {
01161
01162 LibLogger::log_warn("WorldInfoTransceiver", "Too many opponents marked valid in message "
01163 "(got %zu but expected a maximum of %zu), ignoring",
01164 fat_msg->num_opponents, WORLDINFO_FATMSG_NUMOPPS);
01165 } else {
01166 for ( unsigned int i = 0; i < fat_msg->num_opponents; ++i ) {
01167 (*hit)->opponent_pose_rcvd(hostname,
01168 fat_msg->opponents[i].uid,
01169 fat_msg->opponents[i].dist,
01170 fat_msg->opponents[i].bearing,
01171 fat_msg->opponents[i].covariance);
01172 }
01173 }
01174 }
01175 } else {
01176 LibLogger::log_warn("WorldInfoTransceiver", "Invalid fat message received "
01177 "(got %zu bytes but expected %zu bytes), ignoring",
01178 msg_size, sizeof(worldinfo_fat_message_t));
01179 }
01180 break;
01181
01182
01183 default:
01184 LibLogger::log_warn("WorldInfoTransceiver", "Unknown message type %u received "
01185 ", ignoring", msg_type);
01186 }
01187
01188 inbound_bytes -= msg_size;
01189 inbound_buffer += msg_size;
01190 }
01191
01192 } while ( s->available() && (num_msgs <= max_num_msgs) );
01193
01194 handlers.unlock();
01195 }
01196
01197
01198
01199
01200
01201
01202 void *
01203 WorldInfoTransceiver::last_sent_plain_buffer()
01204 {
01205 return out_buffer;
01206 }
01207
01208
01209
01210
01211
01212
01213 size_t
01214 WorldInfoTransceiver::last_sent_plain_buffer_size()
01215 {
01216 return outbound_bytes;
01217 }
01218
01219
01220
01221
01222
01223
01224 void *
01225 WorldInfoTransceiver::last_sent_crypted_buffer()
01226 {
01227 return crypted_out_buffer;
01228 }
01229
01230
01231
01232
01233
01234
01235 size_t
01236 WorldInfoTransceiver::last_sent_crypted_buffer_size()
01237 {
01238 return crypted_out_bytes;
01239 }
01240
01241 }