Fawkes API  Fawkes Development Version
transceiver.cpp
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