add slam_gmapping

2025-06-06 16:15:07 +08:00
parent 9187b9fb85
commit a7b75c31cb
141 changed files with 15992 additions and 0 deletions
@@ -0,0 +1,8 @@
+add_subdirectory(sensor_base)
+ament_export_libraries(sensor_base)
+
+add_subdirectory(sensor_odometry)
+ament_export_libraries(sensor_odometry)
+
+add_subdirectory(sensor_range)
+ament_export_libraries(sensor_range)
@@ -0,0 +1,5 @@
+-include ../global.mk
+
+SUBDIRS=sensor_base sensor_odometry sensor_range
+
+-include ../build_tools/Makefile.subdirs
@@ -0,0 +1,4 @@
+add_library(sensor_base sensor.cpp sensorreading.cpp)
+install(TARGETS sensor_base DESTINATION lib)
+
+ament_export_libraries(sensor_base)
@@ -0,0 +1,12 @@
+#include <gmapping/sensor/sensor_base/sensor.h>
+
+namespace GMapping{
+
+Sensor::Sensor(const std::string& name){
+	m_name=name;
+}
+
+Sensor::~Sensor(){
+}
+
+};// end namespace
@@ -0,0 +1,18 @@
+#ifndef SENSORREADING_H
+#define SENSORREADING_H
+
+#include "sensor.h"
+namespace GMapping{
+
+class SensorReading{
+	public:
+		SensorReading(const Sensor* s=0, double time=0);
+		inline double getTime() const {return m_time;}
+		inline const Sensor* getSensor() const {return m_sensor;}
+	protected:
+		double m_time;
+		const Sensor* m_sensor;
+};
+
+}; //end namespace
+#endif
@@ -0,0 +1,15 @@
+#include <gmapping/sensor/sensor_base/sensorreading.h>
+
+namespace GMapping{
+
+//SensorReading::SensorReading(const Sensor* s, double t){
+//	m_sensor=s;
+//	m_time=t;
+//}
+//
+//
+//SensorReading::~SensorReading(){
+//}
+
+};
+
@@ -0,0 +1,6 @@
+add_library(sensor_odometry odometryreading.cpp odometrysensor.cpp)
+target_link_libraries(sensor_odometry sensor_base)
+
+install(TARGETS sensor_odometry DESTINATION lib)
+
+ament_export_libraries(sensor_odometry)
@@ -0,0 +1,9 @@
+#include <gmapping/sensor/sensor_odometry/odometryreading.h>
+
+namespace GMapping{
+
+OdometryReading::OdometryReading(const OdometrySensor* odo, double time):
+	SensorReading(odo,time){}
+
+};
+
@@ -0,0 +1,9 @@
+#include <gmapping/sensor/sensor_odometry/odometrysensor.h>
+
+namespace GMapping{
+
+OdometrySensor::OdometrySensor(const std::string& name, bool ideal): Sensor(name){ m_ideal=ideal;}
+
+
+};
+
@@ -0,0 +1,7 @@
+add_library(sensor_range rangereading.cpp rangesensor.cpp)
+target_link_libraries(sensor_range sensor_base)
+#ament_target_dependencies(sensor_range sensor_base)
+
+install(TARGETS sensor_range DESTINATION lib)
+
+ament_export_libraries(sensor_range)
@@ -0,0 +1,114 @@
+#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 << __PRETTY_FUNCTION__ << ": 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<< __PRETTY_FUNCTION__ << 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<< __PRETTY_FUNCTION__ << 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;
+}
+
+};
+
@@ -0,0 +1,30 @@
+#include <gmapping/sensor/sensor_range/rangesensor.h>
+
+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);
+	}
+}
+
+};