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  if (node.path.find("ViewPose") == std::string::npos)
39  {
40  // The ensenso SDK 2.2.x has a structure locked ViewPose item in the global params. So it cannot be set to null.
41  node.setNull();
42  }
43 
44  if (isValid(pose))
45  {
46  auto origin = pose.getOrigin();
47  node[itmTranslation][0] = origin.x() * 1000; // ROS transformation is in
48  // meters, NxLib expects it to
49  // be in millimeters.
50  node[itmTranslation][1] = origin.y() * 1000;
51  node[itmTranslation][2] = origin.z() * 1000;
52 
53  auto rotation = pose.getRotation();
54  node[itmRotation][itmAngle] = rotation.getAngle();
55 
56  auto rotationAxis = rotation.getAxis();
57  node[itmRotation][itmAxis][0] = rotationAxis.x();
58  node[itmRotation][itmAxis][1] = rotationAxis.y();
59  node[itmRotation][itmAxis][2] = rotationAxis.z();
60  }
61  else
62  {
63  ROS_ERROR("Given is pose is not valid for writing to the NxLib. Using identity transform");
64  // Use an identity transformation as a reasonable default value.
65  node[itmTranslation][0] = 0;
66  node[itmTranslation][1] = 0;
67  node[itmTranslation][2] = 0;
68 
69  node[itmRotation][itmAngle] = 0;
70  node[itmRotation][itmAxis][0] = 1;
71  node[itmRotation][itmAxis][1] = 0;
72  node[itmRotation][itmAxis][2] = 0;
73  }
74 }
75 
76 tf2::Transform poseFromNxLib(NxLibItem const& node)
77 {
78  tf2::Transform pose;
79  tf2::Vector3 origin;
80  origin.setX(node[itmTranslation][0].asDouble() / 1000); // NxLib
81  // transformation is
82  // in millimeters, ROS
83  // expects it to be in
84  // meters.
85  origin.setY(node[itmTranslation][1].asDouble() / 1000);
86  origin.setZ(node[itmTranslation][2].asDouble() / 1000);
87  pose.setOrigin(origin);
88 
89  tf2::Vector3 rotationAxis(node[itmRotation][itmAxis][0].asDouble(), node[itmRotation][itmAxis][1].asDouble(),
90  node[itmRotation][itmAxis][2].asDouble());
91  tf2::Quaternion rotation(rotationAxis, node[itmRotation][itmAngle].asDouble());
92  pose.setRotation(rotation);
93 
94  return pose;
95 }
96 
97 geometry_msgs::TransformStamped poseFromNxLib(NxLibItem const& node, std::string const& parentFrame,
98  std::string const& childFrame)
99 {
100  geometry_msgs::TransformStamped stampedTransform;
101  stampedTransform.header.stamp = ros::Time::now();
102  stampedTransform.header.frame_id = parentFrame;
103  stampedTransform.child_frame_id = childFrame;
104 
105  tf2::Transform transform = poseFromNxLib(node);
106  tf2::convert(transform, stampedTransform.transform);
107  return stampedTransform;
108 }
109 
110 geometry_msgs::TransformStamped transformFromPose(geometry_msgs::PoseStamped const& pose, std::string const& childFrame)
111 {
112  geometry_msgs::TransformStamped transform;
113 
114  transform.transform.translation.x = pose.pose.position.x;
115  transform.transform.translation.y = pose.pose.position.y;
116  transform.transform.translation.z = pose.pose.position.z;
117  transform.transform.rotation = pose.pose.orientation;
118 
119  transform.header.stamp = pose.header.stamp;
120  transform.header.frame_id = pose.header.frame_id;
121  transform.child_frame_id = childFrame;
122 
123  return transform;
124 }
125 
126 geometry_msgs::PoseStamped stampedPoseFromTransform(geometry_msgs::TransformStamped const& transform)
127 {
128  geometry_msgs::PoseStamped pose;
129 
130  pose.pose.position.x = transform.transform.translation.x;
131  pose.pose.position.y = transform.transform.translation.y;
132  pose.pose.position.z = transform.transform.translation.z;
133  pose.pose.orientation = transform.transform.rotation;
134 
135  pose.header.stamp = transform.header.stamp;
136  pose.header.frame_id = transform.header.frame_id;
137 
138  return pose;
139 }
140 
141 tf2::Transform fromStampedMessage(geometry_msgs::TransformStamped const& tStamped)
142 {
143  tf2::Transform transform;
144  tf2::Quaternion quat;
145  tf2::convert(tStamped.transform.rotation, quat);
146  transform.setRotation(quat);
147  tf2::Vector3 trans;
148  tf2::convert(tStamped.transform.translation, trans);
149  transform.setOrigin(trans);
150 
151  return transform;
152 }
153 
154 tf2::Transform fromStampedMessage(geometry_msgs::PoseStamped const& pStamped)
155 {
156  tf2::Transform transform;
157  tf2::Quaternion quat;
158  tf2::convert(pStamped.pose.orientation, quat);
159  transform.setRotation(quat);
160  tf2::Vector3 trans;
161  tf2::convert(pStamped.pose.position, trans);
162  transform.setOrigin(trans);
163 
164  return transform;
165 }
166 
167 tf2::Transform fromMsg(geometry_msgs::Transform const& t)
168 {
169  tf2::Transform transform;
170  tf2::Quaternion quat;
171  tf2::convert(t.rotation, quat);
172  transform.setRotation(quat);
173  tf2::Vector3 trans;
174  tf2::convert(t.translation, trans);
175  transform.setOrigin(trans);
176 
177  return transform;
178 }
179 
180 tf2::Transform fromMsg(geometry_msgs::Pose const& p)
181 {
182  tf2::Transform transform;
183  tf2::Quaternion quat;
184  tf2::convert(p.orientation, quat);
185  transform.setRotation(quat);
186  tf2::Vector3 trans;
187  tf2::convert(p.position, trans);
188  transform.setOrigin(trans);
189 
190  return transform;
191 }
192 
193 geometry_msgs::Pose poseFromTransform(tf2::Transform const& transform)
194 {
195  geometry_msgs::Pose pose;
196  tf2::convert(transform.getRotation(), pose.orientation);
197  pose.position.x = transform.getOrigin().x();
198  pose.position.y = transform.getOrigin().y();
199  pose.position.z = transform.getOrigin().z();
200 
201  return pose;
202 }
203 
204 geometry_msgs::TransformStamped fromTfTransform(tf2::Transform const& transform, std::string parentFrame,
205  std::string childFrame)
206 {
207  geometry_msgs::TransformStamped tStamped;
208  tf2::convert(transform.getOrigin(), tStamped.transform.translation);
209  tf2::convert(transform.getRotation(), tStamped.transform.rotation);
210  tStamped.header.frame_id = std::move(parentFrame);
211  tStamped.child_frame_id = std::move(childFrame);
212  tStamped.header.stamp = ros::Time::now();
213 
214  return tStamped;
215 }
216 
217 tf2::Transform getLatestTransform(tf2_ros::Buffer const& tfBuffer, std::string const& cameraFrame,
218  std::string const& targetFrame)
219 {
220  tf2::Transform transform;
221  try
222  {
223  geometry_msgs::TransformStamped stTransform;
224  stTransform = tfBuffer.lookupTransform(cameraFrame, targetFrame, ros::Time(0));
225 
226  transform = fromMsg(stTransform.transform);
227  }
228  catch (tf2::TransformException& ex)
229  {
230  ROS_WARN("%s", ex.what());
231  }
232  return transform;
233 }
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 Thu Feb 6 2020 03:48:46