navigation_utility.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * navigation_utility.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
41 #include <tf/tf.h>
42 
44 
45 #include <cmath>
46 
47 namespace mbf_utility
48 {
49 
50 bool getRobotPose(const TF &tf,
51  const std::string &robot_frame,
52  const std::string &global_frame,
53  const ros::Duration &timeout,
54  geometry_msgs::PoseStamped &robot_pose)
55 {
56  geometry_msgs::PoseStamped local_pose;
57  local_pose.header.frame_id = robot_frame;
58  local_pose.header.stamp = ros::Time(0); // most recent available
59  local_pose.pose.orientation.w = 1.0;
60  bool success = transformPose(tf,
61  global_frame,
62  timeout,
63  local_pose,
64  robot_pose);
65  if (success && ros::Time::now() - robot_pose.header.stamp > timeout)
66  {
67  ROS_WARN("Most recent robot pose is %gs old (tolerance %gs)",
68  (ros::Time::now() - robot_pose.header.stamp).toSec(), timeout.toSec());
69  return false;
70  }
71  return success;
72 }
73 
80 static bool isNormalized(const geometry_msgs::Quaternion& _q, double _epsilon)
81 {
82  const double sq_sum = std::pow(_q.x, 2) + std::pow(_q.y, 2) + std::pow(_q.z, 2) + std::pow(_q.w, 2);
83  return std::abs(sq_sum - 1.) <= _epsilon;
84 }
85 
86 bool transformPose(const TF &tf,
87  const std::string &target_frame,
88  const ros::Duration &timeout,
89  const geometry_msgs::PoseStamped &in,
90  geometry_msgs::PoseStamped &out)
91 {
92  // Note: The tf-library does not check if the input is well formed.
93  if (!isNormalized(in.pose.orientation, 0.01))
94  {
95  ROS_WARN_STREAM("The given quaterinon " << in.pose.orientation << " is not normalized");
96  return false;
97  }
98 
99  std::string error_msg;
100 
101 #ifdef USE_OLD_TF
102  bool success = tf.waitForTransform(target_frame,
103  in.header.frame_id,
104  in.header.stamp,
105  timeout,
106  ros::Duration(0.01),
107  &error_msg);
108 #else
109  bool success = tf.canTransform(target_frame,
110  in.header.frame_id,
111  in.header.stamp,
112  timeout,
113  &error_msg);
114 #endif
115 
116  if (!success)
117  {
118  ROS_WARN_STREAM("Failed to look up transform from frame '" << in.header.frame_id << "' into frame '" << target_frame
119  << "': " << error_msg);
120  return false;
121  }
122 
123  try
124  {
125 #ifdef USE_OLD_TF
126  tf.transformPose(target_frame, in, out);
127 #else
128  tf.transform(in, out, target_frame);
129 #endif
130  }
131  catch (const TFException &ex)
132  {
133  ROS_WARN_STREAM("Failed to transform pose from frame '" << in.header.frame_id << " ' into frame '"
134  << target_frame << "' with exception: " << ex.what());
135  return false;
136  }
137  return true;
138 }
139 
140 bool transformPoint(const TF &tf,
141  const std::string &target_frame,
142  const ros::Duration &timeout,
143  const geometry_msgs::PointStamped &in,
144  geometry_msgs::PointStamped &out)
145 {
146  std::string error_msg;
147 
148 #ifdef USE_OLD_TF
149  bool success = tf.waitForTransform(target_frame,
150  in.header.frame_id,
151  in.header.stamp,
152  timeout,
153  ros::Duration(0.01),
154  &error_msg);
155 #else
156  bool success = tf.canTransform(target_frame,
157  in.header.frame_id,
158  in.header.stamp,
159  timeout,
160  &error_msg);
161 #endif
162 
163  if (!success)
164  {
165  ROS_WARN_STREAM("Failed to look up transform from frame '" << in.header.frame_id << "' into frame '" << target_frame
166  << "': " << error_msg);
167  return false;
168  }
169 
170  try
171  {
172 #ifdef USE_OLD_TF
173  tf.transformPoint(target_frame, in, out);
174 #else
175  tf.transform(in, out, target_frame);
176 #endif
177  }
178  catch (const TFException &ex)
179  {
180  ROS_WARN_STREAM("Failed to transform point from frame '" << in.header.frame_id << " ' into frame '"
181  << target_frame << "' with exception: " << ex.what());
182  return false;
183  }
184  return true;
185 }
186 
187 double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
188 {
189  const geometry_msgs::Point &p1 = pose1.pose.position;
190  const geometry_msgs::Point &p2 = pose2.pose.position;
191  const double dx = p1.x - p2.x;
192  const double dy = p1.y - p2.y;
193  const double dz = p1.z - p2.z;
194  return sqrt(dx * dx + dy * dy + dz * dz);
195 }
196 
197 double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
198 {
199  const geometry_msgs::Quaternion &q1 = pose1.pose.orientation;
200  const geometry_msgs::Quaternion &q2 = pose2.pose.orientation;
201  tf::Quaternion rot1, rot2;
202  tf::quaternionMsgToTF(q1, rot1);
203  tf::quaternionMsgToTF(q2, rot2);
204  return rot1.angleShortestPath(rot2);
205 }
206 
207 } /* namespace mbf_utility */
mbf_utility::distance
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
Computes the Euclidean-distance between two poses.
Definition: navigation_utility.cpp:187
tf::quaternionMsgToTF
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
tf::Quaternion::angleShortestPath
tfScalar angleShortestPath(const Quaternion &q) const
mbf_utility::isNormalized
static bool isNormalized(const geometry_msgs::Quaternion &_q, double _epsilon)
Returns true, if the given quaternion is normalized.
Definition: navigation_utility.cpp:80
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
mbf_utility::transformPose
bool transformPose(const TF &tf, const std::string &target_frame, const ros::Duration &timeout, const geometry_msgs::PoseStamped &in, geometry_msgs::PoseStamped &out)
Transforms a pose from one frame into another.
Definition: navigation_utility.cpp:86
mbf_utility::angle
double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
computes the smallest angle between two poses.
Definition: navigation_utility.cpp:197
ROS_WARN
#define ROS_WARN(...)
mbf_utility::transformPoint
bool transformPoint(const TF &tf, const std::string &target_frame, const ros::Duration &timeout, const geometry_msgs::PointStamped &in, geometry_msgs::PointStamped &out)
Transforms a point from one frame into another.
Definition: navigation_utility.cpp:140
mbf_utility
Definition: exe_path_exception.h:7
tf.h
ros::Time
mbf_utility::getRobotPose
bool getRobotPose(const TF &tf, const std::string &robot_frame, const std::string &global_frame, const ros::Duration &timeout, geometry_msgs::PoseStamped &robot_pose)
Computes the robot pose.
Definition: navigation_utility.cpp:50
tf::TransformListener
tf
DurationBase< Duration >::toSec
double toSec() const
tf2::TransformException
ros::Duration
tf::Quaternion
navigation_utility.h
ros::Time::now
static Time now()


mbf_utility
Author(s): Sebastian Pütz
autogenerated on Wed Mar 2 2022 00:33:44