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( m_addr.c_str(),
00062                                             m_port,
00063                                             m_key.c_str(),
00064                                             m_iv.c_str(),
00065                                             m_resolver );
00066   m_transceiver->add_handler(this);
00067 }
00068 
00069 /** Destructor. */
00070 WorldInfoViewerBackendThread::~WorldInfoViewerBackendThread()
00071 {
00072   delete m_transceiver;
00073   delete m_resolver;
00074
00075   m_avahi->cancel();
00076   m_avahi->join();
00077   delete m_avahi;
00078 }
00079 
00080 /** Access the dispatcher that is emitted whenever new data has
00081  * arrived.
00082  * @return reference to the dispatcher
00083  */
00084 Glib::Dispatcher&
00085 WorldInfoViewerBackendThread::new_worldinfo_data()
00086 {
00087   return m_signal_new_worldinfo_data;
00088 }
00089 
00090 /** Access the dispatcher that is emitted whenever new game state data
00091  * has arrived.
00092  * @return reference to the dispatcher
00093  */
00094 Glib::Dispatcher&
00095 WorldInfoViewerBackendThread::new_gamestate_data()
00096 {
00097   return m_signal_new_gamestate_data;
00098 }
00099
00100 void
00101 WorldInfoViewerBackendThread::loop()
00102 {
00103   m_transceiver->flush_sequence_numbers( 10 );
00104   m_transceiver->recv(true, 100);
00105   usleep(100000);
00106 }
00107
00108 void
00109 WorldInfoViewerBackendThread::pose_rcvd( const char* from_host,
00110                                          float x,
00111                                          float y,
00112                                          float theta,
00113                                          float* covariance )
00114 {
00115 #ifdef DEBUG_PRINT
00116   printf( "Received pose data from host %s: x=%.3f y=%.3f theta=%.3f\n",
00117           from_host, x, y, theta );
00118 #endif /* DEBUG_PRING */
00119
00120   m_data_container->set_robot_pose( from_host, x, y, theta, covariance );
00121   m_signal_new_worldinfo_data();
00122 }
00123
00124 void
00125 WorldInfoViewerBackendThread::velocity_rcvd( const char* from_host,
00126                                              float vel_x,
00127                                              float vel_y,
00128                                              float vel_theta,
00129                                              float* covariance )
00130 {
00131 #ifdef DEBUG_PRINT
00132   printf( "Received velocity data from host %s: vx=%.3f vy=%.3f vtheta=%.3f\n",
00133           from_host, vel_x, vel_y, vel_theta );
00134 #endif /* DEBUG_PRINT */
00135
00136   m_data_container->set_robot_velocity( from_host, vel_x, vel_y, vel_theta,
00137                                         covariance );
00138   m_signal_new_worldinfo_data();
00139 }
00140
00141 void
00142 WorldInfoViewerBackendThread::ball_pos_rcvd( const char* from_host,
00143                                              bool visible,
00144                                              int visibility_history,
00145                                              float dist,
00146                                              float bearing,
00147                                              float slope,
00148                                              float* covariance )
00149 {
00150 #ifdef DEBUG_PRINT
00151   if ( visible )
00152   { printf( "Received ball data from host %s: dist=%.3f bearing=%.3f\n",
00153             from_host, dist, bearing ); }
00154   else
00155   { printf( "Received ball not visible from host %s\n", from_host ); }
00156 #endif /* DEBUG_PRINT */
00157
00158   m_data_container->set_ball_pos( from_host, visible, visibility_history,
00159                                   dist, bearing, slope, covariance );
00160   m_signal_new_worldinfo_data();
00161 }
00162
00163 void
00164 WorldInfoViewerBackendThread::global_ball_pos_rcvd( const char* from_host,
00165                                                     bool visible,
00166                                                     int visibility_history,
00167                                                     float x,
00168                                                     float y,
00169                                                     float z,
00170                                                     float* covariance )
00171 {
00172 //   m_data_container->set_ball_pos_global( from_host, visible, visibility_history,
00173 //                                       x, y, z, covariance );
00174   m_signal_new_worldinfo_data();
00175 }
00176
00177 void
00178 WorldInfoViewerBackendThread::ball_velocity_rcvd( const char* from_host,
00179                                                   float vel_x,
00180                                                   float vel_y,
00181                                                   float vel_z,
00182                                                   float* covariance )
00183 {
00184   m_data_container->set_ball_velocity( from_host, vel_x, vel_y, vel_z,
00185                                        covariance );
00186   m_signal_new_worldinfo_data();
00187 }
00188
00189 void
00190 WorldInfoViewerBackendThread::global_ball_velocity_rcvd( const char *from_host,
00191                                                          float vel_x,
00192                                                          float vel_y,
00193                                                          float vel_z,
00194                                                          float *covariance )
00195 {
00196   // TODO
00197
00198 //   m_data_container->set_global_ball_velocity( from_host, vel_x, vel_y, vel_z,
00199 //                                            covariance );
00200 //   m_signal_new_worldinfo_data();
00201 }
00202
00203
00204 void
00205 WorldInfoViewerBackendThread::opponent_pose_rcvd( const char* from_host,
00206                                                   unsigned int uid,
00207                                                   float distance,
00208                                                   float angle,
00209                                                   float* covariance )
00210 {
00211 // #ifdef DEBUG_PRINT
00212 //   printf("Received opponent pose data form host %s\n", from_host );
00213 // #endif /* DEBUG_PRINT */
00214
00215   m_data_container->set_opponent_pos( from_host, uid, distance, angle,
00216                                       covariance );
00217   m_signal_new_worldinfo_data();
00218 }
00219
00220
00221 void
00222 WorldInfoViewerBackendThread::opponent_disapp_rcvd( const char *from_host,
00223                                                     unsigned int uid )
00224 {
00225   m_data_container->opponent_disappeared( from_host, uid );
00226   m_signal_new_worldinfo_data();
00227 }
00228
00229
00230 void
00231 WorldInfoViewerBackendThread::gamestate_rcvd( const char* from_host,
00232                                               unsigned int game_state,
00233                                               worldinfo_gamestate_team_t state_team,
00234                                               unsigned int score_cyan,
00235                                               unsigned int score_magenta,
00236                                               worldinfo_gamestate_team_t own_team,
00237                                               worldinfo_gamestate_goalcolor_t own_goal_color,
00238                                               worldinfo_gamestate_half_t half )
00239 {
00240 #ifdef DEBUG_PRINT
00241   printf( "Received gamestate data from host %s\n", from_host );
00242 #endif /* DEBUG_PRINT */
00243
00244   m_data_container->set_game_state( game_state, state_team,
00245                                     score_cyan, score_magenta,
00246                                     own_team, own_goal_color, half );
00247   m_signal_new_gamestate_data();
00248 }
00249
00250 void
00251 WorldInfoViewerBackendThread::penalty_rcvd(const char *from_host,
00252                                            unsigned int player,
00253                                            unsigned int penalty,
00254                                            unsigned int seconds_remaining)
00255 {
00256 }