Fawkes API  Fawkes Development Version
wm_thread.cpp
1 
2 /***************************************************************************
3  * wm_thread.cpp - Fawkes WorldModel Plugin Thread
4  *
5  * Created: Fri Jun 29 11:56:48 2007 (on flight to RoboCup 2007, Atlanta)
6  * Copyright 2006-2008 Tim Niemueller [www.niemueller.de]
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 "wm_thread.h"
24 #include "net_thread.h"
25 
26 #include "fusers/single_copy.h"
27 #include "fusers/multi_copy.h"
28 #include "fusers/objpos_average.h"
29 #include "fusers/objpos_majority.h"
30 
31 #include <netcomm/worldinfo/transceiver.h>
32 #include <utils/system/pathparser.h>
33 #include <geometry/hom_point.h>
34 #include <geometry/hom_vector.h>
35 
36 #include <interfaces/GameStateInterface.h>
37 #include <interfaces/ObjectPositionInterface.h>
38 
39 #include <cmath>
40 #include <cstring>
41 
42 using namespace std;
43 using namespace fawkes;
44 
45 /** @class WorldModelThread <plugins/worldmodel/wm_thread.h>
46  * Main thread of worldmodel plugin.
47  * @author Tim Niemueller
48  * @author Daniel Beck
49  */
50 
51 /** Constructor.
52  * @param net_thread pointer to a WorldModelNetworkThread
53  */
55  : Thread("WorldModelThread", Thread::OPMODE_WAITFORWAKEUP),
56  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
57 {
58  __net_thread = net_thread;
59 
60  __wi_send_enabled = false;
61  __wi_send_interval = 20;
62  __wi_send_counter = 1;
63 
64  __wi_send_pose = NULL;
65  __wi_send_ball = NULL;
66 
67 
68  __wi_send_interval = 15;
69  __wi_send_counter = 1;
70 }
71 
72 
73 /** Destructor. */
75 {
76 }
77 
78 void
80 {
81  try {
82  __cfg_confspace = config->get_string("/worldmodel/confspace");
83 
84  logger->log_debug("WorldModelThread", "Config space: %s", __cfg_confspace.c_str());
85 
86  std::string prefix = "/worldmodel/interfaces/" + __cfg_confspace + "/";
87  std::list<std::string> combos;
88  // Read interfaces
89  Configuration::ValueIterator *vi = config->search(prefix.c_str());
90  while (vi->next()) {
91  if (strcmp(vi->type(), "string") == 0) {
92  PathParser pp(vi->path());
93  if ( pp.size() > 1 ) {
94  combos.push_back(pp[pp.size() - 2]);
95  }
96  }
97  }
98  combos.sort();
99  combos.unique();
100 
101  for (std::list<std::string>::iterator i = combos.begin(); i != combos.end(); ++i) {
102  std::string type = config->get_string((prefix + *i + "/type").c_str());
103  std::string from_id = config->get_string((prefix + *i + "/from_id").c_str());
104  std::string to_id = config->get_string((prefix + *i + "/to_id").c_str());
105  std::string method = config->get_string((prefix + *i + "/method").c_str());
106 
107  if (method == "copy") {
108  if (from_id.find_first_of("*?[") == std::string::npos) {
109  logger->log_debug(name(), "Instantiating SingleCopyFuser for %s -> %s (type: %s)",
110  from_id.c_str(), to_id.c_str(), type.c_str());
112  from_id.c_str(), to_id.c_str());
113  __fusers.push_back(fuser);
114  } else {
115  logger->log_debug(name(), "Instantiating MultiCopyFuser for %s -> %s (type: %s)",
116  from_id.c_str(), to_id.c_str(), type.c_str());
118  from_id.c_str(), to_id.c_str());
119  __fusers.push_back(fuser);
120  }
121  } else if (method == "average") {
122  // sanity checks
123  if (type != "ObjectPositionInterface") {
124  throw Exception("Can only average interfaces of type ObjectPositionInterface");
125  }
126  logger->log_debug(name(), "Instantiating ObjPosAverageFuser for %s -> %s (type: %s)",
127  from_id.c_str(), to_id.c_str(), type.c_str());
129  from_id.c_str(), to_id.c_str());
130  __fusers.push_back(fuser);
131  } else if (method == "majority") {
132  // sanity checks
133  if (type != "ObjectPositionInterface") {
134  throw Exception("Can only average interfaces of type ObjectPositionInterface");
135  }
136  std::string own_id = config->get_string((prefix + *i + "/own_id").c_str());
137  float self_confidence_radius = config->get_float((prefix + *i + "/confidence_radius").c_str());
138  logger->log_debug(name(), "Instantiating MajorityFuser for %s -> %s (type: %s)",
139  from_id.c_str(), to_id.c_str(), type.c_str());
141  own_id.c_str(),
142  from_id.c_str(),
143  to_id.c_str(),
144  self_confidence_radius);
145  __fusers.push_back(fuser);
146  } else {
147  throw Exception("Unknown fuse method '%s', for interface %s -> %s (type %s)",
148  method.c_str(), from_id.c_str(), to_id.c_str(), type.c_str());
149  }
150  }
151 
152  } catch (Exception &e) {
153  e.print_trace();
154  }
155 
156  __wi_send_enabled = false;
157  try {
158  std::string prefix = "/worldmodel/wi_send/" + __cfg_confspace + "/";
159  __wi_send_enabled = config->get_bool((prefix + "enable_send").c_str());
160 
161  if (__wi_send_enabled) {
162  logger->log_debug(name(), "Sending worldinfo messages enabled");
163 
164  std::string pose_id = config->get_string((prefix + "pose_id").c_str());
165  std::string ball_id = config->get_string((prefix + "ball_id").c_str());
166 
167  logger->log_debug(name(), "Obtaining pose worldinfo data from interface %s.",
168  pose_id.c_str());
169  logger->log_debug(name(), "Obtaining ball worldinfo data from interface %s.",
170  ball_id.c_str());
171 
172  __wi_send_pose = blackboard->open_for_reading<ObjectPositionInterface>(pose_id.c_str());
173  __wi_send_ball = blackboard->open_for_reading<ObjectPositionInterface>(ball_id.c_str());
174 
175  } else {
176  logger->log_debug(name(), "Sending worldinfo messages disabled");
177  }
178 
179  } catch (Exception& e) {
180  if ( __wi_send_enabled) {
181  throw;
182  } else {
183  logger->log_debug(name(), "Sending worldinfo messages disabled (enable not set)");
184  }
185  }
186 
187 }
188 
189 
190 /** Clean up when init failed.
191  * You may only call this from init(). Never ever call it from anywhere
192  * else!
193  */
194 void
196 {
197 }
198 
199 
200 void
202 {
203  for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
204  delete *__fit;
205  }
206  __fusers.clear();
207 
208  if (__wi_send_enabled) {
209  try {
210  blackboard->close(__wi_send_pose);
211  blackboard->close(__wi_send_ball);
212  } catch (Exception& e) {
213  e.print_trace();
214  }
215  }
216 }
217 
218 
219 void
221 {
222  for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
223  (*__fit)->fuse();
224  }
225 
226  // only send every __wi_send_interval loop
227  if ( 0 != (__wi_send_counter % __wi_send_interval) ) {
228  ++__wi_send_counter;
229  return;
230  }
231 
232  __wi_send_counter = 1;
233 
234  WorldInfoTransceiver* transceiver = __net_thread->get_transceiver();
235 
236  if (__wi_send_enabled) {
237  __wi_send_pose->read();
238  __wi_send_ball->read();
239 
240  bool do_send = false;
241 
242  // pose
243  HomPoint pos;
244  pos.x( __wi_send_pose->world_x() );
245  pos.y( __wi_send_pose->world_y() );
246  float yaw = __wi_send_pose->yaw();
247  if (__wi_send_pose->has_writer()) {
248  do_send = true;
249  transceiver->set_pose(pos.x(), pos.y(), yaw,
250  __wi_send_pose->world_xyz_covariance() /* TODO */);
251  transceiver->set_velocity(__wi_send_pose->world_x_velocity(),
252  __wi_send_pose->world_y_velocity(),
253  __wi_send_pose->world_z_velocity(),
254  __wi_send_pose->world_xyz_velocity_covariance());
255 
256  // ball
257  if (__wi_send_ball->has_writer() && __wi_send_ball->is_valid()) {
258  if (__wi_send_ball->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
259  transceiver->set_glob_ball_pos(__wi_send_ball->world_x(),
260  __wi_send_ball->world_y(),
261  __wi_send_ball->world_z(),
262  __wi_send_ball->world_xyz_covariance() );
263  } else {
264  // compute global ball position
265  HomVector relative_ball;
266  relative_ball.x( __wi_send_ball->relative_x() );
267  relative_ball.y( __wi_send_ball->relative_y() );
268  relative_ball.rotate_z( yaw );
269  HomPoint global_ball = pos + relative_ball;
270 
271  transceiver->set_glob_ball_pos(global_ball.x(), global_ball.y(), 0.0,
272  __wi_send_ball->dbs_covariance() /* TODO */);
273  }
274  transceiver->set_glob_ball_visible(__wi_send_ball->is_visible(),
275  __wi_send_ball->visibility_history());
276 
277  // TODO
278 // transceiver->set_glob_ball_velocity(__wi_send_ball->relative_x_velocity(),
279 // __wi_send_ball->relative_y_velocity(),
280 // __wi_send_ball->relative_z_velocity(),
281 // __wi_send_ball->relative_xyz_velocity_covariance());
282  }
283  }
284 
285  if (do_send) {
286  transceiver->send();
287  }
288  }
289 }
float relative_y() const
Get relative_y value.
void send()
Send information.
float * world_xyz_velocity_covariance() const
Get world_xyz_velocity_covariance value.
virtual const char * type() const =0
Type of value.
Class to send and receive world information.
Definition: transceiver.h:52
virtual float y() const
RO-getter for y.
Definition: hom_coord.cpp:115
float relative_x() const
Get relative_x value.
void init_failure_cleanup()
Clean up when init failed.
Definition: wm_thread.cpp:195
ObjectPositionInterface Fawkes BlackBoard Interface.
void set_glob_ball_pos(float x, float y, float z, float *covariance)
Set global ball position.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void loop()
Code to execute in the thread.
Definition: wm_thread.cpp:220
uint32_t flags() const
Get flags value.
float world_x_velocity() const
Get world_x_velocity value.
float world_z() const
Get world_z value.
STL namespace.
virtual void finalize()
Finalize the thread.
Definition: wm_thread.cpp:201
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual ~WorldModelThread()
Destructor.
Definition: wm_thread.cpp:74
float yaw() const
Get yaw value.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Multi interface copy fuser.
Definition: multi_copy.h:38
virtual const char * path() const =0
Path of value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
float world_x() const
Get world_x value.
Single interface copy fuser.
Definition: single_copy.h:33
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:782
float world_y_velocity() const
Get world_y_velocity value.
ObjectPositionInterface majority fuser.
Thread aspect to use blocked timing.
fawkes::WorldInfoTransceiver * get_transceiver()
Access the WI transceiver.
Definition: net_thread.cpp:210
A homogeneous point.
Definition: hom_point.h:33
void set_pose(float x, float y, float theta, float *covariance)
Set global pose of robot.
Base class for exceptions in Fawkes.
Definition: exception.h:36
float * dbs_covariance() const
Get dbs_covariance value.
bool is_visible() const
Get visible value.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
WorldModelThread(WorldModelNetworkThread *net_thread)
Constructor.
Definition: wm_thread.cpp:54
float world_z_velocity() const
Get world_z_velocity value.
virtual void init()
Initialize the thread.
Definition: wm_thread.cpp:79
ObjectPositionModel average fuser.
const char * name() const
Get name of thread.
Definition: thread.h:95
A homogeneous vector.
Definition: hom_vector.h:31
int32_t visibility_history() const
Get visibility_history value.
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
virtual HomCoord & rotate_z(float rad)
Convenience function to rotate the HomCoord around the z-axis.
Definition: hom_coord.cpp:234
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float world_y() const
Get world_y value.
bool is_valid() const
Get valid value.
Iterator interface to iterate over config values.
Definition: config.h:68
Path parser.
Definition: pathparser.h:32
float * world_xyz_covariance() const
Get world_xyz_covariance value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
void set_glob_ball_visible(bool visible, int visibility_history)
Set ball visibility for the global ball.
Network thread of worldmodel plugin.
Definition: net_thread.h:45
virtual float x() const
RO-getter for x.
Definition: hom_coord.cpp:85
void set_velocity(float vel_x, float vel_y, float vel_theta, float *covariance)
Set velocity of the robot.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
virtual void close(Interface *interface)=0
Close interface.