sonar.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 "sonar.hpp"
00022 
00023 namespace naoqi
00024 {
00025 namespace recorder
00026 {
00027 
00028 SonarRecorder::SonarRecorder(const std::vector<std::string>& topics, float buffer_frequency ):
00029   topics_(topics),
00030   buffer_duration_(helpers::recorder::bufferDefaultDuration),
00031   buffer_frequency_(buffer_frequency),
00032   counter_(1)
00033 {}
00034 
00035 void SonarRecorder::write(const std::vector<sensor_msgs::Range>& sonar_msgs)
00036 {
00037   if ( topics_.size() != sonar_msgs.size() )
00038   {
00039     std::cerr << "Incorrect number of sonar range messages in sonar recorder. " << sonar_msgs.size() << "/" << topics_.size() << std::endl;
00040     return;
00041   }
00042 
00043   for( size_t i=0; i<sonar_msgs.size(); ++i)
00044   {
00045     if (!sonar_msgs[i].header.stamp.isZero()) {
00046       gr_->write(topics_[i], sonar_msgs[i], sonar_msgs[i].header.stamp);
00047     }
00048     else {
00049       gr_->write(topics_[i], sonar_msgs[i]);
00050     }
00051   }
00052 }
00053 
00054 void SonarRecorder::writeDump(const ros::Time& time)
00055 {
00056   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
00057   boost::circular_buffer< std::vector<sensor_msgs::Range> >::iterator it;
00058   for (it = buffer_.begin(); it != buffer_.end(); it++)
00059   {
00060     write(*it);
00061   }
00062 }
00063 
00064 void SonarRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
00065 {
00066   gr_ = gr;
00067   conv_frequency_ = conv_frequency;
00068   if (buffer_frequency_ != 0)
00069   {
00070     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
00071     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
00072   }
00073   else
00074   {
00075     max_counter_ = 1;
00076     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
00077   }
00078   buffer_.resize(buffer_size_);
00079   is_initialized_ = true;
00080 }
00081 
00082 void SonarRecorder::bufferize(const std::vector<sensor_msgs::Range>& sonar_msgs )
00083 {
00084   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00085   if (counter_ < max_counter_)
00086   {
00087     counter_++;
00088   }
00089   else
00090   {
00091     counter_ = 1;
00092     buffer_.push_back(sonar_msgs);
00093   }
00094 }
00095 
00096 void SonarRecorder::setBufferDuration(float duration)
00097 {
00098   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00099   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
00100   buffer_duration_ = duration;
00101   buffer_.set_capacity(buffer_size_);
00102 }
00103 
00104 } //publisher
00105 } // naoqi


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