enum_sensor_orientation.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2015 Nuno Marques.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <unordered_map>
18 
19 #include <mavros/utils.h>
20 #include <mavros/frame_tf.h>
21 #include <ros/console.h>
22 
23 namespace mavros {
24 namespace utils {
25 using mavlink::common::MAV_SENSOR_ORIENTATION;
26 
27 // internal type: name - rotation
28 using OrientationPair = std::pair<const std::string, const Eigen::Quaterniond>;
29 
30 // internal data initializer
31 static const OrientationPair make_orientation(const std::string &name,
32  const double roll,
33  const double pitch,
34  const double yaw)
35 {
36  constexpr auto DEG_TO_RAD = (M_PI / 180.0);
37  const Eigen::Quaterniond rot = ftf::quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw) * DEG_TO_RAD);
38  return std::make_pair(name, rot);
39 }
40 
41 // [[[cog:
42 // import attr
43 // import pymavlink.dialects.v20.common as common
44 // ename = 'MAV_SENSOR_ORIENTATION'
45 // pfx2 = 'MAV_SENSOR_ROTATION_'
46 //
47 // enum = sorted(common.enums[ename].items())
48 // enum.pop() # remove ENUM_END
49 //
50 // @attr.s(auto_attribs=True)
51 // class Vector3:
52 // Roll: float = 0.0
53 // Pitch: float = 0.0
54 // Yaw: float = 0.0
55 //
56 // @classmethod
57 // def parse_rpy(cls, desc):
58 // try:
59 // pairs = {
60 // f.strip(): float(v)
61 // for f, v in [v.split(":") for v in desc.split(',')]
62 // }
63 // return cls(**pairs)
64 // except Exception as ex:
65 // print(f"Parse Error: {ex}, desc: {desc}")
66 // return cls()
67 //
68 // cog.outl(f"static const std::unordered_map<typename std::underlying_type<{ename}>::type, const OrientationPair> sensor_orientations{{{{")
69 // for k, e in enum:
70 // name_short = e.name[len(pfx2):]
71 // vec = Vector3.parse_rpy(e.description)
72 // whitespace = ' ' * (27 - len(name_short))
73 // cog.outl(f"""{{ {k:>3}, make_orientation("{name_short}",{whitespace}{vec.Roll:>5}, {vec.Pitch:>5}, {vec.Yaw:>5}) }},""")
74 //
75 // cog.outl("}};")
76 // ]]]
77 static const std::unordered_map<typename std::underlying_type<MAV_SENSOR_ORIENTATION>::type, const OrientationPair> sensor_orientations{{
78 { 0, make_orientation("NONE", 0.0, 0.0, 0.0) },
79 { 1, make_orientation("YAW_45", 0.0, 0.0, 45.0) },
80 { 2, make_orientation("YAW_90", 0.0, 0.0, 90.0) },
81 { 3, make_orientation("YAW_135", 0.0, 0.0, 135.0) },
82 { 4, make_orientation("YAW_180", 0.0, 0.0, 180.0) },
83 { 5, make_orientation("YAW_225", 0.0, 0.0, 225.0) },
84 { 6, make_orientation("YAW_270", 0.0, 0.0, 270.0) },
85 { 7, make_orientation("YAW_315", 0.0, 0.0, 315.0) },
86 { 8, make_orientation("ROLL_180", 180.0, 0.0, 0.0) },
87 { 9, make_orientation("ROLL_180_YAW_45", 180.0, 0.0, 45.0) },
88 { 10, make_orientation("ROLL_180_YAW_90", 180.0, 0.0, 90.0) },
89 { 11, make_orientation("ROLL_180_YAW_135", 180.0, 0.0, 135.0) },
90 { 12, make_orientation("PITCH_180", 0.0, 180.0, 0.0) },
91 { 13, make_orientation("ROLL_180_YAW_225", 180.0, 0.0, 225.0) },
92 { 14, make_orientation("ROLL_180_YAW_270", 180.0, 0.0, 270.0) },
93 { 15, make_orientation("ROLL_180_YAW_315", 180.0, 0.0, 315.0) },
94 { 16, make_orientation("ROLL_90", 90.0, 0.0, 0.0) },
95 { 17, make_orientation("ROLL_90_YAW_45", 90.0, 0.0, 45.0) },
96 { 18, make_orientation("ROLL_90_YAW_90", 90.0, 0.0, 90.0) },
97 { 19, make_orientation("ROLL_90_YAW_135", 90.0, 0.0, 135.0) },
98 { 20, make_orientation("ROLL_270", 270.0, 0.0, 0.0) },
99 { 21, make_orientation("ROLL_270_YAW_45", 270.0, 0.0, 45.0) },
100 { 22, make_orientation("ROLL_270_YAW_90", 270.0, 0.0, 90.0) },
101 { 23, make_orientation("ROLL_270_YAW_135", 270.0, 0.0, 135.0) },
102 { 24, make_orientation("PITCH_90", 0.0, 90.0, 0.0) },
103 { 25, make_orientation("PITCH_270", 0.0, 270.0, 0.0) },
104 { 26, make_orientation("PITCH_180_YAW_90", 0.0, 180.0, 90.0) },
105 { 27, make_orientation("PITCH_180_YAW_270", 0.0, 180.0, 270.0) },
106 { 28, make_orientation("ROLL_90_PITCH_90", 90.0, 90.0, 0.0) },
107 { 29, make_orientation("ROLL_180_PITCH_90", 180.0, 90.0, 0.0) },
108 { 30, make_orientation("ROLL_270_PITCH_90", 270.0, 90.0, 0.0) },
109 { 31, make_orientation("ROLL_90_PITCH_180", 90.0, 180.0, 0.0) },
110 { 32, make_orientation("ROLL_270_PITCH_180", 270.0, 180.0, 0.0) },
111 { 33, make_orientation("ROLL_90_PITCH_270", 90.0, 270.0, 0.0) },
112 { 34, make_orientation("ROLL_180_PITCH_270", 180.0, 270.0, 0.0) },
113 { 35, make_orientation("ROLL_270_PITCH_270", 270.0, 270.0, 0.0) },
114 { 36, make_orientation("ROLL_90_PITCH_180_YAW_90", 90.0, 180.0, 90.0) },
115 { 37, make_orientation("ROLL_90_YAW_270", 90.0, 0.0, 270.0) },
116 { 38, make_orientation("ROLL_90_PITCH_68_YAW_293", 90.0, 68.0, 293.0) },
117 { 39, make_orientation("PITCH_315", 0.0, 315.0, 0.0) },
118 { 40, make_orientation("ROLL_90_PITCH_315", 90.0, 315.0, 0.0) },
119 { 100, make_orientation("CUSTOM", 0.0, 0.0, 0.0) },
120 }};
121 // [[[end]]] (checksum: 92ae7d7d313456c3645ce7c189d2b6df)
122 
123 
124 std::string to_string(MAV_SENSOR_ORIENTATION orientation)
125 {
126  const auto idx = enum_value(orientation);
127  auto it = sensor_orientations.find(idx);
128 
129  if (it == sensor_orientations.end()) {
130  ROS_ERROR_NAMED("uas", "SENSOR: wrong orientation index: %d", idx);
131  return std::to_string(idx);
132  }
133 
134  return it->second.first;
135 }
136 
137 Eigen::Quaterniond sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation)
138 {
139  const auto idx = enum_value(orientation);
140  auto it = sensor_orientations.find(idx);
141 
142  if (it == sensor_orientations.end()) {
143  ROS_ERROR_NAMED("uas", "SENSOR: wrong orientation index: %d", idx);
144  return Eigen::Quaterniond::Identity();
145  }
146 
147  return it->second.second;
148 }
149 
150 int sensor_orientation_from_str(const std::string &sensor_orientation)
151 {
152  // XXX bsearch
153 
154  // 1. try to find by name
155  for (const auto& kv : sensor_orientations) {
156  if (kv.second.first == sensor_orientation)
157  return kv.first;
158  }
159 
160  // 2. try convert integer
161  // fallback for old configs that uses numeric orientation.
162  try {
163  int idx = std::stoi(sensor_orientation, 0, 0);
164  if (0 > idx || sensor_orientations.find(idx) == sensor_orientations.end()) {
165  ROS_ERROR_NAMED("uas", "SENSOR: orientation index out of bound: %d", idx);
166  return -1;
167  }
168  else
169  return idx;
170  }
171  catch (std::invalid_argument &ex) {
172  // failed
173  }
174 
175  ROS_ERROR_STREAM_NAMED("uas", "SENSOR: wrong orientation str: " << sensor_orientation);
176 
177  return -1;
178 }
179 
180 } // namespace utils
181 } // namespace mavros
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
mavros::utils::to_string
std::string to_string(MAV_SENSOR_ORIENTATION orientation)
Definition: enum_sensor_orientation.cpp:124
mavros::utils::sensor_orientation_matching
Eigen::Quaterniond sensor_orientation_matching(mavlink::common::MAV_SENSOR_ORIENTATION orientation)
Function to match the received orientation received by MAVLink msg and the rotation of the sensor rel...
mavros::utils::make_orientation
static const OrientationPair make_orientation(const std::string &name, const double roll, const double pitch, const double yaw)
Definition: enum_sensor_orientation.cpp:31
mavros::utils::to_string
std::string to_string(timesync_mode e)
Definition: enum_to_string.cpp:316
mavros::utils::OrientationPair
std::pair< const std::string, const Eigen::Quaterniond > OrientationPair
Definition: enum_sensor_orientation.cpp:28
mavros::ftf::quaternion_from_rpy
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Convert euler angles to quaternion.
Definition: ftf_quaternion_utils.cpp:27
mavros::utils::sensor_orientation_from_str
int sensor_orientation_from_str(const std::string &sensor_orientation)
Retrieve sensor orientation number from alias name.
Definition: enum_sensor_orientation.cpp:150
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
frame_tf.h
Frame transformation utilities.
mavros::utils::sensor_orientations
static const std::unordered_map< typename std::underlying_type< MAV_SENSOR_ORIENTATION >::type, const OrientationPair > sensor_orientations
Definition: enum_sensor_orientation.cpp:77
mavros::utils::enum_value
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55

mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 13 2023 02:17:50