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,41 @@
+#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_linear_pose      = (dL + dR) / 2;
+
+    // Compute angular delta for robot in rad
+    const double delta_angular_pose      = (dR - dL) / parameters_.track_width_mm();
+
+    // Compute angle in rad between -pi and pi
+    double O_rad = DEG2RAD(pose_.O());
+    O_rad = limit_angle_rad(O_rad + delta_angular_pose);
+
+    // Compute x and y coordinates in mm
+    pose_.set_x(pose_.x() + delta_linear_pose * cos(O_rad)); 
+    pose_.set_y(pose_.y() + delta_linear_pose * sin(O_rad));
+    pose_.set_O(RAD2DEG(O_rad));
+
+    // Save polar position delta since last call
+    polar_.set_distance(delta_linear_pose);
+    polar_.set_angle(delta_angular_pose);
+
+    return 0;
+}
+
+} // namespace odometer
+
+} // namespace cogip

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

@@ -0,0 +1,85 @@
+#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 pose
+    ///
+    /// @note this function should be called to reset robot pose and defined a new default one
+    ///
+    /// @param x X coordinate (mm)
+    /// @param y Y coordinate (mm)
+    /// @param O angle (deg)
+    void set_pose(double x, double y, double O) override
+    {
+        pose_.set_x(x);
+        pose_.set_y(y);
+        pose_.set_O(O);
+    }
+
+    /// @brief Set the default odometry pose
+    ///
+    /// @note this function should be called to reset robot pose and defined a new default one
+    ///
+    /// @param pose postion reference
+    void set_pose(const cogip::cogip_defs::Pose &pose) override
+    {
+        pose_.set_x(pose.x());
+        pose_.set_y(pose.y());
+        pose_.set_O(pose.O());
+    }
+
+    /// @brief Get current pose using cogip def format
+    /// @note Data units:
+    ///         - x, y: mm
+    ///         - O: deg
+    /// @return pose cogip::cogip_defs::Pose current pose reference
+    const cogip::cogip_defs::Pose& pose() override
+    {
+        return pose_;
+    }
+
+    /// @brief Get current polar pose delta cogip def format
+    /// @note Data units:
+    ///         - linear: mm
+    ///         - O: deg
+    /// @return velocity cogip::cogip_defs::Polar current polar pose delta reference
+    const cogip::cogip_defs::Polar& polar() override
+    {
+        return polar_;
+    }
+
+    /// @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_;
+
+    cogip::cogip_defs::Pose pose_;
+    cogip::cogip_defs::Polar polar_;
+};
+
+} // 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,51 @@
+#pragma once
+
+#include <cstdint>
+
+#include "cogip_defs/Polar.hpp"
+#include "cogip_defs/Pose.hpp"
+
+namespace cogip {
+
+namespace odometer {
+
+class OdometerInterface {
+public:
+    /// @brief Set the default odometry pose
+    ///
+    /// @note this function should be called to reset robot pose and defined a new default one
+    ///
+    /// @param x X coordinate (mm)
+    /// @param y Y coordinate (mm)
+    /// @param O angle (deg)
+    virtual void set_pose(double x, double y, double O) = 0;
+
+    /// @brief Set the default odometry pose
+    ///
+    /// @note this function should be called to reset robot pose and defined a new default one
+    ///
+    /// @param pose postion reference
+    virtual void set_pose(const cogip::cogip_defs::Pose &pose) = 0;
+
+    /// @brief Get current pose using cogip def format
+    /// @note Data units:
+    ///         - x, y: mm
+    ///         - O: deg
+    /// @return pose cogip::cogip_defs::Pose current pose reference
+    virtual const cogip::cogip_defs::Pose& pose() = 0;
+
+    /// @brief Get current polar pose delta cogip def format
+    /// @note Data units:
+    ///         - linear: mm
+    ///         - O: deg
+    /// @return velocity cogip::cogip_defs::Polar current polar pose delta reference
+    virtual const cogip::cogip_defs::Polar& polar() = 0;
+
+    /// @brief update new robot pose (x, y, O)
+    /// @return int 0 on success, negative on failure.
+    virtual int update() = 0;
+};
+
+} /// namespace odometer
+
+} ///namespace cogip