Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
robot_utils.cpp
1 // Copyright (c) 2018 Intel Corporation
2 // Copyright (c) 2019 Steven Macenski
3 // Copyright (c) 2019 Samsung Research America
4 //
5 // Licensed under the Apache License, Version 2.0 (the "License");
6 // you may not use this file except in compliance with the License.
7 // You may obtain a copy of the License at
8 //
9 // http://www.apache.org/licenses/LICENSE-2.0
10 //
11 // Unless required by applicable law or agreed to in writing, software
12 // distributed under the License is distributed on an "AS IS" BASIS,
13 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 // See the License for the specific language governing permissions and
15 // limitations under the License.
16 
17 #include <string>
18 #include <cmath>
19 #include <memory>
20 
21 #include "tf2/convert.hpp"
22 #include "tf2/utils.hpp"
23 #include "nav2_ros_common/tf2_factories.hpp"
24 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
25 
26 #include "nav2_util/robot_utils.hpp"
27 #include "rclcpp/logger.hpp"
28 
29 namespace nav2_util
30 {
31 
32 bool lookupTransformWithStalenessCheck(
33  nav2::TransformBuffer & tf_buffer,
34  const std::string & target_frame,
35  const std::string & source_frame,
36  const rclcpp::Time & current_time,
37  double staleness_threshold,
38  geometry_msgs::msg::TransformStamped & transform)
39 {
40  if (target_frame == source_frame) {
41  geometry_msgs::msg::TransformStamped identity;
42  identity.header.frame_id = target_frame;
43  identity.header.stamp = current_time;
44  identity.child_frame_id = source_frame;
45  identity.transform.rotation.w = 1.0;
46  transform = identity;
47  return true;
48  }
49 
50  geometry_msgs::msg::TransformStamped latest_transform;
51  try {
52  latest_transform = tf_buffer.lookupTransform(
53  target_frame, source_frame, tf2::TimePointZero);
54  } catch (const tf2::TransformException & ex) {
55  RCLCPP_ERROR(
56  rclcpp::get_logger("lookupTransformWithStalenessCheck"),
57  "Failed to get transform: %s", ex.what());
58  return false;
59  }
60 
61  const bool has_timestamp =
62  latest_transform.header.stamp.sec != 0 || latest_transform.header.stamp.nanosec != 0;
63  if (staleness_threshold > 0.0 && has_timestamp) {
64  const auto transform_time = rclcpp::Time(
65  latest_transform.header.stamp, current_time.get_clock_type());
66  const double transform_age = (current_time - transform_time).seconds();
67  if (transform_age > staleness_threshold) {
68  RCLCPP_ERROR(
69  rclcpp::get_logger("lookupTransformWithStalenessCheck"),
70  "Transform from frame '%s' to frame '%s' is stale: age %fs exceeds threshold %fs",
71  source_frame.c_str(), target_frame.c_str(), transform_age, staleness_threshold);
72  return false;
73  }
74  }
75  transform = latest_transform;
76  return true;
77 }
78 
79 geometry_msgs::msg::PoseStamped transformToPoseStamped(
80  const geometry_msgs::msg::TransformStamped & transform)
81 {
82  geometry_msgs::msg::PoseStamped pose;
83  pose.header = transform.header;
84  pose.pose.position.x = transform.transform.translation.x;
85  pose.pose.position.y = transform.transform.translation.y;
86  pose.pose.position.z = transform.transform.translation.z;
87  pose.pose.orientation = transform.transform.rotation;
88  return pose;
89 }
90 
91 geometry_msgs::msg::TransformStamped poseToTransformStamped(
92  const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame)
93 {
94  geometry_msgs::msg::TransformStamped transform;
95  transform.header = pose.header;
96  transform.child_frame_id = child_frame;
97  transform.transform.translation.x = pose.pose.position.x;
98  transform.transform.translation.y = pose.pose.position.y;
99  transform.transform.translation.z = pose.pose.position.z;
100  transform.transform.rotation = pose.pose.orientation;
101  return transform;
102 }
103 
104 geometry_msgs::msg::TransformStamped invertTransform(
105  const geometry_msgs::msg::TransformStamped & transform)
106 {
107  tf2::Transform tf_transform;
108  tf2::fromMsg(transform.transform, tf_transform);
109  geometry_msgs::msg::TransformStamped inverse;
110  inverse.header.stamp = transform.header.stamp;
111  inverse.header.frame_id = transform.child_frame_id;
112  inverse.child_frame_id = transform.header.frame_id;
113  inverse.transform = tf2::toMsg(tf_transform.inverse());
114  return inverse;
115 }
116 
117 bool getFreshPose(
118  nav2::TransformBuffer & tf_buffer,
119  const std::string & target_frame,
120  const std::string & source_frame,
121  const rclcpp::Time & current_time,
122  double staleness_threshold,
123  geometry_msgs::msg::PoseStamped & pose)
124 {
125  geometry_msgs::msg::TransformStamped transform;
126  if (lookupTransformWithStalenessCheck(
127  tf_buffer, target_frame, source_frame, current_time, staleness_threshold, transform))
128  {
129  pose = transformToPoseStamped(transform);
130  return true;
131  }
132  return false;
133 }
134 
135 bool getCurrentPose(
136  geometry_msgs::msg::PoseStamped & global_pose,
137  nav2::TransformBuffer & tf_buffer, const std::string global_frame,
138  const std::string robot_frame, const double transform_timeout,
139  const rclcpp::Time stamp)
140 {
141  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
142  global_pose.header.frame_id = robot_frame;
143  global_pose.header.stamp = stamp;
144 
145  return transformPoseInTargetFrame(
146  global_pose, global_pose, tf_buffer, global_frame, transform_timeout);
147 }
148 
149 bool transformPoseInTargetFrame(
150  const geometry_msgs::msg::PoseStamped & input_pose,
151  geometry_msgs::msg::PoseStamped & transformed_pose,
152  nav2::TransformBuffer & tf_buffer, const std::string target_frame,
153  const double transform_timeout)
154 {
155  static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
156 
157  if (input_pose.header.frame_id == target_frame) {
158  transformed_pose = input_pose;
159  return true;
160  }
161 
162  try {
163  transformed_pose = tf_buffer.transform(
164  input_pose, target_frame,
165  tf2::durationFromSec(transform_timeout));
166  return true;
167  } catch (tf2::LookupException & ex) {
168  RCLCPP_ERROR(
169  logger,
170  "No Transform available Error looking up target frame: %s\n", ex.what());
171  } catch (tf2::ConnectivityException & ex) {
172  RCLCPP_ERROR(
173  logger,
174  "Connectivity Error looking up target frame: %s\n", ex.what());
175  } catch (tf2::ExtrapolationException & ex) {
176  RCLCPP_ERROR(
177  logger,
178  "Extrapolation Error looking up target frame: %s\n", ex.what());
179  } catch (tf2::TimeoutException & ex) {
180  RCLCPP_ERROR(
181  logger,
182  "Transform timeout with tolerance: %.4f", transform_timeout);
183  } catch (tf2::TransformException & ex) {
184  RCLCPP_ERROR(
185  logger, "Failed to transform from %s to %s",
186  input_pose.header.frame_id.c_str(), target_frame.c_str());
187  }
188 
189  return false;
190 }
191 
192 bool getTransform(
193  const std::string & source_frame_id,
194  const std::string & target_frame_id,
195  const tf2::Duration & transform_tolerance,
196  const nav2::TransformBuffer::SharedPtr tf_buffer,
197  geometry_msgs::msg::TransformStamped & transform_msg)
198 {
199  if (source_frame_id == target_frame_id) {
200  // We are already in required frame
201  return true;
202  }
203 
204  try {
205  // Obtaining the transform to get data from source to target frame
206  transform_msg = tf_buffer->lookupTransform(
207  target_frame_id, source_frame_id,
208  tf2::TimePointZero, transform_tolerance);
209  } catch (tf2::TransformException & e) {
210  RCLCPP_ERROR(
211  rclcpp::get_logger("getTransform"),
212  "Failed to get \"%s\"->\"%s\" frame transform: %s",
213  source_frame_id.c_str(), target_frame_id.c_str(), e.what());
214  return false;
215  }
216  return true;
217 }
218 
219 bool getTransform(
220  const std::string & source_frame_id,
221  const std::string & target_frame_id,
222  const tf2::Duration & transform_tolerance,
223  const nav2::TransformBuffer::SharedPtr tf_buffer,
224  tf2::Transform & tf2_transform)
225 {
226  tf2_transform.setIdentity(); // initialize by identical transform
227  geometry_msgs::msg::TransformStamped transform;
228  if (getTransform(source_frame_id, target_frame_id, transform_tolerance, tf_buffer, transform)) {
229  // Convert TransformStamped to TF2 transform
230  tf2::fromMsg(transform.transform, tf2_transform);
231  return true;
232  }
233  return false;
234 }
235 
236 bool getTransform(
237  const std::string & source_frame_id,
238  const rclcpp::Time & source_time,
239  const std::string & target_frame_id,
240  const rclcpp::Time & target_time,
241  const std::string & fixed_frame_id,
242  const tf2::Duration & transform_tolerance,
243  const nav2::TransformBuffer::SharedPtr tf_buffer,
244  geometry_msgs::msg::TransformStamped & transform_msg)
245 {
246  try {
247  // Obtaining the transform to get data from source to target frame.
248  // This also considers the time shift between source and target.
249  transform_msg = tf_buffer->lookupTransform(
250  target_frame_id, target_time,
251  source_frame_id, source_time,
252  fixed_frame_id, transform_tolerance);
253  } catch (tf2::TransformException & ex) {
254  RCLCPP_ERROR(
255  rclcpp::get_logger("getTransform"),
256  "Failed to get \"%s\"->\"%s\" frame transform: %s",
257  source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
258  return false;
259  }
260 
261  return true;
262 }
263 
264 bool getTransform(
265  const std::string & source_frame_id,
266  const rclcpp::Time & source_time,
267  const std::string & target_frame_id,
268  const rclcpp::Time & target_time,
269  const std::string & fixed_frame_id,
270  const tf2::Duration & transform_tolerance,
271  const nav2::TransformBuffer::SharedPtr tf_buffer,
272  tf2::Transform & tf2_transform)
273 {
274  geometry_msgs::msg::TransformStamped transform;
275  tf2_transform.setIdentity(); // initialize by identical transform
276  if (getTransform(
277  source_frame_id, source_time, target_frame_id, target_time, fixed_frame_id,
278  transform_tolerance, tf_buffer, transform))
279  {
280  // Convert TransformStamped to TF2 transform
281  tf2::fromMsg(transform.transform, tf2_transform);
282  return true;
283  }
284 
285  return false;
286 }
287 
288 bool validateTwist(const geometry_msgs::msg::Twist & msg)
289 {
290  if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) {
291  return false;
292  }
293 
294  if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) {
295  return false;
296  }
297 
298  if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) {
299  return false;
300  }
301 
302  if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) {
303  return false;
304  }
305 
306  if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) {
307  return false;
308  }
309 
310  if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) {
311  return false;
312  }
313 
314  return true;
315 }
316 
317 } // end namespace nav2_util