motor_trace_buffer.cpp
Go to the documentation of this file.
1 
28 #include <string>
29 
31 {
35  MotorTraceBuffer::MotorTraceBuffer(unsigned trace_size) :
36  trace_size_(trace_size),
37  trace_index_(0),
38  published_traces_(0)
39  {
40  assert(trace_size_ > 0);
41  trace_buffer_.reserve(trace_size_);
42  reset();
43  }
44 
46  {
47  publish_delay_ = -1;
48  publish_level_ = -1;
49  publish_reason_ = "OK";
50  }
51 
52 
55  bool MotorTraceBuffer::initialize(const sr_edc_ethercat_drivers::ActuatorInfo &actuator_info)
56  {
57  std::string topic("motor_trace");
58  if (!actuator_info.name.empty())
59  {
60  topic = topic + "/" + actuator_info.name;
61  }
63  true);
64  if (publisher_ == NULL)
65  {
66  return false;
67  }
68 
69  sr_edc_ethercat_drivers::MotorTrace &msg(publisher_->msg_);
70  msg.actuator_info = actuator_info;
71  msg.samples.reserve(trace_size_);
72 
73  return true;
74  }
75 
76 
80  {
81  if (publish_delay_ < 0)
82  {
83  return;
84  }
86  if (publish_delay_ >= 0)
87  {
88  return;
89  }
90 
92 
93  assert(publisher_ != NULL);
94  if ((publisher_ == NULL) || (!publisher_->trylock()))
95  {
96  return;
97  }
98 
99  sr_edc_ethercat_drivers::MotorTrace &msg(publisher_->msg_);
100 
101  msg.header.stamp = ros::Time::now();
102  msg.reason = publish_reason_;
103  unsigned size = trace_buffer_.size();
104  msg.samples.clear();
105  msg.samples.reserve(size);
106 
107  // @todo is there a beter way to copy data between two std::vectors?
108  for (unsigned i = 0; i < size; ++i)
109  {
110  msg.samples.push_back(trace_buffer_.at((trace_index_ + 1 + i) % size));
111  }
112 
113  // Cancel any delayed publishing from occuring
114  publish_delay_ = -1;
115  publish_level_ = -1;
116 
118  }
119 
120 
125  void MotorTraceBuffer::flagPublish(const std::string &reason, int level, int delay)
126  {
127  if (delay < 0)
128  {
129  delay = 0;
130  }
131  else if (delay > 900)
132  {
133  delay = 900;
134  }
135  if (level > publish_level_)
136  {
137  publish_reason_ = reason;
138  publish_delay_ = delay;
139  publish_level_ = level;
140  }
141  }
142 
143 
146  void MotorTraceBuffer::sample(const sr_edc_ethercat_drivers::MotorTraceSample &s)
147  {
148  { // Add motor trace sample to trace buffer
149  assert(trace_buffer_.size() <= trace_size_);
150  if (trace_buffer_.size() >= trace_size_)
151  {
152  trace_index_ = (trace_index_ + 1) % trace_buffer_.size();
153  trace_buffer_.at(trace_index_) = s;
154  }
155  else
156  {
157  trace_index_ = trace_buffer_.size();
158  trace_buffer_.push_back(s);
159  }
160  }
161  }
162 } // namespace sr_edc_ethercat_drivers
msg
void flagPublish(const std::string &reason, int level, int delay)
flags delayed publish of motor trace.
void sample(const sr_edc_ethercat_drivers::MotorTraceSample &s)
Adds sample to motor trace.
bool initialize(const sr_edc_ethercat_drivers::ActuatorInfo &actuator_info)
Initializes motor trace publisher.
realtime_tools::RealtimePublisher< sr_edc_ethercat_drivers::MotorTrace > * publisher_
Publishes the last second of the motor controller.
static Time now()
void checkPublish()
Publishes motor trace if delay time is up.
std::vector< sr_edc_ethercat_drivers::MotorTraceSample > trace_buffer_


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:53