mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
msg_extractor.h
Go to the documentation of this file.
1 // clang: TomasFormat
6 #ifndef MRS_LIB_MSG_EXTRACTOR_H
7 #define MRS_LIB_MSG_EXTRACTOR_H
8 
9 #include <mrs_msgs/TrackerCommand.h>
10 #include <mrs_msgs/Reference.h>
11 #include <mrs_msgs/ReferenceStamped.h>
12 
13 #include <nav_msgs/Odometry.h>
14 
15 #include <mrs_lib/attitude_converter.h>
16 
17 namespace mrs_lib
18 {
19 
20 /* geometry_msgs::Point //{ */
21 
29 std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
30 
31  double x = data.x;
32  double y = data.y;
33  double z = data.z;
34 
35  return std::tuple(x, y, z);
36 }
37 
45 std::tuple<double, double, double> getXYZ(const geometry_msgs::PointConstPtr& data) {
46 
47  return getXYZ(*data);
48 }
49 
50 //}
51 
52 /* geometry_msgs::Vector3 //{ */
53 
61 std::tuple<double, double, double> getXYZ(const geometry_msgs::Vector3& data) {
62 
63  double x = data.x;
64  double y = data.y;
65  double z = data.z;
66 
67  return std::tuple(x, y, z);
68 }
69 
77 std::tuple<double, double, double> getXYZ(const geometry_msgs::Vector3ConstPtr& data) {
78 
79  return getXYZ(*data);
80 }
81 
82 //}
83 
84 /* geometry_msgs::Pose //{ */
85 
86 /* getPosition() //{ */
87 
95 std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
96 
97  return getXYZ(data.position);
98 }
99 
107 std::tuple<double, double, double> getPosition(const geometry_msgs::PoseConstPtr& data) {
108 
109  return getPosition(*data);
110 }
111 
112 //}
113 
114 /* getHeading() //{ */
115 
123 double getHeading(const geometry_msgs::Pose& data) {
124 
125  return mrs_lib::AttitudeConverter(data.orientation).getHeading();
126 }
127 
135 double getHeading(const geometry_msgs::PoseConstPtr& data) {
136 
137  return getHeading(*data);
138 }
139 
140 //}
141 
142 /* getYaw() //{ */
143 
151 double getYaw(const geometry_msgs::Pose& data) {
152 
153  return mrs_lib::AttitudeConverter(data.orientation).getYaw();
154 }
155 
163 double getYaw(const geometry_msgs::PoseConstPtr& data) {
164 
165  return getYaw(*data);
166 }
167 
168 //}
169 
170 //}
171 
172 /* geometry_msgs::PoseWithCovariance //{ */
173 
174 /* getPosition() //{ */
175 
183 std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
184 
185  return getPosition(data.pose);
186 }
187 
195 std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovarianceConstPtr& data) {
196 
197  return getPosition(*data);
198 }
199 
200 //}
201 
202 /* getHeading() //{ */
203 
211 double getHeading(const geometry_msgs::PoseWithCovariance& data) {
212 
213  return getHeading(data.pose);
214 }
215 
223 double getHeading(const geometry_msgs::PoseWithCovarianceConstPtr& data) {
224 
225  return getHeading(*data);
226 }
227 
228 //}
229 
230 /* getYaw() //{ */
231 
239 double getYaw(const geometry_msgs::PoseWithCovariance& data) {
240 
241  return getYaw(data.pose);
242 }
243 
251 double getYaw(const geometry_msgs::PoseWithCovarianceConstPtr& data) {
252 
253  return getYaw(*data);
254 }
255 
256 //}
257 
258 //}
259 
260 /* geometry_msgs::Twist //{ */
261 
262 /* getVelocity() //{ */
263 
271 std::tuple<double, double, double> getVelocity(const geometry_msgs::Twist& data) {
272 
273  return getXYZ(data.linear);
274 }
275 
283 std::tuple<double, double, double> getPosition(const geometry_msgs::TwistConstPtr& data) {
284 
285  return getVelocity(*data);
286 }
287 
288 //}
289 
290 //}
291 
292 /* geometry_msgs::TwistWithCovariance //{ */
293 
294 /* getVelocity() //{ */
295 
303 std::tuple<double, double, double> getVelocity(const geometry_msgs::TwistWithCovariance& data) {
304 
305  return getVelocity(data.twist);
306 }
307 
315 std::tuple<double, double, double> getPosition(const geometry_msgs::TwistWithCovarianceConstPtr& data) {
316 
317  return getVelocity(*data);
318 }
319 
320 //}
321 
322 //}
323 
324 /* nav_msgs::Odometry //{ */
325 
326 /* getPosition() //{ */
327 
335 std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
336 
337  return getPosition(data.pose);
338 }
339 
347 std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
348 
349  return getPosition(*data);
350 }
351 
352 //}
353 
354 /* getVelocity() //{ */
355 
363 std::tuple<double, double, double> getVelocity(const nav_msgs::Odometry& data) {
364 
365  return getVelocity(data.twist);
366 }
367 
375 std::tuple<double, double, double> getVelocity(const nav_msgs::OdometryConstPtr& data) {
376 
377  return getVelocity(*data);
378 }
379 
380 //}
381 
382 /* getHeading() //{ */
383 
391 double getHeading(const nav_msgs::Odometry& data) {
392 
393  return getHeading(data.pose);
394 }
395 
403 double getHeading(const nav_msgs::OdometryConstPtr& data) {
404 
405  return getHeading(*data);
406 }
407 
408 //}
409 
410 /* getYaw() //{ */
411 
419 double getYaw(const nav_msgs::Odometry& data) {
420 
421  return getYaw(data.pose);
422 }
423 
431 double getYaw(const nav_msgs::OdometryConstPtr& data) {
432 
433  return getYaw(*data);
434 }
435 
436 //}
437 
438 /* getPose() //{ */
439 
447 geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
448 
449  return data.pose.pose;
450 }
451 
459 geometry_msgs::Pose getPose(const nav_msgs::OdometryConstPtr& data) {
460 
461  return getPose(*data);
462 }
463 
464 //}
465 
466 //}
467 
468 /* mrs_msgs::TrackerCommand //{ */
469 
470 /* getPosition() //{ */
471 
479 std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
480 
481  return getXYZ(data.position);
482 }
483 
491 std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommandConstPtr& data) {
492 
493  return getPosition(*data);
494 }
495 
496 //}
497 
498 /* getVelocity() //{ */
499 
507 std::tuple<double, double, double> getVelocity(const mrs_msgs::TrackerCommand& data) {
508 
509  return getXYZ(data.velocity);
510 }
511 
519 std::tuple<double, double, double> getVelocity(const mrs_msgs::TrackerCommandConstPtr& data) {
520 
521  return getVelocity(*data);
522 }
523 
524 //}
525 
526 /* getHeading() //{ */
527 
535 double getHeading(const mrs_msgs::TrackerCommand& data) {
536 
537  double heading = 0;
538 
539  if (data.use_heading) {
540 
541  heading = data.heading;
542 
543  } else if (data.use_orientation) {
544 
545  heading = mrs_lib::AttitudeConverter(data.orientation).getHeading();
546  }
547 
548  return heading;
549 }
550 
558 double getHeading(const mrs_msgs::TrackerCommandConstPtr& data) {
559 
560  return getHeading(*data);
561 }
562 
563 //}
564 
565 //}
566 
567 /* mrs_msgs::Reference //{ */
568 
569 /* getPosition() //{ */
570 
578 std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
579 
580  return getXYZ(data.position);
581 }
582 
590 std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceConstPtr& data) {
591 
592  return getPosition(*data);
593 }
594 
595 //}
596 
597 /* getHeading() //{ */
598 
606 double getHeading(const mrs_msgs::Reference& data) {
607 
608  return data.heading;
609 }
610 
618 double getHeading(const mrs_msgs::ReferenceConstPtr& data) {
619 
620  return getHeading(*data);
621 }
622 
623 //}
624 
625 //}
626 
627 /* mrs_msgs::ReferenceStamped //{ */
628 
629 /* getPosition() //{ */
630 
638 std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
639 
640  return getPosition(data.reference);
641 }
642 
650 std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStampedConstPtr& data) {
651 
652  return getPosition(*data);
653 }
654 
655 //}
656 
657 /* getHeading() //{ */
658 
666 double getHeading(const mrs_msgs::ReferenceStamped& data) {
667 
668  return getHeading(data.reference);
669 }
670 
678 double getHeading(const mrs_msgs::ReferenceStampedConstPtr& data) {
679 
680  return getHeading(*data);
681 }
682 
683 //}
684 
685 //}
686 
687 } // namespace mrs_lib
688 
689 //}
690 
691 #endif // MRS_LIB_MSG_EXTRACTOR_H
mrs_lib::AttitudeConverter::getHeading
double getHeading(void)
get the angle of the rotated x-axis in the original XY plane, a.k.a
Definition: attitude_converter.cpp:254
mrs_lib::getXYZ
std::tuple< double, double, double > getXYZ(const geometry_msgs::Point &data)
get XYZ from geometry_msgs::Point
Definition: msg_extractor.h:29
attitude_converter.h
Conversions between various representations of object attitude in 3D. Supports Quaternions,...
mrs_lib::getPosition
std::tuple< double, double, double > getPosition(const geometry_msgs::Pose &data)
get position from geometry_msgs::Pose
Definition: msg_extractor.h:95
mrs_lib::AttitudeConverter::getYaw
double getYaw(void)
get the yaw angle
Definition: attitude_converter.cpp:233
mrs_lib::getVelocity
std::tuple< double, double, double > getVelocity(const geometry_msgs::Twist &data)
get velocity from geometry_msgs::Twist
Definition: msg_extractor.h:271
mrs_lib::AttitudeConverter
The main convertor class. Instantiate with any type in constructor and get the value in any other typ...
Definition: attitude_converter.h:153
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition: attitude_converter.h:29
mrs_lib::getHeading
double getHeading(const geometry_msgs::Pose &data)
get heading from geometry_msgs::Pose
Definition: msg_extractor.h:123
mrs_lib::getYaw
double getYaw(const geometry_msgs::Pose &data)
get yaw from geometry_msgs::Pose
Definition: msg_extractor.h:151
mrs_lib::getPose
geometry_msgs::Pose getPose(const nav_msgs::Odometry &data)
returns the Pose part of the nav_msgs::Odometry message
Definition: msg_extractor.h:447