joint_state.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 "joint_state.hpp"
00022 
00023 namespace naoqi
00024 {
00025 namespace recorder
00026 {
00027 
00028 JointStateRecorder::JointStateRecorder( 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 JointStateRecorder::write( const sensor_msgs::JointState& js_msg,
00038                                 const std::vector<geometry_msgs::TransformStamped>& tf_transforms )
00039 {
00040   if (!js_msg.header.stamp.isZero()) {
00041     gr_->write(topic_, js_msg, js_msg.header.stamp);
00042   }
00043   else {
00044     gr_->write(topic_, js_msg);
00045   }
00046   gr_->write("/tf", tf_transforms);
00047 }
00048 
00049 void JointStateRecorder::writeDump(const ros::Time& time)
00050 {
00051   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
00052   boost::circular_buffer< std::vector<geometry_msgs::TransformStamped> >::iterator it_tf;
00053   for (it_tf = bufferTF_.begin(); it_tf != bufferTF_.end(); it_tf++)
00054   {
00055     gr_->write("/tf", *it_tf);
00056   }
00057   for (boost::circular_buffer<sensor_msgs::JointState>::iterator it_js = bufferJoinState_.begin();
00058        it_js != bufferJoinState_.end(); it_js++)
00059   {
00060     if (!it_js->header.stamp.isZero()) {
00061       gr_->write(topic_, *it_js, it_js->header.stamp);
00062     }
00063     else {
00064       gr_->write(topic_, *it_js);
00065     }
00066   }
00067 }
00068 
00069 void JointStateRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
00070 {
00071   gr_ = gr;
00072   conv_frequency_ = conv_frequency;
00073   if (buffer_frequency_ != 0)
00074   {
00075     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
00076     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
00077   }
00078   else
00079   {
00080     max_counter_ = 1;
00081     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
00082   }
00083   bufferJoinState_.resize(buffer_size_);
00084   bufferTF_.resize(buffer_size_);
00085   is_initialized_ = true;
00086 }
00087 
00088 void JointStateRecorder::bufferize( const sensor_msgs::JointState& js_msg,
00089                 const std::vector<geometry_msgs::TransformStamped>& tf_transforms )
00090 {
00091   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00092   if (counter_ < max_counter_)
00093   {
00094     counter_++;
00095   }
00096   else
00097   {
00098     counter_ = 1;
00099     bufferJoinState_.push_back(js_msg);
00100     bufferTF_.push_back(tf_transforms);
00101   }
00102 }
00103 
00104 void JointStateRecorder::setBufferDuration(float duration)
00105 {
00106   boost::mutex::scoped_lock lock_bufferize( mutex_ );
00107   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
00108   buffer_duration_ = duration;
00109   bufferJoinState_.set_capacity(buffer_size_);
00110   bufferTF_.set_capacity(buffer_size_);
00111 }
00112 
00113 } //publisher
00114 } // naoqi


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