NextState estimation

Entities/State2D.cpp

@@ -0,0 +1,70 @@
+#include "State2D.hpp"
+
+#include <cmath>
+
+using namespace astar;
+
+// simple constructor
+State2D::State2D() : position(0.0, 0.0), orientation(0.0), wheel_angle(0.0), time(0.0), v(0.0), gear(ForwardGear){}
+
+// basic pose constructor with Vector2D
+State2D::State2D(const Vector2D<double> &pos, double o, double w_angle = 0.0, double vel = 0.0, double dt = 0.0, Gear g = ForwardGear) :
+        position(pos), orientation(o), wheel_angle(w_angle), v(vel), time(dt), gear(g) {}
+
+
+// explicit constructor
+State2D::State2D(double x_, double y_, double o, double w_angle = 0.0, double vel = 0.0, double dt = 0.0, Gear g = ForwardGear) :
+		position(x_, y_), orientation(o), wheel_angle(w_angle), v(vel), time(0), gear(ForwardGear) {}
+
+// copy constructor
+State2D::State2D(const State2D &s) :
+        position(s.position), orientation(s.orientation), wheel_angle(s.wheel_angle), v(s.v), time(s.time), gear(s.gear) {}
+
+// distance between two poses
+double State2D::Distance(const State2D &p)
+{
+    return position.Distance(p.position);
+}
+
+// distance squared between two poses
+// Euclidean distance
+double State2D::Distance2(const State2D &p)
+{
+    return position.Distance2(p.position);
+}
+
+// get the minimum difference between two angles
+double State2D::GetOrientationDiff(const State2D &p)
+{
+    return mrpt::math::angDistance<double>(orientation, p.orientation);
+}
+
+// get difference between two orientations (yaw), overloading
+double State2D::GetOrientationDiff(double t)
+{
+    return mrpt::math::angDistance<double>(orientation, t);
+}
+
+// the assignment operator
+void State2D::operator=(const State2D &p)
+{
+    position = p.position;
+    orientation = p.orientation;
+    wheel_angle = p.wheel_angle;
+    v = p.v;
+    gear = p.gear;
+    timestamp = p.timestamp;
+}
+
+// == operator overloading
+bool State2D::operator ==(const State2D &p)
+{
+    return std::fabs(position.x - p.position.x) < 0.001 && std::fabs(position.y - p.position.y) < 0.001 && std::fabs(orientation - p.orientation) < 0.001;
+}
+
+// != operator overloading
+bool State2D::operator !=(const State2D &p)
+{
+    return !(std::fabs(position.x - p.position.x) < 0.001 && std::fabs(position.y - p.position.y) < 0.001 && std::fabs(orientation - p.orientation) < 0.001);
+}
+

Entities/State2D.hpp

@@ -0,0 +1,98 @@
+#ifndef STATE_2D_HPP
+#define STATE_2D_HPP
+
+#include <list>
+#include <vector>
+
+#include "../Helpers/wrap2pi.hpp"
+#include "Vector2D.hpp"
+#include "ReedsShepp/ReedsSheppAction.hpp"
+
+namespace astar {
+
+class State2D {
+
+    public:
+
+        // PUBLIC ATTRIBUTES
+
+        // the position, x and y coordinates
+        astar::Vector2D<double> position;
+
+        // the heading
+        double orientation;
+
+        // registering the wheel angle
+        double wheel_angle;
+
+        // the speed at the current pose
+        double v;
+
+        // how much time
+        double time;
+
+        // the gear
+        astar::Gear gear;
+
+        // simple constructor
+        State2D();
+
+        // basic constructor, only position and orientation are required
+        State2D(const astar::Vector2D<double> &pos, double o, double w_angle = 0, double vel = 0, double dt = 0, astar::Gear g = ForwardGear);
+
+        // most explicit constructor
+        State2D(double x_, double y_, double o, double w_angle = 0, double vel = 0, double dt = 0, astar::Gear g = ForwardGear);
+
+        // copy constructor
+        State2D(const State2D&);
+
+        // distance between two poses
+        double Distance(const State2D&);
+
+        // distance squared between two poses
+        double Distance2(const State2D&);
+
+        // get difference between orientations (yaw)
+        double GetOrientationDiff(const State2D&);
+
+        // get difference between orientations (yaw), overloaded version
+        double GetOrientationDiff(double);
+
+        // the assignment operator
+        void operator=(const State2D&);
+
+        // == operator overloading
+        bool operator==(const State2D&);
+
+        // != operator overloading
+        bool operator!=(const State2D&);
+
+};
+
+// a helper class to avoid copies
+class StateList {
+
+public:
+
+    // EVERYTHING PUBLIC
+    std::list<State2D> states;
+
+};
+
+typedef StateList* StateListPtr;
+
+//
+class StateArray {
+
+public:
+
+    // EVERYTHING PUBLIC
+    std::vector<State2D> states;
+
+};
+
+typedef StateArray* StateArrayPtr;
+
+}
+
+#endif

Entities/Vector2D.hpp

@@ -23,7 +23,7 @@ class Vector2D {
     T x, y;
 
     // basic constructor'   '
-    Vector2D() : x(0.0), y(0.0) {}
+    Vector2D() : x(0), y(0) {}
 
     // copy constructor
     Vector2D(const Vector2D &v) : x(v.x), y(v.y) {}
@@ -32,13 +32,13 @@ class Vector2D {
     Vector2D(T x_, T y_) : x(x_), y(y_) {}
 
     // distance between two vectors
-    T Distance(const Vector2D &v)
+    T Distance(Vector2D &v)
     {
         return std::sqrt((x - v.x)*(x - v.x) + (y - v.y)*(y - v.y));
     }
 
     // squared distance between two vectors
-    T Distance2(const Vector2D &v)
+    T Distance2(Vector2D &v)
     {
         return (x - v.x)*(x - v.x) + (y - v.y)*(y - v.y);
     }

PathFinding/GridMap/GVDLau.hpp

@@ -31,6 +31,9 @@ class GVDLau {
         // set a given cell as obstacle
         void SetObstacle(int x, int y);
 
+        // set a given cell as free space
+        void UnsetObstacle(int x, int y);
+
 };
 
 }