Program Listing for File geometry.hpp

↰ Return to documentation for file (include/rp_utils/geometry.hpp)

#ifndef RP_UTILS_GEOMETRY_HPP_
#define RP_UTILS_GEOMETRY_HPP_

#include <rclcpp/duration.hpp>

#include <geometry_msgs/msg/pose_array.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

namespace rp_utils::geometry
{

geometry_msgs::msg::PoseArray Transform(const geometry_msgs::msg::PoseArray &input_array,
                                        const std::string &target_frame,
                                        const std::shared_ptr<tf2_ros::Buffer> &tf_buffer,
                                        const tf2::Duration &timeout = tf2::durationFromSec(0.1))
{
    geometry_msgs::msg::PoseArray transformed_array;
    transformed_array.header.frame_id = target_frame;
    transformed_array.header.stamp = input_array.header.stamp;

    for (const auto &pose : input_array.poses)
    {
        geometry_msgs::msg::PoseStamped pose_stamped;
        pose_stamped.header = input_array.header;
        pose_stamped.pose = pose;

        // Transform pose to the desired frame
        geometry_msgs::msg::PoseStamped transformed_pose;
        transformed_pose = tf_buffer->transform(pose_stamped, target_frame, timeout);

        transformed_array.poses.push_back(transformed_pose.pose);
    }

    return transformed_array;
}

} // namespace rp_utils::geometry

#endif // RP_UTILS_GEOMETRY_HPP_