00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
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
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
00082
00083
00084
00085 Glib::Dispatcher&
00086 WorldInfoViewerBackendThread::new_worldinfo_data()
00087 {
00088 return m_signal_new_worldinfo_data;
00089 }
00090
00091
00092
00093
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
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
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
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
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
00206
00207
00208
00209
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
00221
00222
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
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 }