X-lanni
154 lines
3.6 KiB
C++
154 lines
3.6 KiB
C++
#include "sensorlog.h"
|
|
|
|
#include <iostream>
|
|
#include <sstream>
|
|
#include <assert.h>
|
|
#include <sensor_odometry/odometrysensor.h>
|
|
#include <sensor_range/rangesensor.h>
|
|
|
|
#define LINEBUFFER_SIZE 100000
|
|
|
|
namespace GMapping {
|
|
|
|
using namespace std;
|
|
|
|
SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){
|
|
}
|
|
|
|
SensorLog::~SensorLog(){
|
|
for (iterator it=begin(); it!=end(); it++)
|
|
if (*it) delete (*it);
|
|
}
|
|
|
|
istream& SensorLog::load(istream& is){
|
|
for (iterator it=begin(); it!=end(); it++)
|
|
if (*it) delete (*it);
|
|
clear();
|
|
|
|
char buf[LINEBUFFER_SIZE];
|
|
while (is){
|
|
is.getline(buf, LINEBUFFER_SIZE);
|
|
istringstream lis(buf);
|
|
|
|
string sensorname;
|
|
|
|
if (lis)
|
|
lis >>sensorname;
|
|
else
|
|
continue;
|
|
|
|
|
|
|
|
SensorMap::const_iterator it=m_sensorMap.find(sensorname);
|
|
if (it==m_sensorMap.end()){
|
|
continue;
|
|
}
|
|
|
|
Sensor* sensor=it->second;
|
|
|
|
SensorReading* reading=0;
|
|
OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
|
|
if (odometry)
|
|
reading=parseOdometry(lis, odometry);
|
|
|
|
RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
|
|
if (range)
|
|
reading=parseRange(lis, range);
|
|
if (reading)
|
|
push_back(reading);
|
|
}
|
|
return is;
|
|
|
|
}
|
|
|
|
OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{
|
|
OdometryReading* reading=new OdometryReading(osen);
|
|
OrientedPoint pose;
|
|
OrientedPoint speed;
|
|
OrientedPoint accel;
|
|
is >> pose.x >> pose.y >> pose.theta;
|
|
is >> speed.x >>speed.theta;
|
|
speed.y=0;
|
|
is >> accel.x;
|
|
accel.y=accel.theta=0;
|
|
reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
|
|
return reading;
|
|
}
|
|
|
|
RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{
|
|
if(rs->newFormat){
|
|
string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
|
|
is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode;
|
|
}
|
|
|
|
unsigned int size;
|
|
is >> size;
|
|
assert(size==rs->beams().size());
|
|
|
|
RangeReading* reading=new RangeReading(rs);
|
|
//cerr << "#R=" << size << endl;
|
|
reading->resize(size);
|
|
for (unsigned int i=0; i<size; i++){
|
|
is >> (*reading)[i];
|
|
}
|
|
if (rs->newFormat){
|
|
int reflectionBeams;
|
|
is >> reflectionBeams;
|
|
double reflection;
|
|
for (int i=0; i<reflectionBeams; i++)
|
|
is >> reflection;
|
|
}
|
|
//FIXME XXX
|
|
OrientedPoint laserPose;
|
|
is >> laserPose.x >> laserPose.y >> laserPose.theta;
|
|
OrientedPoint pose;
|
|
is >> pose.x >> pose.y >> pose.theta;
|
|
reading->setPose(pose);
|
|
double a,b,c;
|
|
if (rs->newFormat){
|
|
string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
|
|
is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
|
|
} else {
|
|
is >> a >> b >> c;
|
|
}
|
|
string s;
|
|
is >> a >> s;
|
|
is >> a;
|
|
reading->setTime(a);
|
|
return reading;
|
|
}
|
|
|
|
OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const {
|
|
xmin=ymin=1e6;
|
|
xmax=ymax=-1e6;
|
|
bool first=true;
|
|
OrientedPoint start;
|
|
for (const_iterator it=begin(); it!=end(); it++){
|
|
double lxmin=0., lxmax=0., lymin=0., lymax=0.;
|
|
const SensorReading* reading=*it;
|
|
const OdometryReading* odometry=dynamic_cast<const OdometryReading*> (reading);
|
|
if (odometry){
|
|
lxmin=lxmax=odometry->getPose().x;
|
|
lymin=lymax=odometry->getPose().y;
|
|
}
|
|
|
|
const RangeReading* rangeReading=dynamic_cast<const RangeReading*> (reading);
|
|
if (rangeReading){
|
|
lxmin=lxmax=rangeReading->getPose().x;
|
|
lymin=lymax=rangeReading->getPose().y;
|
|
if (first){
|
|
first=false;
|
|
start=rangeReading->getPose();
|
|
}
|
|
}
|
|
xmin=xmin<lxmin?xmin:lxmin;
|
|
xmax=xmax>lxmax?xmax:lxmax;
|
|
ymin=ymin<lymin?lymin:lymin;
|
|
ymax=ymax>lymax?ymax:lymax;
|
|
}
|
|
return start;
|
|
}
|
|
|
|
};
|
|
|