rangereading.cpp

Go to the documentation of this file.

#include <limits>
#include <iostream>
#include <assert.h>
#include <sys/types.h>
#include <gmapping/utils/gvalues.h>
#include "gmapping/sensor/sensor_range/rangereading.h"
 
namespace GMapping{
 
using namespace std;
 
RangeReading::RangeReading(const RangeSensor* rs, double time):
        SensorReading(rs,time){}
 
RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time):
        SensorReading(rs,time){
        assert(n_beams==rs->beams().size());
        resize(n_beams);
        for (unsigned int i=0; i<size(); i++)
                (*this)[i]=d[i];
}
 
RangeReading::~RangeReading(){
//      cerr << __func__ << ": CAZZZZZZZZZZZZZZZZZZZZOOOOOOOOOOO" << endl;
}
 
unsigned int RangeReading::rawView(double* v, double density) const{
        if (density==0){
                for (unsigned int i=0; i<size(); i++)
                        v[i]=(*this)[i];
        } else {
                Point lastPoint(0,0);
                uint suppressed=0;
                for (unsigned int i=0; i<size(); i++){
                        const RangeSensor* rs=dynamic_cast<const RangeSensor*>(getSensor());
                        assert(rs);
                        Point lp(
                                cos(rs->beams()[i].pose.theta)*(*this)[i],
                                sin(rs->beams()[i].pose.theta)*(*this)[i]);
                        Point dp=lastPoint-lp;
                        double distance=sqrt(dp*dp);
                        if (distance<density){
                          //                            v[i]=MAXDOUBLE;
                                v[i]=std::numeric_limits<double>::max();
                                suppressed++;
                        }
                        else{
                                lastPoint=lp;
                                v[i]=(*this)[i];
                        }
                        //std::cerr<< __func__ << std::endl;
                        //std::cerr<< "suppressed " << suppressed <<"/"<<size() << std::endl;
                }
        }
        //      return size();
        return static_cast<unsigned int>(size());
 
};
 
unsigned int RangeReading::activeBeams(double density) const{
        if (density==0.)
                return size();
                int ab=0;
        Point lastPoint(0,0);
        uint suppressed=0;
        for (unsigned int i=0; i<size(); i++){
                const RangeSensor* rs=dynamic_cast<const RangeSensor*>(getSensor());
                assert(rs);
                Point lp(
                        cos(rs->beams()[i].pose.theta)*(*this)[i],
                        sin(rs->beams()[i].pose.theta)*(*this)[i]);
                Point dp=lastPoint-lp;
                double distance=sqrt(dp*dp);
                if (distance<density){
                        suppressed++;
                }
                else{
                        lastPoint=lp;
                        ab++;
                }
                //std::cerr<< __func__ << std::endl;
                //std::cerr<< "suppressed " << suppressed <<"/"<<size() << std::endl;
        }
        return ab;
}
 
std::vector<Point> RangeReading::cartesianForm(double maxRange) const{
        const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>(getSensor());
        assert(rangeSensor && rangeSensor->beams().size());
        //      uint m_beams=rangeSensor->beams().size();
        uint m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
        std::vector<Point> cartesianPoints(m_beams);
        double px,py,ps,pc;
        px=rangeSensor->getPose().x;
        py=rangeSensor->getPose().y;
        ps=sin(rangeSensor->getPose().theta);
        pc=cos(rangeSensor->getPose().theta);
        for (unsigned int i=0; i<m_beams; i++){
                const double& rho=(*this)[i];
                const double& s=rangeSensor->beams()[i].s;
                const double& c=rangeSensor->beams()[i].c;
                if (rho>=maxRange){
                        cartesianPoints[i]=Point(0,0);
                } else {
                        Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho);
                        cartesianPoints[i].x=px+pc*p.x-ps*p.y;
                        cartesianPoints[i].y=py+ps*p.x+pc*p.y;
                }
        }
        return cartesianPoints;
}
 
};