diagnostics.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Aldebaran
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019 * LOCAL includes
00020 */
00021 #include "diagnostics.hpp"
00022 
00023 namespace naoqi
00024 {
00025 namespace recorder
00026 {
00027 
00028 DiagnosticsRecorder::DiagnosticsRecorder( const std::string& topic, float buffer_frequency ):
00029   topic_( topic ),
00030   buffer_duration_( helpers::recorder::bufferDefaultDuration ),
00031   is_initialized_( false ),
00032   is_subscribed_( false ),
00033   buffer_frequency_(buffer_frequency),
00034   counter_(1)
00035 {}
00036 
00037 void DiagnosticsRecorder::write(diagnostic_msgs::DiagnosticArray& msg)
00038 {
00039   if (!msg.header.stamp.isZero()) {
00040     gr_->write(topic_, msg, msg.header.stamp);
00041   }
00042   else {
00043     gr_->write(topic_, msg);
00044   }
00045 }
00046 
00047 void DiagnosticsRecorder::writeDump(const ros::Time& time)
00048 {
00049   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
00050   boost::circular_buffer<diagnostic_msgs::DiagnosticArray>::iterator it;
00051   for (it = buffer_.begin(); it != buffer_.end(); it++)
00052   {
00053     if (!it->header.stamp.isZero()) {
00054       gr_->write(topic_, *it, it->header.stamp);
00055     }
00056     else {
00057       gr_->write(topic_, *it);
00058     }
00059   }
00060 }
00061 
00062 void DiagnosticsRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
00063 {
00064   gr_ = gr;
00065   conv_frequency_ = conv_frequency;
00066   if (buffer_frequency_ != 0)
00067   {
00068     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
00069     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
00070   }
00071   else
00072   {
00073     max_counter_ = 1;
00074     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
00075   }
00076   buffer_.resize(buffer_size_);
00077   is_initialized_ = true;
00078 }
00079 
00080 void DiagnosticsRecorder::bufferize(diagnostic_msgs::DiagnosticArray& msg )
00081 {
00082   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00083   if (counter_ < max_counter_)
00084   {
00085     counter_++;
00086   }
00087   else
00088   {
00089     counter_ = 1;
00090     buffer_.push_back(msg);
00091   }
00092 }
00093 
00094 void DiagnosticsRecorder::setBufferDuration(float duration)
00095 {
00096   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00097   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
00098   buffer_duration_ = duration;
00099   buffer_.set_capacity(buffer_size_);
00100 }
00101 
00102 } //publisher
00103 } // naoqi


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14