pose_utilities.cpp
Go to the documentation of this file.
2 
3 #include <string>
4 
5 bool isValid(tf2::Transform const& pose)
6 {
7  auto origin = pose.getOrigin();
8  if (!isValid(origin))
9  {
10  return false;
11  }
12 
13  auto rotation = pose.getRotation();
14  if (std::isnan(rotation.getAngle()))
15  {
16  return false;
17  }
18 
19  auto rotationAxis = rotation.getAxis();
20  return isValid(rotationAxis);
21 }
22 
23 bool isValid(tf2::Vector3 const& vector)
24 {
25  return (!std::isnan(vector.x()) && !std::isnan(vector.y()) && !std::isnan(vector.z()));
26 }
27 
28 bool isValid(geometry_msgs::Transform const& pose)
29 {
30  return isValid(fromMsg(pose));
31 }
32 
33 void writePoseToNxLib(tf2::Transform const& pose, NxLibItem const& node)
34 {
35  // Initialize the node to be empty. This is necessary, because there is a bug in some versions of the NxLib that
36  // overwrites the whole transformation node with an identity transformation as soon as a new node in /Links gets
37  // created.
38  node.setNull();
39 
40  if (isValid(pose))
41  {
42  auto origin = pose.getOrigin();
43  node[itmTranslation][0] = origin.x() * 1000; // ROS transformation is in
44  // meters, NxLib expects it to
45  // be in millimeters.
46  node[itmTranslation][1] = origin.y() * 1000;
47  node[itmTranslation][2] = origin.z() * 1000;
48 
49  auto rotation = pose.getRotation();
50  node[itmRotation][itmAngle] = rotation.getAngle();
51 
52  auto rotationAxis = rotation.getAxis();
53  node[itmRotation][itmAxis][0] = rotationAxis.x();
54  node[itmRotation][itmAxis][1] = rotationAxis.y();
55  node[itmRotation][itmAxis][2] = rotationAxis.z();
56  }
57  else
58  {
59  ROS_ERROR("Given is pose is not valid for writing to the NxLib. Using identity transform");
60  // Use an identity transformation as a reasonable default value.
61  node[itmTranslation][0] = 0;
62  node[itmTranslation][1] = 0;
63  node[itmTranslation][2] = 0;
64 
65  node[itmRotation][itmAngle] = 0;
66  node[itmRotation][itmAxis][0] = 1;
67  node[itmRotation][itmAxis][1] = 0;
68  node[itmRotation][itmAxis][2] = 0;
69  }
70 }
71 
72 tf2::Transform poseFromNxLib(NxLibItem const& node)
73 {
74  tf2::Transform pose;
75  tf2::Vector3 origin;
76  origin.setX(node[itmTranslation][0].asDouble() / 1000); // NxLib
77  // transformation is
78  // in millimeters, ROS
79  // expects it to be in
80  // meters.
81  origin.setY(node[itmTranslation][1].asDouble() / 1000);
82  origin.setZ(node[itmTranslation][2].asDouble() / 1000);
83  pose.setOrigin(origin);
84 
85  tf2::Vector3 rotationAxis(node[itmRotation][itmAxis][0].asDouble(), node[itmRotation][itmAxis][1].asDouble(),
86  node[itmRotation][itmAxis][2].asDouble());
87  tf2::Quaternion rotation(rotationAxis, node[itmRotation][itmAngle].asDouble());
88  pose.setRotation(rotation);
89 
90  return pose;
91 }
92 
93 geometry_msgs::TransformStamped poseFromNxLib(NxLibItem const& node, std::string const& parentFrame,
94  std::string const& childFrame)
95 {
96  geometry_msgs::TransformStamped stampedTransform;
97  stampedTransform.header.stamp = ros::Time::now();
98  stampedTransform.header.frame_id = parentFrame;
99  stampedTransform.child_frame_id = childFrame;
100 
101  tf2::Transform transform = poseFromNxLib(node);
102  tf2::convert(transform, stampedTransform.transform);
103  return stampedTransform;
104 }
105 
106 geometry_msgs::TransformStamped transformFromPose(geometry_msgs::PoseStamped const& pose, std::string const& childFrame)
107 {
108  geometry_msgs::TransformStamped transform;
109 
110  transform.transform.translation.x = pose.pose.position.x;
111  transform.transform.translation.y = pose.pose.position.y;
112  transform.transform.translation.z = pose.pose.position.z;
113  transform.transform.rotation = pose.pose.orientation;
114 
115  transform.header.stamp = pose.header.stamp;
116  transform.header.frame_id = pose.header.frame_id;
117  transform.child_frame_id = childFrame;
118 
119  return transform;
120 }
121 
122 geometry_msgs::PoseStamped stampedPoseFromTransform(geometry_msgs::TransformStamped const& transform)
123 {
124  geometry_msgs::PoseStamped pose;
125 
126  pose.pose.position.x = transform.transform.translation.x;
127  pose.pose.position.y = transform.transform.translation.y;
128  pose.pose.position.z = transform.transform.translation.z;
129  pose.pose.orientation = transform.transform.rotation;
130 
131  pose.header.stamp = transform.header.stamp;
132  pose.header.frame_id = transform.header.frame_id;
133 
134  return pose;
135 }
136 
137 tf2::Transform fromStampedMessage(geometry_msgs::TransformStamped const& tStamped)
138 {
139  tf2::Transform transform;
140  tf2::Quaternion quat;
141  tf2::convert(tStamped.transform.rotation, quat);
142  transform.setRotation(quat);
143  tf2::Vector3 trans;
144  tf2::convert(tStamped.transform.translation, trans);
145  transform.setOrigin(trans);
146 
147  return transform;
148 }
149 
150 tf2::Transform fromStampedMessage(geometry_msgs::PoseStamped const& pStamped)
151 {
152  tf2::Transform transform;
153  tf2::Quaternion quat;
154  tf2::convert(pStamped.pose.orientation, quat);
155  transform.setRotation(quat);
156  tf2::Vector3 trans;
157  tf2::convert(pStamped.pose.position, trans);
158  transform.setOrigin(trans);
159 
160  return transform;
161 }
162 
163 tf2::Transform fromMsg(geometry_msgs::Transform const& t)
164 {
165  tf2::Transform transform;
166  tf2::Quaternion quat;
167  tf2::convert(t.rotation, quat);
168  transform.setRotation(quat);
169  tf2::Vector3 trans;
170  tf2::convert(t.translation, trans);
171  transform.setOrigin(trans);
172 
173  return transform;
174 }
175 
176 tf2::Transform fromMsg(geometry_msgs::Pose const& p)
177 {
178  tf2::Transform transform;
179  tf2::Quaternion quat;
180  tf2::convert(p.orientation, quat);
181  transform.setRotation(quat);
182  tf2::Vector3 trans;
183  tf2::convert(p.position, trans);
184  transform.setOrigin(trans);
185 
186  return transform;
187 }
188 
189 geometry_msgs::Pose poseFromTransform(tf2::Transform const& transform)
190 {
191  geometry_msgs::Pose pose;
192  tf2::convert(transform.getRotation(), pose.orientation);
193  pose.position.x = transform.getOrigin().x();
194  pose.position.y = transform.getOrigin().y();
195  pose.position.z = transform.getOrigin().z();
196 
197  return pose;
198 }
199 
200 geometry_msgs::TransformStamped fromTfTransform(tf2::Transform const& transform, std::string parentFrame,
201  std::string childFrame)
202 {
203  geometry_msgs::TransformStamped tStamped;
204  tf2::convert(transform.getOrigin(), tStamped.transform.translation);
205  tf2::convert(transform.getRotation(), tStamped.transform.rotation);
206  tStamped.header.frame_id = std::move(parentFrame);
207  tStamped.child_frame_id = std::move(childFrame);
208  tStamped.header.stamp = ros::Time::now();
209 
210  return tStamped;
211 }
212 
213 tf2::Transform getLatestTransform(tf2_ros::Buffer const& tfBuffer, std::string const& cameraFrame,
214  std::string const& targetFrame)
215 {
216  tf2::Transform transform;
217  try
218  {
219  geometry_msgs::TransformStamped stTransform;
220  stTransform = tfBuffer.lookupTransform(cameraFrame, targetFrame, ros::Time(0));
221 
222  transform = fromMsg(stTransform.transform);
223  }
224  catch (tf2::TransformException& ex)
225  {
226  ROS_WARN("%s", ex.what());
227  }
228  return transform;
229 }
tf2::Transform fromStampedMessage(geometry_msgs::TransformStamped const &tStamped)
geometry_msgs::Pose poseFromTransform(tf2::Transform const &transform)
Quaternion getRotation() const
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf2::Transform getLatestTransform(tf2_ros::Buffer const &tfBuffer, std::string const &cameraFrame, std::string const &targetFrame)
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
geometry_msgs::TransformStamped fromTfTransform(tf2::Transform const &transform, std::string parentFrame, std::string childFrame)
geometry_msgs::TransformStamped transformFromPose(geometry_msgs::PoseStamped const &pose, std::string const &childFrame)
#define ROS_WARN(...)
tf2::Transform fromMsg(geometry_msgs::Transform const &t)
tf2Scalar getAngle() const
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
geometry_msgs::PoseStamped stampedPoseFromTransform(geometry_msgs::TransformStamped const &transform)
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
bool isValid(tf2::Transform const &pose)
void writePoseToNxLib(tf2::Transform const &pose, NxLibItem const &node)
Vector3 getAxis() const
static Time now()
void convert(const A &a, B &b)
tf2::Transform poseFromNxLib(NxLibItem const &node)
#define ROS_ERROR(...)


ensenso_camera
Author(s): Ensenso
autogenerated on Sat Jul 27 2019 03:51:24