DeviceLogKML.cpp
Go to the documentation of this file.
1 /*
2 MIT LICENSE
3 
4 Copyright (c) 2014-2021 Inertial Sense, Inc. - http://inertialsense.com
5 
6 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :
7 
8 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
9 
10 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
11 */
12 
13 #include <ctime>
14 #include <string>
15 #include <sstream>
16 #include <sys/types.h>
17 #include <sys/stat.h>
18 #include <iomanip>
19 #include <iostream>
20 #include <fstream>
21 #include <stdio.h>
22 #include <stdlib.h>
23 #include <stddef.h>
24 #include "ISPose.h"
25 #include "DeviceLogKML.h"
26 #include "ISLogger.h"
27 #include "ISConstants.h"
28 
29 using namespace std;
30 
31 void cDeviceLogKML::InitDeviceForWriting(int pHandle, std::string timestamp, std::string directory, uint64_t maxDiskSpace, uint32_t maxFileSize)
32 {
33  memset(&m_Log, 0, sizeof(m_Log));
34 
35  cDeviceLog::InitDeviceForWriting(pHandle, timestamp, directory, maxDiskSpace, maxFileSize);
36 }
37 
38 
40 {
42 
43  for (int kid = 0; kid < cDataKML::MAX_NUM_KID; kid++)
44  {
45  // Close file
46  CloseWriteFile(kid, m_Log[kid]);
47  }
48 
49  return true;
50 }
51 
52 
54 {
55  // Ensure we have data
56  int64_t dataSize = (int64_t)log.data.size();
57  if (dataSize <= 0)
58  {
59  return false;
60  }
61 
62  // Ensure directory exists
63  if (m_directory.empty())
64  {
65  return false;
66  }
67 
68  _MKDIR(m_directory.c_str());
69 
70  // Create filename
71  log.fileCount++;
72  int serNum = m_devInfo.serialNumber;
73  if (!serNum)
74  {
75  serNum = m_pHandle;
76  }
77 
78  log.fileName = GetNewFileName(serNum, log.fileCount, m_kml.GetDatasetName(kid).c_str());
79 
80 #define BUF_SIZE 100
81  char buf[BUF_SIZE];
82  int styleCnt = 1;
83  string altitudeMode;
84  string iconScale = "0.5";
85  string labelScale = "0.5";
86  string iconUrl;
87 
88  if (m_showPointTimestamps)
89  labelScale = "0.5";
90  else
91  labelScale = "0.01";
92 
93  // Altitude mode
94  if(m_altClampToGround)
95  altitudeMode = "clampToGround";
96  else
97  altitudeMode = "absolute";
98 
99  // Style
100  string styleStr, colorStr;
101 
102  iconUrl = "http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png";
103  // iconUrl = "http://earth.google.com/images/kml-icons/track-directional/track-none.png";
104 
105  // colors are ABGR
106  switch (kid)
107  {
108  case cDataKML::KID_INS:
109  iconUrl = "http://earth.google.com/images/kml-icons/track-directional/track-0.png";
110  colorStr = "ff00ffff"; // yellow
111  break;
112  case cDataKML::KID_GPS:
113  colorStr = "ff0000ff"; // red
114  break;
115  case cDataKML::KID_GPS1:
116  colorStr = "ffff0000"; // blue
117  break;
118  case cDataKML::KID_RTK:
119  colorStr = "ffffff00"; // cyan
120  break;
121  case cDataKML::KID_REF:
122  iconUrl = "http://earth.google.com/images/kml-icons/track-directional/track-0.png";
123  colorStr = "ffff00ff"; // magenta
124  break;
125  }
126 
127  // Add XML version info and document
129  *Style, *LineStyle, *IconStyle, *LabelStyle,
130  *Placemark, *Point, *LineString,
131  *heading,
132  *elem, *href;
133 
134  TiXmlDocument tDoc;
135 
136  TiXmlDeclaration* decl = new TiXmlDeclaration("1.0", "UTF-8", "");
137  tDoc.LinkEndChild(decl);
138 
139  TiXmlElement* kml = new TiXmlElement("kml");
140  tDoc.LinkEndChild(kml);
141 
142  TiXmlElement* Document = new TiXmlElement("Document");
143  kml->LinkEndChild(Document);
144  kml->SetAttribute("xmlns", "http://www.opengis.net/kml/2.2");
145  kml->SetAttribute("xmlns:gx", "http://www.google.com/kml/ext/2.2");
146 
147  // Show sample (dot)
148  if (m_showPoints)
149  {
150  // Style
151  styleStr = string("stylesel_");
152 
153  Style = new TiXmlElement("Style");
154  Style->SetAttribute("id", styleStr);
155  Document->LinkEndChild(Style);
156 
157  // Icon style
158  IconStyle = new TiXmlElement("IconStyle");
159  Style->LinkEndChild(IconStyle);
160 
161  elem = new TiXmlElement("color");
162  elem->LinkEndChild(new TiXmlText(colorStr));
163  IconStyle->LinkEndChild(elem);
164 
165  elem = new TiXmlElement("colorMode");
166  elem->LinkEndChild(new TiXmlText("normal"));
167  IconStyle->LinkEndChild(elem);
168 
169  elem = new TiXmlElement("scale");
170  elem->LinkEndChild(new TiXmlText(iconScale));
171  IconStyle->LinkEndChild(elem);
172 
173  heading = new TiXmlElement("heading");
174  heading->LinkEndChild(new TiXmlText("0"));
175  IconStyle->LinkEndChild(heading);
176 
177  href = new TiXmlElement("href");
178  href->LinkEndChild(new TiXmlText(iconUrl));
179  elem = new TiXmlElement("Icon");
180  elem->LinkEndChild(href);
181  IconStyle->LinkEndChild(elem);
182 
183  // Label style
184  LabelStyle = new TiXmlElement("LabelStyle");
185  Style->LinkEndChild(LabelStyle);
186 
187  elem = new TiXmlElement("colorMode");
188  elem->LinkEndChild(new TiXmlText("normal"));
189  LabelStyle->LinkEndChild(elem);
190 
191  elem = new TiXmlElement("scale");
192  elem->LinkEndChild(new TiXmlText(labelScale));
193  LabelStyle->LinkEndChild(elem);
194 
195 
196  double nextTime = log.data[0].time;
197  for (size_t i = 0; i < log.data.size(); i++)
198  {
199  sKmlLogData& item = (log.data[i]);
200 
201  // Log only every iconUpdatePeriodSec
202  if (item.time < nextTime)
203  {
204  continue;
205  }
206  nextTime = item.time - fmod(item.time, m_pointUpdatePeriodSec) + m_pointUpdatePeriodSec;
207 
208  // Placemark
209  Placemark = new TiXmlElement("Placemark");
210  Document->LinkEndChild(Placemark);
211 
212  // Name
213  elem = new TiXmlElement("name");
214  ostringstream timeStream;
215  switch (kid)
216  {
217  case cDataKML::KID_GPS:
218  case cDataKML::KID_GPS1:
219  case cDataKML::KID_GPS2:
220  case cDataKML::KID_RTK:
221  timeStream << fixed << setprecision(1) << item.time;
222  break;
223  default:
224  timeStream << fixed << setprecision(3) << item.time;
225  break;
226  }
227  elem->LinkEndChild(new TiXmlText(timeStream.str()));
228  Placemark->LinkEndChild(elem);
229 
230  // styleUrl
231  elem = new TiXmlElement("styleUrl");
232  if (item.theta[2] != 0.0f)
233  { // Style - to indicate heading
234  string styleCntStr = styleStr + to_string(styleCnt++);
235  TiXmlNode *StyleNode = Style->Clone();
236  StyleNode->ToElement()->SetAttribute("id", styleCntStr);
237  StyleNode->FirstChildElement("IconStyle")->FirstChildElement("heading")->FirstChild()->ToText()->SetValue(to_string(item.theta[2] * C_RAD2DEG_F));
238  Document->LinkEndChild(StyleNode);
239 
240  elem->LinkEndChild(new TiXmlText("#" + styleCntStr));
241  }
242  else
243  {
244  elem->LinkEndChild(new TiXmlText("#" + styleStr));
245  }
246  Placemark->LinkEndChild(elem);
247 
248  Point = new TiXmlElement("Point");
249  Placemark->LinkEndChild(Point);
250 
251  elem = new TiXmlElement("coordinates");
252 #if 1
253  double lat = _CLAMP(item.lla[0] * DEG2RADMULT, -C_PIDIV2, C_PIDIV2) * RAD2DEGMULT;
254  double lon = _CLAMP(item.lla[1] * DEG2RADMULT, -C_PI, C_PI) * RAD2DEGMULT;
255  double alt = _CLAMP(item.lla[2], -1000, 100000);
256  snprintf(buf, BUF_SIZE, "%.8lf,%.8lf,%.3lf", lon, lat, alt);
257  elem->LinkEndChild(new TiXmlText(buf));
258 #else
259  ostringstream coordinateStream; // Not getting digits of precision
260  coordinateStream
261  << fixed
262  << setprecision(8)
263  << item->lla[1] << "," // Lat
264  << item->lla[0] << "," // Lon
265  << setprecision(3)
266  << item->lla[2] << " "; // Alt
267  elem->LinkEndChild(new TiXmlText(coordinateStream.str()));
268 #endif
269  Point->LinkEndChild(elem);
270 
271  elem = new TiXmlElement("altitudeMode");
272  elem->LinkEndChild(new TiXmlText(altitudeMode));
273  Point->LinkEndChild(elem);
274  }
275  }// if (m_showSample)
276 
277 
278  // Show path (track)
279  if (m_showTracks)
280  {
281  // Track style
282  styleStr = string("stylesel_") + to_string(styleCnt++);
283 
284  Style = new TiXmlElement("Style");
285  Style->SetAttribute("id", styleStr);
286  Document->LinkEndChild(Style);
287 
288  LineStyle = new TiXmlElement("LineStyle");
289  Style->LinkEndChild(LineStyle);
290 
291  elem = new TiXmlElement("color");
292  elem->LinkEndChild(new TiXmlText(colorStr));
293  LineStyle->LinkEndChild(elem);
294 
295  elem = new TiXmlElement("colorMode");
296  elem->LinkEndChild(new TiXmlText("normal"));
297  LineStyle->LinkEndChild(elem);
298 
299  elem = new TiXmlElement("width");
300  elem->LinkEndChild(new TiXmlText("2"));
301  LineStyle->LinkEndChild(elem);
302 
303  // Track
304  Placemark = new TiXmlElement("Placemark");
305  Document->LinkEndChild(Placemark);
306 
307  elem = new TiXmlElement("name");
308  elem->LinkEndChild(new TiXmlText("Tracks"));
309  Placemark->LinkEndChild(elem);
310 
311  elem = new TiXmlElement("description");
312  elem->LinkEndChild(new TiXmlText("SN tracks"));
313  Placemark->LinkEndChild(elem);
314 
315  elem = new TiXmlElement("styleUrl");
316  elem->LinkEndChild(new TiXmlText("#" + styleStr));
317  Placemark->LinkEndChild(elem);
318 
319  LineString = new TiXmlElement("LineString");
320  Placemark->LinkEndChild(LineString);
321 
322  ostringstream coordinateStream;
323  int j = 0;
324  for (size_t i = 0; i < log.data.size(); i++)
325  {
326  sKmlLogData& item = (log.data[i]);
327  double lat = _CLAMP(item.lla[0] * DEG2RADMULT, -C_PIDIV2, C_PIDIV2) * RAD2DEGMULT;
328  double lon = _CLAMP(item.lla[1] * DEG2RADMULT, -C_PI, C_PI) * RAD2DEGMULT;
329  double alt = _CLAMP(item.lla[2], -1000, 100000);
330 
331  if (i >= log.data.size()-2)
332  {
333  j++;
334  }
335 
336  snprintf(buf, BUF_SIZE, "%.8lf,%.8lf,%.3lf ", lon, lat, alt);
337 // if (strcmp("-111.65863637,40.05570543,1418.282 ", buf) == 0)
338 // {
339 // j++;
340 // }
341 // qDebug() << string(buf);
342 // std::cout << "Value of str is : ";
343  coordinateStream << string(buf);
344  }
345 
346  elem = new TiXmlElement("coordinates");
347  elem->LinkEndChild(new TiXmlText(coordinateStream.str()));
348  LineString->LinkEndChild(elem);
349 
350  elem = new TiXmlElement("extrude");
351  elem->LinkEndChild(new TiXmlText("1"));
352  LineString->LinkEndChild(elem);
353 
354  elem = new TiXmlElement("altitudeMode");
355  elem->LinkEndChild(new TiXmlText(altitudeMode));
356  LineString->LinkEndChild(elem);
357  }
358 
359  // Write XML to file
360  if (!tDoc.SaveFile(log.fileName.c_str()))
361  {
362  return false;
363  }
364 
365  // Remove data
366  log.data.clear();
367 
368  return true;
369 }
370 
371 
373 {
374 
375 #if PLATFORM_IS_WINDOWS
376 
377  for (int kid = 0; kid < cDataKML::MAX_NUM_KID; kid++)
378  {
379  sKmlLog &log = m_Log[kid];
380 
381  if (log.fileName.empty())
382  {
383  continue;
384  }
385 
386  std::wstring stemp = std::wstring(log.fileName.begin(), log.fileName.end());
387  LPCWSTR filename = stemp.c_str();
388  ShellExecuteW(0, 0, filename, 0, 0, SW_SHOW);
389  }
390 
391 #endif
392 
393  return true;
394 }
395 
396 
397 bool cDeviceLogKML::SaveData(p_data_hdr_t *dataHdr, const uint8_t *dataBuf)
398 {
399  cDeviceLog::SaveData(dataHdr, dataBuf);
400 
401  // Save data to file
402  if (!WriteDateToFile(dataHdr, dataBuf))
403  {
404  return false;
405  }
406 
407  // Convert formats
408  uDatasets& d = (uDatasets&)(*dataBuf);
409  switch (dataHdr->id)
410  {
411  case DID_INS_2:
412  ins_1_t ins1;
413  ins1.week = d.ins2.week;
414  ins1.timeOfWeek = d.ins2.timeOfWeek;
415  ins1.hdwStatus = d.ins2.hdwStatus;
416  ins1.insStatus = d.ins2.insStatus;
417  quat2euler(d.ins2.qn2b, ins1.theta);
418  memcpy(ins1.uvw, d.ins2.uvw, 12);
419  memcpy(ins1.lla, d.ins2.lla, 24);
420  p_data_hdr_t dHdr = { DID_INS_1 , sizeof(ins_1_t), 0 };
421  // Save data to file
422  if (!WriteDateToFile(&dHdr, (uint8_t*)&ins1))
423  {
424  return false;
425  }
426  break;
427  }
428 
429  return true;
430 }
431 
432 
433 bool cDeviceLogKML::WriteDateToFile(const p_data_hdr_t *dataHdr, const uint8_t* dataBuf)
434 {
435  int kid = cDataKML::DID_TO_KID(dataHdr->id);
436 
437  // Only use selected data
438  if (kid < 0)
439  {
440  return true;
441  }
442 
443  // Reference current log
444  sKmlLog &log = m_Log[kid];
445 
446  // Write date to file
447  m_kml.WriteDataToFile(log.data, dataHdr, dataBuf);
448 
449  // File byte size
450  int nBytes = (int)(log.data.size() * cDataKML::BYTES_PER_KID(kid));
451  log.fileSize = nBytes;
452  m_fileSize = _MAX(m_fileSize, log.fileSize);
453  m_logSize = nBytes;
454 
455  if (log.fileSize >= m_maxFileSize)
456  {
457  // Close existing file
458  if (!CloseWriteFile(kid, log))
459  {
460  return false;
461  }
462  }
463 
464  return true;
465 }
466 
467 
469 {
470  p_data_t* data = NULL;
471 
472  // Read data from chunk
473  while (!(data = ReadDataFromChunk()))
474  {
475  // Read next chunk from file
476  if (!ReadChunkFromFile())
477  {
478  return NULL;
479  }
480  }
481 
482  // Read is good
484  return data;
485 }
486 
487 
489 {
490  return NULL;
491 }
492 
493 
495 {
496  return true;
497 }
498 
499 
500 void cDeviceLogKML::SetSerialNumber(uint32_t serialNumber)
501 {
502  m_devInfo.serialNumber = serialNumber;
503 }
504 
505 
506 
double lla[3]
Definition: DataKML.h:32
d
bool ReadChunkFromFile()
float theta[3]
Definition: DataKML.h:33
ins_2_t ins2
Definition: data_sets.h:3529
filename
float qn2b[4]
Definition: data_sets.h:534
void OnReadData(p_data_t *data)
Definition: DeviceLog.cpp:267
virtual bool CloseAllFiles()
Definition: DeviceLog.cpp:77
uint32_t fileSize
Definition: DeviceLogKML.h:36
virtual const TiXmlElement * ToElement() const
Cast to a more defined type. Will return null if not of the requested type.
Definition: tinyxml.h:704
uint32_t id
Definition: ISComm.h:376
is_can_uvw uvw
Definition: CAN_comm.h:257
#define DID_INS_1
Definition: data_sets.h:38
p_data_t * ReadDataFromChunk()
virtual const TiXmlText * ToText() const
Cast to a more defined type. Will return null if not of the requested type.
Definition: tinyxml.h:707
double time
Definition: DataKML.h:31
struct PACKED ins_1_t
void SetSerialNumber(uint32_t serialNumber) OVERRIDE
#define DID_INS_2
Definition: data_sets.h:39
#define NULL
Definition: nm_bsp.h:52
#define C_PIDIV2
Definition: ISConstants.h:544
void InitDeviceForWriting(int pHandle, std::string timestamp, std::string directory, uint64_t maxDiskSpace, uint32_t maxFileSize) OVERRIDE
tf::Vector3 Point
void SetValue(const char *_value)
Definition: tinyxml.h:514
std::vector< sKmlLogData > data
Definition: DeviceLogKML.h:31
uint32_t week
Definition: CAN_comm.h:26
std::string fileName
Definition: DeviceLogKML.h:33
TiXmlNode * LinkEndChild(TiXmlNode *addThis)
Definition: tinyxml.cpp:182
bool SaveFile() const
Save a file using the current document value. Returns true if successful.
Definition: tinyxml.cpp:956
virtual bool SaveData(p_data_hdr_t *dataHdr, const uint8_t *dataBuf)
Definition: DeviceLog.cpp:106
const TiXmlElement * FirstChildElement() const
Convenience function to get through elements.
Definition: tinyxml.cpp:448
double timeOfWeek
Definition: data_sets.h:417
bool WriteDateToFile(const p_data_hdr_t *dataHdr, const uint8_t *dataBuf)
bool CloseAllFiles() OVERRIDE
void SetAttribute(const char *name, const char *_value)
Definition: tinyxml.cpp:776
uint32_t hdwStatus
Definition: CAN_comm.h:40
virtual void InitDeviceForWriting(int pHandle, string timestamp, string directory, uint64_t maxDiskSpace, uint32_t maxFileSize)
Definition: DeviceLog.cpp:57
def quat2euler(q)
Definition: pose.py:126
#define C_RAD2DEG_F
Definition: ISConstants.h:562
double lla[3]
Definition: data_sets.h:511
static int DID_TO_KID(int did)
Definition: DataKML.h:71
bool SaveData(p_data_hdr_t *dataHdr, const uint8_t *dataBuf) OVERRIDE
#define _CLAMP(v, minV, maxV)
Definition: ISConstants.h:302
#define DEG2RADMULT
Definition: ISConstants.h:366
USBInterfaceDescriptor data
uint32_t fileCount
Definition: DeviceLogKML.h:34
#define _MAX(a, b)
Definition: ISConstants.h:294
uint32_t insStatus
Definition: CAN_comm.h:37
const TiXmlNode * FirstChild() const
The first child of this node. Will be null if there are no children.
Definition: tinyxml.h:528
float theta[3]
Definition: data_sets.h:505
virtual TiXmlNode * Clone() const
Creates a new Element and returns it - the returned element is a copy.
Definition: tinyxml.cpp:885
#define snprintf
Definition: test_suite.cpp:86
#define _MKDIR(dir)
Definition: ISConstants.h:213
bool CloseWriteFile(int kid, sKmlLog &log)
#define C_PI
Definition: ISConstants.h:545
#define BUF_SIZE
p_data_t * ReadData() OVERRIDE
#define RAD2DEGMULT
Definition: ISConstants.h:369
bool OpenWithSystemApp(void) OVERRIDE
static int BYTES_PER_KID(int kid)
Definition: DataKML.h:84


inertial_sense_ros
Author(s):
autogenerated on Sun Feb 28 2021 03:17:57