00001
00002
00003
00004
00005
00006
00007 #include "burst_calc/cat_creator.h"
00008 #include <cstdio>
00009
00013 CatCreator::~CatCreator()
00014 {
00015 if (cat_file_.is_open())
00016 cat_file_.close();
00017 }
00018
00019
00026 void CatCreator::init()
00027 {
00028 is_init_ = false;
00029 save_to_file_ = true;
00030
00031 getParams();
00032
00033
00034 cat_pub_ = n_.advertise<burst_calc::cat>("cats", 1000);
00035 ca_pub_ = n_.advertise<burst_calc::ca>("cas", 1000);
00036
00037
00038 ROS_INFO("Waiting for subscribers...");
00039 while (cat_pub_.getNumSubscribers() < 1 && ca_pub_.getNumSubscribers() &&
00040 ros::ok());
00041
00042
00043 burst_sub_ = n_.subscribe("bursts_to_cat_creator", 1000, &CatCreator::callback, this);
00044 ranges_sub_ = n_.subscribe("ranges_to_cat_creator", 1, &CatCreator::rangesCallback,
00045 this);
00046
00047 if (save_to_file_)
00048 initFile(file_name_.c_str());
00049
00050 ros::spin();
00051 }
00052
00056 void CatCreator::getParams()
00057 {
00058
00059 if (!n_.getParam("cat_log_path", file_name_))
00060 {
00061 ROS_WARN("Could not load burst_log_path parameter, logging will be disabled");
00062 save_to_file_ = false;
00063 }
00064 }
00065
00076 void CatCreator::updateOffsets(const neuro_recv::dish_state& d)
00077 {
00078 for (int i = 0; i < 60; i++)
00079 {
00080 if (d.samples[i] < offsets_[i])
00081 {
00082 offsets_[i] = d.samples[i];
00083
00084 }
00085 }
00086 }
00087
00096 void CatCreator::callback(const burst_calc::burst::ConstPtr& b)
00097 {
00098 if (is_init_)
00099 {
00100
00101 if (save_to_file_)
00102 headerToFile(*b);
00103
00104
00105 burst_calc::cat cat;
00106 cat.header.stamp = b->header.stamp;
00107 cat.end = b->end;
00108 cat.channels = b->channels;
00109
00110 for (unsigned int i = 0; i < b->dishes.size(); i++)
00111 {
00112
00113 updateOffsets(b->dishes[i]);
00114
00115 if (caExists(b->dishes[i]))
00116 {
00117
00118
00119 burst_calc::ca ca = getCa(b->dishes[i]);
00120 cat.cas.push_back(ca);
00121 ca_pub_.publish(ca);
00122 if (save_to_file_)
00123 toFile(i, b->dishes[i], ca);
00124 }
00125 else
00126 {
00127
00128
00129 if (save_to_file_)
00130 toFile(i, b->dishes[i]);
00131 }
00132 }
00133
00134
00135 cat_pub_.publish(cat);
00136 ROS_INFO("CAT of size %d created from burst of size %d",
00137 static_cast<int>(cat.cas.size()), static_cast<int>(b->dishes.size()));
00138 }
00139 else
00140 ROS_ERROR("Minimum voltages not initialized, skipping CAT creation");
00141 }
00142
00152 void CatCreator::rangesCallback(const burst_calc::ranges::ConstPtr& r)
00153 {
00154 ROS_INFO("CAT Creator initialized");
00155
00156 for (int i = 0; i < 60; i++)
00157 {
00158 offsets_[i] = r->min_volts[i];
00159 thresholds_[i] = r->thresholds[i];
00160 }
00161
00162 if (save_to_file_)
00163 {
00164 cat_file_ << "threshold_volts,,,,";
00165 for (int i = 0; i < 60; i++)
00166 cat_file_ << r->thresholds[i] << ',';
00167 cat_file_ << "\n\n";
00168 }
00169
00170 is_init_ = true;
00171 }
00172
00181 bool CatCreator::caExists(const neuro_recv::dish_state& d)
00182 {
00183
00184 for (int i = 0; i < 60; i ++)
00185 {
00186 if (d.samples[i] > thresholds_[i])
00187 return true;
00188 }
00189
00190
00191 return false;
00192 }
00193
00202 const burst_calc::ca CatCreator::getCa(const neuro_recv::dish_state& d)
00203 {
00204
00205 double x_sum = 0.0;
00206 double y_sum = 0.0;
00207 double activity = 0.0;
00208
00209 for (int i = 0; i < 60; i++)
00210 {
00211
00212 if (d.samples[i] > thresholds_[i])
00213 {
00214 double this_activity = d.samples[i] - offsets_[i];
00215 if (this_activity < 0.0)
00216 {
00217 ROS_ERROR("Activity is lower than recorded minimum, CA will not be accurate");
00218 ROS_ERROR("%d: %f = %f - %f\n", i, this_activity, d.samples[i],
00219 offsets_[i]);
00220 this_activity = -this_activity;
00221 }
00222
00223 x_sum += this_activity * X_COORD_[i];
00224 y_sum += this_activity * Y_COORD_[i];
00225 activity += this_activity;
00226 }
00227 }
00228
00229 burst_calc::ca ca;
00230 ca.header.stamp = d.header.stamp;
00231 if (activity > 0.0)
00232 {
00233 ca.x = x_sum / activity;
00234 ca.y = y_sum / activity;
00235 }
00236 else
00237 ROS_ERROR("No activity recorded for this CA");
00238
00239
00240
00241
00242 return ca;
00243 }
00244
00249 void CatCreator::initFile(const char* cat_file)
00250 {
00251 cat_file_.open(cat_file, std::ios_base::trunc | std::ios_base::out);
00252 if (!cat_file_.is_open())
00253 {
00254 ROS_ERROR("Cannot open %s. CSV logging will be disabled.", cat_file);
00255 save_to_file_ = false;
00256 return;
00257 }
00258
00259 cat_file_ << "index,seconds,ca_x,ca_y,";
00260 for (int i = 0; i < 60; i++)
00261 {
00262 cat_file_ << "channel_" << i << ',';
00263 }
00264 cat_file_ << '\n';
00265 }
00266
00271 void CatCreator::headerToFile(const burst_calc::burst& b)
00272 {
00273 cat_file_ << "begin_time," << b.header.stamp.toSec() << ",\n";
00274 cat_file_ << "end_time," << b.end.toSec() << ",\n";
00275 cat_file_ << "duration," << (b.end - b.header.stamp).toSec() << ",\n";
00276 cat_file_ << "bursting_channels,\n";
00277 for (unsigned int i = 0; i < b.channels.size(); i++)
00278 cat_file_ << static_cast<int>(b.channels[i]) << ",\n";
00279 }
00280
00286 void CatCreator::toFile(int i, const neuro_recv::dish_state& d,
00287 const burst_calc::ca& c)
00288 {
00289 cat_file_ << i << ',' << d.header.stamp.toSec() << ',' << c.x << ','
00290 << c.y << ',';
00291 for (int j = 0; j < 60; j++)
00292 cat_file_ << d.samples[j] << ',';
00293 cat_file_ << '\n';
00294 }
00295
00300 void CatCreator::toFile(int i, const neuro_recv::dish_state& d)
00301 {
00302 cat_file_ << i << ',' << d.header.stamp.toSec() << ",,,";
00303 for (int j = 0; j < 60; j++)
00304 cat_file_ << d.samples[j] << ',';
00305 cat_file_ << '\n';
00306 }
00307
00311 int main(int argc, char** argv)
00312 {
00313 ros::init(argc, argv, "cat_creator");
00314 CatCreator cat_creator;
00315 return 0;
00316 }