dish_viz.cpp
Go to the documentation of this file.
00001 /*
00002  * dish_viz.cpp
00003  * Copyright 2013 University of Massachusetts Lowell
00004  * Authors: Abraham Shultz, Jonathan Hasenzahl
00005  */
00006 
00007 #include "dish_viz/dish_viz.h"
00008 #include "time_server/time_srv.h"
00009 
00016 void DataHandler::init()
00017 {
00018     // Get parameters
00019     getParams();
00020 
00021     // Initialize the visualizer
00022     dviz_.init(color_mode_);
00023 
00024     // Initialize the client and subscribers
00025     time_client_ = n_.serviceClient<time_server::time_srv>("time_service");
00026 
00027     dish_sub_ = n_.subscribe("dish_states_to_dish_viz", 1000,
00028                              &DataHandler::dishCallback, this);
00029     ca_sub_ = n_.subscribe("cas", 1000, &DataHandler::caCallback, this);
00030     burst_sub_ = n_.subscribe("bursts_to_dish_viz", 1, &DataHandler::burstCallback,
00031                               this);
00032     ranges_sub_ = n_.subscribe("ranges_to_dish_viz", 1, &DataHandler::rangesCallback,
00033                                this);
00034 
00035     // Wait for start signal
00036     ROS_INFO("Waiting to start...");
00037     start_ = false;
00038     while (!start_ && ros::ok())
00039         ros::spinOnce();
00040 
00041     // Run the visualizer
00042     run();
00043 }
00044 
00048 void DataHandler::getParams()
00049 {
00050     // Get loop rate parameter
00051     if (!n_.getParam("loop_rate", loop_rate_))
00052     {
00053         ROS_WARN("Could not load loop_rate parameter: default is 200");
00054         loop_rate_ = 200;
00055     }
00056 
00057     // Get color mode parameter
00058     if (!n_.getParam("visualizer_color_mode", color_mode_))
00059     {
00060         ROS_WARN("Could not load visualizer_color_mode parameter: default is 0");
00061         color_mode_ = 0;
00062     }
00063 }
00064 
00070 void DataHandler::run()
00071 {
00072     ROS_INFO("Running visualizer");
00073 
00074     ros::Rate loop_rate(loop_rate_);
00075 
00076     while (!queue_.empty() && ros::ok())
00077     {
00078         ros::spinOnce();
00079 
00080         // Update the visualizer channels
00081         neuro_recv::dish_state d = queue_.front();
00082         //printf("Queue size: %d\n", static_cast<int>(queue_.size()));
00083         for (int i = 0; i < 60; i++)
00084             dviz_.update(i, static_cast<double>(d.samples[i]));
00085         queue_.pop();
00086 
00087         // Plot CA if conditions are met
00088         plotCa();
00089 
00090         loop_rate.sleep();
00091     }
00092 
00093     ROS_INFO("Visualizer queue empty: shutting down");
00094 }
00095 
00099 void DataHandler::plotCa()
00100 {
00101     if (!cas_.empty())
00102     {
00103         time_server::time_srv ca_check;
00104         ca_check.request.target = cas_.front().header.stamp;
00105 
00106         if (time_client_.call(ca_check))
00107         {
00108             printf("CA Time - Server Time = %f\n", ca_check.response.delta.toSec());
00109 
00110             // Send the CA to the visualizer if its equal to server time, with
00111             // a small room for error. If the CA is more than 0.1s behind server
00112             // time, then something has gone wrong.
00113             if (ca_check.response.delta <= ros::Duration(0.05) &&
00114                 ca_check.response.delta >= ros::Duration(-0.1))
00115             {
00116                 dviz_.updateCa(cas_.front());
00117                 cas_.pop();
00118             }
00119             else if (ca_check.response.delta < ros::Duration(-0.1))
00120             {
00121                 ROS_ERROR("CA time is behind visualizer time and will not display");
00122                 cas_.pop();
00123             }
00124         }
00125         else
00126             ROS_ERROR("Time server is not responding");
00127     }
00128 }
00129 
00138 void DataHandler::updateMinMax(const neuro_recv::dish_state& d)
00139 {
00140     for (int i = 0; i < 60; i++)
00141     {
00142         if (d.samples[i] > dviz_.getMaxVolt(i))
00143         {
00144             dviz_.setMaxVolt(i, d.samples[i]);
00145             //printf("Range correction: max of channel %d set to %f\n", i,
00146             //       d.samples[i]);
00147         }
00148         else if (d.samples[i] < dviz_.getMinVolt(i))
00149         {
00150             dviz_.setMinVolt(i, d.samples[i]);
00151             //printf("Range correction: min of channel %d set to %f\n", i,
00152             //       d.samples[i]);
00153         }
00154     }
00155 }
00156 
00166 void DataHandler::dishCallback(const neuro_recv::dish_state::ConstPtr &d)
00167 {
00168     updateMinMax(*d);
00169     queue_.push(*d);
00170     //printf("Queue size: %d\n", static_cast<int>(queue_.size()));
00171 }
00172 
00181 void DataHandler::caCallback(const burst_calc::ca::ConstPtr& c)
00182 {
00183     cas_.push(*c);
00184 }
00185 
00195 void DataHandler::burstCallback(const burst_calc::burst::ConstPtr &b)
00196 {
00197     start_ = true;
00198 }
00199 
00209 void DataHandler::rangesCallback(const burst_calc::ranges::ConstPtr& r)
00210 {
00211     dviz_.setVoltRanges(r->baselines, r->thresholds, r->min_volts, r->max_volts);
00212 
00213     /*
00214     for (int i = 0; i < 60; i++)
00215         printf("%d: Min[%.5f] Base[%.5f] Thresh[%.5f] Max[%.5f]\n", i, r->min_volts[i],
00216                r->baselines[i], r->thresholds[i], r->max_volts[i]);
00217     */
00218 }
00219 
00223 int main(int argc, char **argv)
00224 {
00225         ros::init(argc, argv, "dish_viz");
00226         DataHandler dh;
00227         return 0;
00228 }
00229 


dish_viz
Author(s): Abraham Shultz
autogenerated on Sun Jan 5 2014 11:12:31