| 1 | /**
|
---|
| 2 | * \file ipc_base.h
|
---|
| 3 | * \brief definition de structures
|
---|
| 4 | * \author {
|
---|
| 5 | * Sébastien Ambroziak
|
---|
| 6 | * Paul Sauvage
|
---|
| 7 | * }
|
---|
| 8 | * \version 0.1
|
---|
| 9 | * \date 5 novembre 2020
|
---|
| 10 | *
|
---|
| 11 | * Definitions des structures pour la message queue
|
---|
| 12 | *
|
---|
| 13 | */
|
---|
| 14 |
|
---|
| 15 |
|
---|
| 16 | #ifndef IPC_BASE
|
---|
| 17 | #define IPC_BASE
|
---|
| 18 |
|
---|
| 19 | #include <string>
|
---|
| 20 | #include <iostream>
|
---|
| 21 |
|
---|
| 22 | #define ROS2 1
|
---|
| 23 | #define FLAIR 2
|
---|
| 24 | #define ARDUPILOT 3
|
---|
| 25 |
|
---|
| 26 | #include <ctime>
|
---|
| 27 |
|
---|
| 28 | //#define MAX_TOPIC_SIZE 25
|
---|
| 29 | //#define MAX_CHAR_ARRAY_SIZE 255
|
---|
| 30 |
|
---|
| 31 | // structures for message queue
|
---|
| 32 | namespace ipc{
|
---|
| 33 | namespace type{
|
---|
| 34 |
|
---|
| 35 | struct vector2D {
|
---|
| 36 | double x;
|
---|
| 37 | double y;
|
---|
| 38 | };
|
---|
| 39 |
|
---|
| 40 | struct vector2D_32 {
|
---|
| 41 | float x;
|
---|
| 42 | float y;
|
---|
| 43 | float z;
|
---|
| 44 | };
|
---|
| 45 |
|
---|
| 46 | inline std::ostream &operator<<(std::ostream &out, vector2D vecteur) {
|
---|
| 47 | out << "\tx : " << vecteur.x << std::endl;
|
---|
| 48 | out << "\ty : " << vecteur.y << std::endl;
|
---|
| 49 | return out;
|
---|
| 50 | }
|
---|
| 51 |
|
---|
| 52 | struct vector3D {
|
---|
| 53 | double x;
|
---|
| 54 | double y;
|
---|
| 55 | double z;
|
---|
| 56 | };
|
---|
| 57 |
|
---|
| 58 | struct vector3D_32 {
|
---|
| 59 | float x;
|
---|
| 60 | float y;
|
---|
| 61 | float z;
|
---|
| 62 | };
|
---|
| 63 |
|
---|
| 64 | inline std::ostream &operator<<(std::ostream &out, vector3D vecteur) {
|
---|
| 65 | out << "\tx : " << vecteur.x << std::endl;
|
---|
| 66 | out << "\ty : " << vecteur.y << std::endl;
|
---|
| 67 | out << "\tz : " << vecteur.z << std::endl;
|
---|
| 68 | return out;
|
---|
| 69 | }
|
---|
| 70 |
|
---|
| 71 | inline std::ostream &operator<<(std::ostream &out, vector3D_32 vecteur) {
|
---|
| 72 | out << "\tx : " << vecteur.x << std::endl;
|
---|
| 73 | out << "\ty : " << vecteur.y << std::endl;
|
---|
| 74 | out << "\tz : " << vecteur.z << std::endl;
|
---|
| 75 | return out;
|
---|
| 76 | }
|
---|
| 77 |
|
---|
| 78 | struct acceleration {
|
---|
| 79 | vector3D linear;
|
---|
| 80 | vector3D angular;
|
---|
| 81 | };
|
---|
| 82 |
|
---|
| 83 | struct vector4D {
|
---|
| 84 | double x;
|
---|
| 85 | double y;
|
---|
| 86 | double z;
|
---|
| 87 | double w;
|
---|
| 88 | };
|
---|
| 89 |
|
---|
| 90 | struct vector4D_32 {
|
---|
| 91 | float x;
|
---|
| 92 | float y;
|
---|
| 93 | float z;
|
---|
| 94 | float w;
|
---|
| 95 | };
|
---|
| 96 |
|
---|
| 97 | inline std::ostream &operator<<(std::ostream &out, vector4D vecteur) {
|
---|
| 98 | out << "\tx : " << vecteur.x << std::endl;
|
---|
| 99 | out << "\ty : " << vecteur.y << std::endl;
|
---|
| 100 | out << "\tz : " << vecteur.z << std::endl;
|
---|
| 101 | out << "\tw : " << vecteur.w << std::endl;
|
---|
| 102 | return out;
|
---|
| 103 | }
|
---|
| 104 |
|
---|
| 105 | inline std::ostream &operator<<(std::ostream &out, vector4D_32 vecteur) {
|
---|
| 106 | out << "\tx : " << vecteur.x << std::endl;
|
---|
| 107 | out << "\ty : " << vecteur.y << std::endl;
|
---|
| 108 | out << "\tz : " << vecteur.z << std::endl;
|
---|
| 109 | out << "\tw : " << vecteur.w << std::endl;
|
---|
| 110 | return out;
|
---|
| 111 | }
|
---|
| 112 |
|
---|
| 113 | struct matrix {
|
---|
| 114 | int width;
|
---|
| 115 | int height;
|
---|
| 116 | float data[5][5];
|
---|
| 117 | };
|
---|
| 118 |
|
---|
| 119 | inline std::ostream &operator<<(std::ostream &out, matrix M) {
|
---|
| 120 |
|
---|
| 121 | for(int j = 0;j < M.height;j++){
|
---|
| 122 | out<<"\t| "<<M.data[0][j];
|
---|
| 123 | for(int i = 1;i < M.width;i++){
|
---|
| 124 | out << "\t " << M.data[i][j];
|
---|
| 125 | }
|
---|
| 126 | out << " |" << std::endl;
|
---|
| 127 | }
|
---|
| 128 | return out;
|
---|
| 129 | }
|
---|
| 130 |
|
---|
| 131 | struct imu{
|
---|
| 132 | vector4D orientation;
|
---|
| 133 | double orientation_covariance[9];
|
---|
| 134 |
|
---|
| 135 | vector3D angular_velocity;
|
---|
| 136 | double angular_velocity_covariance[9];
|
---|
| 137 |
|
---|
| 138 | vector3D linear_acceleration;
|
---|
| 139 | double linear_acceleration_covariance[9];
|
---|
| 140 | };
|
---|
| 141 |
|
---|
| 142 | inline std::ostream &operator<<(std::ostream &out, imu msg) {
|
---|
| 143 | out << "orientation : \n" << msg.orientation << std::endl;
|
---|
| 144 | out << "orientation covarience : " ;
|
---|
| 145 | for (int i=0;i<9;i++){out << msg.orientation_covariance[i] << ' ';}
|
---|
| 146 | out << std::endl;
|
---|
| 147 |
|
---|
| 148 | out << "angular_velocity : \n" << msg.angular_velocity << std::endl;
|
---|
| 149 | out << "angular_velocity_covariance : " ;
|
---|
| 150 | for (int i=0;i<9;i++){out << msg.orientation_covariance[i] << ' ';}
|
---|
| 151 | out << std::endl;
|
---|
| 152 |
|
---|
| 153 | out << "linear_acceleration : \n" << msg.linear_acceleration << std::endl;
|
---|
| 154 | out << "linear_acceleration_covariance : " ;
|
---|
| 155 | for (int i=0;i<9;i++){out << msg.linear_acceleration_covariance[i] << ' ';}
|
---|
| 156 | out << std::endl;
|
---|
| 157 |
|
---|
| 158 | return out;
|
---|
| 159 | }
|
---|
| 160 |
|
---|
| 161 | struct ahrs{
|
---|
| 162 | vector4D_32 quaternion;
|
---|
| 163 | vector3D_32 angular_rate;
|
---|
| 164 | };
|
---|
| 165 |
|
---|
| 166 | inline std::ostream &operator<<(std::ostream &out, ahrs msg) {
|
---|
| 167 | out << "quaternion : \n" << msg.quaternion << std::endl;
|
---|
| 168 | out << "angular_rate : \n" << msg.angular_rate << std::endl;
|
---|
| 169 | return out;
|
---|
| 170 | }
|
---|
| 171 |
|
---|
| 172 |
|
---|
| 173 | /**
|
---|
| 174 | * \struct ipc_message
|
---|
| 175 | * \brief Structure classique de message ipc.
|
---|
| 176 | *
|
---|
| 177 | * \param T permet de choisir le type de l'élément data
|
---|
| 178 | * \param N permet de choisir la taille du tableau d'objet du message
|
---|
| 179 | * Si N = 0 : data est une structure de type T
|
---|
| 180 | * Si N > 0 : data est un tableau d'élement de type T et de taille N
|
---|
| 181 | */
|
---|
| 182 |
|
---|
| 183 |
|
---|
| 184 |
|
---|
| 185 | struct timestamp{
|
---|
| 186 | uint32_t sec=0;
|
---|
| 187 | uint32_t nanosec=0;
|
---|
| 188 |
|
---|
| 189 | void now(){
|
---|
| 190 | struct timespec now;
|
---|
| 191 | timespec_get( &now, TIME_UTC );
|
---|
| 192 | sec = (int32_t)now.tv_sec;
|
---|
| 193 | nanosec = (uint32_t)now.tv_nsec;
|
---|
| 194 | }
|
---|
| 195 |
|
---|
| 196 | uint64_t toNanosec(){
|
---|
| 197 | return (uint64_t)((uint64_t)sec*1000000000 + (uint64_t)nanosec) ;
|
---|
| 198 | }
|
---|
| 199 | };
|
---|
| 200 |
|
---|
| 201 | inline std::ostream &operator<<(std::ostream &out, timestamp stamp) {
|
---|
| 202 | out << "[" << stamp.sec << '.' << stamp.nanosec << ']';
|
---|
| 203 | return out;
|
---|
| 204 | }
|
---|
| 205 |
|
---|
| 206 | inline timestamp operator-(timestamp const& a, timestamp const& b)
|
---|
| 207 | {
|
---|
| 208 | timestamp resultat;
|
---|
| 209 | resultat.sec = a.sec - b.sec;
|
---|
| 210 | resultat.nanosec = a.nanosec - b.nanosec;
|
---|
| 211 | return resultat;
|
---|
| 212 | }
|
---|
| 213 |
|
---|
| 214 | //message ipc
|
---|
| 215 |
|
---|
| 216 | template <typename T>
|
---|
| 217 | struct ipc_message {
|
---|
| 218 | long msg_type = 1;
|
---|
| 219 | struct {
|
---|
| 220 | struct timestamp stamp;
|
---|
| 221 | T value;
|
---|
| 222 | } data;
|
---|
| 223 |
|
---|
| 224 | void time_now(){
|
---|
| 225 | data.stamp.now();
|
---|
| 226 | }
|
---|
| 227 | };
|
---|
| 228 |
|
---|
| 229 | template <typename T>
|
---|
| 230 | struct ipc_tab {
|
---|
| 231 | long msg_type = 1;
|
---|
| 232 | struct {
|
---|
| 233 | struct timestamp stamp;
|
---|
| 234 | T values[1];
|
---|
| 235 | } data;
|
---|
| 236 |
|
---|
| 237 | void time_now(){
|
---|
| 238 | data.stamp.now();
|
---|
| 239 | }
|
---|
| 240 | };
|
---|
| 241 |
|
---|
| 242 | } // end namespace type
|
---|
| 243 | } // end namespace ipc
|
---|
| 244 |
|
---|
| 245 |
|
---|
| 246 |
|
---|
| 247 | #endif |
---|