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"
26 #include "nav2_util/robot_utils.hpp"
27 #include "rclcpp/logger.hpp"
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)
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;
50 geometry_msgs::msg::TransformStamped latest_transform;
52 latest_transform = tf_buffer.lookupTransform(
53 target_frame, source_frame, tf2::TimePointZero);
54 }
catch (
const tf2::TransformException & ex) {
56 rclcpp::get_logger(
"lookupTransformWithStalenessCheck"),
57 "Failed to get transform: %s", ex.what());
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) {
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);
75 transform = latest_transform;
79 geometry_msgs::msg::PoseStamped transformToPoseStamped(
80 const geometry_msgs::msg::TransformStamped & transform)
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;
91 geometry_msgs::msg::TransformStamped poseToTransformStamped(
92 const geometry_msgs::msg::PoseStamped & pose,
const std::string & child_frame)
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;
104 geometry_msgs::msg::TransformStamped invertTransform(
105 const geometry_msgs::msg::TransformStamped & transform)
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());
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)
125 geometry_msgs::msg::TransformStamped transform;
126 if (lookupTransformWithStalenessCheck(
127 tf_buffer, target_frame, source_frame, current_time, staleness_threshold, transform))
129 pose = transformToPoseStamped(transform);
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)
141 tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
142 global_pose.header.frame_id = robot_frame;
143 global_pose.header.stamp = stamp;
145 return transformPoseInTargetFrame(
146 global_pose, global_pose, tf_buffer, global_frame, transform_timeout);
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)
155 static rclcpp::Logger logger = rclcpp::get_logger(
"transformPoseInTargetFrame");
157 if (input_pose.header.frame_id == target_frame) {
158 transformed_pose = input_pose;
163 transformed_pose = tf_buffer.transform(
164 input_pose, target_frame,
165 tf2::durationFromSec(transform_timeout));
167 }
catch (tf2::LookupException & ex) {
170 "No Transform available Error looking up target frame: %s\n", ex.what());
171 }
catch (tf2::ConnectivityException & ex) {
174 "Connectivity Error looking up target frame: %s\n", ex.what());
175 }
catch (tf2::ExtrapolationException & ex) {
178 "Extrapolation Error looking up target frame: %s\n", ex.what());
179 }
catch (tf2::TimeoutException & ex) {
182 "Transform timeout with tolerance: %.4f", transform_timeout);
183 }
catch (tf2::TransformException & ex) {
185 logger,
"Failed to transform from %s to %s",
186 input_pose.header.frame_id.c_str(), target_frame.c_str());
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)
199 if (source_frame_id == target_frame_id) {
206 transform_msg = tf_buffer->lookupTransform(
207 target_frame_id, source_frame_id,
208 tf2::TimePointZero, transform_tolerance);
209 }
catch (tf2::TransformException & e) {
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());
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)
226 tf2_transform.setIdentity();
227 geometry_msgs::msg::TransformStamped transform;
228 if (getTransform(source_frame_id, target_frame_id, transform_tolerance, tf_buffer, transform)) {
230 tf2::fromMsg(transform.transform, tf2_transform);
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)
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) {
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());
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)
274 geometry_msgs::msg::TransformStamped transform;
275 tf2_transform.setIdentity();
277 source_frame_id, source_time, target_frame_id, target_time, fixed_frame_id,
278 transform_tolerance, tf_buffer, transform))
281 tf2::fromMsg(transform.transform, tf2_transform);
288 bool validateTwist(
const geometry_msgs::msg::Twist & msg)
290 if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) {
294 if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) {
298 if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) {
302 if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) {
306 if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) {
310 if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) {