Fawkes API
Fawkes Development Version
|
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 }