backend_thread.cpp

00001 
00002 /***************************************************************************
00003  *  backend_thread.cpp - World Info Viewer backend thread
00004  *
00005  *  Created: Thu April 10 22:00:08 2008
00006  *  Copyright  2008  Daniel Beck
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.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "backend_thread.h"
00024 #include <worldinfo_utils/data_container.h>
00025 #include <netcomm/worldinfo/transceiver.h>
00026 
00027 using namespace fawkes;
00028 
00029 /** @class WorldInfoViewerBackendThread backend_thread.h <tools/worldinfo_viewer/backend_thread.h>
00030  * The backend thread of the worldinfo viewer application.
00031  * @author Daniel Beck
00032  */
00033 
00034 /** Constructor.  
00035  * @param data_container pointer to the central instance of the
00036  * WorldInfoDataContainer
00037  * @param addr multicast address to use for worldinfo communication
00038  * @param port port
00039  * @param key de-/encryption key
00040  * @param iv initialization vector for de-/encryption
00041  */ 
00042 WorldInfoViewerBackendThread::WorldInfoViewerBackendThread( WorldInfoDataContainer* data_container,
00043                                                             const char* addr,
00044                                                             unsigned short port,
00045                                                             const char* key,
00046                                                             const char* iv )
00047   : Thread("WorldInfoViewerBackendThread")
00048 {
00049   m_data_container = data_container;
00050 
00051   m_addr = addr;
00052   m_port = port;
00053   m_key  = key;
00054   m_iv   = iv;
00055 
00056   m_avahi = new AvahiThread();
00057   m_avahi->start();
00058 
00059   m_resolver = new NetworkNameResolver( m_avahi );
00060 
00061   m_transceiver = new WorldInfoTransceiver( WorldInfoTransceiver::MULTICAST,
00062                                             m_addr.c_str(),
00063                                             m_port, 
00064                                             m_key.c_str(),
00065                                             m_iv.c_str(),
00066                                             m_resolver );
00067   m_transceiver->add_handler(this);
00068 }
00069 
00070 /** Destructor. */
00071 WorldInfoViewerBackendThread::~WorldInfoViewerBackendThread()
00072 {
00073   delete m_transceiver;
00074   delete m_resolver;
00075 
00076   m_avahi->cancel();
00077   m_avahi->join();
00078   delete m_avahi;
00079 }
00080 
00081 /** Access the dispatcher that is emitted whenever new data has
00082  * arrived.
00083  * @return reference to the dispatcher
00084  */
00085 Glib::Dispatcher&
00086 WorldInfoViewerBackendThread::new_worldinfo_data()
00087 {
00088   return m_signal_new_worldinfo_data;
00089 }
00090 
00091 /** Access the dispatcher that is emitted whenever new game state data
00092  * has arrived.
00093  * @return reference to the dispatcher
00094  */
00095 Glib::Dispatcher&
00096 WorldInfoViewerBackendThread::new_gamestate_data()
00097 {
00098   return m_signal_new_gamestate_data;
00099 }
00100 
00101 void
00102 WorldInfoViewerBackendThread::loop()
00103 {
00104   m_transceiver->flush_sequence_numbers( 10 );
00105   m_transceiver->recv(true, 100);
00106   usleep(100000);
00107 }
00108 
00109 void
00110 WorldInfoViewerBackendThread::pose_rcvd( const char* from_host,
00111                                          float x,
00112                                          float y,
00113                                          float theta,
00114                                          float* covariance )
00115 {
00116 #ifdef DEBUG_PRINT
00117   printf( "Received pose data from host %s: x=%.3f y=%.3f theta=%.3f\n",
00118           from_host, x, y, theta );
00119 #endif /* DEBUG_PRING */
00120   
00121   m_data_container->set_robot_pose( from_host, x, y, theta, covariance );
00122   m_signal_new_worldinfo_data();
00123 }
00124 
00125 void
00126 WorldInfoViewerBackendThread::velocity_rcvd( const char* from_host,
00127                                              float vel_x,
00128                                              float vel_y,
00129                                              float vel_theta,
00130                                              float* covariance )
00131 {
00132 #ifdef DEBUG_PRINT
00133   printf( "Received velocity data from host %s: vx=%.3f vy=%.3f vtheta=%.3f\n",
00134           from_host, vel_x, vel_y, vel_theta );
00135 #endif /* DEBUG_PRINT */
00136   
00137   m_data_container->set_robot_velocity( from_host, vel_x, vel_y, vel_theta,
00138                                         covariance );
00139   m_signal_new_worldinfo_data();
00140 }
00141 
00142 void
00143 WorldInfoViewerBackendThread::ball_pos_rcvd( const char* from_host,
00144                                              bool visible,
00145                                              int visibility_history,
00146                                              float dist,
00147                                              float bearing,
00148                                              float slope,
00149                                              float* covariance )
00150 {
00151 #ifdef DEBUG_PRINT
00152   if ( visible )
00153   { printf( "Received ball data from host %s: dist=%.3f bearing=%.3f\n",
00154             from_host, dist, bearing ); }
00155   else
00156   { printf( "Received ball not visible from host %s\n", from_host ); }
00157 #endif /* DEBUG_PRINT */
00158   
00159   m_data_container->set_ball_pos( from_host, visible, visibility_history,
00160                                   dist, bearing, slope, covariance );
00161   m_signal_new_worldinfo_data();
00162 }
00163 
00164 void
00165 WorldInfoViewerBackendThread::global_ball_pos_rcvd( const char* from_host,
00166                                                     bool visible,
00167                                                     int visibility_history,
00168                                                     float x,
00169                                                     float y,
00170                                                     float z,
00171                                                     float* covariance )
00172 {
00173 #ifdef DEBUG_PRINT
00174   if ( visible )
00175   { printf( "Received global ball data from host %s: x=%.3f y=%.3f\n",
00176             from_host, x, y ); }
00177   else
00178   { printf( "Received global ball not visible from host %s\n", from_host ); }
00179 #endif /* DEBUG_PRINT */
00180   m_data_container->set_ball_pos_global( from_host, visible,
00181                                          visibility_history,
00182                                          x, y, z, covariance );
00183   m_signal_new_worldinfo_data();
00184 }
00185 
00186 void
00187 WorldInfoViewerBackendThread::ball_velocity_rcvd( const char* from_host,
00188                                                   float vel_x,
00189                                                   float vel_y,
00190                                                   float vel_z,
00191                                                   float* covariance )
00192 {
00193   m_data_container->set_ball_velocity( from_host, vel_x, vel_y, vel_z,
00194                                        covariance );
00195   m_signal_new_worldinfo_data();
00196 }
00197 
00198 void
00199 WorldInfoViewerBackendThread::global_ball_velocity_rcvd( const char *from_host,
00200                                                          float vel_x,
00201                                                          float vel_y,
00202                                                          float vel_z,
00203                                                          float *covariance )
00204 {
00205   // TODO
00206 
00207 //   m_data_container->set_global_ball_velocity( from_host, vel_x, vel_y, vel_z,
00208 //                                            covariance );
00209 //   m_signal_new_worldinfo_data();
00210 }
00211 
00212 
00213 void
00214 WorldInfoViewerBackendThread::opponent_pose_rcvd( const char* from_host,
00215                                                   unsigned int uid,
00216                                                   float distance,
00217                                                   float angle,
00218                                                   float* covariance )
00219 {
00220 // #ifdef DEBUG_PRINT
00221 //   printf("Received opponent pose data form host %s\n", from_host );
00222 // #endif /* DEBUG_PRINT */
00223 
00224   m_data_container->set_opponent_pos( from_host, uid, distance, angle,
00225                                       covariance );
00226   m_signal_new_worldinfo_data();
00227 }
00228 
00229 
00230 void
00231 WorldInfoViewerBackendThread::opponent_disapp_rcvd( const char *from_host,
00232                                                     unsigned int uid )
00233 {
00234   m_data_container->opponent_disappeared( from_host, uid );
00235   m_signal_new_worldinfo_data();
00236 }
00237 
00238 
00239 void
00240 WorldInfoViewerBackendThread::gamestate_rcvd( const char* from_host, 
00241                                               unsigned int game_state, 
00242                                               worldinfo_gamestate_team_t state_team, 
00243                                               unsigned int score_cyan, 
00244                                               unsigned int score_magenta, 
00245                                               worldinfo_gamestate_team_t own_team, 
00246                                               worldinfo_gamestate_goalcolor_t own_goal_color,
00247                                               worldinfo_gamestate_half_t half )
00248 {
00249 #ifdef DEBUG_PRINT
00250   printf( "Received gamestate data from host %s\n", from_host );
00251 #endif /* DEBUG_PRINT */
00252 
00253   m_data_container->set_game_state( game_state, state_team,
00254                                     score_cyan, score_magenta,
00255                                     own_team, own_goal_color, half );
00256   m_signal_new_gamestate_data();
00257 }
00258 
00259 void
00260 WorldInfoViewerBackendThread::penalty_rcvd(const char *from_host,
00261                                            unsigned int player,
00262                                            unsigned int penalty,
00263                                            unsigned int seconds_remaining)
00264 {
00265 }