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 <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

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1