Add interfaces, plugins and class headers for new localization (ros-n…

…avigation#1391)

nav2_localization/include/interfaces/matcher_base.hpp

@@ -0,0 +1,21 @@
+#ifndef NAV2_LOCALIZATION__MATCHER_BASE_HPP_
+#define NAV2_LOCALIZATION__MATCHER_BASE_HPP_
+
+#include <map.hpp>
+#include <measurement.hpp>
+#include <particle.hpp>
+#include <vector>
+
+namespace nav2_localization_base
+{
+    class Matcher2d
+    {
+        public:
+            virtual float get_overlap(const nav2_localization::measurement_t& measurement, const pose_2d_t& pos, const nav2_localization::ndt_grid_2d_t& map); // Computes the overlap between a measurement and what should be measured given the current position and a map
+
+        protected:
+            Matcher2d(){}
+    };  
+};
+
+#endif // NAV2_LOCALIZATION__MATCHER_BASE_HPP_

nav2_localization/include/interfaces/motion_model_base.hpp

@@ -0,0 +1,19 @@
+#ifndef NAV2_LOCALIZATION__MOTION_MODEL_BASE_HPP_
+#define NAV2_LOCALIZATION__MOTION_MODEL_BASE_HPP_
+
+#include <vector>
+
+namespace nav2_localization_base
+{
+    class MotionModel
+    {   
+        public:
+			// TODO - How to send odometry?
+            virtual std::vector<float> get_transformation(const Odometry& odom); // Applies a motion model plugin to a certain odometry
+
+        protected:
+            MotionModel(){}
+    };  
+};
+
+#endif // NAV2_LOCALIZATION__MOTION_MODEL_BASE_HPP_

nav2_localization/include/interfaces/solver_base.hpp

@@ -0,0 +1,21 @@
+#ifndef NAV2_LOCALIZATION__SOLVER_BASE_HPP_
+#define NAV2_LOCALIZATION__SOLVER_BASE_HPP_
+
+#include <interfaces/matcher_base.hpp>
+#include <interfaces/motion_model_base.hpp>
+
+namespace nav2_localization_base
+{
+	class Solver2d
+	{
+		public:
+			// From nav2 localization interfaces
+			Matcher2d matcher;
+			MotionModel motion_model;
+
+		protected:
+			Solver2d(){}
+	};
+};
+
+#endif // NAV2_LOCALIZATION__SOLVER_BASE_HPP_

nav2_localization/include/map.hpp

@@ -0,0 +1,31 @@
+#ifndef NAV2_LOCALIZATION__MAP_HPP_
+#define NAV2_LOCALIZATION__MAP_HPP_
+
+#include <vector>
+
+namespace nav2_localization
+{
+	// NDT Grid cell for NDT Grid Map
+	typedef struct
+	{   
+	    float mean;
+	    float covariance;
+	} ndt_cell_t;
+
+
+	// 2D NDT Grid Map for NDT matching
+	typedef struct
+	{   
+	    std::vector<std::vector<NDTCell>>
+	} ndt_grid_2d_t;
+
+/*
+	// 3D NDT Grid Map for NDT matching
+	typedef struct
+	{   
+	    std::vector<std::vector<std::vector<NDTCell>>>
+	} ndt_grid_3d_t;
+*/
+}
+
+#endif // NAV2_LOCALIZATION__MAP_HPP_

nav2_localization/include/measurement.hpp

@@ -0,0 +1,17 @@
+#ifndef NAV2_LOCALIZATION__MEASUREMENT_HPP_
+#define NAV2_LOCALIZATION__MEASUREMENT_HPP_
+
+#include <vector>
+
+namespace nav2_localization
+{
+	typedef struct
+	{
+		// TODO - How to represent keypoints?
+		std::vector<std::vector<float>> keypoints; // Vector of 3D points
+		// TODO - How to represent descriptors?? Plugin??
+		std::vector<std::vector<float>> kps_descriptors // Vector of descriptors
+	} measurement_t;
+};
+
+#endif // NAV2_LOCALIZATION__MEASUREMENT_HPP_

nav2_localization/include/particle.hpp

@@ -0,0 +1,81 @@
+#ifndef NAV2_LOCALIZATION__PARTICLE_HPP_
+#define NAV2_LOCALIZATION__PARTICLE_HPP_
+
+#include <map.hpp>
+#include <measurement.hpp>
+#include <vector>
+
+namespace nav2_localization
+{
+	// Position and orientation for 2D environments
+	typedef struct
+	{
+		// 2D Position (Cartesian coords.)
+		float x;
+		float y;
+
+		// 2D Orientation (rotation over Z axis)
+		float theta;
+	} pose_2d_t;
+
+/*
+	// Position and orientation for 3D environments
+	typedef struct
+	{
+		// 3D Position
+		float x;
+		float y;
+		float z;
+
+		// 3D Orientation
+		float alpha;
+		float beta;
+		float gamma;
+	} pose_3d_t; 
+*/
+
+
+	// Base Particle class
+	class Particle:
+	{
+		protected:
+			float weight; // Particle's importance weight
+
+			bool decide_suicide(); // Use the particle's weight to pseudo-randomly decide if the particle should be killed
+
+			virtual void move(); // Move the particle
+			virtual void compute_weight(); // Update the weight
+	};
+
+	// 2D particle
+	class Particle2d : Particle
+	{
+		private:
+			// 2D pose (Cartesian Coords.) and orientation (Z axis)
+			pose_2d_t pose;
+
+		public:
+			Particle2d(const float& x, const float& y, const float& theta); // Class constructor. Takes initial position (cartesian coords.) and orientation (Z axis)
+
+			void move(const float& dx, const float& dy, const float& dtheta); // Move the particle with the specified variation of position and orientation
+			void compute_weight(const ndt_grid_2d_t& map, const measurement_t& measurement); // Update the weight with the given map and observation
+	};
+
+/*
+	// 3D particle	
+	class Particle3d : Particle
+	{
+		private:
+			// 3D pose (Cartesian Coords.) and orientation (Euler Angles)
+			pose_3d_t pose;
+
+		public:
+			Particle(const float& x, const float& y, const float& z, const float& alpha, const float& beta, const float& gamma); // Class constructor. Takes initial position (cartesian coords.) and orientation (Euler Angles)
+
+			void move(const float& dx, const float& dy, const float& dz, const float& dalpha, const float& dbeta, const float& dgamma); // Move the particle with the specified variation of position and orientation
+			void compute_weight(const ndt_grid_3d_t& map, const measurement_t& measurement); // Update the weight with the given map and observation
+	}; 
+*/
+};
+
+#endif // NAV2_LOCALIZATION__PARTICLE_HPP_

nav2_localization/include/particle_filter.hpp

@@ -0,0 +1,30 @@
+#ifndef NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_
+#define NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_
+
+#include <map.hpp>
+#include <measurement.hpp>
+#include <particle.hpp>
+#include <vector>
+
+namespace nav2_localization
+{
+	// Implements a Particle Filter (PF) for 2D localization purposes
+	class ParticleFilter2d
+	{
+		private:
+			std::vector<nav2_localization::Particle2d> particles; // Vector containing all the particles
+			unsigned int n_particles; // Number of particles
+
+		public:
+			ParticleFilter2d(){}
+
+			void sample(const ndt_grid_2d_t& map, const unsigned int& n_parts); // Takes the map to sample and the number of particles with which the map will be sampled
+			void apply_motion(const float& dx, const float& dy, const float& dtheta); // Applies a certain pose variation to every particle of the PF
+			void compute_weights(const ndt_grid_2d_t& map, const measurement_t& measurement); // Update the weights of all the particles with the given map and observation
+			void purge(); // Kills particles pseudo-randomly according to their importance weights
+			// TODO - Update to use KLD Sampling (as AMCL does)
+			void resample(); // Generate new particles
+	};
+};
+
+#endif // NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_

nav2_localization/include/plugins/matcher_plugins.hpp

@@ -0,0 +1,25 @@
+#ifndef NAV2_LOCALIZATION__MATCHER_PLUGINS_HPP_
+#define NAV2_LOCALIZATION__MATCHER_PLUGINS_HPP_
+
+#include <interfaces/matcher_base.hpp>
+#include <map.hpp>
+#include <measurement.hpp>
+#include <vector>
+
+namespace nav2_localization_plugins
+{
+    // Implements a vanilla matcher for 2D NDT maps (for NDT-MCL localization)
+    class Vanilla2d : public nav2_localization_base::Matcher
+    {
+		private:
+            ndt_grid_2d_t map;
+
+        public:
+			Vanilla2d(){}
+
+            // TODO - How to send position?
+            float get_overlap(const nav2_localization::measurement_t& measurement, const vector<float>& pos, const nav2_localization::ndt_grid_2d_t& map); // Computes the overlap between a measurement and what should be measured given the current position and a map
+    };  
+};
+
+#endif // NAV2_LOCALIZATION__MATCHER_PLUGINS_HPP_

nav2_localization/include/plugins/motion_model_plugins.hpp

@@ -0,0 +1,47 @@
+#ifndef NAV2_LOCALIZATION__MOTION_MODEL_PLUGINS_HPP_
+#define NAV2_LOCALIZATION__MOTION_MODEL_PLUGINS_HPP_
+
+#include <interfaces/motion_model_base.hpp>
+#include <vector>
+
+namespace nav2_localization_plugins
+{
+    // Implements a Bicycle motion model
+    class Bicycle : public nav2_localization_base::MotionModel
+    {   
+        public:
+            Bicycle(){}
+			// TODO - How to send odometry?
+    		std::vector<float> get_transformation(const Odometry& odom); // Get the position and orientation transformation using a given odometry
+    };
+
+    // Implements an Ackermann motion model
+	//// TODO - Use this to simplify ackermann vehicle to bicycle model and then use the Bicycle model
+    class Ackermann : public nav2_localization_base::MotionModel
+    {   
+        public:
+            Ackermann(){}
+			// TODO - How to send odometry?
+    		std::vector<float> get_transformation(const Odometry& odom); // Get the position and orientation transformation using a given odometry
+    };
+
+    // Implements a Differential motion model
+    class Differential : public nav2_localization_base::MotionModel
+    {   
+        public:
+            Differential(){}
+			// TODO - How to send odometry?
+    		std::vector<float> get_transformation(const Odometry& odom); // Get the position and orientation transformation using a given odometry
+    };  
+
+    // Implements an Omnidirectional motion model
+    class Omnidirectional : public nav2_localization_base::MotionModel
+    {   
+        public:
+            Omnidirectional(){}
+			// TODO - How to send odometry?
+    		std::vector<float> get_transformation(const Odometry& odom); // Get the position and orientation transformation using a given odometry
+    };  
+};
+
+#endif // NAV2_LOCALIZATION__MOTION_MODEL_PLUGINS_HPP_

nav2_localization/include/plugins/solver_plugins.hpp

@@ -0,0 +1,29 @@
+#ifndef NAV2_LOCALIZATION__SOLVER_PLUGINS_HPP_
+#define NAV2_LOCALIZATION__SOLVER_PLUGINS_HPP_
+
+#include <interfaces/solver_base.hpp>
+#include <map.hpp>
+#include <particle_filter.hpp>
+
+namespace nav2_localization_plugins
+{
+	// Implements a 2D Monte-Carlo Localization (MCL) algorithm to solve the state estimation problem.
+	class MCL2d : public nav2_localization_base::Solver
+	{
+		private:
+			nav2_localization::ParticleFilter2d particle_filter; // MCL solver, a Particle Filter (PF)
+
+		public:
+			MCL2d(){} // Class constructor. Empty bc the PF will be initialized in the "sample()" step
+
+			// Methods implementing the MCL steps:
+			// TODO - How to send map?
+			void sample(const nav2_localization::ndt_grid_2d_t& map, const unsigned int& n_particles); // Sample the map with the specified number of particles 
+			// TODO - How to send odometry?
+			void update(const Odometry& odom, const nav2_localization_base::MotionModel& motion_model); // Update the particles' position using a given odometry and motion model
+			// TODO - How to send map?
+			void resample(const nav2_localization::ndt_grid_2d_t& map); // Compute particles' weights, decide which to kill and generate new samples
+	};
+};
+
+#endif // NAV2_LOCALIZATION__SOLVER_PLUGINS_HPP_