Fawkes API  Fawkes Development Version
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  */
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. */
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&
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&
97 {
98  return m_signal_new_gamestate_data;
99 }
100 
101 void
103 {
104  m_transceiver->flush_sequence_numbers( 10 );
105  m_transceiver->recv(true, 100);
106  usleep(100000);
107 }
108 
109 void
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
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
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
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
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
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
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
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
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,
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
261  unsigned int player,
262  unsigned int penalty,
263  unsigned int seconds_remaining)
264 {
265 }
virtual void global_ball_velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_z, float *covariance)
Ball velocity information received.
Data container to store and exchange worldinfo data.
WorldInfoViewerBackendThread(fawkes::WorldInfoDataContainer *data_container, const char *addr, unsigned short port, const char *key, const char *iv)
Constructor.
Glib::Dispatcher & new_worldinfo_data()
Access the dispatcher that is emitted whenever new data has arrived.
virtual void pose_rcvd(const char *from_host, float x, float y, float theta, float *covariance)
Pose information received.
Class to send and receive world information.
Definition: transceiver.h:52
void set_ball_velocity(const char *from_host, float vel_x, float vel_y, float vel_z, float *covariance)
Set the ball velocity as it is estimated by the specified robot.
Fawkes library namespace.
void set_ball_pos_global(const char *from_host, bool visible, int visibility_history, float x, float y, float z, float *covariance)
Set the global ball position estimation of a robot.
void opponent_disappeared(const char *from_host, unsigned int uid)
Remove the opponent with the given ID form the list of opponents seen by the given robot...
Thread class encapsulation of pthreads.
Definition: thread.h:42
void set_game_state(int game_state, worldinfo_gamestate_team_t state_team, unsigned int score_cyan, unsigned int score_magenta, worldinfo_gamestate_team_t own_team, worldinfo_gamestate_goalcolor_t own_goal_color, worldinfo_gamestate_half_t half)
Set the gamestate.
Glib::Dispatcher & new_gamestate_data()
Access the dispatcher that is emitted whenever new game state data has arrived.
virtual void global_ball_pos_rcvd(const char *from_host, bool visible, int visibility_history, float x, float y, float z, float *covariance)
Global ball position information received.
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:70
virtual void gamestate_rcvd(const char *from_host, unsigned int game_state, fawkes::worldinfo_gamestate_team_t state_team, unsigned int score_cyan, unsigned int score_magenta, fawkes::worldinfo_gamestate_team_t our_team, fawkes::worldinfo_gamestate_goalcolor_t our_goal_color, fawkes::worldinfo_gamestate_half_t half)
Gamestate information received.
void loop()
Code to execute in the thread.
virtual ~WorldInfoViewerBackendThread()
Destructor.
void recv(bool block=false, unsigned int max_num_msgs=0)
Receive information.
Avahi main thread.
Definition: avahi_thread.h:55
void cancel()
Cancel a thread.
Definition: thread.cpp:640
void flush_sequence_numbers(unsigned int sec)
Flush sequence numbers conditionally.
virtual void opponent_pose_rcvd(const char *from_host, unsigned int uid, float distance, float angle, float *covarianceconst)
Opponent information received.
void set_opponent_pos(const char *from_host, unsigned int uid, float distance, float angle, float *covariance)
Set the position of a detected opponent.
Network name and address resolver.
Definition: resolver.h:48
void set_robot_velocity(const char *from_host, float vel_x, float vel_y, float vel_theta, float *covariance)
Set the velocity of the robot.
virtual void ball_velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_z, float *covariance)
Ball velocity information received.
worldinfo_gamestate_team_t
Team.
Definition: enums.h:54
virtual void velocity_rcvd(const char *from_host, float vel_x, float vel_y, float vel_theta, float *covariance)
Robot velocity information received.
virtual void ball_pos_rcvd(const char *from_host, bool visible, int visibility_history, float dist, float pitch, float yaw, float *covariance)
Ball position information received.
void set_robot_pose(const char *from_host, float x, float y, float theta, float *covariance)
Set the pose of a robot.
void join()
Join the thread.
Definition: thread.cpp:599
void add_handler(WorldInfoHandler *h)
Add a handler for world information.
void set_ball_pos(const char *from_host, bool visible, int visibility_history, float dist, float bearing, float slope, float *covariance)
Set the ball position estimation of a robot.
virtual void opponent_disapp_rcvd(const char *from_host, unsigned int uid)
Opponent disappeared.
virtual void penalty_rcvd(const char *from_host, unsigned int player, unsigned int penalty, unsigned int seconds_remaining)
Penalty info received.
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:63
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:507