SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | Private Attributes | Static Private Attributes | List of all members
cl_nav2z::Pose Class Reference

#include <cp_pose.hpp>

Inheritance diagram for cl_nav2z::Pose:
Inheritance graph
Collaboration diagram for cl_nav2z::Pose:
Collaboration graph

Public Member Functions

 Pose (std::string poseFrameName="base_link", std::string referenceFrame="odom")
 
 Pose (StandardReferenceFrames referenceFrame)
 
void onInitialize () override
 
void update () override
 
void waitTransformUpdate (rclcpp::Rate r=rclcpp::Rate(20))
 
geometry_msgs::msg::Pose toPoseMsg ()
 
geometry_msgs::msg::PoseStamped toPoseStampedMsg ()
 
float getYaw ()
 
float getX ()
 
float getY ()
 
float getZ ()
 
const std::string & getReferenceFrame () const
 
const std::string & getFrameId () const
 
- Public Member Functions inherited from smacc2::ISmaccComponent
 ISmaccComponent ()
 
virtual ~ISmaccComponent ()
 
virtual std::string getName () const
 
- Public Member Functions inherited from smacc2::ISmaccUpdatable
 ISmaccUpdatable ()
 
 ISmaccUpdatable (rclcpp::Duration duration)
 
void executeUpdate (rclcpp::Node::SharedPtr node)
 
void setUpdatePeriod (rclcpp::Duration duration)
 

Public Attributes

bool isInitialized
 

Private Attributes

geometry_msgs::msg::PoseStamped pose_
 
std::string poseFrameName_
 
std::string referenceFrame_
 
std::mutex m_mutex_
 

Static Private Attributes

static std::shared_ptr< tf2_ros::Buffer > tfBuffer_
 
static std::shared_ptr< tf2_ros::TransformListener > tfListener_
 
static std::mutex listenerMutex_
 

Additional Inherited Members

- Protected Member Functions inherited from smacc2::ISmaccComponent
virtual void onInitialize ()
 
template<typename EventType >
void postEvent (const EventType &ev)
 
template<typename EventType >
void postEvent ()
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
template<typename TComponent >
void requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)
 
template<typename TComponent >
void requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)
 
template<typename TClient >
void requiresClient (TClient *&requiredClientStorage)
 
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType * createSiblingComponent (TArgs... targs)
 
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType * createSiblingNamedComponent (std::string name, TArgs... targs)
 
rclcpp::Node::SharedPtr getNode ()
 
rclcpp::Logger getLogger () const
 
ISmaccStateMachinegetStateMachine ()
 
virtual void update ()=0
 
- Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachinestateMachine_
 
ISmaccClientowner_
 

Detailed Description

Definition at line 43 of file cp_pose.hpp.

Constructor & Destructor Documentation

◆ Pose() [1/2]

cl_nav2z::Pose::Pose ( std::string  poseFrameName = "base_link",
std::string  referenceFrame = "odom" 
)

Definition at line 35 of file cp_pose.cpp.

36: isInitialized(false), poseFrameName_(targetFrame), referenceFrame_(referenceFrame)
37
38{
39 this->pose_.header.frame_id = referenceFrame_;
40}
bool isInitialized
Definition: cp_pose.hpp:80
std::string referenceFrame_
Definition: cp_pose.hpp:91
geometry_msgs::msg::PoseStamped pose_
Definition: cp_pose.hpp:83
std::string poseFrameName_
Definition: cp_pose.hpp:90

References pose_, and referenceFrame_.

◆ Pose() [2/2]

cl_nav2z::Pose::Pose ( StandardReferenceFrames  referenceFrame)

Definition at line 55 of file cp_pose.cpp.

56: Pose("base_link", referenceFrameToString(referenceFrame))
57{
58}
Pose(std::string poseFrameName="base_link", std::string referenceFrame="odom")
Definition: cp_pose.cpp:35
std::string referenceFrameToString(StandardReferenceFrames referenceFrame)
Definition: cp_pose.cpp:42

Member Function Documentation

◆ getFrameId()

const std::string & cl_nav2z::Pose::getFrameId ( ) const
inline

Definition at line 78 of file cp_pose.hpp.

78{ return poseFrameName_; }

References poseFrameName_.

◆ getReferenceFrame()

const std::string & cl_nav2z::Pose::getReferenceFrame ( ) const
inline

◆ getX()

float cl_nav2z::Pose::getX ( )

Definition at line 120 of file cp_pose.cpp.

120{ return pose_.pose.position.x; }

References pose_.

◆ getY()

float cl_nav2z::Pose::getY ( )

Definition at line 121 of file cp_pose.cpp.

