lib: add new odometry class

Create a class that use encoder objects to compute 
odometry.

This library only support approximation by segment.

Add -ng suffix in order to keep last odometry lib until 
the refactoring job is done.

lib/odometry-ng/Makefile

@@ -0,0 +1 @@
+include $(RIOTBASE)/Makefile.base

lib/odometry-ng/Makefile.dep

@@ -0,0 +1,3 @@
+USEMODULE += cogip_defs
+USEMODULE += trigonometry
+USEMODULE += encoder

lib/odometry-ng/Makefile.include

@@ -0,0 +1,2 @@
+USEMODULE_INCLUDES_odometry-ng := $(LAST_MAKEFILEDIR)/include
+USEMODULE_INCLUDES += $(USEMODULE_INCLUDES_odometry-ng)

lib/odometry-ng/OdometerDifferential.cpp

@@ -0,0 +1,39 @@
+#include <cmath>
+
+#include "odometer/OdometerDifferential.hpp"
+#include "trigonometry.h"
+#include "utils.hpp"
+
+namespace cogip {
+
+namespace odometer {
+
+int OdometerDifferential::update()
+{
+    // Compute encoders wheels left and right linear delta in mm
+    const double dL = left_encoder_.get_angle_and_reset() * parameters_.left_wheel_diameter_mm() * parameters_.left_polarity();
+    const double dR = right_encoder_.get_angle_and_reset() * parameters_.right_wheel_diameter_mm() * parameters_.right_polarity();
+
+    // Compute linear delta for robot in mm
+    const double delta_lin_pos      = (dL + dR) / 2;
+
+    // Compute angular delta for robot in rad
+    const double delta_ang_pos      = (dR - dL) / parameters_.track_width_mm();
+
+    // Compute angle in rad between -pi and pi
+    pos_.O = limit_angle_rad(pos_.O + delta_ang_pos);
+
+    // Compute x and y coordinates in mm
+    pos_.x += delta_lin_pos * cos(pos_.O);
+    pos_.y += delta_lin_pos * sin(pos_.O); 
+
+    // velocites are in mm/period and rad/period so the current velocites are the deltas since last call
+    vel_.linear  = delta_lin_pos;
+    vel_.angular = delta_ang_pos;
+
+    return 0;
+}
+
+} // namespace odometer
+
+} // namespace cogip

lib/odometry-ng/include/odometer/OdometerDifferential.hpp

@@ -0,0 +1,86 @@
+#pragma once
+
+#include <cstdint>
+
+#include "encoder/EncoderInterface.hpp"
+#include "OdometerInterface.hpp"
+#include "OdometerDifferentialParameters.hpp"
+
+namespace cogip {
+
+namespace odometer {
+
+class OdometerDifferential: public OdometerInterface {
+
+public:
+    /// @brief Construct a new differential odometer object
+    /// @param parameters Odometer parameters
+    explicit OdometerDifferential(OdometerDifferentialParameters &parameters, 
+                                  cogip::encoder::EncoderInterface &left_encoder,
+                                  cogip::encoder::EncoderInterface &right_encoder) : 
+                                  parameters_(parameters), left_encoder_(left_encoder), right_encoder_(right_encoder) {}
+
+    /// @brief Set the default odometry position
+    ///
+    /// @note this function should be called to reset robot position and defined a new default one
+    ///
+    /// @param x X coordinate (mm)
+    /// @param y Y coordinate (mm)
+    /// @param O angle (deg)
+    void set_position(double x, double y, double O) override
+    {
+        pos_.x = x;
+        pos_.y = y;
+        pos_.O = DEG2RAD(O);
+    }
+
+
+    /// @brief Set the default odometry position
+    ///
+    /// @note this function should be called to reset robot position and defined a new default one
+    ///
+    /// @param pose postion reference
+    void set_position(const cogip::cogip_defs::Pose &pose) override
+    {
+        pos_.x = pose.coords().x();
+        pos_.y = pose.coords().y();
+        pos_.O = DEG2RAD(pose.O());
+    }
+
+    /// @brief Copy current position using cogip def format
+    /// @note Data units:
+    ///         - x, y: mm
+    ///         - O: deg
+    /// @param pose cogip::cogip_defs::Pose current pose
+    void copy_position(cogip::cogip_defs::Pose &pose) override
+    { 
+        pose.set_x(pos_.x);
+        pose.set_y(pos_.y);
+        pose.set_O(RAD2DEG(pos_.O));
+    }
+
+    /// @brief Copy current velocities using cogip def format
+    /// @note Data units:
+    ///         - linear: mm/s
+    ///         - O: deg/s
+    /// @param vel cogip::cogip_defs::Polar current velocities
+    void copy_velocities(cogip::cogip_defs::Polar &velocity) override
+    { 
+        velocity.set_distance(vel_.linear);
+        velocity.set_angle(RAD2DEG(vel_.angular));
+    }
+
+    /// @brief update new robot pose (x, y, O)
+    /// @return int 0 on success, negative on failure.
+    int update() override;
+
+private:
+    OdometerDifferentialParameters &parameters_;
+
+    cogip::encoder::EncoderInterface &left_encoder_;
+    cogip::encoder::EncoderInterface &right_encoder_;
+};
+
+} // namespace odometer
+
+} // namespace cogip

