17#include <rclcpp/rclcpp.hpp>
18#include <geometry_msgs/msg/pose_stamped.hpp>
19#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
20#include <geometry_msgs/msg/pose.hpp>
21#include <geometry_msgs/msg/transform_stamped.hpp>
22#include <geometry_msgs/msg/transform.hpp>
30void convert(
const geometry_msgs::msg::Pose& pose, geometry_msgs::msg::Transform& trans)
32 trans.rotation = pose.orientation;
33 trans.translation.x = pose.position.x;
34 trans.translation.y = pose.position.y;
35 trans.translation.z = pose.position.z;
38void convert(
const geometry_msgs::msg::PoseWithCovariance& pose, geometry_msgs::msg::Transform& trans)
43void convert(
const geometry_msgs::msg::PoseStamped& pose, geometry_msgs::msg::TransformStamped& trans)
45 convert(pose.pose, trans.transform);
46 trans.header = pose.header;
49void convert(
const geometry_msgs::msg::PoseWithCovarianceStamped& pose, geometry_msgs::msg::TransformStamped& trans)
51 convert(pose.pose, trans.transform);
52 trans.header = pose.header;
61 : config_(config), transform_pub_(transform_pub), node_(node)
67 geometry_msgs::msg::TransformStamped out_tf;
75 geometry_msgs::msg::TransformStamped out_tf;
83 geometry_msgs::msg::TransformStamped out_tf;
85 out_tf.header.stamp =
node_->now();
93 geometry_msgs::msg::TransformStamped out_tf;
95 out_tf.header.stamp =
node_->now();
void poseStampedCallback(geometry_msgs::msg::PoseStamped::UniquePtr msg)
Callback for new pose stamped messages.
PoseToTF2(PoseToTF2Config config, TransformPublisher transform_pub, std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > node)
Constructor.
void poseWithCovarianceStampedCallback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg)
Callback for new pose stamped messages.
std::function< void(const geometry_msgs::msg::TransformStamped &)> TransformPublisher
void poseWithCovarianceCallback(geometry_msgs::msg::PoseWithCovariance::UniquePtr msg)
Callback for new pose with covariance messages.
void poseCallback(geometry_msgs::msg::Pose::UniquePtr msg)
Callback for new pose stamped messages.
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > node_
TransformPublisher transform_pub_
void convert(const geometry_msgs::msg::PoseWithCovarianceStamped &pose, geometry_msgs::msg::TransformStamped &trans)
Stuct containing the frame configuration values for pose_to_tf.
std::string default_parent_frame