00001
00005
#include <utils/Linear.h>
00006
#include <utils/ConfigFile.h>
00007
00008
#include "VehPoseLogger.h"
00009
00010 bool VehPoseLogger::open(
const char* name,
00011
const utils::ConfigFile* user_header)
00012 {
00013 utils::ConfigFile header;
00014
00015
00016
if (user_header) {
00017 utils::ConfigFile::copy(*user_header, header);
00018 }
00019
00020
00021 header.setInt(
"int DataFormat.version_major", 1);
00022 header.setInt(
"int DataFormat.version_minor", 0);
00023
00024
00025 _logger.close();
00026
00027
00028 _logger.declare(
"x",
"double", &_output_area.
x);
00029 _logger.declare(
"y",
"double", &_output_area.
y);
00030 _logger.declare(
"z",
"float", &_output_area.
z);
00031 _logger.declare(
"ori",
"[float : 4]", &_output_area.
ori);
00032
00033
00034
return _logger.open(name, header);
00035 }
00036
00037 bool VehPoseLogger::open(utils::ConfigFile& header)
00038 {
00039
return open(header.getString(
"name",
"VehPose.raw"), &header);
00040 }
00041
00042 bool VehPoseLogger::logPoints(utils::Time time,
const VehPose& pose)
00043 {
00044
00045 _output_area.
x = pose.
pos.x;
00046 _output_area.
y = pose.
pos.y;
00047 _output_area.
z = pose.
pos.z;
00048 _output_area.
ori[0] = pose.
ori[0];
00049 _output_area.
ori[1] = pose.
ori[1];
00050 _output_area.
ori[2] = pose.
ori[2];
00051 _output_area.
ori[3] = pose.
ori[3];
00052
00053
00054
bool res = _logger.log(time);
00055
00056
00057
return res;
00058 }
00059