00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "wm_thread.h"
00024 #include "net_thread.h"
00025
00026 #include "fusers/single_copy.h"
00027 #include "fusers/multi_copy.h"
00028 #include "fusers/objpos_average.h"
00029 #include "fusers/objpos_majority.h"
00030
00031 #include <netcomm/worldinfo/transceiver.h>
00032 #include <utils/system/pathparser.h>
00033 #include <geometry/hom_point.h>
00034 #include <geometry/hom_vector.h>
00035
00036 #include <interfaces/GameStateInterface.h>
00037 #include <interfaces/ObjectPositionInterface.h>
00038
00039 #include <cmath>
00040 #include <cstring>
00041
00042 using namespace std;
00043 using namespace fawkes;
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 WorldModelThread::WorldModelThread(WorldModelNetworkThread* net_thread)
00055 : Thread("WorldModelThread", Thread::OPMODE_WAITFORWAKEUP),
00056 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
00057 {
00058 __net_thread = net_thread;
00059
00060 __wi_send_enabled = false;
00061 __wi_send_interval = 20;
00062 __wi_send_counter = 1;
00063
00064 __wi_send_pose = NULL;
00065 __wi_send_ball = NULL;
00066
00067
00068 __wi_send_interval = 15;
00069 __wi_send_counter = 1;
00070 }
00071
00072
00073
00074 WorldModelThread::~WorldModelThread()
00075 {
00076 }
00077
00078 void
00079 WorldModelThread::init()
00080 {
00081 try {
00082 __cfg_confspace = config->get_string("/worldmodel/confspace");
00083
00084 logger->log_debug("WorldModelThread", "Config space: %s", __cfg_confspace.c_str());
00085
00086 std::string prefix = "/worldmodel/interfaces/" + __cfg_confspace + "/";
00087 std::list<std::string> combos;
00088
00089 Configuration::ValueIterator *vi = config->search(prefix.c_str());
00090 while (vi->next()) {
00091 if (strcmp(vi->type(), "string") == 0) {
00092 PathParser pp(vi->path());
00093 if ( pp.size() > 1 ) {
00094 combos.push_back(pp[pp.size() - 2]);
00095 }
00096 }
00097 }
00098 combos.sort();
00099 combos.unique();
00100
00101 for (std::list<std::string>::iterator i = combos.begin(); i != combos.end(); ++i) {
00102 std::string type = config->get_string((prefix + *i + "/type").c_str());
00103 std::string from_id = config->get_string((prefix + *i + "/from_id").c_str());
00104 std::string to_id = config->get_string((prefix + *i + "/to_id").c_str());
00105 std::string method = config->get_string((prefix + *i + "/method").c_str());
00106
00107 if (method == "copy") {
00108 if (from_id.find_first_of("*?[") == std::string::npos) {
00109 logger->log_debug(name(), "Instantiating SingleCopyFuser for %s -> %s (type: %s)",
00110 from_id.c_str(), to_id.c_str(), type.c_str());
00111 WorldModelSingleCopyFuser *fuser = new WorldModelSingleCopyFuser(blackboard, type.c_str(),
00112 from_id.c_str(), to_id.c_str());
00113 __fusers.push_back(fuser);
00114 } else {
00115 logger->log_debug(name(), "Instantiating MultiCopyFuser for %s -> %s (type: %s)",
00116 from_id.c_str(), to_id.c_str(), type.c_str());
00117 WorldModelMultiCopyFuser *fuser = new WorldModelMultiCopyFuser(blackboard, type.c_str(),
00118 from_id.c_str(), to_id.c_str());
00119 __fusers.push_back(fuser);
00120 }
00121 } else if (method == "average") {
00122
00123 if (type != "ObjectPositionInterface") {
00124 throw Exception("Can only average interfaces of type ObjectPositionInterface");
00125 }
00126 logger->log_debug(name(), "Instantiating ObjPosAverageFuser for %s -> %s (type: %s)",
00127 from_id.c_str(), to_id.c_str(), type.c_str());
00128 WorldModelObjPosAverageFuser *fuser = new WorldModelObjPosAverageFuser(logger, blackboard,
00129 from_id.c_str(), to_id.c_str());
00130 __fusers.push_back(fuser);
00131 } else if (method == "majority") {
00132
00133 if (type != "ObjectPositionInterface") {
00134 throw Exception("Can only average interfaces of type ObjectPositionInterface");
00135 }
00136 std::string own_id = config->get_string((prefix + *i + "/own_id").c_str());
00137 float self_confidence_radius = config->get_float((prefix + *i + "/confidence_radius").c_str());
00138 logger->log_debug(name(), "Instantiating MajorityFuser for %s -> %s (type: %s)",
00139 from_id.c_str(), to_id.c_str(), type.c_str());
00140 WorldModelObjPosMajorityFuser *fuser = new WorldModelObjPosMajorityFuser(logger, blackboard,
00141 own_id.c_str(),
00142 from_id.c_str(),
00143 to_id.c_str(),
00144 self_confidence_radius);
00145 __fusers.push_back(fuser);
00146 } else {
00147 throw Exception("Unknown fuse method '%s', for interface %s -> %s (type %s)",
00148 method.c_str(), from_id.c_str(), to_id.c_str(), type.c_str());
00149 }
00150 }
00151
00152 } catch (Exception &e) {
00153 e.print_trace();
00154 }
00155
00156 __wi_send_enabled = false;
00157 try {
00158 std::string prefix = "/worldmodel/wi_send/" + __cfg_confspace + "/";
00159 __wi_send_enabled = config->get_bool((prefix + "enable_send").c_str());
00160
00161 if (__wi_send_enabled) {
00162 logger->log_debug(name(), "Sending worldinfo messages enabled");
00163
00164 std::string pose_id = config->get_string((prefix + "pose_id").c_str());
00165 std::string ball_id = config->get_string((prefix + "ball_id").c_str());
00166
00167 logger->log_debug(name(), "Obtaining pose worldinfo data from interface %s.",
00168 pose_id.c_str());
00169 logger->log_debug(name(), "Obtaining ball worldinfo data from interface %s.",
00170 ball_id.c_str());
00171
00172 __wi_send_pose = blackboard->open_for_reading<ObjectPositionInterface>(pose_id.c_str());
00173 __wi_send_ball = blackboard->open_for_reading<ObjectPositionInterface>(ball_id.c_str());
00174
00175 } else {
00176 logger->log_debug(name(), "Sending worldinfo messages disabled");
00177 }
00178
00179 } catch (Exception& e) {
00180 if ( __wi_send_enabled) {
00181 throw;
00182 } else {
00183 logger->log_debug(name(), "Sending worldinfo messages disabled (enable not set)");
00184 }
00185 }
00186
00187 }
00188
00189
00190
00191
00192
00193
00194 void
00195 WorldModelThread::init_failure_cleanup()
00196 {
00197 }
00198
00199
00200 void
00201 WorldModelThread::finalize()
00202 {
00203 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
00204 delete *__fit;
00205 }
00206 __fusers.clear();
00207
00208 if (__wi_send_enabled) {
00209 try {
00210 blackboard->close(__wi_send_pose);
00211 blackboard->close(__wi_send_ball);
00212 } catch (Exception& e) {
00213 e.print_trace();
00214 }
00215 }
00216 }
00217
00218
00219 void
00220 WorldModelThread::loop()
00221 {
00222 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
00223 (*__fit)->fuse();
00224 }
00225
00226
00227 if ( 0 != (__wi_send_counter % __wi_send_interval) ) {
00228 ++__wi_send_counter;
00229 return;
00230 }
00231
00232 __wi_send_counter = 1;
00233
00234 WorldInfoTransceiver* transceiver = __net_thread->get_transceiver();
00235
00236 if (__wi_send_enabled) {
00237 __wi_send_pose->read();
00238 __wi_send_ball->read();
00239
00240 bool do_send = false;
00241
00242
00243 HomPoint pos;
00244 pos.x( __wi_send_pose->world_x() );
00245 pos.y( __wi_send_pose->world_y() );
00246 float yaw = __wi_send_pose->yaw();
00247 if (__wi_send_pose->has_writer()) {
00248 do_send = true;
00249 transceiver->set_pose(pos.x(), pos.y(), yaw,
00250 __wi_send_pose->world_xyz_covariance() );
00251 transceiver->set_velocity(__wi_send_pose->world_x_velocity(),
00252 __wi_send_pose->world_y_velocity(),
00253 __wi_send_pose->world_z_velocity(),
00254 __wi_send_pose->world_xyz_velocity_covariance());
00255
00256
00257 if (__wi_send_ball->has_writer() && __wi_send_ball->is_valid()) {
00258 if (__wi_send_ball->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
00259 transceiver->set_glob_ball_pos(__wi_send_ball->world_x(),
00260 __wi_send_ball->world_y(),
00261 __wi_send_ball->world_z(),
00262 __wi_send_ball->world_xyz_covariance() );
00263 } else {
00264
00265 HomVector relative_ball;
00266 relative_ball.x( __wi_send_ball->relative_x() );
00267 relative_ball.y( __wi_send_ball->relative_y() );
00268 relative_ball.rotate_z( yaw );
00269 HomPoint global_ball = pos + relative_ball;
00270
00271 transceiver->set_glob_ball_pos(global_ball.x(), global_ball.y(), 0.0,
00272 __wi_send_ball->dbs_covariance() );
00273 }
00274 transceiver->set_glob_ball_visible(__wi_send_ball->is_visible(),
00275 __wi_send_ball->visibility_history());
00276
00277
00278
00279
00280
00281
00282 }
00283 }
00284
00285 if (do_send) {
00286 transceiver->send();
00287 }
00288 }
00289 }