23#include <smacc2/component.h>
24#include <smacc2/smacc_updatable.h>
26#include <tf2/transform_datatypes.h>
27#include <tf2/transform_listener.h>
28#include <geometry_msgs/msg/pose.hpp>
37 geometry_msgs::msg::PoseStamped
pose_;
53 tf2::Stamped<tf2::Transform> transform;
59 poseTrack->referenceBaseFrame_, poseTrack->targetPoseFrame_, rclcpp::Time(0),
64 std::lock_guard<std::mutex> guard(
m_mutex_);
65 poseTrack->pose_.pose = tf2::toMsg(transform);
66 poseTrack->pose_.header.stamp = transform.stamp_;
67 poseTrack->pose_.header.frame_id = poseTrack->referenceBaseFrame_;
68 poseTrack->isInitialized =
true;
71 catch (tf2::TransformException ex)
73 RCLCPP_ERROR_STREAM_THROTTLE(
74 1,
"[Component pose] (getLogger(), " << poseFrameName_ <<
"/[" << referenceFrame_
75 <<
"] ) is failing on pose update : " << ex.what());
81 std::string & targetPoseFrameName, std::string & referenceBaseFrame,
82 geometry_msgs::msg::Pose & out)
87 std::string & targetName, std::string & referenceBaseFrame)
93 static std::shared_ptr<tf2_ros::TransformListener>
tfListener_;
static std::shared_ptr< tf2_ros::TransformListener > tfListener_
virtual void update() override
std::list< std::shared_ptr< TfPoseTrack > > poseTracks_
std::future< geometry_msgs::msg::Pose > waitForNextTransform(std::string &targetName, std::string &referenceBaseFrame)
void getLastTransform(std::string &targetPoseFrameName, std::string &referenceBaseFrame, geometry_msgs::msg::Pose &out)
static std::mutex listenerMutex_
std::string targetPoseFrame_
geometry_msgs::msg::PoseStamped pose_
std::string referenceBaseFrame_