camera.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 "camera.hpp"
00022 
00023 namespace naoqi
00024 {
00025 namespace recorder
00026 {
00027 
00028 CameraRecorder::CameraRecorder( const std::string& topic_, float buffer_frequency ):
00029   buffer_frequency_(buffer_frequency),
00030   buffer_duration_( helpers::recorder::bufferDefaultDuration ),
00031   counter_(1)
00032 {
00033   topic_info_ = topic_ + "/camera_info";
00034   topic_img_ = topic_ + "/image_raw";
00035 }
00036 
00037 void CameraRecorder::write(const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info)
00038 {
00039   if (!img->header.stamp.isZero()) {
00040     gr_->write(topic_img_, *img, img->header.stamp);
00041   }
00042   else {
00043     gr_->write(topic_img_, *img);
00044   }
00045   if (!camera_info.header.stamp.isZero()) {
00046     gr_->write(topic_info_, camera_info, camera_info.header.stamp);
00047   }
00048   else {
00049     gr_->write(topic_info_, camera_info);
00050   }
00051 }
00052 
00053 void CameraRecorder::writeDump(const ros::Time& time)
00054 {
00055   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
00056   boost::circular_buffer< std::pair<sensor_msgs::ImagePtr, sensor_msgs::CameraInfo> >::iterator it;
00057   for (it = buffer_.begin(); it != buffer_.end(); it++)
00058   {
00059     if (it->first != NULL)
00060     {
00061       write(it->first, it->second);
00062     }
00063   }
00064 }
00065 
00066 void CameraRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
00067 {
00068   gr_ = gr;
00069   conv_frequency_ = conv_frequency;
00070   max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
00071   buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
00072   buffer_.resize(buffer_size_);
00073   is_initialized_ = true;
00074 }
00075 
00076 void CameraRecorder::bufferize( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info )
00077 {
00078   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00079   if (counter_ < max_counter_)
00080   {
00081     counter_++;
00082   }
00083   else
00084   {
00085     counter_ = 1;
00086     buffer_.push_back(std::make_pair(img, camera_info));
00087   }
00088 }
00089 
00090 void CameraRecorder::setBufferDuration(float duration)
00091 {
00092   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00093   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
00094   buffer_duration_ = duration;
00095   buffer_.set_capacity(buffer_size_);
00096 }
00097 
00098 } //publisher
00099 } // naoqi


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