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; }