Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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
00187 } catch(const std::exception& e) {
00188 ROS_WARN("Error writing to file \"%s\"", file_name_.c_str());
00189 }
00190 }
00191 }
00192