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 <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 /** @class WorldInfoException transceiver.h <netcomm/worldinfo/transceiver.h> 00046 * Thrown on critical errors in world info handling. 00047 * @ingroup NetComm 00048 */ 00049 00050 /** Constructor. 00051 * @param msg message 00052 */ 00053 WorldInfoException::WorldInfoException(const char *msg) 00054 : Exception(msg) 00055 { 00056 } 00057 00058 00059 /** @class WorldInfoTransceiver transceiver.h <netcomm/worldinfo/transceiver.h> 00060 * Class to send and receive world information. 00061 * An important point in a domain of cooperating soccer robots is transmitting 00062 * and receiving a robot's belief of its surrounding. The world info 00063 * transceiver does exactly that. It allows for sending information about the 00064 * robot's pose and velocity and its perception of the ball and other robots 00065 * on the field. 00066 * 00067 * The unit for distances and positions is meter (m), speed is given in 00068 * meter per second (m/s), angles are given in radiant (rad). Angles can be in 00069 * the range 0 to 2 * PI or -PI to PI. Since they can be converted easily 00070 * between these ranges without further information users of such information 00071 * shall be able to process both. 00072 * 00073 * Coordinates are given in a right-handed coordinate system with the origin in 00074 * center of the field, X pointing towards the opponent goal, Y to the right 00075 * and Z downwards. 00076 * 00077 * Information is transmitted with a simple protocol via UDP Multicast or 00078 * Broadcast packets. 00079 * 00080 * A call to send() will reset all information, thus all opponents are removed 00081 * from the list to be sent, positions of robot and ball are marked invalid. 00082 * You have to call the appropriate set methods before the information is sent. 00083 * You can thus call send() at any time but only changed information 00084 * (information set since last send() call) is transmitted over the network. 00085 * 00086 * @ingroup NetComm 00087 * @author Tim Niemueller 00088 */ 00089 00090 00091 /** Constructor. 00092 * @param socket_type either multicast or broadcast socket 00093 * @param addr multicast or broadcast address to send information to and receive from 00094 * @param port UDP port to send information to and receive from 00095 * @param key encryption key 00096 * @param iv encryption initialisation vector 00097 * @param resolver An initialized network resolver, is NULL is supplied 00098 * an internal resolver will be created without mDNS support. 00099 * @exception OutOfMemoryException thrown if internal buffers cannot be created 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 // set maximum size buffer to get valid results from encryptor 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 /** Destructor. */ 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 /** Set loopback of sent packets. 00200 * This sets whether packets should be looped back to local sockets for multicast 00201 * communication. 00202 * @param loop true to deliver sent packets to local sockets, false prevent delivering 00203 * @see MulticastDatagramSocket::set_loop() 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 /** Enable or disable sending of fat message. 00217 * The fat message is considered to be legacy code and therefore disabled by default. 00218 * If you happen to need the fat message you can enable it using this method and then 00219 * it will be send for every call to send(). 00220 * @param fatmsg_enabled true to enable sending of fat message, false otherwise 00221 */ 00222 void 00223 WorldInfoTransceiver::set_fatmsg_enabled(bool fatmsg_enabled) 00224 { 00225 if ( this->fatmsg_enabled && ! fatmsg_enabled ) { 00226 // fatmsg turned off 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 // fatmsg turned on 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 } // else unchanged 00243 00244 this->fatmsg_enabled = fatmsg_enabled; 00245 } 00246 00247 00248 /** Add a handler for world information. 00249 * World information will be dispatched to all registered handlers as soon it 00250 * is received. 00251 * @param h handler to register 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 /** Remove handler for world information. 00265 * The handler is removed from the list of handlers that incoming information 00266 * is dispatched to. No error is thrown if the handler was never registered 00267 * so it is safe to call this for any handler. 00268 * @param h handler to remove from subscriber list 00269 */ 00270 void 00271 WorldInfoTransceiver::rem_handler(WorldInfoHandler *h) 00272 { 00273 handlers.remove_locked(h); 00274 } 00275 00276 00277 /** Flush sequence numbers conditionally. 00278 * This will conditionally flush the sequence numbers stored per sender. The 00279 * sequence numbers are stored per IP. With this method you can flush the 00280 * sequence numbers that have been inactive for a specified time. A recommended 00281 * value is 10 seconds. You may NOT call this concurrently to recv()! 00282 * @param sec number of seconds since that must have passed without a message 00283 * to remove a specific IP from sequence list 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 /** Set global pose of robot. 00306 * Global pose of sensing robot (x, y, theta) with the origin in the 00307 * middle of the field, right handed coordinate system (y to opponent goal, 00308 * x to the right, z pointing upwards, same as in simulation league). 00309 * Theta points in y direction (theta = 0 iff robot front points to opponent 00310 * goal). 00311 * The confidence about the robot's pose is transmitted as a 3x3 covariance 00312 * matrix. 00313 * @param x x position of robot 00314 * @param y y position of robot 00315 * @param theta rotation of robot 00316 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00317 * rows (first row, three floats, second row, three floats, third row, three 00318 * floats). No length check or whatsoever is done. This will crash if c is not 00319 * long enough! c will not be copied but referenced so it has to exist when 00320 * send() is called! 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 /** Set velocity of the robot. 00334 * Set the current velocity of the robot. 00335 * @param vel_x velocity in x direction 00336 * @param vel_y velocity in y direction 00337 * @param vel_theta rotational velocity, positive velocity means clockwise 00338 * rotation, negative velocity means counter-clockwise. 00339 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00340 * rows (first row, three floats, second row, three floats, third row, three 00341 * floats). No length check or whatsoever is done. This will crash if c is not 00342 * long enough! c will not be copied but referenced so it has to exist when 00343 * send() is called! 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 /** Set ball position. 00357 * Set the ball perception relative to the current robot position. 00358 * Note that the ball position is given in polar coordinates in 00359 * 3D space! 00360 * The confidence about the ball position is transmitted as a 3x3 covariance 00361 * matrix. 00362 * @param dist distance to ball in meters 00363 * @param bearing bearing angle to ball 00364 * @param slope slope angle to ball 00365 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00366 * rows (first row, three floats, second row, three floats, third row, three 00367 * floats). No length check or whatsoever is done. This will crash if c is not 00368 * long enough! c will not be copied but referenced so it has to exist when 00369 * send() is called! 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 /** Set global ball position. 00383 * Note that the ball position is given in polar coordinates in 00384 * 3D space! 00385 * The confidence about the ball position is transmitted as a 3x3 covariance 00386 * matrix. 00387 * @param x the x-coordinate of the global ball position 00388 * @param y the y-coordinate of the global ball position 00389 * @param z the z-coordinate of the global ball position 00390 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00391 * rows (first row, three floats, second row, three floats, third row, three 00392 * floats). No length check or whatsoever is done. This will crash if c is not 00393 * long enough! c will not be copied but referenced so it has to exist when 00394 * send() is called! 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 /** Set ball visibility. 00408 * This method defines if the ball is currently visible or not. Additionally more detailed 00409 * information is provided in the visibility history. The history shall be 0 only if the 00410 * vision has just been initialized. It shall be positive if the ball is visible and shall 00411 * have the number of vision cycles in which the ball was visible in a row. It shall be 00412 * negative if the ball is not visible and shall be the negative value of the number 00413 * of frames where the ball was not visible. A value of 30 for example means that the 00414 * ball has been continuously visible for 30 frames, it was never lost. A value of 00415 * -20 means that the ball was not seen for the last 20 frames. 00416 * @param visible true if the ball is visible, false otherwise 00417 * @param visibility_history visibility history, see above. 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 /** Set ball visibility for the global ball. 00429 * Same semantics as set_ball_visible(). 00430 * @param visible true if the ball is visible, false otherwise 00431 * @param visibility_history visibility history, see above. 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 /** Set ball velocity. 00443 * Set the current velocity of the robot. 00444 * @param vel_x velocity in x direction 00445 * @param vel_y velocity in y direction 00446 * @param vel_z velocity in z direction 00447 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00448 * rows (first row, three floats, second row, three floats, third row, three 00449 * floats). No length check or whatsoever is done. This will crash if c is not 00450 * long enough! c will not be copied but referenced so it has to exist when 00451 * send() is called! 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 /** Set global ball velocity. 00466 * Set the current, global velocity of the robot. 00467 * @param vel_x velocity in x direction wrt. to the global frame 00468 * @param vel_y velocity in y direction wrt. to the global frame 00469 * @param vel_z velocity in z direction wrt. to the global frame 00470 * @param covariance covariance matrix with 9 entries, ordered as three concatenated 00471 * rows (first row, three floats, second row, three floats, third row, three 00472 * floats). No length check or whatsoever is done. This will crash if c is not 00473 * long enough! c will not be copied but referenced so it has to exist when 00474 * send() is called! 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 /** Set current game state. 00489 * @param gamestate current game state 00490 * @param state_team team referenced by the game state 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 /** Set score. 00506 * @param score_cyan current score of team cyan 00507 * @param score_magenta current score of team magenta 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 /** Add penalty message. 00519 * @param player player for which the penalty applies 00520 * @param penalty penalty code 00521 * @param seconds_remaining estimated time in seconds until the penalty is lifted 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 /** Set team and goal info. 00536 * @param our_team our team color 00537 * @param goal_color our goal color 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 /** Set current half of the game time. 00550 * @param half current half 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 /** Clear opponents list. 00561 * Clear the list of opponents that has to be transmitted. This is done 00562 * implicitly in send(). 00563 */ 00564 void 00565 WorldInfoTransceiver::clear_opponents() 00566 { 00567 opponents.clear(); 00568 } 00569 00570 00571 /** Add opponent to transmit list. 00572 * Add an opponent to the list of opponents to be transmitted on next send() 00573 * call. Opponents are given in a 2D polar coordinate system (assumption is that 00574 * robots don't fly in the soccer domain). 00575 * @param uid unique ID of this opponent. The caller shall assign the same UID to an 00576 * opponent if and only if the object is the same (for example an opponent that was 00577 * tracked) 00578 * @param distance to opponent 00579 * @param bearing to opponent (angle is zero if opponent is in front of robot, 00580 * positive if right of robot, negative if left of robot). 00581 * @param covariance covariance matrix with 4 entries, ordered as two concatenated 00582 * rows (first row, two floats, second row, two floats. No length check or 00583 * whatsoever is done. This will crash if c is not 00584 * long enough! c will not be copied but referenced so it has to exist when 00585 * send() is called! 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 /** Add disappeared opponent. 00597 * Add any opponent that you had added in an earlier cycle (before the last send()) with 00598 * add_opponent() and that is no longer visible. After it has been marked as disappeared 00599 * the unique ID may not be reused. Gibt it another new ID instead. 00600 * @param uid Unique ID of opponent that disappeared 00601 */ 00602 void 00603 WorldInfoTransceiver::add_disappeared_opponent(unsigned int uid) 00604 { 00605 disappeared_opponents.push_back(uid); 00606 } 00607 00608 /** Append packet to outbound buffer. 00609 * @param msg_type message type 00610 * @param msg message buffer 00611 * @param msg_size size of message buffer 00612 * @exception OutOfMemoryException thrown if message is too big or if the 00613 * remaining size in the outbound buffer is not big enough 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 // per message header 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 // message body 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 /** Reset outbound buffer. 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 /** Send information. 00665 * All information that has been set since last call is sent over the network. 00666 * This implicitly resets all information and flushes the opponent list. 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 // fill fat msg 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 // fill fat msg 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 // fill fat msg 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 // fill fat msg 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 // Append penalties 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 // Append opponents 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 // fill fat msg 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 // send slim msgs 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 // send fat msg 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 /** Receive information. 00854 * This checks if there is information on the network waiting to be received 00855 * and if so receives and processes the information and dispatches it to all 00856 * registered handlers. If you order it to block this method will block until 00857 * information has been received and dispatched (useful if running in a 00858 * thread). 00859 * 00860 * Received packets will be ignored if 00861 * - they do not start with 0xBEEF 00862 * - they are of an incompatible version 00863 * - the sequence number is smaller or equal to an already received packet 00864 * They will only be partially handled if 00865 * - a packet has been truncated (truncated message is ignored) 00866 * - an unknown message type is encountered (message is ignored) 00867 * - a message size does not match the expected size for a given type (message is ignored) 00868 * 00869 * @param block set to true for blocking operation, in this case recv() will 00870 * block until data is available, false for non-blocking operation where recv() 00871 * will immediately return if there is no data available 00872 * @param max_num_msgs maximum number of messages to process in a single 00873 * call to recv(). Set to 0 for an unlimited number of messages per call (this 00874 * can block for an infinite time if messages are coming in fast). 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 // skip message if it is looped 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 // decryptor decrypts to in_buffer, see constructor 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 cout << "Plain:"; 00918 for (size_t i = 0; i < inbound_bytes; ++i) { 00919 unsigned int u = *((unsigned char *)in_buffer + i); 00920 printf("%02x ", u); 00921 } 00922 cout << endl; 00923 */ 00924 00925 // Process 00926 worldinfo_header_t *header = (worldinfo_header_t *)in_buffer; 00927 if ( ntohs(header->beef) != 0xBEEF ) { 00928 // throw WorldInfoException("Incorrect message received, wrong key?"); 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 // Sequence number handling per client, IPv4 only, for IPv6 in the pre-128-bit era 00939 // we would need a custom compare function 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 // Already received (loop) or replay attack, just ignore 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 // Go through messages 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 //printf("Message type: %u size: %u ntype: %u nsize: %u\n", 00968 // msg_type, msg_size, msgh->type, msgh->size); 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 // We can't handle this 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 } // end for each handler 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 // there is more to process 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 /** Get last sent plain buffer. 01199 * This method is meant to be used for debugging and testing purposes only. 01200 * @return last plain text message buffer 01201 */ 01202 void * 01203 WorldInfoTransceiver::last_sent_plain_buffer() 01204 { 01205 return out_buffer; 01206 } 01207 01208 01209 /** Get last sent plain buffer size. 01210 * This method is meant to be used for debugging and testing purposes only. 01211 * @return last plain text message buffer size 01212 */ 01213 size_t 01214 WorldInfoTransceiver::last_sent_plain_buffer_size() 01215 { 01216 return outbound_bytes; 01217 } 01218 01219 01220 /** Get last sent crypted buffer. 01221 * This method is meant to be used for debugging and testing purposes only. 01222 * @return last crytped message buffer 01223 */ 01224 void * 01225 WorldInfoTransceiver::last_sent_crypted_buffer() 01226 { 01227 return crypted_out_buffer; 01228 } 01229 01230 01231 /** Get last sent crypted buffer size. 01232 * This method is meant to be used for debugging and testing purposes only. 01233 * @return last crypted message buffer size 01234 */ 01235 size_t 01236 WorldInfoTransceiver::last_sent_crypted_buffer_size() 01237 { 01238 return crypted_out_bytes; 01239 } 01240 01241 } // end namespace fawkes