Pose.hpp

Go to the documentation of this file.

// Copyright (C) 2021 COGIP Robotics association <cogip35@gmail.com>
// This file is subject to the terms and conditions of the GNU Lesser
// General Public License v2.1. See the file LICENSE in the top level
// directory for more details.
 
 
#pragma once
 
#include "cogip_defs/Coords.hpp"
#include "cogip_defs/Polar.hpp"
#include "PB_Pose.hpp"
#include "trigonometry.h"
 
namespace cogip {
 
namespace cogip_defs {
 
class Pose : public Coords {
public:
    Pose(
        double x=0.0,         
        double y=0.0,         
        double O=0.0          
        ) : Coords(x, y), O_(O) {};
 
    explicit Pose(const PB_Pose &pose) : Coords(pose.get_x(), pose.get_y()), O_(pose.get_O()) {};
 
    Coords coords(void) const { return Coords(x_, y_); };
 
    void set_coords(
        const Coords &coords  
        ) { x_ = coords.x(); y_ = coords.y();};
 
    double O(void) const { return O_; };
 
    void set_O(
        double O              
        ) { O_ = O; };
 
    bool operator == (
        const Pose other      
        ) const { return x_ == other.x_ && y_ == other.y_ && O_ == other.O_; };
 
    void pb_copy(
        PB_Pose &pose         
        ) const {
        pose.set_x(x_);
        pose.set_y(y_);
        pose.set_O(O_);
    };
 
    Polar operator-(const Pose& p) {
        double error_x = x_ - p.x();
        double error_y = y_ - p.y();
 
        double error_O = limit_angle_rad(atan2(error_y, error_x) - DEG2RAD(p.O()));
 
        return Polar(
            sqrt(square(error_x) + square(error_y)),
            RAD2DEG(error_O)
        );
    };
 
protected:
    double O_;                
};
 
} // namespace cogip_defs
 
} // namespace cogip