Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * transceiver.h - World Info Transceiver 00004 * 00005 * Created: Sun Jan 21 14:15:32 2007 00006 * Copyright 2006-2007 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 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_WRE file in the doc directory. 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 <arpa/inet.h> 00037 #include <netinet/in.h> 00038 #include <cstdlib> 00039 #include <cstring> 00040 00041 namespace fawkes { 00042 00043 /** @class WorldInfoException transceiver.h <netcomm/worldinfo/transceiver.h> 00044 * Thrown on critical errors in world info handling. 00045 * @ingroup NetComm 00046 */ 00047 00048 /** Constructor. 00049 * @param msg message 00050 */ 00051 WorldInfoException::WorldInfoException(const char *msg) 00052 : Exception(msg) 00053 { 00054 } 00055 00056 00057 /** @class WorldInfoTransceiver transceiver.h <netcomm/worldinfo/transceiver.h> 00058 * Class to send and receive world information. 00059 * An important point in a domain of cooperating soccer robots is transmitting 00060 * and receiving a robot's belief of its surrounding. The world info 00061 * transceiver does exactly that. It allows for sending information about the 00062 * robot's pose and velocity and its perception of the ball and other robots 00063 * on the field. 00064 * 00065 * The unit for distances and positions is meter (m), speed is given in 00066 * meter per second (m/s), angles are given in radiant (rad). Angles can be in 00067 * the range 0 to 2 * PI or -PI to PI. Since they can be converted easily 00068 * between these ranges without further information users of such information 00069 * shall be able to process both. 00070 * 00071 * Coordinates are given in a right-handed coordinate system with the origin in 00072 * center of the field, X pointing towards the opponent goal, Y to the right 00073 * and Z downwards. 00074 * 00075 * Information is transmitted with a simple protocol via UDP Multicast or 00076 * Broadcast packets. 00077 * 00078 * A call to send() will reset all information, thus all opponents are removed 00079 * from the list to be sent, positions of robot and ball are marked invalid. 00080 * You have to call the appropriate set methods before the information is sent. 00081 * You can thus call send() at any time but only changed information 00082 * (information set since last send() call) is transmitted over the network. 00083 * 00084 * @ingroup NetComm 00085 * @author Tim Niemueller 00086 */ 00087 00088 00089 /** Constructor. 00090 * @param socket_type either multicast or broadcast socket 00091 * @param addr multicast or broadcast address to send information to and receive from 00092 * @param port UDP port to send information to and receive from 00093 * @param key encryption key 00094 * @param iv encryption initialisation vector 00095 * @param resolver An initialized network resolver, is NULL is supplied 00096 * an internal resolver will be created without mDNS support. 00097 * @exception OutOfMemoryException thrown if internal buffers cannot be created 00098 */ 00099 WorldInfoTransceiver::WorldInfoTransceiver(SocketType socket_type, 00100 const char *addr, unsigned short port, 00101 const char *key, const char *iv, 00102 NetworkNameResolver *resolver) : 00103 pose_changed( false ), 00104 vel_changed( false ), 00105 rel_ball_changed( false ), 00106 rel_ball_vel_changed( false ), 00107 glob_ball_changed( false ), 00108 glob_ball_vel_changed( false ), 00109 gamestate_changed( false ) 00110 { 00111 try { 00112 switch (socket_type) { 00113 case MULTICAST: { 00114 MulticastDatagramSocket* ms = new MulticastDatagramSocket(addr, port); 00115 ms->bind(); 00116 s = ms; 00117 break; 00118 } 00119 case BROADCAST: { 00120 BroadcastDatagramSocket* bs = new BroadcastDatagramSocket(addr, port); 00121 bs->bind(); 00122 s = bs; 00123 break; 00124 } 00125 } 00126 set_loop(false); 00127 } catch (SocketException &e) { 00128 e.append("WorldInfoTransceiver cannot instantiate socket for %s:%u", addr, port); 00129 throw; 00130 } 00131 00132 in_buffer = malloc(WORLDINFO_MTU); 00133 out_buffer = malloc(WORLDINFO_MTU); 00134 if (! in_buffer || ! out_buffer) { 00135 throw OutOfMemoryException(); 00136 } 00137 00138 fatmsg_enabled = false; 00139 fatmsg_bufsize = 0; 00140 fatmsg_buf = NULL; 00141 fatmsg_header = NULL; 00142 fatmsg_msgheader = NULL; 00143 fatmsg = NULL; 00144 00145 __key = strdup(key); 00146 __iv = strdup(iv); 00147 00148 encryptor = new WorldInfoMessageEncryptor((const unsigned char *)__key, (const unsigned char *)__iv); 00149 decryptor = new WorldInfoMessageDecryptor((const unsigned char *)__key, (const unsigned char *)__iv); 00150 00151 // set maximum size buffer to get valid results from encryptor 00152 encryptor->set_plain_buffer(out_buffer, WORLDINFO_MTU); 00153 00154 crypt_buffer_size = encryptor->recommended_crypt_buffer_size(); 00155 crypted_out_buffer = malloc(crypt_buffer_size); 00156 crypted_in_buffer = malloc(crypt_buffer_size); 00157 00158 if (! crypted_in_buffer || ! crypted_out_buffer) { 00159 throw OutOfMemoryException(); 00160 } 00161 00162 encryptor->set_crypt_buffer(crypted_out_buffer, crypt_buffer_size); 00163 00164 decryptor->set_plain_buffer(in_buffer, WORLDINFO_MTU); 00165 00166 if ( resolver == NULL ) { 00167 this->resolver = new NetworkNameResolver(); 00168 resolver_delete = true; 00169 } else { 00170 this->resolver = resolver; 00171 resolver_delete = false; 00172 } 00173 00174 out_seq = 0; 00175 } 00176 00177 00178 /** Destructor. */ 00179 WorldInfoTransceiver::~WorldInfoTransceiver() 00180 { 00181 set_fatmsg_enabled(false); 00182 free(out_buffer); 00183 free(in_buffer); 00184 free(crypted_out_buffer); 00185 free(crypted_in_buffer); 00186 free(__key); 00187 free(__iv); 00188 delete s; 00189 delete encryptor; 00190 delete decryptor; 00191 if ( resolver_delete ) { 00192 delete resolver; 00193 } 00194 } 00195 00196 00197 /** Set loopback of sent packets. 00198 * This sets whether packets should be looped back to local sockets for multicast 00199 * communication. 00200 * @param loop true to deliver sent packets to local sockets, false prevent delivering 00201 * @see MulticastDatagramSocket::set_loop() 00202 */ 00203 void 00204 WorldInfoTransceiver::set_loop(bool loop) 00205 { 00206 MulticastDatagramSocket* ms = dynamic_cast<MulticastDatagramSocket*>(s); 00207 if (s) { 00208 ms->set_loop( loop ); 00209 } 00210 this->loop = loop; 00211 } 00212 00213 00214 /** Enable or disable sending of fat message. 00215 * The fat message is considered to be legacy code and therefore disabled by default. 00216 * If you happen to need the fat message you can enable it using this method and then 00217 * it will be send for every call to send(). 00218 * @param fatmsg_enabled true to enable sending of fat message, false otherwise 00219 */ 00220 void 00221 WorldInfoTransceiver::set_fatmsg_enabled(bool fatmsg_enabled) 00222 { 00223 if ( this->fatmsg_enabled && ! fatmsg_enabled ) { 00224 // fatmsg turned off 00225 free(fatmsg_buf); 00226 fatmsg_buf = NULL; 00227 fatmsg_msgheader = NULL; 00228 fatmsg_header = NULL; 00229 fatmsg = NULL; 00230 fatmsg_bufsize = 0; 00231 } else if (! this->fatmsg_enabled && fatmsg_enabled ) { 00232 // fatmsg turned on 00233 fatmsg_bufsize = sizeof(worldinfo_header_t) + sizeof(worldinfo_message_header_t) 00234 + sizeof(worldinfo_fat_message_t); 00235 fatmsg_buf = calloc(1, fatmsg_bufsize); 00236 fatmsg_header = (worldinfo_header_t *)fatmsg_buf; 00237 fatmsg_msgheader = (worldinfo_message_header_t *)((char *)fatmsg_buf + sizeof(worldinfo_header_t)); 00238 fatmsg = (worldinfo_fat_message_t *)((char *)fatmsg_buf + sizeof(worldinfo_header_t) 00239 + sizeof(worldinfo_message_header_t)); 00240 } // else unchanged 00241 00242 this->fatmsg_enabled = fatmsg_enabled; 00243 } 00244 00245 00246 /** Add a handler for world information. 00247 * World information will be dispatched to all registered handlers as soon it 00248 * is received. 00249 * @param h handler to register 00250 */ 00251 void 00252 WorldInfoTransceiver::add_handler(WorldInfoHandler *h) 00253 { 00254 handlers.lock(); 00255 handlers.push_back(h); 00256 handlers.sort(); 00257 handlers.unique(); 00258 handlers.unlock(); 00259 } 00260 00261 00262 /** Remove handler for world information. 00263 * The handler is removed from the list of handlers that incoming information 00264 * is dispatched to. No error is thrown if the handler was never registered 00265 * so it is safe to call this for any handler. 00266 * @param h handler to remove from subscriber list 00267 */ 00268 void 00269 WorldInfoTransceiver::rem_handler(WorldInfoHandler *h) 00270 { 00271 handlers.remove_locked(h); 00272 } 00273 00274 00275 /** Flush sequence numbers conditionally. 00276 * This will conditionally flush the sequence numbers stored per sender. The 00277 * sequence numbers are stored per IP. With this method you can flush the 00278 * sequence numbers that have been inactive for a specified time. A recommended 00279 * value is 10 seconds. You may NOT call this concurrently to recv()! 00280 * @param sec number of seconds since that must have passed without a message 00281 * to remove a specific IP from sequence list 00282 */ 00283 void 00284 WorldInfoTransceiver::flush_sequence_numbers(unsigned int sec) 00285 { 00286 time_t limit = time(NULL) - sec; 00287 00288 std::map<uint32_t, time_t>::iterator lrtit2; 00289 lrtit = last_received_time.begin(); 00290 while (lrtit != last_received_time.end()) { 00291 if ( (*lrtit).second < limit ) { 00292 sequence_numbers.erase((*lrtit).first); 00293 lrtit2 = lrtit; 00294 ++lrtit; 00295 last_received_time.erase(lrtit2); 00296 } else { 00297 ++lrtit; 00298 } 00299 } 00300 } 00301 00302 00303 /** Set global pose of robot. 00304 * Global pose of sensing robot (x, y, theta) with the origin in the 00305 * middle of the field, right handed coordinate system (y to opponent goal, 00306 * x to the right, z pointing upwards, same as in simulation league). 00307 * Theta points in y direction (theta = 0 iff robot front points to opponent 00308 * goal). 00309 * The confidence about the robot's pose is transmitted as a 3x3 covariance 00310 * matrix. 00311 * @param x x position of robot 00312 * @param y y position of robot 00313 * @param theta rotation of robot 00314 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00315 * rows (first row, three floats, second row, three floats, third row, three 00316 * floats). No length check or whatsoever is done. This will crash if c is not 00317 * long enough! c will not be copied but referenced so it has to exist when 00318 * send() is called! 00319 */ 00320 void 00321 WorldInfoTransceiver::set_pose(float x, float y, float theta, float *covariance) 00322 { 00323 pose_x = x; 00324 pose_y = y; 00325 pose_theta = theta; 00326 pose_covariance = covariance; 00327 pose_changed = true; 00328 } 00329 00330 00331 /** Set velocity of the robot. 00332 * Set the current velocity of the robot. 00333 * @param vel_x velocity in x direction 00334 * @param vel_y velocity in y direction 00335 * @param vel_theta rotational velocity, positive velocity means clockwise 00336 * rotation, negative velocity means counter-clockwise. 00337 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00338 * rows (first row, three floats, second row, three floats, third row, three 00339 * floats). No length check or whatsoever is done. This will crash if c is not 00340 * long enough! c will not be copied but referenced so it has to exist when 00341 * send() is called! 00342 */ 00343 void 00344 WorldInfoTransceiver::set_velocity(float vel_x, float vel_y, float vel_theta, float *covariance) 00345 { 00346 this->vel_x = vel_x; 00347 this->vel_y = vel_y; 00348 this->vel_theta = vel_theta; 00349 this->vel_covariance = covariance; 00350 this->vel_changed = true; 00351 } 00352 00353 00354 /** Set ball position. 00355 * Set the ball perception relative to the current robot position. 00356 * Note that the ball position is given in polar coordinates in 00357 * 3D space! 00358 * The confidence about the ball position is transmitted as a 3x3 covariance 00359 * matrix. 00360 * @param dist distance to ball in meters 00361 * @param bearing bearing angle to ball 00362 * @param slope slope angle to ball 00363 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00364 * rows (first row, three floats, second row, three floats, third row, three 00365 * floats). No length check or whatsoever is done. This will crash if c is not 00366 * long enough! c will not be copied but referenced so it has to exist when 00367 * send() is called! 00368 */ 00369 void 00370 WorldInfoTransceiver::set_rel_ball_pos(float dist, float bearing, float slope, float *covariance) 00371 { 00372 rel_ball_dist = dist; 00373 rel_ball_bearing = bearing; 00374 rel_ball_slope = slope; 00375 rel_ball_covariance = covariance; 00376 rel_ball_changed = true; 00377 } 00378 00379 00380 /** Set global ball position. 00381 * Note that the ball position is given in polar coordinates in 00382 * 3D space! 00383 * The confidence about the ball position is transmitted as a 3x3 covariance 00384 * matrix. 00385 * @param x the x-coordinate of the global ball position 00386 * @param y the y-coordinate of the global ball position 00387 * @param z the z-coordinate of the global ball position 00388 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00389 * rows (first row, three floats, second row, three floats, third row, three 00390 * floats). No length check or whatsoever is done. This will crash if c is not 00391 * long enough! c will not be copied but referenced so it has to exist when 00392 * send() is called! 00393 */ 00394 void 00395 WorldInfoTransceiver::set_glob_ball_pos(float x, float y, float z, float *covariance) 00396 { 00397 glob_ball_x = x; 00398 glob_ball_y = y; 00399 glob_ball_z = z; 00400 glob_ball_covariance = covariance; 00401 glob_ball_changed = true; 00402 } 00403 00404 00405 /** Set ball visibility. 00406 * This method defines if the ball is currently visible or not. Additionally more detailed 00407 * information is provided in the visibility history. The history shall be 0 only if the 00408 * vision has just been initialized. It shall be positive if the ball is visible and shall 00409 * have the number of vision cycles in which the ball was visible in a row. It shall be 00410 * negative if the ball is not visible and shall be the negative value of the number 00411 * of frames where the ball was not visible. A value of 30 for example means that the 00412 * ball has been continuously visible for 30 frames, it was never lost. A value of 00413 * -20 means that the ball was not seen for the last 20 frames. 00414 * @param visible true if the ball is visible, false otherwise 00415 * @param visibility_history visibility history, see above. 00416 */ 00417 void 00418 WorldInfoTransceiver::set_rel_ball_visible(bool visible, int visibility_history) 00419 { 00420 rel_ball_visible = visible; 00421 rel_ball_visibility_history = visibility_history; 00422 rel_ball_changed = true; 00423 } 00424 00425 00426 /** Set ball visibility for the global ball. 00427 * Same semantics as set_ball_visible(). 00428 * @param visible true if the ball is visible, false otherwise 00429 * @param visibility_history visibility history, see above. 00430 */ 00431 void 00432 WorldInfoTransceiver::set_glob_ball_visible(bool visible, int visibility_history) 00433 { 00434 glob_ball_visible = visible; 00435 glob_ball_visibility_history = visibility_history; 00436 glob_ball_changed = true; 00437 } 00438 00439 00440 /** Set ball velocity. 00441 * Set the current velocity of the robot. 00442 * @param vel_x velocity in x direction 00443 * @param vel_y velocity in y direction 00444 * @param vel_z velocity in z direction 00445 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00446 * rows (first row, three floats, second row, three floats, third row, three 00447 * floats). No length check or whatsoever is done. This will crash if c is not 00448 * long enough! c will not be copied but referenced so it has to exist when 00449 * send() is called! 00450 */ 00451 void 00452 WorldInfoTransceiver::set_rel_ball_velocity(float vel_x, float vel_y, float vel_z, 00453 float *covariance) 00454 { 00455 rel_ball_vel_x = vel_x; 00456 rel_ball_vel_y = vel_y; 00457 rel_ball_vel_z = vel_z; 00458 rel_ball_vel_covariance = covariance; 00459 rel_ball_vel_changed = true; 00460 } 00461 00462 00463 /** Set global ball velocity. 00464 * Set the current, global velocity of the robot. 00465 * @param vel_x velocity in x direction wrt. to the global frame 00466 * @param vel_y velocity in y direction wrt. to the global frame 00467 * @param vel_z velocity in z direction wrt. to the global frame 00468 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00469 * rows (first row, three floats, second row, three floats, third row, three 00470 * floats). No length check or whatsoever is done. This will crash if c is not 00471 * long enough! c will not be copied but referenced so it has to exist when 00472 * send() is called! 00473 */ 00474 void 00475 WorldInfoTransceiver::set_glob_ball_velocity(float vel_x, float vel_y, float vel_z, 00476 float *covariance) 00477 { 00478 glob_ball_vel_x = vel_x; 00479 glob_ball_vel_y = vel_y; 00480 glob_ball_vel_z = vel_z; 00481 glob_ball_vel_covariance = covariance; 00482 glob_ball_vel_changed = true; 00483 } 00484 00485 00486 /** Set current game state. 00487 * @param gamestate current game state 00488 * @param state_team team referenced by the game state 00489 */ 00490 void 00491 WorldInfoTransceiver::set_gamestate(int gamestate, 00492 worldinfo_gamestate_team_t state_team) 00493 { 00494 if ((gamestate < 0) || (gamestate >= 16)) { 00495 throw OutOfBoundsException("Illegal gamestate", gamestate, 0, 15); 00496 } 00497 gamestate_msg.game_state = gamestate; 00498 gamestate_msg.state_team = state_team; 00499 gamestate_changed = true; 00500 } 00501 00502 00503 /** Set score. 00504 * @param score_cyan current score of team cyan 00505 * @param score_magenta current score of team magenta 00506 */ 00507 void 00508 WorldInfoTransceiver::set_score(unsigned int score_cyan, unsigned int score_magenta) 00509 { 00510 gamestate_msg.score_cyan = score_cyan; 00511 gamestate_msg.score_magenta = score_magenta; 00512 gamestate_changed = true; 00513 } 00514 00515 00516 /** Add penalty message. 00517 * @param player player for which the penalty applies 00518 * @param penalty penalty code 00519 * @param seconds_remaining estimated time in seconds until the penalty is lifted 00520 */ 00521 void 00522 WorldInfoTransceiver::add_penalty(unsigned int player, unsigned int penalty, 00523 unsigned int seconds_remaining) 00524 { 00525 worldinfo_penalty_message_t pm; 00526 pm.reserved = 0; 00527 pm.player = player; 00528 pm.penalty = penalty; 00529 pm.seconds_remaining = seconds_remaining; 00530 penalties[player] = pm; 00531 } 00532 00533 /** Set team and goal info. 00534 * @param our_team our team color 00535 * @param goal_color our goal color 00536 */ 00537 void 00538 WorldInfoTransceiver::set_team_goal(worldinfo_gamestate_team_t our_team, 00539 worldinfo_gamestate_goalcolor_t goal_color) 00540 { 00541 gamestate_msg.our_team = our_team; 00542 gamestate_msg.our_goal_color = goal_color; 00543 gamestate_changed = true; 00544 } 00545 00546 00547 /** Set current half of the game time. 00548 * @param half current half 00549 */ 00550 void 00551 WorldInfoTransceiver::set_half(worldinfo_gamestate_half_t half) 00552 { 00553 gamestate_msg.half = half; 00554 gamestate_changed = true; 00555 } 00556 00557 00558 /** Clear opponents list. 00559 * Clear the list of opponents that has to be transmitted. This is done 00560 * implicitly in send(). 00561 */ 00562 void 00563 WorldInfoTransceiver::clear_opponents() 00564 { 00565 opponents.clear(); 00566 } 00567 00568 00569 /** Add opponent to transmit list. 00570 * Add an opponent to the list of opponents to be transmitted on next send() 00571 * call. Opponents are given in a 2D polar coordinate system (assumption is that 00572 * robots don't fly in the soccer domain). 00573 * @param uid unique ID of this opponent. The caller shall assign the same UID to an 00574 * opponent if and only if the object is the same (for example an opponent that was 00575 * tracked) 00576 * @param distance to opponent 00577 * @param bearing to opponent (angle is zero if opponent is in front of robot, 00578 * positive if right of robot, negative if left of robot). 00579 * @param covariance covariance matrix with 4 entries, ordered as two concatenated 00580 * rows (first row, two floats, second row, two floats. No length check or 00581 * whatsoever is done. This will crash if c is not 00582 * long enough! c will not be copied but referenced so it has to exist when 00583 * send() is called! 00584 */ 00585 void 00586 WorldInfoTransceiver::add_opponent(unsigned int uid, 00587 float distance, float bearing, float *covariance) 00588 { 00589 opponent_t o = { uid, distance, bearing, covariance }; 00590 opponents.push_back(o); 00591 } 00592 00593 00594 /** Add disappeared opponent. 00595 * Add any opponent that you had added in an earlier cycle (before the last send()) with 00596 * add_opponent() and that is no longer visible. After it has been marked as disappeared 00597 * the unique ID may not be reused. Gibt it another new ID instead. 00598 * @param uid Unique ID of opponent that disappeared 00599 */ 00600 void 00601 WorldInfoTransceiver::add_disappeared_opponent(unsigned int uid) 00602 { 00603 disappeared_opponents.push_back(uid); 00604 } 00605 00606 /** Append packet to outbound buffer. 00607 * @param msg_type message type 00608 * @param msg message buffer 00609 * @param msg_size size of message buffer 00610 * @exception OutOfMemoryException thrown if message is too big or if the 00611 * remaining size in the outbound buffer is not big enough 00612 */ 00613 void 00614 WorldInfoTransceiver::append_outbound(uint16_t msg_type, 00615 void *msg, uint16_t msg_size) 00616 { 00617 worldinfo_message_header_t mh; 00618 00619 if ( (outbound_bytes + sizeof(mh) + msg_size ) > WORLDINFO_MTU ) { 00620 throw OutOfMemoryException(); 00621 } 00622 00623 // per message header 00624 mh.type = htons(msg_type); 00625 mh.size = htons(msg_size); 00626 memcpy(outbound_buffer, &mh, sizeof(mh)); 00627 00628 outbound_bytes += sizeof(mh); 00629 outbound_buffer += sizeof(mh); 00630 00631 // message body 00632 memcpy(outbound_buffer, msg, msg_size); 00633 outbound_bytes += msg_size; 00634 outbound_buffer += msg_size; 00635 ++outbound_num_msgs; 00636 } 00637 00638 00639 /** Reset outbound buffer. 00640 */ 00641 void 00642 WorldInfoTransceiver::reset_outbound() 00643 { 00644 worldinfo_header_t *header = (worldinfo_header_t *)out_buffer; 00645 header->beef = htons(0xBEEF); 00646 header->version = WORLDINFO_VERSION; 00647 00648 if ( fatmsg_enabled ) { 00649 memset(fatmsg_buf, 0, fatmsg_bufsize); 00650 fatmsg_header->beef = htons(0xBEEF); 00651 fatmsg_header->version = WORLDINFO_VERSION; 00652 fatmsg_msgheader->type = htons(WORLDINFO_MSGTYPE_FAT_WORLDINFO); 00653 fatmsg_msgheader->size = htons(sizeof(worldinfo_fat_message_t)); 00654 } 00655 00656 outbound_buffer = (unsigned char *)out_buffer + sizeof(worldinfo_header_t); 00657 outbound_bytes = sizeof(worldinfo_header_t); 00658 outbound_num_msgs = 0; 00659 } 00660 00661 00662 /** Send information. 00663 * All information that has been set since last call is sent over the network. 00664 * This implicitly resets all information and flushes the opponent list. 00665 */ 00666 void 00667 WorldInfoTransceiver::send() 00668 { 00669 worldinfo_header_t *header = (worldinfo_header_t *)out_buffer; 00670 00671 reset_outbound(); 00672 00673 if ( pose_changed ) { 00674 worldinfo_pose_message_t pm; 00675 pm.x = pose_x; 00676 pm.y = pose_y; 00677 pm.theta = pose_theta; 00678 memcpy(&(pm.covariance), pose_covariance, sizeof(pm.covariance)); 00679 pose_changed = false; 00680 00681 append_outbound(WORLDINFO_MSGTYPE_POSE, &pm, sizeof(pm)); 00682 00683 if ( fatmsg_enabled ) { 00684 // fill fat msg 00685 memcpy(&(fatmsg->pose), &pm, sizeof(pm)); 00686 fatmsg->valid_pose = 1; 00687 } 00688 } else { 00689 if ( fatmsg_enabled ) { 00690 fatmsg->valid_pose = 0; 00691 } 00692 } 00693 00694 if ( vel_changed ) { 00695 worldinfo_velocity_message_t vm; 00696 vm.vel_x = vel_x; 00697 vm.vel_y = vel_y; 00698 vm.vel_theta = vel_theta; 00699 memcpy(&(vm.covariance), vel_covariance, sizeof(vm.covariance)); 00700 vel_changed = false; 00701 00702 append_outbound(WORLDINFO_MSGTYPE_VELO, &vm, sizeof(vm)); 00703 00704 if ( fatmsg_enabled ) { 00705 // fill fat msg 00706 memcpy(&(fatmsg->velo), &vm, sizeof(vm)); 00707 fatmsg->valid_velo = 1; 00708 } 00709 } else { 00710 if ( fatmsg_enabled ) { 00711 fatmsg->valid_velo = 0; 00712 } 00713 } 00714 00715 if ( rel_ball_changed ) { 00716 worldinfo_relballpos_message_t bm; 00717 bm.dist = rel_ball_dist; 00718 bm.bearing = rel_ball_bearing; 00719 bm.slope = rel_ball_slope; 00720 bm.history = rel_ball_visibility_history; 00721 bm.visible = rel_ball_visible ? -1 : 0; 00722 memcpy(&(bm.covariance), rel_ball_covariance, sizeof(bm.covariance)); 00723 00724 rel_ball_changed = false; 00725 00726 append_outbound(WORLDINFO_MSGTYPE_GLOBBALL, &bm, sizeof(bm)); 00727 00728 if ( fatmsg_enabled ) { 00729 // fill fat msg 00730 memcpy(&(fatmsg->relball_pos), &bm, sizeof(bm)); 00731 fatmsg->valid_relball_pos = 1; 00732 } 00733 } else { 00734 if ( fatmsg_enabled ) { 00735 fatmsg->valid_relball_pos = 0; 00736 } 00737 } 00738 00739 if ( glob_ball_changed ) { 00740 worldinfo_globballpos_message_t bm; 00741 bm.x = glob_ball_x; 00742 bm.y = glob_ball_y; 00743 bm.z = glob_ball_z; 00744 bm.history = glob_ball_visibility_history; 00745 bm.visible = glob_ball_visible ? -1 : 0; 00746 memcpy(&(bm.covariance), glob_ball_covariance, sizeof(bm.covariance)); 00747 00748 glob_ball_changed = false; 00749 00750 append_outbound(WORLDINFO_MSGTYPE_GLOBBALL, &bm, sizeof(bm)); 00751 } 00752 00753 if ( gamestate_changed ) { 00754 append_outbound(WORLDINFO_MSGTYPE_GAMESTATE, 00755 &gamestate_msg, sizeof(worldinfo_gamestate_message_t)); 00756 gamestate_changed = false; 00757 } 00758 00759 if ( rel_ball_vel_changed ) { 00760 worldinfo_relballvelo_message_t rbvm; 00761 rbvm.vel_x = rel_ball_vel_x; 00762 rbvm.vel_y = rel_ball_vel_y; 00763 rbvm.vel_z = rel_ball_vel_z; 00764 memcpy(&(rbvm.covariance), rel_ball_vel_covariance, sizeof(rbvm.covariance)); 00765 rel_ball_vel_changed = false; 00766 00767 append_outbound(WORLDINFO_MSGTYPE_RELBALLVELO, &rbvm, sizeof(rbvm)); 00768 00769 if ( fatmsg_enabled ) { 00770 // fill fat msg 00771 memcpy(&(fatmsg->relball_velo), &rbvm, sizeof(rbvm)); 00772 fatmsg->valid_relball_velo = 1; 00773 } 00774 } else { 00775 if ( fatmsg_enabled ) { 00776 fatmsg->valid_relball_velo = 0; 00777 } 00778 } 00779 00780 if ( glob_ball_vel_changed ) { 00781 worldinfo_globballvelo_message_t rbvm; 00782 rbvm.vel_x = glob_ball_vel_x; 00783 rbvm.vel_y = glob_ball_vel_y; 00784 rbvm.vel_z = glob_ball_vel_z; 00785 memcpy(&(rbvm.covariance), glob_ball_vel_covariance, sizeof(rbvm.covariance)); 00786 glob_ball_vel_changed = false; 00787 00788 append_outbound(WORLDINFO_MSGTYPE_GLOBBALLVELO, &rbvm, sizeof(rbvm)); 00789 } 00790 00791 // Append penalties 00792 for (penit = penalties.begin(); penit != penalties.end(); ++penit) { 00793 append_outbound(WORLDINFO_MSGTYPE_PENALTY, 00794 &(penit->second), sizeof(worldinfo_penalty_message_t)); 00795 } 00796 penalties.clear(); 00797 00798 // Append opponents 00799 unsigned int num_opponents = 0; 00800 for ( oppit = opponents.begin(); oppit != opponents.end(); ++oppit) { 00801 worldinfo_opppose_message_t opm; 00802 opm.uid = (*oppit).uid; 00803 opm.dist = (*oppit).distance; 00804 opm.bearing = (*oppit).bearing; 00805 memcpy(&(opm.covariance), (*oppit).covariance, sizeof(opm.covariance)); 00806 00807 append_outbound(WORLDINFO_MSGTYPE_OPP_POSE, &opm, sizeof(opm)); 00808 00809 if ( fatmsg_enabled ) { 00810 if ( num_opponents < WORLDINFO_FATMSG_NUMOPPS ) { 00811 // fill fat msg 00812 memcpy(&(fatmsg->opponents[num_opponents]), &opm, sizeof(opm)); 00813 ++num_opponents; 00814 fatmsg->num_opponents = num_opponents; 00815 } 00816 } 00817 } 00818 opponents.clear(); 00819 00820 for ( doppit = disappeared_opponents.begin(); doppit != disappeared_opponents.end(); ++doppit) { 00821 worldinfo_oppdisappeared_message_t opdm; 00822 opdm.uid = *doppit; 00823 00824 append_outbound(WORLDINFO_MSGTYPE_OPP_DISAPP, &opdm, sizeof(opdm)); 00825 } 00826 disappeared_opponents.clear(); 00827 00828 if ( outbound_num_msgs > 0 ) { 00829 // send slim msgs 00830 header->seq = htonl(out_seq++); 00831 00832 encryptor->set_plain_buffer(out_buffer, outbound_bytes); 00833 crypted_out_bytes = encryptor->encrypt(); 00834 00835 s->send(crypted_out_buffer, crypted_out_bytes); 00836 00837 if ( fatmsg_enabled ) { 00838 // send fat msg 00839 fatmsg_header->seq = htonl(out_seq++); 00840 00841 encryptor->set_plain_buffer(fatmsg_buf, fatmsg_bufsize); 00842 crypted_out_bytes = encryptor->encrypt(); 00843 00844 s->send(crypted_out_buffer, crypted_out_bytes); 00845 } 00846 } 00847 00848 } 00849 00850 00851 /** Receive information. 00852 * This checks if there is information on the network waiting to be received 00853 * and if so receives and processes the information and dispatches it to all 00854 * registered handlers. If you order it to block this method will block until 00855 * information has been received and dispatched (useful if running in a 00856 * thread). 00857 * 00858 * Received packets will be ignored if 00859 * - they do not start with 0xBEEF 00860 * - they are of an incompatible version 00861 * - the sequence number is smaller or equal to an already received packet 00862 * They will only be partially handled if 00863 * - a packet has been truncated (truncated message is ignored) 00864 * - an unknown message type is encountered (message is ignored) 00865 * - a message size does not match the expected size for a given type (message is ignored) 00866 * 00867 * @param block set to true for blocking operation, in this case recv() will 00868 * block until data is available, false for non-blocking operation where recv() 00869 * will immediately return if there is no data available 00870 * @param max_num_msgs maximum number of messages to process in a single 00871 * call to recv(). Set to 0 for an unlimited number of messages per call (this 00872 * can block for an infinite time if messages are coming in fast). 00873 */ 00874 void 00875 WorldInfoTransceiver::recv(bool block, unsigned int max_num_msgs) 00876 { 00877 if ( ! block ) { 00878 if ( ! s->available() ) { 00879 return; 00880 } 00881 } 00882 00883 handlers.lock(); 00884 00885 unsigned int num_msgs = (max_num_msgs == 0 ? 0 : 1); 00886 do { 00887 struct sockaddr_in from; 00888 socklen_t addr_len = sizeof(from); 00889 size_t bytes = crypt_buffer_size; 00890 00891 if ( max_num_msgs != 0 ) ++num_msgs; 00892 00893 bytes = s->recv(crypted_in_buffer, bytes, (struct sockaddr *)&from, &addr_len); 00894 00895 // skip message if it is looped 00896 if (!loop) { 00897 struct in_addr localhost; 00898 ::inet_aton("127.0.0.1", &localhost); 00899 if (from.sin_addr.s_addr == localhost.s_addr) { 00900 continue; 00901 } 00902 } 00903 00904 // decryptor decrypts to in_buffer, see constructor 00905 decryptor->set_crypt_buffer(crypted_in_buffer, bytes); 00906 try { 00907 inbound_bytes = decryptor->decrypt(); 00908 } catch (MessageDecryptionException &e) { 00909 //LibLogger::log_warn("WorldInfoTransceiver", "Message decryption failed, ignoring"); 00910 //LibLogger::log_warn("WorldInfoTransceiver", e); 00911 continue; 00912 } 00913 00914 /* 00915 cout << "Plain:"; 00916 for (size_t i = 0; i < inbound_bytes; ++i) { 00917 unsigned int u = *((unsigned char *)in_buffer + i); 00918 printf("%02x ", u); 00919 } 00920 cout << endl; 00921 */ 00922 00923 // Process 00924 worldinfo_header_t *header = (worldinfo_header_t *)in_buffer; 00925 if ( ntohs(header->beef) != 0xBEEF ) { 00926 // throw WorldInfoException("Incorrect message received, wrong key?"); 00927 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid message received (no 0xBEEF), ignoring"); 00928 continue; 00929 } 00930 00931 if ( header->version != WORLDINFO_VERSION ) { 00932 //LibLogger::log_warn("WorldInfoTransceiver", "Unsupported version of world info data received, ignoring"); 00933 continue; 00934 } 00935 00936 // Sequence number handling per client, IPv4 only, for IPv6 in the pre-128-bit era 00937 // we would need a custom compare function 00938 unsigned int cseq = ntohl(header->seq); 00939 if ( sequence_numbers.find(from.sin_addr.s_addr) != sequence_numbers.end() ) { 00940 if ( cseq <= sequence_numbers[from.sin_addr.s_addr] ) { 00941 // Already received (loop) or replay attack, just ignore 00942 //LibLogger::log_warn("WorldInfoTransceiver", "Received packet twice, ignoring"); 00943 continue; 00944 } 00945 } 00946 sequence_numbers[from.sin_addr.s_addr] = cseq; 00947 last_received_time[from.sin_addr.s_addr] = time(NULL); 00948 00949 inbound_bytes -= sizeof(worldinfo_header_t); 00950 inbound_buffer = (unsigned char *)in_buffer + sizeof(worldinfo_header_t); 00951 00952 std::string hostname_s; 00953 if ( ! resolver->resolve_address((struct sockaddr *)&from, sizeof(from), hostname_s) ) { 00954 hostname_s = "unknown"; 00955 } 00956 const char *hostname = hostname_s.c_str(); 00957 00958 // Go through messages 00959 while ( inbound_bytes > 0 ) { 00960 worldinfo_message_header_t *msgh = (worldinfo_message_header_t *)inbound_buffer; 00961 inbound_bytes -= sizeof(worldinfo_message_header_t); 00962 inbound_buffer += sizeof(worldinfo_message_header_t); 00963 uint16_t msg_type = ntohs(msgh->type); 00964 uint16_t msg_size = ntohs(msgh->size); 00965 //printf("Message type: %u size: %u ntype: %u nsize: %u\n", 00966 // msg_type, msg_size, msgh->type, msgh->size); 00967 if ( inbound_bytes < msg_size ) { 00968 //LibLogger::log_warn("WorldInfoTransceiver", "Truncated packet received or protocol " 00969 // "error, ignoring rest of packet (got %zu bytes, but expected " 00970 // "%zu bytes)", inbound_bytes, msg_size); 00971 break; 00972 } 00973 switch ( msg_type ) { 00974 case WORLDINFO_MSGTYPE_POSE: 00975 if ( msg_size == sizeof(worldinfo_pose_message_t) ) { 00976 worldinfo_pose_message_t *pose_msg = (worldinfo_pose_message_t *)inbound_buffer; 00977 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 00978 (*hit)->pose_rcvd(hostname, 00979 pose_msg->x, pose_msg->y, pose_msg->theta, 00980 pose_msg->covariance); 00981 } 00982 } else { 00983 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid pose message received " 00984 // "(got %zu bytes but expected %zu bytes), ignoring", 00985 // msg_size, sizeof(worldinfo_pose_message_t)); 00986 } 00987 break; 00988 00989 case WORLDINFO_MSGTYPE_VELO: 00990 if ( msg_size == sizeof(worldinfo_velocity_message_t) ) { 00991 worldinfo_velocity_message_t *velo_msg = (worldinfo_velocity_message_t *)inbound_buffer; 00992 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 00993 (*hit)->velocity_rcvd(hostname, 00994 velo_msg->vel_x, velo_msg->vel_y, velo_msg->vel_theta, 00995 velo_msg->covariance); 00996 } 00997 } else { 00998 // LibLogger::log_warn("WorldInfoTransceiver", "Invalid velocity message received " 00999 // "(got %zu bytes but expected %zu bytes), ignoring", 01000 // msg_size, sizeof(worldinfo_velocity_message_t)); 01001 } 01002 break; 01003 01004 case WORLDINFO_MSGTYPE_RELBALL: 01005 if ( msg_size == sizeof(worldinfo_relballpos_message_t) ) { 01006 worldinfo_relballpos_message_t *ball_msg = (worldinfo_relballpos_message_t *)inbound_buffer; 01007 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01008 (*hit)->ball_pos_rcvd(hostname, 01009 (ball_msg->visible == -1), ball_msg->history, 01010 ball_msg->dist, ball_msg->bearing, ball_msg->slope, 01011 ball_msg->covariance); 01012 } 01013 } else { 01014 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid relative ball pos message received " 01015 // "(got %zu bytes but expected %zu bytes), ignoring", 01016 // msg_size, sizeof(worldinfo_relballpos_message_t)); 01017 } 01018 break; 01019 01020 case WORLDINFO_MSGTYPE_GLOBBALL: 01021 if ( msg_size == sizeof(worldinfo_globballpos_message_t) ) { 01022 worldinfo_globballpos_message_t *ball_msg = (worldinfo_globballpos_message_t *)inbound_buffer; 01023 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01024 (*hit)->global_ball_pos_rcvd(hostname, 01025 (ball_msg->visible == -1), ball_msg->history, 01026 ball_msg->x, ball_msg->y, ball_msg->z, 01027 ball_msg->covariance); 01028 } 01029 } else { 01030 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid global ball pos message received " 01031 // "(got %zu bytes but expected %zu bytes), ignoring", 01032 // msg_size, sizeof(worldinfo_globballpos_message_t)); 01033 } 01034 break; 01035 01036 case WORLDINFO_MSGTYPE_RELBALLVELO: 01037 if ( msg_size == sizeof(worldinfo_relballvelo_message_t) ) { 01038 worldinfo_relballvelo_message_t *bvel_msg = (worldinfo_relballvelo_message_t *)inbound_buffer; 01039 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01040 (*hit)->ball_velocity_rcvd(hostname, 01041 bvel_msg->vel_x, bvel_msg->vel_y, bvel_msg->vel_z, 01042 bvel_msg->covariance); 01043 } 01044 } else { 01045 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid relative ball velocity message received " 01046 // "(got %zu bytes but expected %zu bytes), ignoring", 01047 // msg_size, sizeof(worldinfo_relballvelo_message_t)); 01048 } 01049 break; 01050 01051 case WORLDINFO_MSGTYPE_GLOBBALLVELO: 01052 if ( msg_size == sizeof(worldinfo_globballvelo_message_t) ) { 01053 worldinfo_globballvelo_message_t *bvel_msg = (worldinfo_globballvelo_message_t *)inbound_buffer; 01054 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01055 (*hit)->global_ball_velocity_rcvd(hostname, 01056 bvel_msg->vel_x, bvel_msg->vel_y, bvel_msg->vel_z, 01057 bvel_msg->covariance); 01058 } 01059 } else { 01060 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid global ball velocity message received " 01061 // "(got %zu bytes but expected %zu bytes), ignoring", 01062 // msg_size, sizeof(worldinfo_globballvelo_message_t)); 01063 } 01064 break; 01065 01066 case WORLDINFO_MSGTYPE_OPP_POSE: 01067 if ( msg_size == sizeof(worldinfo_opppose_message_t) ) { 01068 worldinfo_opppose_message_t *oppp_msg = (worldinfo_opppose_message_t *)inbound_buffer; 01069 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01070 (*hit)->opponent_pose_rcvd(hostname, 01071 oppp_msg->uid, oppp_msg->dist, oppp_msg->bearing, 01072 oppp_msg->covariance); 01073 } 01074 } else { 01075 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid opponent pose message received " 01076 // "(got %zu bytes but expected %zu bytes), ignoring", 01077 // msg_size, sizeof(worldinfo_opppose_message_t)); 01078 } 01079 break; 01080 01081 case WORLDINFO_MSGTYPE_OPP_DISAPP: 01082 if ( msg_size == sizeof(worldinfo_oppdisappeared_message_t) ) { 01083 worldinfo_oppdisappeared_message_t *oppd_msg = (worldinfo_oppdisappeared_message_t *)inbound_buffer; 01084 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01085 (*hit)->opponent_disapp_rcvd(hostname, oppd_msg->uid); 01086 } 01087 } else { 01088 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid opponent disappeared message received " 01089 // "(got %zu bytes but expected %zu bytes), ignoring", 01090 // msg_size, sizeof(worldinfo_oppdisappeared_message_t)); 01091 } 01092 break; 01093 01094 case WORLDINFO_MSGTYPE_GAMESTATE: 01095 if ( msg_size == sizeof(worldinfo_gamestate_message_t) ) { 01096 worldinfo_gamestate_message_t *gs_msg = (worldinfo_gamestate_message_t *)inbound_buffer; 01097 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01098 (*hit)->gamestate_rcvd(hostname, 01099 gs_msg->game_state, 01100 (worldinfo_gamestate_team_t)gs_msg->state_team, 01101 gs_msg->score_cyan, gs_msg->score_magenta, 01102 (worldinfo_gamestate_team_t)gs_msg->our_team, 01103 (worldinfo_gamestate_goalcolor_t)gs_msg->our_goal_color, 01104 (worldinfo_gamestate_half_t)gs_msg->half); 01105 } 01106 } else { 01107 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid gamestate message received " 01108 // "(got %zu bytes but expected %zu bytes), ignoring", 01109 // msg_size, sizeof(worldinfo_gamestate_message_t)); 01110 } 01111 break; 01112 01113 case WORLDINFO_MSGTYPE_PENALTY: 01114 if ( msg_size == sizeof(worldinfo_penalty_message_t) ) { 01115 worldinfo_penalty_message_t *p_msg = (worldinfo_penalty_message_t *)inbound_buffer; 01116 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01117 (*hit)->penalty_rcvd(hostname, 01118 p_msg->player, p_msg->penalty, p_msg->seconds_remaining); 01119 } 01120 01121 } else { 01122 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid penalty message received " 01123 // "(got %zu bytes but expected %zu bytes), ignoring", 01124 // msg_size, sizeof(worldinfo_penalty_message_t)); 01125 } 01126 break; 01127 01128 case WORLDINFO_MSGTYPE_FAT_WORLDINFO: 01129 if ( msg_size == sizeof(worldinfo_fat_message_t) ) { 01130 worldinfo_fat_message_t *fat_msg = (worldinfo_fat_message_t *)inbound_buffer; 01131 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) { 01132 if ( fat_msg->valid_pose ) { 01133 (*hit)->pose_rcvd(hostname, 01134 fat_msg->pose.x, fat_msg->pose.y, fat_msg->pose.theta, 01135 fat_msg->pose.covariance); 01136 } 01137 01138 if ( fat_msg->valid_velo ) { 01139 (*hit)->velocity_rcvd(hostname, 01140 fat_msg->velo.vel_x, fat_msg->velo.vel_y, 01141 fat_msg->velo.vel_theta, fat_msg->velo.covariance); 01142 } 01143 if ( fat_msg->valid_relball_pos ) { 01144 (*hit)->ball_pos_rcvd(hostname, 01145 (fat_msg->relball_pos.visible == -1), 01146 fat_msg->relball_pos.history, 01147 fat_msg->relball_pos.dist, fat_msg->relball_pos.bearing, 01148 fat_msg->relball_pos.slope, fat_msg->relball_pos.covariance); 01149 } 01150 if ( fat_msg->valid_relball_velo ) { 01151 (*hit)->ball_velocity_rcvd(hostname, 01152 fat_msg->relball_velo.vel_x, 01153 fat_msg->relball_velo.vel_y, 01154 fat_msg->relball_velo.vel_z, 01155 fat_msg->relball_velo.covariance); 01156 } 01157 01158 if ( fat_msg->num_opponents > WORLDINFO_FATMSG_NUMOPPS ) { 01159 // We can't handle this 01160 //LibLogger::log_warn("WorldInfoTransceiver", "Too many opponents marked valid in message " 01161 // "(got %zu but expected a maximum of %zu), ignoring", 01162 // fat_msg->num_opponents, WORLDINFO_FATMSG_NUMOPPS); 01163 } else { 01164 for ( unsigned int i = 0; i < fat_msg->num_opponents; ++i ) { 01165 (*hit)->opponent_pose_rcvd(hostname, 01166 fat_msg->opponents[i].uid, 01167 fat_msg->opponents[i].dist, 01168 fat_msg->opponents[i].bearing, 01169 fat_msg->opponents[i].covariance); 01170 } 01171 } 01172 } // end for each handler 01173 } else { 01174 //LibLogger::log_warn("WorldInfoTransceiver", "Invalid fat message received " 01175 // "(got %zu bytes but expected %zu bytes), ignoring", 01176 // msg_size, sizeof(worldinfo_fat_message_t)); 01177 } 01178 break; 01179 01180 01181 //default: 01182 //LibLogger::log_warn("WorldInfoTransceiver", "Unknown message type %u received " 01183 // ", ignoring", msg_type); 01184 } 01185 // there is more to process 01186 inbound_bytes -= msg_size; 01187 inbound_buffer += msg_size; 01188 } 01189 01190 } while ( s->available() && (num_msgs <= max_num_msgs) ); 01191 01192 handlers.unlock(); 01193 } 01194 01195 01196 /** Get last sent plain buffer. 01197 * This method is meant to be used for debugging and testing purposes only. 01198 * @return last plain text message buffer 01199 */ 01200 void * 01201 WorldInfoTransceiver::last_sent_plain_buffer() 01202 { 01203 return out_buffer; 01204 } 01205 01206 01207 /** Get last sent plain buffer size. 01208 * This method is meant to be used for debugging and testing purposes only. 01209 * @return last plain text message buffer size 01210 */ 01211 size_t 01212 WorldInfoTransceiver::last_sent_plain_buffer_size() 01213 { 01214 return outbound_bytes; 01215 } 01216 01217 01218 /** Get last sent crypted buffer. 01219 * This method is meant to be used for debugging and testing purposes only. 01220 * @return last crytped message buffer 01221 */ 01222 void * 01223 WorldInfoTransceiver::last_sent_crypted_buffer() 01224 { 01225 return crypted_out_buffer; 01226 } 01227 01228 01229 /** Get last sent crypted buffer size. 01230 * This method is meant to be used for debugging and testing purposes only. 01231 * @return last crypted message buffer size 01232 */ 01233 size_t 01234 WorldInfoTransceiver::last_sent_crypted_buffer_size() 01235 { 01236 return crypted_out_bytes; 01237 } 01238 01239 } // end namespace fawkes