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