raw_data_pa.cpp
Go to the documentation of this file.
00001 //==============================================================================
00002 // Copyright (c) 2019, Peter Weissig, TU Chemnitz
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"
00018 // AND
00019 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00022 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00027 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 //==============================================================================
00029 
00030 // This file is an addon from TUC-ProAut (https://github.com/TUC-ProAut/).
00031 // It was created to log all data from the ublox to a logfile and to publish
00032 // the data as ros messages. This is used by our group to also evaluate the
00033 // measured data with the rtklib.
00034 
00035 #include "ublox_gps/raw_data_pa.h"
00036 #include <cmath>
00037 #include <string>
00038 #include <sstream>
00039 
00040 #include <sys/types.h>
00041 #include <sys/stat.h>
00042 #include <time.h>
00043 
00044 using namespace ublox_node;
00045 
00046 //
00047 // ublox_node namespace
00048 //
00049 
00050 RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) :
00051   pnh_(ros::NodeHandle("~")),
00052   flag_publish_(false),
00053   is_ros_subscriber_(is_ros_subscriber) {
00054 
00055 }
00056 
00057 void RawDataStreamPa::getRosParams() {
00058 
00059     if (is_ros_subscriber_) {
00060         pnh_.param<std::string>("dir", file_dir_, "");
00061     } else {
00062         pnh_.param<std::string>("raw_data_stream/dir", file_dir_, "");
00063         pnh_.param("raw_data_stream/publish", flag_publish_, false);
00064     }
00065 }
00066 
00067 bool RawDataStreamPa::isEnabled() {
00068 
00069     if (is_ros_subscriber_) {
00070         return !file_dir_.empty();
00071     } else {
00072         return flag_publish_ || (!file_dir_.empty());
00073     }
00074 }
00075 
00076 
00077 void RawDataStreamPa::initialize() {
00078 
00079     if (is_ros_subscriber_) {
00080         ROS_INFO("Subscribing to raw data stream.");
00081         static ros::Subscriber subscriber =
00082           nh_.subscribe ("raw_data_stream", 100,
00083             &RawDataStreamPa::msgCallback, this);
00084     } else if (flag_publish_) {
00085         ROS_INFO("Publishing raw data stream.");
00086         RawDataStreamPa::publishMsg(std::string());
00087     }
00088 
00089     if (!file_dir_.empty()) {
00090         struct stat stat_info;
00091         if (stat(file_dir_.c_str(), &stat_info ) != 0) {
00092             ROS_ERROR("Can't log raw data to file. "
00093               "Directory \"%s\" does not exist.", file_dir_.c_str());
00094 
00095         } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) {
00096             ROS_ERROR("Can't log raw data to file. "
00097               "\"%s\" exists, but is not a directory.", file_dir_.c_str());
00098 
00099         } else {
00100             if (file_dir_.back() != '/') {
00101                 file_dir_ += '/';
00102             }
00103 
00104             time_t t = time(NULL);
00105             struct tm time_struct = *localtime(&t);
00106 
00107             std::stringstream filename;
00108             filename.width(4); filename.fill('0');
00109               filename << time_struct.tm_year + 1900;
00110               filename.width(0); filename << '_';
00111             filename.width(2); filename.fill('0');
00112               filename << time_struct.tm_mon  + 1;
00113               filename.width(0); filename << '_';
00114             filename.width(2); filename.fill('0');
00115               filename << time_struct.tm_mday;
00116               filename.width(0); filename << '_';
00117             filename.width(2); filename.fill('0');
00118               filename << time_struct.tm_hour;
00119             filename.width(2); filename.fill('0');
00120               filename << time_struct.tm_min ;
00121             filename.width(0); filename << ".log";
00122             file_name_ = file_dir_ + filename.str();
00123 
00124             try {
00125                 file_handle_.open(file_name_);
00126                 ROS_INFO("Logging raw data to file \"%s\"",
00127                   file_name_.c_str());
00128             } catch(const std::exception& e) {
00129                 ROS_ERROR("Can't log raw data to file. "
00130                   "Can't create file \"%s\".", file_name_.c_str());
00131             }
00132         }
00133     }
00134 }
00135 
00136 void RawDataStreamPa::ubloxCallback(const unsigned char* data,
00137   const std::size_t size) {
00138 
00139     std::string str((const char*) data, size);
00140 
00141     if (flag_publish_) {
00142         publishMsg(str);
00143     }
00144 
00145     saveToFile(str);
00146 }
00147 
00148 void RawDataStreamPa::msgCallback(
00149   const std_msgs::UInt8MultiArray::ConstPtr& msg) {
00150 
00151     std::string str(msg->data.size(), ' ');
00152     std::copy(msg->data.begin(), msg->data.end(), str.begin());
00153     saveToFile(str);
00154 }
00155 
00156 std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8(
00157  const std::string str) {
00158 
00159     std_msgs::UInt8MultiArray msg;
00160 
00161     msg.layout.data_offset = 0;
00162     msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
00163     msg.layout.dim[0].size   = str.length();
00164     msg.layout.dim[0].stride = 1;
00165     msg.layout.dim[0].label  = "raw_data_stream";
00166 
00167     msg.data.resize(str.length());
00168     std::copy(str.begin(), str.end(), msg.data.begin());
00169 
00170     return msg;
00171 }
00172 
00173 void RawDataStreamPa::publishMsg(const std::string str) {
00174 
00175     static ros::Publisher publisher =
00176       pnh_.advertise<std_msgs::UInt8MultiArray>("raw_data_stream", 100);
00177 
00178     publisher.publish(RawDataStreamPa::str2uint8(str));
00179 }
00180 
00181 void RawDataStreamPa::saveToFile(const std::string str) {
00182 
00183     if (file_handle_.is_open()) {
00184         try {
00185             file_handle_ << str;
00186             // file_handle_.flush();
00187         } catch(const std::exception& e) {
00188             ROS_WARN("Error writing to file \"%s\"", file_name_.c_str());
00189         }
00190     }
00191 }
00192 


ublox_gps
Author(s): Johannes Meyer
autogenerated on Fri Jun 14 2019 19:26:13