globalrecorder.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 <naoqi_driver/recorder/globalrecorder.hpp>
00022 
00023 /*
00024 * STANDARD includes
00025 */
00026 #include <ctime>
00027 #include <sstream>
00028 
00029 /*
00030 * ROS includes
00031 */
00032 #include <std_msgs/Int32.h>
00033 #include <tf2_msgs/TFMessage.h>
00034 #include <ros/ros.h>
00035 #include <rosbag/bag.h>
00036 #include <rosbag/view.h>
00037 #include <geometry_msgs/TransformStamped.h>
00038 
00039 /*
00040 * BOOST includes
00041 */
00042 #include <boost/filesystem.hpp>
00043 #include <boost/make_shared.hpp>
00044 #include <boost/shared_ptr.hpp>
00045 
00046 /*
00047 * ALDEBARAN includes
00048 */
00049 #include <qi/log.hpp>
00050 
00051 qiLogCategory("ros.Recorder");
00052 
00053 namespace naoqi
00054 {
00055 namespace recorder
00056 {
00057 
00058   GlobalRecorder::GlobalRecorder(const std::string& prefix_topic):
00059     _bag()
00060   , _processMutex()
00061   , _nameBag("")
00062   , _isStarted(false)
00063   {
00064     if (!prefix_topic.empty())
00065     {
00066       _prefix_topic = "/"+prefix_topic+"/";
00067     }
00068     else
00069     {
00070       _prefix_topic = "/";
00071     }
00072   }
00073 
00074   void GlobalRecorder::startRecord(const std::string& prefix_bag) {
00075     boost::mutex::scoped_lock startLock( _processMutex );
00076     if (!_isStarted) {
00077       try {
00078         // Get current path
00079         boost::filesystem::path cur_path( boost::filesystem::current_path() );
00080 
00081         // Get time
00082         time_t rawtime;
00083         struct tm * timeinfo;
00084         char buffer[80];
00085         std::time(&rawtime);
00086         timeinfo = std::localtime(&rawtime);
00087         std::strftime(buffer,80,"%d-%m-%Y_%I:%M:%S",timeinfo);
00088 
00089         if (!prefix_bag.empty()) {
00090           _nameBag = cur_path.string()+"/"+prefix_bag+"_"+buffer;
00091         }
00092         else {
00093           _nameBag = cur_path.string()+"/"+buffer;
00094         }
00095         _nameBag.append(".bag");
00096 
00097         _bag.open(_nameBag, rosbag::bagmode::Write);
00098         _isStarted = true;
00099         std::cout << YELLOW << "The bag " << BOLDCYAN << _nameBag << RESETCOLOR << YELLOW << " is opened" << RESETCOLOR << std::endl;
00100       } catch (std::exception e){
00101         throw std::runtime_error(e.what());
00102       }
00103     }
00104     else {
00105       qiLogError() << "Cannot start a record. The module is already recording.";
00106     }
00107   }
00108 
00109   std::string GlobalRecorder::stopRecord(const std::string& robot_ip) {
00110     boost::mutex::scoped_lock stopLock( _processMutex );
00111     if (_isStarted) {
00112       _bag.close();
00113       _isStarted = false;
00114 
00115       std::stringstream message;
00116       message << _nameBag;
00117       std::cout << YELLOW << "The bag " << BOLDCYAN << _nameBag << RESETCOLOR << YELLOW << " is closed" << RESETCOLOR << std::endl;
00118 
00119       // Check if we are on a robot
00120       char* current_path;
00121       current_path = getenv("HOME");
00122       std::string cp = current_path;
00123       if (!(cp.find("nao") == std::string::npos)) {
00124         std::cout << BOLDRED << "To download this bag on your computer:" << RESETCOLOR << std::endl
00125                      << GREEN << "\t$ scp nao@" << robot_ip << ":" << _nameBag << " <LOCAL_PATH>" << RESETCOLOR
00126                         << std::endl;
00127       }
00128 
00129       _nameBag.clear();
00130       return message.str();
00131     }
00132     else {
00133       qiLogError() << "Cannot stop recording while it has not been started.";
00134       return "Cannot stop recording while it has not been started.";
00135     }
00136   }
00137 
00138   bool GlobalRecorder::isStarted() {
00139     return _isStarted;
00140   }
00141 
00142   void GlobalRecorder::write(const std::string& topic, const std::vector<geometry_msgs::TransformStamped>& msgtf) {
00143     if (!msgtf.empty())
00144     {
00145       std::string ros_topic;
00146       if (topic[0]!='/')
00147       {
00148         ros_topic = _prefix_topic+topic;
00149       }
00150       else
00151       {
00152         ros_topic = topic;
00153       }
00154       tf2_msgs::TFMessage message;
00155       ros::Time now = ros::Time::now();
00156       if (!msgtf[0].header.stamp.isZero()) {
00157         now = msgtf[0].header.stamp;
00158       }
00159       for (std::vector<geometry_msgs::TransformStamped>::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it)
00160       {
00161         message.transforms.push_back(*it);
00162       }
00163       boost::mutex::scoped_lock writeLock( _processMutex );
00164       if (_isStarted) {
00165         _bag.write(ros_topic, now, message);
00166       }
00167     }
00168   }
00169 
00170 } // recorder
00171 } // naoqi


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