LaserReading* CarmenLogReader::parseFLaser(std::istream& _stream) const{
std::string sensorName, robotName;
std::vector<double> phi,rho;
unsigned int number;
OrientedPoint2D laserPose, robotPose;
double timestamp;
double start, fov, resolution, maxRange;
_stream >> sensorName >> number;
phi.resize(number);
rho.resize(number);
start = -M_PI_2;
fov = M_PI;
resolution = fov/number;
maxRange = 81.9;
for(uint i=0; i<number; i++){
phi[i] = start + i*resolution;
_stream >> rho[i];
}
_stream >> laserPose.x >> laserPose.y >> laserPose.theta;
_stream >> robotPose.x >> robotPose.y >> robotPose.theta;
_stream >> timestamp >> robotName;
LaserReading *result = new LaserReading(phi, rho, timestamp, sensorName, robotName);
result->setMaxRange(maxRange);
result->setLaserPose(laserPose);
result->setRobotPose(robotPose);
return result;
}
LaserReading* CarmenLogReader::parseRobotLaser(std::istream& _stream) const{
std::string sensorName, robotName;
std::vector<double> phi,rho,remission;
unsigned int number=0, remissionNumber=0;
OrientedPoint2D laserPose, robotPose;
double timestamp;
double start, fov, resolution, maxRange, accuracy;
int laserType, remissionMode;
double tv, rv, forward_safety_dist, side_safety_dist, turn_axis;
_stream >> sensorName >> laserType >> start >> fov >> resolution >> maxRange >> accuracy >> remissionMode; // Laser sensor parameters
_stream >> number;
phi.resize(number);
rho.resize(number);
for(uint i=0; i < number; i++){
phi[i] = start + i*resolution;
_stream >> rho[i];
}
_stream >> remissionNumber;
remission.resize(remissionNumber);
for(uint i=0; i<remissionNumber; i++){
_stream >> remission[i];
}
_stream >> laserPose.x >> laserPose.y >> laserPose.theta;
_stream >> robotPose.x >> robotPose.y >> robotPose.theta;
_stream >> tv >> rv >> forward_safety_dist >> side_safety_dist >> turn_axis;
_stream >> timestamp >> robotName;
LaserReading *result = new LaserReading(phi, rho, timestamp, sensorName, robotName);
result->setMaxRange(maxRange);
result->setRemission(remission);
result->setLaserPose(laserPose);
result->setRobotPose(robotPose);
return result;
}
