23 #include "wm_thread.h"
24 #include "net_thread.h"
26 #include "fusers/single_copy.h"
27 #include "fusers/multi_copy.h"
28 #include "fusers/objpos_average.h"
29 #include "fusers/objpos_majority.h"
31 #include <netcomm/worldinfo/transceiver.h>
32 #include <utils/system/pathparser.h>
33 #include <geometry/hom_point.h>
34 #include <geometry/hom_vector.h>
36 #include <interfaces/GameStateInterface.h>
37 #include <interfaces/ObjectPositionInterface.h>
43 using namespace fawkes;
55 :
Thread(
"WorldModelThread",
Thread::OPMODE_WAITFORWAKEUP),
58 __net_thread = net_thread;
60 __wi_send_enabled =
false;
61 __wi_send_interval = 20;
62 __wi_send_counter = 1;
64 __wi_send_pose = NULL;
65 __wi_send_ball = NULL;
68 __wi_send_interval = 15;
69 __wi_send_counter = 1;
84 logger->
log_debug(
"WorldModelThread",
"Config space: %s", __cfg_confspace.c_str());
86 std::string prefix =
"/worldmodel/interfaces/" + __cfg_confspace +
"/";
87 std::list<std::string> combos;
91 if (strcmp(vi->
type(),
"string") == 0) {
93 if ( pp.size() > 1 ) {
94 combos.push_back(pp[pp.size() - 2]);
101 for (std::list<std::string>::iterator i = combos.begin(); i != combos.end(); ++i) {
103 std::string from_id =
config->
get_string((prefix + *i +
"/from_id").c_str());
107 if (method ==
"copy") {
108 if (from_id.find_first_of(
"*?[") == std::string::npos) {
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);
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);
121 }
else if (method ==
"average") {
123 if (type !=
"ObjectPositionInterface") {
124 throw Exception(
"Can only average interfaces of type ObjectPositionInterface");
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") {
133 if (type !=
"ObjectPositionInterface") {
134 throw Exception(
"Can only average interfaces of type ObjectPositionInterface");
137 float self_confidence_radius =
config->
get_float((prefix + *i +
"/confidence_radius").c_str());
139 from_id.c_str(), to_id.c_str(), type.c_str());
144 self_confidence_radius);
145 __fusers.push_back(fuser);
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());
156 __wi_send_enabled =
false;
158 std::string prefix =
"/worldmodel/wi_send/" + __cfg_confspace +
"/";
159 __wi_send_enabled =
config->
get_bool((prefix +
"enable_send").c_str());
161 if (__wi_send_enabled) {
180 if ( __wi_send_enabled) {
203 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
208 if (__wi_send_enabled) {
222 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
227 if ( 0 != (__wi_send_counter % __wi_send_interval) ) {
232 __wi_send_counter = 1;
236 if (__wi_send_enabled) {
237 __wi_send_pose->
read();
238 __wi_send_ball->
read();
240 bool do_send =
false;
246 float yaw = __wi_send_pose->
yaw();
258 if (__wi_send_ball->
flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
269 HomPoint global_ball = pos + relative_ball;