motor_trace_buffer.cpp

Go to the documentation of this file.

#include <sr_edc_ethercat_drivers/motor_trace_buffer.h>
#include <string>

namespace sr_edc_ethercat_drivers
{
  MotorTraceBuffer::MotorTraceBuffer(unsigned trace_size) :
          trace_size_(trace_size),
          trace_index_(0),
          published_traces_(0)
  {
    assert(trace_size_ > 0);
    trace_buffer_.reserve(trace_size_);
    reset();
  }

  void MotorTraceBuffer::reset()
  {
    publish_delay_ = -1;
    publish_level_ = -1;
    publish_reason_ = "OK";
  }


  bool MotorTraceBuffer::initialize(const sr_edc_ethercat_drivers::ActuatorInfo &actuator_info)
  {
    std::string topic("motor_trace");
    if (!actuator_info.name.empty())
    {
      topic = topic + "/" + actuator_info.name;
    }
    publisher_ = new realtime_tools::RealtimePublisher<sr_edc_ethercat_drivers::MotorTrace>(ros::NodeHandle(), topic, 1,
                                                                                            true);
    if (publisher_ == NULL)
    {
      return false;
    }

    sr_edc_ethercat_drivers::MotorTrace &msg(publisher_->msg_);
    msg.actuator_info = actuator_info;
    msg.samples.reserve(trace_size_);

    return true;
  }


  void MotorTraceBuffer::checkPublish()
  {
    if (publish_delay_ < 0)
    {
      return;
    }
    --publish_delay_;
    if (publish_delay_ >= 0)
    {
      return;
    }

    ++published_traces_;

    assert(publisher_ != NULL);
    if ((publisher_ == NULL) || (!publisher_->trylock()))
    {
      return;
    }

    sr_edc_ethercat_drivers::MotorTrace &msg(publisher_->msg_);

    msg.header.stamp = ros::Time::now();
    msg.reason = publish_reason_;
    unsigned size = trace_buffer_.size();
    msg.samples.clear();
    msg.samples.reserve(size);

    // @todo is there a beter way to copy data between two std::vectors?
    for (unsigned i = 0; i < size; ++i)
    {
      msg.samples.push_back(trace_buffer_.at((trace_index_ + 1 + i) % size));
    }

    // Cancel any delayed publishing from occuring
    publish_delay_ = -1;
    publish_level_ = -1;

    publisher_->unlockAndPublish();
  }


  void MotorTraceBuffer::flagPublish(const std::string &reason, int level, int delay)
  {
    if (delay < 0)
    {
      delay = 0;
    }
    else if (delay > 900)
    {
      delay = 900;
    }
    if (level > publish_level_)
    {
      publish_reason_ = reason;
      publish_delay_ = delay;
      publish_level_ = level;
    }
  }


  void MotorTraceBuffer::sample(const sr_edc_ethercat_drivers::MotorTraceSample &s)
  {
    {  // Add motor trace sample to trace buffer
      assert(trace_buffer_.size() <= trace_size_);
      if (trace_buffer_.size() >= trace_size_)
      {
        trace_index_ = (trace_index_ + 1) % trace_buffer_.size();
        trace_buffer_.at(trace_index_) = s;
      }
      else
      {
        trace_index_ = trace_buffer_.size();
        trace_buffer_.push_back(s);
      }
    }
  }
}  // namespace sr_edc_ethercat_drivers