Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
robot_utils.hpp
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 #ifndef NAV2_UTIL__ROBOT_UTILS_HPP_
18 #define NAV2_UTIL__ROBOT_UTILS_HPP_
19 
20 #include <string>
21 #include <memory>
22 
23 #include "geometry_msgs/msg/pose_stamped.hpp"
24 #include "geometry_msgs/msg/twist.hpp"
25 #include "geometry_msgs/msg/twist_stamped.hpp"
26 #include "geometry_msgs/msg/transform_stamped.hpp"
27 #include "tf2/time.hpp"
28 #include "tf2/transform_datatypes.hpp"
29 #include "nav2_ros_common/tf2_factories.hpp"
30 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
31 #include "rclcpp/time.hpp"
32 
33 namespace nav2_util
34 {
35 
48 bool lookupTransformWithStalenessCheck(
49  nav2::TransformBuffer & tf_buffer,
50  const std::string & target_frame,
51  const std::string & source_frame,
52  const rclcpp::Time & current_time,
53  double staleness_threshold,
54  geometry_msgs::msg::TransformStamped & transform);
55 
61 geometry_msgs::msg::PoseStamped transformToPoseStamped(
62  const geometry_msgs::msg::TransformStamped & transform);
63 
70 geometry_msgs::msg::TransformStamped poseToTransformStamped(
71  const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame);
72 
78 geometry_msgs::msg::TransformStamped invertTransform(
79  const geometry_msgs::msg::TransformStamped & transform);
80 
92 bool getFreshPose(
93  nav2::TransformBuffer & tf_buffer,
94  const std::string & target_frame,
95  const std::string & source_frame,
96  const rclcpp::Time & current_time,
97  double staleness_threshold,
98  geometry_msgs::msg::PoseStamped & pose);
99 
109 bool getCurrentPose(
110  geometry_msgs::msg::PoseStamped & global_pose,
111  nav2::TransformBuffer & tf_buffer, const std::string global_frame = "map",
112  const std::string robot_frame = "base_link", const double transform_timeout = 0.1,
113  const rclcpp::Time stamp = rclcpp::Time());
114 
124 bool transformPoseInTargetFrame(
125  const geometry_msgs::msg::PoseStamped & input_pose,
126  geometry_msgs::msg::PoseStamped & transformed_pose,
127  nav2::TransformBuffer & tf_buffer, const std::string target_frame,
128  const double transform_timeout = 0.1);
129 
139 bool getTransform(
140  const std::string & source_frame_id,
141  const std::string & target_frame_id,
142  const tf2::Duration & transform_tolerance,
143  const nav2::TransformBuffer::SharedPtr tf_buffer,
144  tf2::Transform & tf2_transform);
145 
159 bool getTransform(
160  const std::string & source_frame_id,
161  const rclcpp::Time & source_time,
162  const std::string & target_frame_id,
163  const rclcpp::Time & target_time,
164  const std::string & fixed_frame_id,
165  const tf2::Duration & transform_tolerance,
166  const nav2::TransformBuffer::SharedPtr tf_buffer,
167  tf2::Transform & tf2_transform);
168 
178 bool getTransform(
179  const std::string & source_frame_id,
180  const std::string & target_frame_id,
181  const tf2::Duration & transform_tolerance,
182  const nav2::TransformBuffer::SharedPtr tf_buffer,
183  geometry_msgs::msg::TransformStamped & transform_msg);
184 
198 bool getTransform(
199  const std::string & source_frame_id,
200  const rclcpp::Time & source_time,
201  const std::string & target_frame_id,
202  const rclcpp::Time & target_time,
203  const std::string & fixed_frame_id,
204  const tf2::Duration & transform_tolerance,
205  const nav2::TransformBuffer::SharedPtr tf_buffer,
206  geometry_msgs::msg::TransformStamped & transform_msg);
207 
213 [[nodiscard]] bool validateTwist(const geometry_msgs::msg::Twist & msg);
214 
215 } // end namespace nav2_util
216 
217 #endif // NAV2_UTIL__ROBOT_UTILS_HPP_