src/tools/worldinfo_viewer/backend_thread.cpp

   1
   2 /***************************************************************************
   3  *  backend_thread.cpp - World Info Viewer backend thread
   4  *
   5  *  Created: Thu April 10 22:00:08 2008
   6  *  Copyright  2008  Daniel Beck
   7  *
   8  ****************************************************************************/
   9
  10 /*  This program is free software; you can redistribute it and/or modify
  11  *  it under the terms of the GNU General Public License as published by
  12  *  the Free Software Foundation; either version 2 of the License, or
  13  *  (at your option) any later version.
  14  *
  15  *  This program is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  *  GNU Library General Public License for more details.
  19  *
  20  *  Read the full text in the LICENSE.GPL file in the doc directory.
  21  */
  22
  23 #include "backend_thread.h"
  24 #include <worldinfo_utils/data_container.h>
  25 #include <netcomm/worldinfo/transceiver.h>
  26
  27 using namespace fawkes;
  28
  29 /** @class WorldInfoViewerBackendThread backend_thread.h <tools/worldinfo_viewer/backend_thread.h>
  30  * The backend thread of the worldinfo viewer application.
  31  * @author Daniel Beck
  32  */
  33
  34 /** Constructor.
  35  * @param data_container pointer to the central instance of the
  36  * WorldInfoDataContainer
  37  * @param addr multicast address to use for worldinfo communication
  38  * @param port port
  39  * @param key de-/encryption key
  40  * @param iv initialization vector for de-/encryption
  41  */
  42 WorldInfoViewerBackendThread::WorldInfoViewerBackendThread( WorldInfoDataContainer* data_container,
  43                                                             const char* addr,
  44                                                             unsigned short port,
  45                                                             const char* key,
  46                                                             const char* iv )
  47   : Thread("WorldInfoViewerBackendThread")
  48 {
  49   m_data_container = data_container;
  50
  51   m_addr = addr;
  52   m_port = port;
  53   m_key  = key;
  54   m_iv   = iv;
  55
  56   m_avahi = new AvahiThread();
  57   m_avahi->start();
  58
  59   m_resolver = new NetworkNameResolver( m_avahi );
  60
  61   m_transceiver = new WorldInfoTransceiver( WorldInfoTransceiver::MULTICAST,
  62                                             m_addr.c_str(),
  63                                             m_port,
  64                                             m_key.c_str(),
  65                                             m_iv.c_str(),
  66                                             m_resolver );
  67   m_transceiver->add_handler(this);
  68 }
  69
  70 /** Destructor. */
  71 WorldInfoViewerBackendThread::~WorldInfoViewerBackendThread()
  72 {
  73   delete m_transceiver;
  74   delete m_resolver;
  75
  76   m_avahi->cancel();
  77   m_avahi->join();
  78   delete m_avahi;
  79 }
  80
  81 /** Access the dispatcher that is emitted whenever new data has
  82  * arrived.
  83  * @return reference to the dispatcher
  84  */
  85 Glib::Dispatcher&
  86 WorldInfoViewerBackendThread::new_worldinfo_data()
  87 {
  88   return m_signal_new_worldinfo_data;
  89 }
  90
  91 /** Access the dispatcher that is emitted whenever new game state data
  92  * has arrived.
  93  * @return reference to the dispatcher
  94  */
  95 Glib::Dispatcher&
  96 WorldInfoViewerBackendThread::new_gamestate_data()
  97 {
  98   return m_signal_new_gamestate_data;
  99 }
 100
 101 void
 102 WorldInfoViewerBackendThread::loop()
 103 {
 104   m_transceiver->flush_sequence_numbers( 10 );
 105   m_transceiver->recv(true, 100);
 106   usleep(100000);
 107 }
 108
 109 void
 110 WorldInfoViewerBackendThread::pose_rcvd( const char* from_host,
 111                                          float x,
 112                                          float y,
 113                                          float theta,
 114                                          float* covariance )
 115 {
 116 #ifdef DEBUG_PRINT
 117   printf( "Received pose data from host %s: x=%.3f y=%.3f theta=%.3f\n",
 118           from_host, x, y, theta );
 119 #endif /* DEBUG_PRING */
 120
 121   m_data_container->set_robot_pose( from_host, x, y, theta, covariance );
 122   m_signal_new_worldinfo_data();
 123 }
 124
 125 void
 126 WorldInfoViewerBackendThread::velocity_rcvd( const char* from_host,
 127                                              float vel_x,
 128                                              float vel_y,
 129                                              float vel_theta,
 130                                              float* covariance )
 131 {
 132 #ifdef DEBUG_PRINT
 133   printf( "Received velocity data from host %s: vx=%.3f vy=%.3f vtheta=%.3f\n",
 134           from_host, vel_x, vel_y, vel_theta );
 135 #endif /* DEBUG_PRINT */
 136
 137   m_data_container->set_robot_velocity( from_host, vel_x, vel_y, vel_theta,
 138                                         covariance );
 139   m_signal_new_worldinfo_data();
 140 }
 141
 142 void
 143 WorldInfoViewerBackendThread::ball_pos_rcvd( const char* from_host,
 144                                              bool visible,
 145                                              int visibility_history,
 146                                              float dist,
 147                                              float bearing,
 148                                              float slope,
 149                                              float* covariance )
 150 {
 151 #ifdef DEBUG_PRINT
 152   if ( visible )
 153   { printf( "Received ball data from host %s: dist=%.3f bearing=%.3f\n",
 154             from_host, dist, bearing ); }
 155   else
 156   { printf( "Received ball not visible from host %s\n", from_host ); }
 157 #endif /* DEBUG_PRINT */
 158
 159   m_data_container->set_ball_pos( from_host, visible, visibility_history,
 160                                   dist, bearing, slope, covariance );
 161   m_signal_new_worldinfo_data();
 162 }
 163
 164 void
 165 WorldInfoViewerBackendThread::global_ball_pos_rcvd( const char* from_host,
 166                                                     bool visible,
 167                                                     int visibility_history,
 168                                                     float x,
 169                                                     float y,
 170                                                     float z,
 171                                                     float* covariance )
 172 {
 173 #ifdef DEBUG_PRINT
 174   if ( visible )
 175   { printf( "Received global ball data from host %s: x=%.3f y=%.3f\n",
 176             from_host, x, y ); }
 177   else
 178   { printf( "Received global ball not visible from host %s\n", from_host ); }
 179 #endif /* DEBUG_PRINT */
 180   m_data_container->set_ball_pos_global( from_host, visible,
 181                                          visibility_history,
 182                                          x, y, z, covariance );
 183   m_signal_new_worldinfo_data();
 184 }
 185
 186 void
 187 WorldInfoViewerBackendThread::ball_velocity_rcvd( const char* from_host,
 188                                                   float vel_x,
 189                                                   float vel_y,
 190                                                   float vel_z,
 191                                                   float* covariance )
 192 {
 193   m_data_container->set_ball_velocity( from_host, vel_x, vel_y, vel_z,
 194                                        covariance );
 195   m_signal_new_worldinfo_data();
 196 }
 197
 198 void
 199 WorldInfoViewerBackendThread::global_ball_velocity_rcvd( const char *from_host,
 200                                                          float vel_x,
 201                                                          float vel_y,
 202                                                          float vel_z,
 203                                                          float *covariance )
 204 {
 205   // TODO
 206
 207 //   m_data_container->set_global_ball_velocity( from_host, vel_x, vel_y, vel_z,
 208 //                                            covariance );
 209 //   m_signal_new_worldinfo_data();
 210 }
 211
 212
 213 void
 214 WorldInfoViewerBackendThread::opponent_pose_rcvd( const char* from_host,
 215                                                   unsigned int uid,
 216                                                   float distance,
 217                                                   float angle,
 218                                                   float* covariance )
 219 {
 220 // #ifdef DEBUG_PRINT
 221 //   printf("Received opponent pose data form host %s\n", from_host );
 222 // #endif /* DEBUG_PRINT */
 223
 224   m_data_container->set_opponent_pos( from_host, uid, distance, angle,
 225                                       covariance );
 226   m_signal_new_worldinfo_data();
 227 }
 228
 229
 230 void
 231 WorldInfoViewerBackendThread::opponent_disapp_rcvd( const char *from_host,
 232                                                     unsigned int uid )
 233 {
 234   m_data_container->opponent_disappeared( from_host, uid );
 235   m_signal_new_worldinfo_data();
 236 }
 237
 238
 239 void
 240 WorldInfoViewerBackendThread::gamestate_rcvd( const char* from_host,
 241                                               unsigned int game_state,
 242                                               worldinfo_gamestate_team_t state_team,
 243                                               unsigned int score_cyan,
 244                                               unsigned int score_magenta,
 245                                               worldinfo_gamestate_team_t own_team,
 246                                               worldinfo_gamestate_goalcolor_t own_goal_color,
 247                                               worldinfo_gamestate_half_t half )
 248 {
 249 #ifdef DEBUG_PRINT
 250   printf( "Received gamestate data from host %s\n", from_host );
 251 #endif /* DEBUG_PRINT */
 252
 253   m_data_container->set_game_state( game_state, state_team,
 254                                     score_cyan, score_magenta,
 255                                     own_team, own_goal_color, half );
 256   m_signal_new_gamestate_data();
 257 }
 258
 259 void
 260 WorldInfoViewerBackendThread::penalty_rcvd(const char *from_host,
 261                                            unsigned int player,
 262                                            unsigned int penalty,
 263                                            unsigned int seconds_remaining)
 264 {
 265 }