lib/odometry-ng/include/odometer/OdometerDifferentialParameters.hpp

@@ -0,0 +1,68 @@
+#pragma once
+
+namespace cogip {
+
+namespace odometer {
+
+class OdometerDifferentialParameters {
+public:
+    /// @brief Construct a new Odometry Params object
+    /// @param left_wheel_diameter left wheel diameter value (mm)
+    /// @param right_wheel_diameter right wheel diameter value (mm)
+    /// @param track_width Track width value (mm)
+    /// @param left_polarity left wheel polarity no unit, 1 or -1
+    /// @param right_polarity riht wheel polarity no unit, 1 or -1
+    OdometerDifferentialParameters(double left_wheel_diameter, 
+    double right_wheel_diameter, double track_width, double left_polarity, double right_polarity) : left_wheel_diameter_(left_wheel_diameter), right_wheel_diameter_(right_wheel_diameter), track_width_(track_width), left_polarity_(left_polarity), right_polarity_(right_polarity) {};
+
+    /// @brief Set the left encoder wheel diameter dimension
+    /// @param wheel_diameter Wheel diameter value (mm)
+    void set_left_wheel_diameter(double wheel_diameter) { left_wheel_diameter_ = wheel_diameter; }
+
+    /// @brief Return left encoder wheel diameter value in mm.
+    /// @return double Wheel diameter value (mm)
+    double left_wheel_diameter_mm() const { return left_wheel_diameter_; }
+
+    /// @brief Set the right wheel encoder diameter dimension
+    /// @param wheel_diameter Wheel diameter value (mm)
+    void set_right_wheel_diameter(double wheel_diameter) { right_wheel_diameter_ = wheel_diameter; }
+
+    /// @brief Return right encoder wheel diameter value in mm.
+    /// @return double Wheel diameter value (mm)
+    double right_wheel_diameter_mm() const { return right_wheel_diameter_; }
+
+    /// @brief Set the axle track dimension
+    /// @param track_width Track width value (mm)
+    void set_track_width(double track_width) { track_width_ = track_width; }
+
+    /// @brief Get the axle track value
+    /// @return double Track width value (mm)
+    double track_width_mm() const { return track_width_; }
+
+    /// @brief Set the left encoder polarity
+    /// @param polarity Encoder polarity. -1 or 1.
+    void set_left_polarity(double polarity) { left_polarity_ = polarity; }
+
+    /// @brief Get the left encoder polarity
+    /// @return double Encoder polarity. -1 or 1
+    double left_polarity() const { return left_polarity_; }
+
+    /// @brief Set the right encoder polarity
+    /// @param polarity Encoder polarity. -1 or 1.
+    void set_right_polarity(double polarity) { right_polarity_ = polarity; }
+
+    /// @brief Get the right encoder polarity
+    /// @return double Encoder polarity. -1 or 1
+    double right_polarity() const { return right_polarity_; }
+
+private:
+    double left_wheel_diameter_;
+    double right_wheel_diameter_;
+    double track_width_;
+    double left_polarity_;
+    double right_polarity_;
+};
+
+} // namespace odometer
+
+} // namespace cogip

lib/odometry-ng/include/odometer/OdometerInterface.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <cstdint>
+
+#include "cogip_defs/Polar.hpp"
+#include "cogip_defs/Pose.hpp"
+
+namespace cogip {
+
+namespace odometer {
+
+class OdometerInterface {
+
+// Internal structures for easier Position and Velocity management
+struct Position {
+    Position() : x(0), y(0), O(0) {}
+    Position(float x, float y, float O) : x(x), y(y), O(O) {}
+
+    float x, y, O;
+};
+
+struct Velocity {
+    Velocity() : linear(0), angular(0) {}
+    Velocity(float linear, float angular) : linear(linear), angular(angular) {}
+
+    float linear, angular;
+};
+
+public:
+    /// @brief Set the default odometry position
+    ///
+    /// @note this function should be called to reset robot position and defined a new default one
+    ///
+    /// @param x X coordinate (mm)
+    /// @param y Y coordinate (mm)
+    /// @param O angle (deg)
+    virtual void set_position(double x, double y, double O) = 0;
+
+    /// @brief Set the default odometry position
+    ///
+    /// @note this function should be called to reset robot position and defined a new default one
+    ///
+    /// @param pose postion reference
+    virtual void set_position(const cogip::cogip_defs::Pose &pose) = 0;
+
+    /// @brief Copy current position using cogip def format
+    /// @note Data units:
+    ///         - x, y: mm
+    ///         - O: deg
+    /// @param pose cogip::cogip_defs::Pose current pose
+    virtual void copy_position(cogip::cogip_defs::Pose &pose) = 0;
+
+    /// @brief Copy current velocities using cogip def format
+    /// @note Data units:
+    ///         - linear: mm/s
+    ///         - O: deg/s
+    /// @param vel cogip::cogip_defs::Polar current velocities
+    virtual void copy_velocities(cogip::cogip_defs::Polar &velocity) = 0;
+
+    /// @brief update new robot pose (x, y, O)
+    /// @return int 0 on success, negative on failure.
+    virtual int update() = 0;
+
+protected:
+    Position pos_;  /// Internal position
+    Velocity vel_;  /// Internal velocity
+};
+
+} /// namespace odometer
+
+} ///namespace cogip