geometry_msgs.hpp
Go to the documentation of this file.
1 #pragma once
2 
15 #include <vector>
16 
17 #include <geometry_msgs/Point.h>
18 #include <geometry_msgs/Point32.h>
19 #include <geometry_msgs/Quaternion.h>
20 #include <geometry_msgs/Transform.h>
21 #include <geometry_msgs/Vector3.h>
22 
23 namespace cras {
24 
25 #define DEFINE_CONVERTING_GET_PARAM_ROS_VECTOR3(resultType, scalarType, defaultUnit) \
26 DEFINE_CONVERTING_GET_PARAM(resultType, std::vector<scalarType>, defaultUnit, [](const ::std::vector<scalarType>& v){ \
27  if (v.size() != 3) \
28  throw ::std::runtime_error( \
29  ::cras::format("Cannot load %s parameter from an array of length %lu", #resultType, v.size())); \
30  resultType m; m.x = v[0]; m.y = v[1]; m.z = v[2]; \
31  return m; \
32 })
33 
34 DEFINE_CONVERTING_GET_PARAM_ROS_VECTOR3(::geometry_msgs::Vector3, double, "")
37 
38 DEFINE_CONVERTING_GET_PARAM(::geometry_msgs::Quaternion, ::std::vector<double>, "", [](const ::std::vector<double>& v) {
39  if (v.size() != 3 && v.size() != 4)
40  throw ::std::runtime_error(::cras::format("Cannot load %s parameter from an array of length %lu",
41  "geometry_msgs::Quaternion", v.size()));
42  ::geometry_msgs::Quaternion m;
43  if (v.size() == 4)
44  {
45  m.x = v[0]; m.y = v[1]; m.z = v[2]; m.w = v[3];
46  }
47  else
48  {
49  ::tf2::Quaternion q; q.setRPY(v[0], v[1], v[2]);
50  m.x = q.getX(); m.y = q.getY(); m.z = q.getZ(); m.w = q.getW();
51  }
52  return m;
53 })
54 
55 DEFINE_CONVERTING_GET_PARAM(::geometry_msgs::Transform, ::std::vector<double>, "", [](const ::std::vector<double>& v) {
56  if (v.size() != 6 && v.size() != 7 && v.size() != 16)
57  throw ::std::runtime_error(::cras::format("Cannot load %s parameter from an array of length %lu",
58  "geometry_msgs::Transform", v.size()));
59  ::geometry_msgs::Transform m;
60  if (v.size() == 6 || v.size() == 7)
61  {
62  m.translation.x = v[0]; m.translation.y = v[1]; m.translation.z = v[2];
63  if (v.size() == 6)
64  {
65  ::tf2::Quaternion q; q.setRPY(v[3], v[4], v[5]);
66  m.rotation.x = q.getX(); m.rotation.y = q.getY(); m.rotation.z = q.getZ(); m.rotation.w = q.getW();
67  }
68  else
69  {
70  m.rotation.x = v[3]; m.rotation.y = v[4]; m.rotation.z = v[5]; m.rotation.w = v[6];
71  }
72  }
73  else
74  {
75  m.translation.x = v[3]; m.translation.y = v[7]; m.translation.z = v[11];
76  ::tf2::Matrix3x3 r(v[0], v[1], v[2], v[4], v[5], v[6], v[8], v[9], v[10]);
78  r.getRotation(q);
79  m.rotation.x = q.getX(); m.rotation.y = q.getY(); m.rotation.z = q.getZ(); m.rotation.w = q.getW();
80  }
81  return m;
82 })
83 
84 }
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
#define DEFINE_CONVERTING_GET_PARAM_ROS_VECTOR3(resultType, scalarType, defaultUnit)
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
inline ::std::string format(const char *format, ::va_list args)
Definition: any.hpp:15
void getRotation(Quaternion &q) const
DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector3d, ::std::vector< double >, "", [](const ::std::vector< double > &v){ checkSize(v, 3, "vector");return ::Eigen::Vector3d(v.data());}) DEFINE_CONVERTING_GET_PARAM(


cras_cpp_common
Author(s): Martin Pecka
autogenerated on Sat Jun 17 2023 02:32:53