SMACC2
Loading...
Searching...
No Matches
cp_pose.hpp
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 *-2020
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20#pragma once
21
22#include <mutex>
23
24#include <geometry_msgs/msg/pose_stamped.h>
25#include <tf2/transform_datatypes.h>
26#include <tf2/utils.h>
27#include <tf2_ros/buffer.h>
28#include <tf2_ros/transform_listener.h>
29#include <geometry_msgs/msg/quaternion_stamped.hpp>
30#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
31
32#include <smacc2/component.hpp>
34
35namespace cl_nav2z
36{
38{
39 Map,
41};
42
44{
45public:
46 Pose(std::string poseFrameName = "base_link", std::string referenceFrame = "odom");
47
48 Pose(StandardReferenceFrames referenceFrame);
49
50 void onInitialize() override;
51
52 void update() override;
53
54 // synchronously waits a transform in the current thread
55 void waitTransformUpdate(rclcpp::Rate r = rclcpp::Rate(20));
56
57 inline geometry_msgs::msg::Pose toPoseMsg()
58 {
59 std::lock_guard<std::mutex> guard(m_mutex_);
60 return this->pose_.pose;
61 }
62
63 inline geometry_msgs::msg::PoseStamped toPoseStampedMsg()
64 {
65 RCLCPP_INFO_STREAM(getLogger(), "[Pose] ToPoseMsg ");
66 std::lock_guard<std::mutex> guard(m_mutex_);
67 return this->pose_;
68 }
69
70 // get yaw in radians
71 float getYaw();
72
73 float getX();
74 float getY();
75 float getZ();
76
77 inline void setReferenceFrame(std::string referenceFrame) { referenceFrame_ = referenceFrame; }
78
79 inline const std::string & getReferenceFrame() const { return referenceFrame_; }
80
81 inline const std::string & getFrameId() const { return poseFrameName_; }
82
84
85 std::optional<rclcpp::Time> frozenReferenceFrameTime;
87 {
88 frozenReferenceFrameTime = getNode()->now() - rclcpp::Duration::from_seconds(1);
89 }
90
92
93private:
94 geometry_msgs::msg::PoseStamped pose_;
95
96 static std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
97 static std::shared_ptr<tf2_ros::TransformListener> tfListener_;
98
99 static std::mutex listenerMutex_;
100
101 std::string poseFrameName_;
102 std::string referenceFrame_;
103
104 std::mutex m_mutex_;
105};
106} // namespace cl_nav2z
void unfreezeReferenceFrame()
Definition cp_pose.hpp:91
static std::mutex listenerMutex_
Definition cp_pose.hpp:99
static std::shared_ptr< tf2_ros::Buffer > tfBuffer_
Definition cp_pose.hpp:96
void setReferenceFrame(std::string referenceFrame)
Definition cp_pose.hpp:77
void update() override
Definition cp_pose.cpp:125
void freezeReferenceFrame()
Definition cp_pose.hpp:86
static std::shared_ptr< tf2_ros::TransformListener > tfListener_
Definition cp_pose.hpp:97
std::mutex m_mutex_
Definition cp_pose.hpp:104
const std::string & getFrameId() const
Definition cp_pose.hpp:81
float getYaw()
Definition cp_pose.cpp:119
geometry_msgs::msg::PoseStamped toPoseStampedMsg()
Definition cp_pose.hpp:63
const std::string & getReferenceFrame() const
Definition cp_pose.hpp:79
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:83
std::string referenceFrame_
Definition cp_pose.hpp:102
std::optional< rclcpp::Time > frozenReferenceFrameTime
Definition cp_pose.hpp:85
geometry_msgs::msg::Pose toPoseMsg()
Definition cp_pose.hpp:57
void waitTransformUpdate(rclcpp::Rate r=rclcpp::Rate(20))
Definition cp_pose.cpp:77
geometry_msgs::msg::PoseStamped pose_
Definition cp_pose.hpp:94
std::string poseFrameName_
Definition cp_pose.hpp:101
rclcpp::Logger getLogger() const
rclcpp::Node::SharedPtr getNode()
StandardReferenceFrames
Definition cp_pose.hpp:38