00001 #include "LaserReading.h" 00002 00003 00004 using namespace std; 00005 00006 LaserReading::LaserReading(const vector<double>& _phi, const vector<double>& _rho, double _time, const string& _name, const string& _robot): 00007 AbstractReading(_time, _name, _robot), 00008 m_maxRange(80), 00009 m_phi(_phi), 00010 m_rho(_rho) 00011 { 00012 m_remission.reserve(m_rho.size()); 00013 m_laserPose.x = 0; 00014 m_laserPose.y = 0; 00015 m_laserPose.theta = 0; 00016 computeLocalCartesian(); 00017 m_worldCartesian = m_cartesian; 00018 } 00019 00020 LaserReading::~LaserReading(){ 00021 00022 } 00023 00024 void LaserReading::setRemission(const vector<double>& _remi){ 00025 if(_remi.size() == m_phi.size()) 00026 m_remission = _remi; 00027 } 00028 00029 void LaserReading::setLaserPose(const OrientedPoint2D& _pose){ 00030 m_laserPose = _pose; 00031 computeWorldCartesian(); 00032 } 00033 00034 void LaserReading::computeWorldCartesian(){ 00035 m_worldCartesian.resize(m_phi.size()); 00036 for(unsigned int i = 0; i < m_phi.size(); i++){ 00037 if(m_rho[i]<m_maxRange){ 00038 m_worldCartesian[i].x = cos(m_phi[i] + m_laserPose.theta)*m_rho[i] + m_laserPose.x; 00039 m_worldCartesian[i].y = sin(m_phi[i] + m_laserPose.theta)*m_rho[i] + m_laserPose.y; 00040 } else { 00041 m_worldCartesian[i].x = cos(m_phi[i] + m_laserPose.theta)*m_maxRange + m_laserPose.x; 00042 m_worldCartesian[i].y = sin(m_phi[i] + m_laserPose.theta)*m_maxRange + m_laserPose.y; 00043 /* m_worldCartesian[i].x = 0; 00044 m_worldCartesian[i].y = 0;*/ 00045 } 00046 } 00047 } 00048 00049 void LaserReading::computeLocalCartesian(){ 00050 m_cartesian.resize(m_phi.size()); 00051 for(unsigned int i = 0; i < m_phi.size(); i++){ 00052 if(m_rho[i]<m_maxRange){ 00053 m_cartesian[i].x = cos(m_phi[i])*m_rho[i]; 00054 m_cartesian[i].y = sin(m_phi[i])*m_rho[i]; 00055 } else { 00056 m_cartesian[i].x = cos(m_phi[i] + m_laserPose.theta)*m_maxRange + m_laserPose.x; 00057 m_cartesian[i].y = sin(m_phi[i] + m_laserPose.theta)*m_maxRange + m_laserPose.y; 00058 } 00059 } 00060 } 00061 00062 AbstractReading* LaserReading::clone() const{ 00063 return new LaserReading(*this); 00064 }