multi_copy.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "multi_copy.h"
00024
00025 #include <core/threading/mutex_locker.h>
00026 #include <blackboard/blackboard.h>
00027 #include <interface/interface.h>
00028 #include <core/exceptions/system.h>
00029
00030 #include <cstdlib>
00031 #include <cstring>
00032 #include <cstdio>
00033
00034 using namespace fawkes;
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056 WorldModelMultiCopyFuser::WorldModelMultiCopyFuser(fawkes::BlackBoard *blackboard,
00057 const char *type,
00058 const char *from_id_pattern,
00059 const char *to_id_format)
00060 {
00061 __blackboard = blackboard;
00062 __from_id_pattern = from_id_pattern;
00063 __to_id_format = to_id_format;
00064
00065 std::string::size_type loc = 0;
00066 loc = __to_id_format.find("%");
00067 if ( (loc == std::string::npos) ||
00068 (__to_id_format.find("%", loc+1) != std::string::npos) ||
00069 (__to_id_format.find("%u") == std::string::npos) ) {
00070 throw Exception("to_id_format ('%s') must contain exactly one occurrence of %%u", to_id_format);
00071 }
00072
00073 std::list<Interface *> exifs;
00074 try {
00075 exifs = blackboard->open_multiple_for_reading(type, from_id_pattern);
00076 unsigned int u = 0;
00077 for (std::list<Interface *>::iterator i = exifs.begin(); i != exifs.end(); ++i) {
00078 char *tid;
00079 if (asprintf(&tid, to_id_format, ++u) != -1) {
00080 std::string sid = tid;
00081 free(tid);
00082 Interface *to_if = blackboard->open_for_writing(type, sid.c_str());
00083 __ifmap[*i] = to_if;
00084 } else {
00085 throw OutOfMemoryException("Could not create interface ID, out of memory");
00086 }
00087 }
00088 } catch (Exception &e) {
00089 for (std::list<Interface *>::iterator i = exifs.begin(); i != exifs.end(); ++i) {
00090 blackboard->close(*i);
00091 }
00092 for (__imi = __ifmap.begin(); __imi != __ifmap.end(); ++__imi) {
00093 blackboard->close(__imi->second);
00094 }
00095 throw;
00096 }
00097
00098 bbio_add_observed_create(type, from_id_pattern);
00099 blackboard->register_observer(this, BlackBoard::BBIO_FLAG_CREATED);
00100 }
00101
00102
00103
00104 WorldModelMultiCopyFuser::~WorldModelMultiCopyFuser()
00105 {
00106 __blackboard->unregister_observer(this);
00107
00108 __ifmap.lock();
00109 for (__imi = __ifmap.begin(); __imi != __ifmap.end(); ++__imi) {
00110 __blackboard->close(__imi->first);
00111 __blackboard->close(__imi->second);
00112 }
00113 __ifmap.clear();
00114 __ifmap.unlock();
00115 }
00116
00117
00118 void
00119 WorldModelMultiCopyFuser::bb_interface_created(const char *type, const char *id) throw()
00120 {
00121 unsigned int u;
00122 if (sscanf(id, __to_id_format.c_str(), &u) == 1) {
00123
00124 return;
00125 }
00126
00127 char *tid;
00128 u = __ifmap.size();
00129 if (asprintf(&tid, __to_id_format.c_str(), u) == -1) {
00130 printf("Could not create ID string, asprintf() ran out of memory");
00131 return;
00132 }
00133 std::string sid = tid;
00134 free(tid);
00135
00136 Interface *from_if = NULL;
00137 Interface *to_if = NULL;
00138
00139 try {
00140 from_if = __blackboard->open_for_reading(type, id);
00141 to_if = __blackboard->open_for_writing(type, sid.c_str());
00142
00143 __ifmap.lock();
00144 __ifmap[from_if] = to_if;
00145 __ifmap.unlock();
00146 } catch (Exception &e) {
00147 __blackboard->close(from_if);
00148 __blackboard->close(to_if);
00149 e.print_trace();
00150 }
00151 }
00152
00153
00154 void
00155 WorldModelMultiCopyFuser::fuse()
00156 {
00157 MutexLocker lock(__ifmap.mutex());
00158 for (__imi = __ifmap.begin(); __imi != __ifmap.end(); ++__imi) {
00159 if (__imi->first->has_writer()) {
00160 __imi->first->read();
00161 __imi->second->copy_values(__imi->first);
00162 __imi->second->write();
00163 }
00164 }
00165 }