121{ return pose_.pose.position.y; }

References pose_.

◆ getYaw()

float cl_nav2z::Pose::getYaw ( )

Definition at line 118 of file cp_pose.cpp.

118{ return tf2::getYaw(pose_.pose.orientation); }

References pose_.

◆ getZ()

float cl_nav2z::Pose::getZ ( )

Definition at line 122 of file cp_pose.cpp.

122{ return pose_.pose.position.z; }

References pose_.

◆ onInitialize()

void cl_nav2z::Pose::onInitialize ( )
overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 60 of file cp_pose.cpp.

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}
static std::mutex listenerMutex_
Definition: cp_pose.hpp:88
static std::shared_ptr< tf2_ros::Buffer > tfBuffer_
Definition: cp_pose.hpp:85
static std::shared_ptr< tf2_ros::TransformListener > tfListener_
Definition: cp_pose.hpp:86
rclcpp::Logger getLogger() const
rclcpp::Node::SharedPtr getNode()

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), listenerMutex_, poseFrameName_, referenceFrame_, tfBuffer_, and tfListener_.

Here is the call graph for this function:

◆ toPoseMsg()

geometry_msgs::msg::Pose cl_nav2z::Pose::toPoseMsg ( )
inline

Definition at line 57 of file cp_pose.hpp.

58 {
59 std::lock_guard<std::mutex> guard(m_mutex_);
60 return this->pose_.pose;
61 }
std::mutex m_mutex_
Definition: cp_pose.hpp:93

References m_mutex_, and pose_.

Referenced by cl_nav2z::CbRotateLookAt::onEntry(), cl_nav2z::CbStopNavigation::onEntry(), and cl_nav2z::WaypointNavigator::sendNextGoal().

Here is the caller graph for this function:

◆ toPoseStampedMsg()

geometry_msgs::msg::PoseStamped cl_nav2z::Pose::toPoseStampedMsg ( )
inline

Definition at line 63 of file cp_pose.hpp.

64 {
65 std::lock_guard<std::mutex> guard(m_mutex_);
66 return this->pose_;
67 }

References m_mutex_, and pose_.

◆ update()

void cl_nav2z::Pose::update ( )
overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 124 of file cp_pose.cpp.

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}

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), isInitialized, listenerMutex_, m_mutex_, pose_, poseFrameName_, referenceFrame_, and tfBuffer_.

Here is the call graph for this function:

◆ waitTransformUpdate()

void cl_nav2z::Pose::waitTransformUpdate ( rclcpp::Rate  r = rclcpp::Rate(20))

Definition at line 77 of file cp_pose.cpp.

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}

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), isInitialized, listenerMutex_, m_mutex_, pose_, poseFrameName_, referenceFrame_, and tfBuffer_.

Referenced by cl_nav2z::CbRotateLookAt::onEntry(), and cl_nav2z::CbWaitPose::onEntry().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ isInitialized

bool cl_nav2z::Pose::isInitialized

Definition at line 80 of file cp_pose.hpp.

Referenced by update(), and waitTransformUpdate().

◆ listenerMutex_

std::mutex cl_nav2z::Pose::listenerMutex_
staticprivate

Definition at line 88 of file cp_pose.hpp.

Referenced by onInitialize(), update(), and waitTransformUpdate().

◆ m_mutex_

std::mutex cl_nav2z::Pose::m_mutex_
private

Definition at line 93 of file cp_pose.hpp.

Referenced by toPoseMsg(), toPoseStampedMsg(), update(), and waitTransformUpdate().

◆ pose_

geometry_msgs::msg::PoseStamped cl_nav2z::Pose::pose_
private

◆ poseFrameName_

std::string cl_nav2z::Pose::poseFrameName_
private

Definition at line 90 of file cp_pose.hpp.

Referenced by getFrameId(), onInitialize(), update(), and waitTransformUpdate().

◆ referenceFrame_

std::string cl_nav2z::Pose::referenceFrame_
private

Definition at line 91 of file cp_pose.hpp.

Referenced by getReferenceFrame(), onInitialize(), Pose(), update(), and waitTransformUpdate().

◆ tfBuffer_

std::shared_ptr< tf2_ros::Buffer > cl_nav2z::Pose::tfBuffer_
staticprivate

Definition at line 85 of file cp_pose.hpp.

Referenced by onInitialize(), update(), and waitTransformUpdate().

◆ tfListener_

std::shared_ptr< tf2_ros::TransformListener > cl_nav2z::Pose::tfListener_
staticprivate

Definition at line 86 of file cp_pose.hpp.

Referenced by onInitialize().


The documentation for this class was generated from the following files: