view.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <rosmatlab/rosbag/view.h>
00030 #include <rosmatlab/rosbag/bag.h>
00031 #include <rosmatlab/rosbag/query.h>
00032 
00033 #include <rosmatlab/options.h>
00034 #include <rosmatlab/connection_header.h>
00035 #include <rosmatlab/conversion.h>
00036 #include <rosmatlab/log.h>
00037 
00038 #include <ros/forwards.h>
00039 #include <introspection/message.h>
00040 
00041 #include <boost/algorithm/string/replace.hpp>
00042 
00043 
00044 namespace rosmatlab {
00045 namespace rosbag {
00046 
00047 template <> const char *Object<View>::class_name_ = "rosbag.View";
00048 
00049 View::View(int nrhs, const mxArray *prhs[])
00050   : Object<View>(this)
00051 {
00052   reset();
00053   if (nrhs > 0) addQuery(nrhs, prhs);
00054 }
00055 
00056 View::View(const Bag& bag, int nrhs, const mxArray *prhs[])
00057   : Object<View>(this)
00058 {
00059   reset();
00060   addQuery(bag, nrhs, prhs);
00061 }
00062 
00063 View::~View()
00064 {
00065 }
00066 
00067 mxArray *View::getSize()
00068 {
00069   return mxCreateDoubleScalar(::rosbag::View::size());
00070 }
00071 
00072 void View::addQuery(int nrhs, const mxArray *prhs[])
00073 {
00074   if (nrhs < 1) throw ArgumentException("View.addQuery", 1);
00075 
00076   const Bag *bag = getObject<Bag>(prhs[0]);
00077   if (!bag) throw Exception("View.addQuery", "first argument is not a valid bag handle");
00078 
00079   addQuery(*bag, nrhs - 1, prhs + 1);
00080 }
00081 
00082 void View::addQuery(const Bag& bag, int nrhs, const mxArray *prhs[])
00083 {
00084   QueryPtr query(new Query(nrhs, prhs));
00085   ::rosbag::View::addQuery(bag, *query, query->getStartTime(), query->getEndTime());
00086   queries_.push_back(query);
00087 }
00088 
00089 void View::reset()
00090 {
00091   current_ = end();
00092   eof_ = false;
00093 }
00094 
00095 bool View::start()
00096 {
00097   current_ = begin();
00098   eof_ = false;
00099   return valid();
00100 }
00101 
00102 void View::increment() {
00103   message_instance_.reset();
00104   if (eof_) return;
00105 
00106   // reset current iterator
00107   if (!valid()) {
00108     start();
00109 
00110   // or increment iterator
00111   } else {
00112     current_++;
00113   }
00114 
00115   if (!valid()) eof_ = true;
00116 }
00117 
00118 bool View::eof()
00119 {
00120   return eof_;
00121 }
00122 
00123 bool View::valid()
00124 {
00125   return current_ != end();
00126 }
00127 
00128 View::iterator& View::operator*()
00129 {
00130   return current_;
00131 }
00132 
00133 MessageInstance* View::operator->()
00134 {
00135   return &(*current_);
00136 }
00137 
00138 void View::get(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00139 {
00140   plhs[0] = getInternal(plhs[0]);
00141   if (nlhs > 1) plhs[1] = getTopic();
00142   if (nlhs > 2) plhs[2] = getDataType();
00143   if (nlhs > 3) plhs[3] = getConnectionHeader();
00144   if (nlhs > 4) plhs[4] = getTime();
00145 }
00146 
00147 void View::next(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00148 {
00149   increment();
00150   if (nlhs > 0) get(nlhs, plhs, nrhs, prhs);
00151 }
00152 
00153 mxArray *View::getInternal(mxArray *target, std::size_t index, std::size_t size)
00154 {
00155    // go to the first entry if the current iterator is not valid
00156   if (!valid()) increment();
00157 
00158   // introspect message
00159   if (valid() && !message_instance_) {
00160     MessagePtr introspection = messageByMD5Sum(current_->getMD5Sum());
00161     if (introspection) {
00162       // copy the serialized message to the read buffer (ugly)
00163       std::size_t size = current_->size();
00164       if (read_buffer_.size() < size) read_buffer_.resize(size);
00165       ros::serialization::OStream ostream(read_buffer_.data(), read_buffer_.size());
00166       current_->write(ostream);
00167 
00168       // deserialize message
00169       ros::serialization::IStream istream(read_buffer_.data(), size);
00170       VoidPtr msg = introspection->deserialize(istream);
00171       if (!msg) ROSMATLAB_WARN("deserialization of a message of type %s failed", current_->getDataType().c_str());
00172 
00173       message_instance_ = introspection->introspect(msg);
00174     } else {
00175       ROSMATLAB_PRINTF("Unknown data type '%s' in bag file", current_->getDataType().c_str());
00176       // throw UnknownDataTypeException(current_->getDataType());
00177     }
00178   }
00179 
00180   // convert message instance to Matlab
00181   if (message_instance_) {
00182     target = Conversion(message_instance_).toMatlab(target, index, size);
00183   } else {
00184     target = mxCreateStructMatrix(0, 0, 0, 0);
00185   }
00186 
00187   return target;
00188 }
00189 
00190 namespace {
00191   struct FieldInfo {
00192     std::string topic;
00193     std::string name;
00194     int fieldnum;
00195     std::size_t index;
00196     std::size_t size;
00197   };
00198 }
00199 
00200 void View::data(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00201 {
00202   std::vector<const ConnectionInfo *> connections = ::rosbag::View::getConnections();
00203 
00204   // extract topic information from Connections
00205   std::map<std::string, FieldInfo> topics;
00206   std::vector<const char *> fieldnames;
00207   fieldnames.reserve(connections.size());
00208 
00209   for(std::vector<const ConnectionInfo *>::iterator it = connections.begin(); it != connections.end(); ++it) {
00210     const ConnectionInfo *c = *it;
00211     FieldInfo field;
00212     field.topic = c->topic;
00213     field.name = c->topic;
00214     boost::algorithm::replace_all(field.name, "/", "_");
00215     if (field.name.at(0) == '_') field.name = field.name.substr(1);
00216     fieldnames.push_back(field.name.c_str());
00217     field.fieldnum = topics.size();
00218     field.index = 0;
00219     field.size = 0;
00220     topics[c->topic] = field;
00221   }
00222 
00223   // create result struct
00224   mxArray *data = mxCreateStructMatrix(1, 1, fieldnames.size(), fieldnames.data());
00225 
00226   // iterate through View
00227   for(start(); valid(); increment()) {
00228     if (!topics.count(current_->getTopic())) continue;
00229     FieldInfo &field = topics[current_->getTopic()];
00230     mxArray *target = mxGetFieldByNumber(data, 0, field.fieldnum);
00231 
00232     if (!target) {
00233       // find out how many messages are in the bag file by iterating over all (rosbag::Queries) and adding a new TopicFilterQuery object for each
00234       ::rosbag::View only_this_topic(reduce_overlap_);
00235       for(std::vector< ::rosbag::BagQuery* >::iterator query = ::rosbag::View::queries_.begin(); query != ::rosbag::View::queries_.end(); ++query) {
00236         only_this_topic.addQuery(*((*query)->bag), TopicFilterQuery((*query)->query, current_->getTopic()));
00237       }
00238       field.size = only_this_topic.size();
00239       // ROSMATLAB_PRINTF("Field %s has %u entries.", field.name.c_str(), field.size);
00240     }
00241 
00242     assert(field.index < field.size);
00243     // ROSMATLAB_PRINTF("Converting entry %u/%u of field %s", field.index, field.size, field.name.c_str());
00244     target = getInternal(target, field.index++, field.size);
00245 //    if (!target) target = mxCreateDoubleScalar(field.size); // debugging only
00246 
00247     mxSetFieldByNumber(data, 0, field.fieldnum, target);
00248   }
00249 
00250   // return result
00251   plhs[0] = data;
00252 }
00253 
00254 mxArray *View::getTime()
00255 {
00256   if (!valid()) return mxCreateEmpty();
00257   return mxCreateTime(current_->getTime());
00258 }
00259 
00260 mxArray *View::getTopic()
00261 {
00262   if (!valid()) return mxCreateEmpty();
00263   return mxCreateString(current_->getTopic().c_str());
00264 }
00265 
00266 mxArray *View::getDataType()
00267 {
00268   if (!valid()) return mxCreateEmpty();
00269   return mxCreateString(current_->getDataType().c_str());
00270 }
00271 
00272 mxArray *View::getMD5Sum()
00273 {
00274   if (!valid()) return mxCreateEmpty();
00275   return mxCreateString(current_->getMD5Sum().c_str());
00276 }
00277 
00278 mxArray *View::getMessageDefinition()
00279 {
00280   if (!valid()) return mxCreateEmpty();
00281   return mxCreateString(current_->getMessageDefinition().c_str());
00282 }
00283 
00284 mxArray *View::getConnectionHeader()
00285 {
00286   if (!valid()) return mxCreateStructMatrix(0, 0, 0, 0);
00287   return ConnectionHeader(current_->getConnectionHeader()).toMatlab();
00288 }
00289 
00290 mxArray *View::getCallerId()
00291 {
00292   if (!valid()) return mxCreateEmpty();
00293   return mxCreateString(current_->getCallerId().c_str());
00294 }
00295 
00296 mxArray *View::isLatching()
00297 {
00298   if (!valid()) return mxCreateLogicalMatrix(0, 0);
00299   return mxCreateLogicalScalar(current_->isLatching());
00300 }
00301 
00302 mxArray *View::getQueries()
00303 {
00304   return Query::toMatlab(queries_);
00305 }
00306 
00307 mxArray *View::getConnections()
00308 {
00309   std::vector<const ConnectionInfo *> connections = ::rosbag::View::getConnections();
00310 
00311   mxArray *result;
00312   static const char *fieldnames[] = { "Id", "Topic", "DataType", "MD5Sum" };
00313   result = mxCreateStructMatrix(connections.size(), 1, 4, fieldnames);
00314 
00315   for(mwIndex i = 0; i < connections.size(); ++i) {
00316     mxSetField(result, i, "Id",       mxCreateDoubleScalar(connections[i]->id));
00317     mxSetField(result, i, "Topic",    mxCreateString(connections[i]->topic.c_str()));
00318     mxSetField(result, i, "DataType", mxCreateString(connections[i]->datatype.c_str()));
00319     mxSetField(result, i, "MD5Sum",   mxCreateString(connections[i]->md5sum.c_str()));
00320   }
00321 
00322   return result;
00323 }
00324 
00325 mxArray *View::getBeginTime()
00326 {
00327   return mxCreateTime(::rosbag::View::getBeginTime());
00328 }
00329 
00330 mxArray *View::getEndTime()
00331 {
00332   return mxCreateTime(::rosbag::View::getEndTime());
00333 }
00334 
00335 } // namespace rosbag
00336 } // namespace rosmatlab


rosmatlab_rosbag
Author(s): Johannes Meyer
autogenerated on Fri Jul 25 2014 06:48:22