SMACC2
cp_pose.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
22
23#include <memory>
24#include <string>
25
26namespace cl_nav2z
27{
28using namespace std::chrono_literals;
29
30// static
31std::shared_ptr<tf2_ros::TransformListener> Pose::tfListener_;
32std::shared_ptr<tf2_ros::Buffer> Pose::tfBuffer_;
33std::mutex Pose::listenerMutex_;
34
35Pose::Pose(std::string targetFrame, std::string referenceFrame)
36: isInitialized(false), poseFrameName_(targetFrame), referenceFrame_(referenceFrame)
37
38{
39 this->pose_.header.frame_id = referenceFrame_;
40}
41
43{
44 switch (referenceFrame)
45 {
47 return "map";
49 return "odom";
50 default:
51 return "odom";
52 }
53}
54
56: Pose("base_link", referenceFrameToString(referenceFrame))
57{
58}
59
61{
62 RCLCPP_INFO(
63 getLogger(), "[Pose] Creating Pose tracker component to track %s in the reference frame %s",
64 poseFrameName_.c_str(), referenceFrame_.c_str());
65
66 {
67 // singleton
68 std::lock_guard<std::mutex> guard(listenerMutex_);
69 if (tfListener_ == nullptr)
70 {
71 tfBuffer_ = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
72 tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
73 }
74 }
75}
76
77void Pose::waitTransformUpdate(rclcpp::Rate r)
78{
79 bool found = false;
80 RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate");
81 while (rclcpp::ok() && !found)
82 {
83 tf2::Stamped<tf2::Transform> transform;
84 try
85 {
86 {
87 RCLCPP_INFO_THROTTLE(
88 getLogger(), *(getNode()->get_clock()), 1000,
89 "[Pose Component] waiting transform %s -> %s", referenceFrame_.c_str(),
90 poseFrameName_.c_str());
91 std::lock_guard<std::mutex> lock(listenerMutex_);
92 auto transformstamped =
93 tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, getNode()->now());
94 tf2::fromMsg(transformstamped, transform);
95 }
96
97 {
98 std::lock_guard<std::mutex> guard(m_mutex_);
99 tf2::toMsg(transform, this->pose_.pose);
100 this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
101 found = true;
102 this->isInitialized = true;
103 }
104 }
105 catch (tf2::TransformException & ex)
106 {
107 RCLCPP_ERROR_STREAM_THROTTLE(
108 getLogger(), *(getNode()->get_clock()), 1000,
109 "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
110 << "] ) is failing on pose update : " << ex.what());
111 }
112
113 r.sleep();
114 }
115 RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate -> pose found!");
116}
117
118float Pose::getYaw() { return tf2::getYaw(pose_.pose.orientation); }
119
120float Pose::getX() { return pose_.pose.position.x; }
121float Pose::getY() { return pose_.pose.position.y; }
122float Pose::getZ() { return pose_.pose.position.z; }
123
125{
126 tf2::Stamped<tf2::Transform> transform;
127 try
128 {
129 {
130 std::lock_guard<std::mutex> lock(listenerMutex_);
131 RCLCPP_DEBUG(getLogger(), "[pose] looking up transform");
132 auto transformstamped =
133 tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, rclcpp::Time(0));
134 tf2::fromMsg(transformstamped, transform);
135 }
136
137 {
138 std::lock_guard<std::mutex> guard(m_mutex_);
139 tf2::toMsg(transform, this->pose_.pose);
140 this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
141 this->isInitialized = true;
142 }
143 }
144 catch (tf2::TransformException & ex)
145 {
146 // RCLCPP_DEBUG(getLogger(), "[pose] EXCEPTION");
147 RCLCPP_ERROR_STREAM_THROTTLE(
148 getLogger(), *(getNode()->get_clock()), 1000,
149 "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
150 << "] ) is failing on pose update : " << ex.what());
151 }
152}
153} // namespace cl_nav2z
float getX()
Definition: cp_pose.cpp:120
static std::mutex listenerMutex_
Definition: cp_pose.hpp:89
static std::shared_ptr< tf2_ros::Buffer > tfBuffer_
Definition: cp_pose.hpp:86
void update() override
Definition: cp_pose.cpp:124
static std::shared_ptr< tf2_ros::TransformListener > tfListener_
Definition: cp_pose.hpp:87
float getY()
Definition: cp_pose.cpp:121
std::mutex m_mutex_
Definition: cp_pose.hpp:94
float getYaw()
Definition: cp_pose.cpp:118
Pose(std::string poseFrameName="base_link", std::string referenceFrame="odom")
Definition: cp_pose.cpp:35
void onInitialize() override
Definition: cp_pose.cpp:60
bool isInitialized
Definition: cp_pose.hpp:81
std::string referenceFrame_
Definition: cp_pose.hpp:92
void waitTransformUpdate(rclcpp::Rate r=rclcpp::Rate(20))
Definition: cp_pose.cpp:77
geometry_msgs::msg::PoseStamped pose_
Definition: cp_pose.hpp:84
float getZ()
Definition: cp_pose.cpp:122
std::string poseFrameName_
Definition: cp_pose.hpp:91
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()
StandardReferenceFrames
Definition: cp_pose.hpp:39
std::string referenceFrameToString(StandardReferenceFrames referenceFrame)
Definition: cp_pose.cpp:42