rangesensor.cpp

Particle filtering implementation and application to people tracking.

#include "rangesensor.h"
//#include <iostream>

namespace GMapping{

RangeSensor::RangeSensor(std::string name): Sensor(name){}

RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name),
	m_pose(position), m_beams(beams_num){
	double angle=-.5*res*beams_num;
	for (unsigned int i=0; i<beams_num; i++, angle+=res){
		RangeSensor::Beam& beam(m_beams[i]);
		beam.span=span;
		beam.pose.x=0;
		beam.pose.y=0;
		beam.pose.theta=angle;
		beam.maxRange=maxrange;
	}
	newFormat=0;
	updateBeamsLookup();
}

void RangeSensor::updateBeamsLookup()
{
	for (unsigned int i=0; i<m_beams.size(); i++)
	{
		RangeSensor::Beam& beam(m_beams[i]);
		beam.s=sin(m_beams[i].pose.theta);
		beam.c=cos(m_beams[i].pose.theta);
		//std::cerr << beam.pose.theta << " " << beam.s << " " << beam.c << std::endl;
	}
}

};