motor_trace_buffer.cpp
Go to the documentation of this file.
00001 
00028 #include <sr_edc_ethercat_drivers/motor_trace_buffer.h>
00029 
00030 namespace sr_edc_ethercat_drivers
00031 {
00032 
00036 MotorTraceBuffer::MotorTraceBuffer(unsigned trace_size) :
00037   trace_size_(trace_size),
00038   trace_index_(0),
00039   published_traces_(0)
00040 {
00041   assert(trace_size_ > 0);
00042   trace_buffer_.reserve(trace_size_);
00043   reset();
00044 }
00045 
00046 void MotorTraceBuffer::reset()
00047 {
00048   publish_delay_ = -1;
00049   publish_level_ = -1;
00050   publish_reason_ = "OK";
00051 }
00052 
00053 
00056 bool MotorTraceBuffer::initialize(const sr_edc_ethercat_drivers::ActuatorInfo &actuator_info)
00057 {
00058   std::string topic("motor_trace");
00059   if (!actuator_info.name.empty())
00060     topic = topic + "/" + actuator_info.name;
00061   publisher_ = new realtime_tools::RealtimePublisher<sr_edc_ethercat_drivers::MotorTrace>(ros::NodeHandle(), topic, 1, true);
00062   if (publisher_ == NULL)
00063     return false;
00064 
00065   sr_edc_ethercat_drivers::MotorTrace &msg(publisher_->msg_);
00066   msg.actuator_info = actuator_info;
00067   msg.samples.reserve(trace_size_);
00068 
00069   return true;
00070 }
00071 
00072 
00075 void MotorTraceBuffer::checkPublish()
00076 {
00077   if (publish_delay_ < 0)
00078     return;
00079   --publish_delay_;
00080   if (publish_delay_ >= 0)
00081     return;
00082 
00083   ++published_traces_;
00084 
00085   assert(publisher_ != NULL);
00086   if ((publisher_==NULL) || (!publisher_->trylock()))
00087     return;
00088 
00089   sr_edc_ethercat_drivers::MotorTrace &msg(publisher_->msg_);
00090 
00091   msg.header.stamp = ros::Time::now();
00092   msg.reason = publish_reason_;
00093   unsigned size=trace_buffer_.size();
00094   msg.samples.clear();
00095   msg.samples.reserve(size);
00096 
00097   // TODO : is there a beter way to copy data between two std::vectors?
00098   for (unsigned i=0; i<size; ++i) {
00099     msg.samples.push_back(trace_buffer_.at((trace_index_+1+i)%size));
00100   }
00101 
00102   // Cancel any delayed publishing from occuring
00103   publish_delay_ = -1;
00104   publish_level_ = -1;
00105 
00106   publisher_->unlockAndPublish();
00107 }
00108 
00109 
00110 
00115 void MotorTraceBuffer::flagPublish(const std::string &reason, int level, int delay)
00116 {
00117   if (delay < 0)
00118     delay = 0;
00119   else if (delay > 900) {
00120     delay = 900;
00121   }
00122   if (level > publish_level_)
00123   {
00124     publish_reason_ = reason;
00125     publish_delay_ = delay;
00126     publish_level_ = level;
00127   }
00128 }
00129 
00130 
00133 void MotorTraceBuffer::sample(const sr_edc_ethercat_drivers::MotorTraceSample &s)
00134 {
00135 
00136   { // Add motor trace sample to trace buffer
00137     assert(trace_buffer_.size() <= trace_size_);
00138     if (trace_buffer_.size() >= trace_size_) {
00139       trace_index_ = (trace_index_+1)%trace_buffer_.size();
00140       trace_buffer_.at(trace_index_) = s;
00141     } else {
00142       trace_index_ = trace_buffer_.size();
00143       trace_buffer_.push_back(s);
00144     }
00145   }
00146 }
00147 
00148 
00149 
00150 };


sr_edc_ethercat_drivers
Author(s): Yann Sionneau, Ugo Cupcic / ugo@shadowrobot.com, software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:38:25