diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index b7d58b67363..8bc750cc717 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -91,21 +91,6 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; - /** - * @brief Obtains a transform from source_frame_id at source_time -> - * to base_frame_id_ at curr_time time - * @param source_frame_id Source frame ID to convert data from - * @param source_time Source timestamp to convert data from - * @param curr_time Current node time to interpolate data to - * @param tf_transform Output source->base transform - * @return True if got correct transform, otherwise false - */ - bool getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, - tf2::Transform & tf_transform) const; - // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index 2b736723687..6ad41224b5a 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -78,33 +78,4 @@ bool Source::sourceValid( return true; } -bool Source::getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, - tf2::Transform & tf2_transform) const -{ - geometry_msgs::msg::TransformStamped transform; - tf2_transform.setIdentity(); // initialize by identical transform - - try { - // Obtaining the transform to get data from source to base frame. - // This also considers the time shift between source and base. - transform = tf_buffer_->lookupTransform( - base_frame_id_, curr_time, - source_frame_id, source_time, - global_frame_id_, transform_tolerance_); - } catch (tf2::TransformException & e) { - RCLCPP_ERROR( - logger_, - "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s", - source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what()); - return false; - } - - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); - return true; -} - } // namespace nav2_collision_monitor diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index b9712044b65..baead03cbaa 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -56,6 +56,57 @@ bool transformPoseInTargetFrame( tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout = 0.1); +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Target time to interpolate to + * @param transform_tolerance Transform tolerance + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ + +/** + * @brief Obtains a transform from source_frame_id -> to target_frame_id + * @param source_frame_id Source frame ID to convert from + * @param target_frame_id Target frame ID to convert to + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); + +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Current node time to interpolate to + * @param fixed_frame_id The frame in which to assume the transform is constant in time + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index e99d9e33349..37ab261230d 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -75,4 +75,70 @@ bool transformPoseInTargetFrame( return false; } +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + if (source_frame_id == target_frame_id) { + // We are already in required frame + return true; + } + + try { + // Obtaining the transform to get data from source to target frame + transform = tf_buffer->lookupTransform( + target_frame_id, source_frame_id, + tf2::TimePointZero, transform_tolerance); + } catch (tf2::TransformException & e) { + RCLCPP_ERROR( + rclcpp::get_logger("getTransform"), + "Failed to get \"%s\"->\"%s\" frame transform: %s", + source_frame_id.c_str(), target_frame_id.c_str(), e.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + try { + // Obtaining the transform to get data from source to target frame. + // This also considers the time shift between source and target. + transform = tf_buffer->lookupTransform( + target_frame_id, target_time, + source_frame_id, source_time, + fixed_frame_id, transform_tolerance); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("getTransform"), + "Failed to get \"%s\"->\"%s\" frame transform: %s", + source_frame_id.c_str(), target_frame_id.c_str(), ex.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + } // end namespace nav2_util