Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007 #include "burst_calc/burst_merger.h"
00008 #include <cstdio>
00009
00013 BurstMerger::BurstMerger()
00014 {
00015 for (int i = 0; i < 60; i++)
00016 times_[i] = NULL;
00017 }
00018
00022 BurstMerger::~BurstMerger()
00023 {
00024 for (int i = 0; i < 60; i++)
00025 {
00026 if (times_[i])
00027 delete times_[i];
00028 }
00029 }
00030
00037 void BurstMerger::update()
00038 {
00039 std::vector<int> delete_indexes;
00040
00041 if (list_.size() > 1)
00042 {
00043
00044 for (unsigned int i = 0; i < list_.size() - 1; i++)
00045 {
00046 bool stop = false;
00047 for (unsigned int j = i + 1; j < list_.size() && !stop; j++)
00048 {
00049
00050
00051
00052 if ((list_[i].header.stamp >= list_[j].header.stamp &&
00053 list_[i].header.stamp <= list_[j].end) ||
00054 (list_[j].header.stamp >= list_[i].header.stamp &&
00055 list_[j].header.stamp <= list_[i].end))
00056 {
00057 printf("** Merge : [Sz %4d] [%6.3f - %6.3f] [Ch",
00058 static_cast<int>(list_[i].dishes.size()),
00059 list_[i].header.stamp.toSec(), list_[i].end.toSec());
00060 for (unsigned int k = 0; k < list_[i].channels.size(); k++)
00061 printf(" %d", list_[i].channels[k]);
00062 printf("]\n");
00063
00064 printf("** + : [Sz %4d] [%6.3f - %6.3f] [Ch",
00065 static_cast<int>(list_[j].dishes.size()),
00066 list_[j].header.stamp.toSec(), list_[j].end.toSec());
00067 for (unsigned int k = 0; k < list_[j].channels.size(); k++)
00068 printf(" %d", list_[j].channels[k]);
00069 printf("]\n");
00070
00071
00072 list_[j] = merge(list_[i], list_[j]);
00073
00074 printf("** Result : [Sz %4d] [%6.3f - %6.3f] [Ch",
00075 static_cast<int>(list_[j].dishes.size()),
00076 list_[j].header.stamp.toSec(), list_[j].end.toSec());
00077 for (unsigned int k = 0; k < list_[j].channels.size(); k++)
00078 printf(" %d", list_[j].channels[k]);
00079 printf("]\n");
00080
00081 delete_indexes.push_back(i);
00082 stop = true;
00083 }
00084 }
00085 }
00086
00087
00088
00089 if (!delete_indexes.empty())
00090 {
00091 std::vector<int>::reverse_iterator r;
00092 for (r = delete_indexes.rbegin(); r < delete_indexes.rend(); r++)
00093 list_.erase(list_.begin() + *r);
00094 delete_indexes.clear();
00095 }
00096
00097 for (unsigned int i = 0; i < list_.size(); i++)
00098 {
00099 bool stop = false;
00100 for (int j = 0; j < 60 && !stop; j++)
00101 {
00102 if (times_[j])
00103 {
00104
00105
00106
00107 if (*times_[j] <= list_[i].end)
00108 stop = true;
00109 }
00110 }
00111
00112 if (!stop)
00113 {
00114
00115
00116 final_.push_back(list_[i]);
00117 delete_indexes.push_back(i);
00118 }
00119 }
00120
00121
00122
00123 if (!delete_indexes.empty())
00124 {
00125 std::vector<int>::reverse_iterator r;
00126 for (r = delete_indexes.rbegin(); r < delete_indexes.rend(); r++)
00127 list_.erase(list_.begin() + *r);
00128 delete_indexes.clear();
00129 }
00130 }
00131 }
00132
00142 void BurstMerger::updateTime(int index, const ros::Time* t)
00143 {
00144 if (times_[index])
00145 delete times_[index];
00146
00147 if (t)
00148
00149 times_[index] = new ros::Time(*t);
00150 else
00151
00152 times_[index] = NULL;
00153 }
00154
00155
00165 burst_calc::burst BurstMerger::merge(const burst_calc::burst& b1,
00166 const burst_calc::burst& b2)
00167 {
00168 burst_calc::burst x = b1;
00169 burst_calc::burst y = b2;
00170
00171
00172
00173
00174
00175 if ((y.end < x.end) ||
00176 ((y.end == x.end) && (x.header.stamp < y.header.stamp)))
00177 {
00178 burst_calc::burst temp = x;
00179 x = y;
00180 y = temp;
00181 }
00182
00183
00184 if (x.header.stamp < y.header.stamp)
00185 {
00186 ros::Time start = x.end;
00187 x.end = y.end;
00188 unsigned int index = 0;
00189
00190 while (start >= y.dishes[index].header.stamp)
00191 index++;
00192
00193 for (unsigned int i = index; i < y.dishes.size(); i++)
00194 x.dishes.push_back(y.dishes[i]);
00195
00196
00197 x.channels.insert(x.channels.end(), y.channels.begin(),
00198 y.channels.end());
00199
00200 return x;
00201 }
00202
00203 else
00204 {
00205
00206 y.channels.insert(y.channels.end(), x.channels.begin(),
00207 x.channels.end());
00208
00209 return y;
00210 }
00211 }