17 #ifndef NAV2_UTIL__ROBOT_UTILS_HPP_
18 #define NAV2_UTIL__ROBOT_UTILS_HPP_
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"
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);
61 geometry_msgs::msg::PoseStamped transformToPoseStamped(
62 const geometry_msgs::msg::TransformStamped & transform);
70 geometry_msgs::msg::TransformStamped poseToTransformStamped(
71 const geometry_msgs::msg::PoseStamped & pose,
const std::string & child_frame);
78 geometry_msgs::msg::TransformStamped invertTransform(
79 const geometry_msgs::msg::TransformStamped & transform);
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);
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());
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);
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);
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);
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);
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);
213 [[nodiscard]]
bool validateTwist(
const geometry_msgs::msg::Twist & msg);