sonar.hpp
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 #ifndef SONAR_RECORDER_HPP
00019 #define SONAR_RECORDER_HPP
00020 
00021 /*
00022 * BOOST includes
00023 */
00024 #include <boost/circular_buffer.hpp>
00025 
00026 /*
00027 * LOCAL includes
00028 */
00029 #include <naoqi_driver/recorder/globalrecorder.hpp>
00030 #include "../helpers/recorder_helpers.hpp"
00031 
00032 /*
00033 * ROS includes
00034 */
00035 #include <sensor_msgs/Range.h>
00036 
00037 namespace naoqi
00038 {
00039 namespace recorder
00040 {
00041 
00042 class SonarRecorder
00043 {
00044 
00045 public:
00046   SonarRecorder( const std::vector<std::string>& topics, float buffer_frequency = 0 );
00047 
00048   void write(const std::vector<sensor_msgs::Range>& sonar_msgs );
00049 
00050   void reset( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr, float conv_frequency );
00051 
00052   void bufferize(const std::vector<sensor_msgs::Range>& sonar_msgs );
00053 
00054   void writeDump(const ros::Time& time);
00055 
00056   void setBufferDuration(float duration);
00057 
00058   inline std::string topic() const
00059   {
00060     return "sonar";
00061   }
00062 
00063   inline bool isInitialized() const
00064   {
00065     return is_initialized_;
00066   }
00067 
00068   inline void subscribe( bool state)
00069   {
00070     is_subscribed_ = state;
00071   }
00072 
00073   inline bool isSubscribed() const
00074   {
00075     return is_subscribed_;
00076   }
00077 
00078 protected:
00079   std::string topic_;
00080 
00081   boost::circular_buffer< std::vector<sensor_msgs::Range> > buffer_;
00082   size_t buffer_size_;
00083   float buffer_duration_;
00084 
00085   boost::mutex mutex_;
00086 
00087   bool is_initialized_;
00088   bool is_subscribed_;
00089 
00090   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
00091   std::vector<std::string> topics_;
00092 
00093   float buffer_frequency_;
00094   float conv_frequency_;
00095   int counter_;
00096   int max_counter_;
00097 
00098 }; // class
00099 
00100 } //publisher
00101 } // naoqi
00102 
00103 #endif


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