pose.hpp

// Copyright (C) 2024 Avular Holding B.V. - All Rights Reserved
// You may use this code under the terms of the Avular
// Software End-User License Agreement.
//
// You should have received a copy of the Avular
// Software End-User License Agreement license with
// this file, or download it from: avular.com/eula
//
/*****************************************************************************
 * Created on: 2024 July 17
 * Author: Robbert-Jan de Jager
 *
 * @file The pose contains a position and orientation.
 ****************************************************************************/
#pragma once
 
#include <array>
#include <creos/messages/generic.hpp>
#include <creos/robot_clock.hpp>
#include <nlohmann/json.hpp>
 
namespace creos_messages {
 
struct Pose {
    Point position = {0, 0, 0};
 
    Quaterniond orientation = {0, 0, 0, 1};
 
    auto operator<=>(const Pose& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Pose, position, orientation)
 
 
struct PoseWithCovariance : public Pose {
    Matrixd<6, 6> covariance;
 
    auto operator<=>(const PoseWithCovariance& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(PoseWithCovariance, position, orientation, covariance)
 
 
struct PoseWithCovarianceStamped : public PoseWithCovariance {
    creos::RobotClock::time_point timestamp;
 
    std::string frame_id;
 
    auto operator<=>(const PoseWithCovarianceStamped& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(PoseWithCovarianceStamped, timestamp, position, orientation, covariance)
 
 
struct PoseStamped : public Pose {
    creos::RobotClock::time_point timestamp;
 
    std::string frame_id;
 
    auto operator<=>(const PoseStamped& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(PoseStamped, timestamp, position, orientation)
 
}  // namespace creos_messages