#include <cp_pose.hpp>

Inheritance diagram for cl_nav2z::Pose:

Collaboration diagram for cl_nav2z::Pose:

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 ()

void	setReferenceFrame (std::string referenceFrame)

const std::string &	getReferenceFrame () const

const std::string &	getFrameId () const

void	freezeReferenceFrame ()

void	unfreezeReferenceFrame ()

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

std::optional< rclcpp::Time >	frozenReferenceFrameTime

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
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

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

ISmaccStateMachine *	getStateMachine ()

Protected Member Functions inherited from smacc2::ISmaccUpdatable
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

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.

: isInitialized(false), poseFrameName_(targetFrame), referenceFrame_(referenceFrame)
 
{
  this->pose_.header.frame_id = referenceFrame_;
}

References pose_, and referenceFrame_.

◆ Pose() [2/2]

cl_nav2z::Pose::Pose ( StandardReferenceFrames referenceFrame )

Definition at line 55 of file cp_pose.cpp.

: Pose("base_link", referenceFrameToString(referenceFrame))
{
}

Member Function Documentation

◆ freezeReferenceFrame()

void cl_nav2z::Pose::freezeReferenceFrame ( )

inline

Definition at line 86 of file cp_pose.hpp.

  {
    frozenReferenceFrameTime = getNode()->now() - rclcpp::Duration::from_seconds(1);
  }

References frozenReferenceFrameTime, and smacc2::ISmaccComponent::getNode().

Here is the call graph for this function:

◆ getFrameId()

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

inline

Definition at line 81 of file cp_pose.hpp.

81{ return poseFrameName_; }

References poseFrameName_.

◆ getReferenceFrame()

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

inline

Definition at line 79 of file cp_pose.hpp.

79{ return referenceFrame_; }

References referenceFrame_.

◆ getX()

float cl_nav2z::Pose::getX ( )

Definition at line 121 of file cp_pose.cpp.

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

References pose_.

◆ getY()

float cl_nav2z::Pose::getY ( )

Definition at line 122 of file cp_pose.cpp.

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

References pose_.

◆ getYaw()

float cl_nav2z::Pose::getYaw ( )

Definition at line 119 of file cp_pose.cpp.

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

References pose_.

◆ getZ()

float cl_nav2z::Pose::getZ ( )

Definition at line 123 of file cp_pose.cpp.

123{ 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.

{
  RCLCPP_INFO(
    getLogger(), "[Pose] Creating Pose tracker component to track %s in the reference frame %s",
    poseFrameName_.c_str(), referenceFrame_.c_str());
 
  {
    // singleton
    std::lock_guard<std::mutex> guard(listenerMutex_);
    if (tfListener_ == nullptr)
    {
      tfBuffer_ = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
      tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
    }
  }
}

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

Here is the call graph for this function:

◆ setReferenceFrame()

void cl_nav2z::Pose::setReferenceFrame ( std::string referenceFrame )

inline

Definition at line 77 of file cp_pose.hpp.

77{ referenceFrame_ = referenceFrame; }

References referenceFrame_.

◆ toPoseMsg()

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

inline

Definition at line 57 of file cp_pose.hpp.

  {
    std::lock_guard<std::mutex> guard(m_mutex_);
    return this->pose_.pose;
  }

References m_mutex_, and pose_.

Referenced by cl_nav2z::CpWaypointNavigator::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.

  {
    RCLCPP_INFO_STREAM(getLogger(), "[Pose] ToPoseMsg ");
    std::lock_guard<std::mutex> guard(m_mutex_);
    return this->pose_;
  }

References smacc2::ISmaccComponent::getLogger(), m_mutex_, and pose_.

Here is the call graph for this function:

◆ unfreezeReferenceFrame()

void cl_nav2z::Pose::unfreezeReferenceFrame ( )

inline

Definition at line 91 of file cp_pose.hpp.

91{ frozenReferenceFrameTime = std::nullopt; }

References frozenReferenceFrameTime.

◆ update()

void cl_nav2z::Pose::update ( )

overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 125 of file cp_pose.cpp.

{
  tf2::Stamped<tf2::Transform> transform;
  try
  {
    if (!frozenReferenceFrameTime)
    {
      std::lock_guard<std::mutex> lock(listenerMutex_);
      RCLCPP_INFO(
        getLogger(), "[pose] looking up transform: %s -> %s", referenceFrame_.c_str(),
        poseFrameName_.c_str());
      auto transformstamped =
        tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, rclcpp::Time(0));
      tf2::fromMsg(transformstamped, transform);
      RCLCPP_INFO(getLogger(), "[pose] transform found");
    }
    else
    {
      RCLCPP_INFO(
        getLogger(), "[pose] looking up transform: %s -> %s", referenceFrame_.c_str(),
        poseFrameName_.c_str());
      auto transformstamped = tfBuffer_->lookupTransform(
        referenceFrame_, *frozenReferenceFrameTime, poseFrameName_, rclcpp::Time(0),
        referenceFrame_, 1s);
      tf2::fromMsg(transformstamped, transform);
      RCLCPP_INFO(getLogger(), "[pose] transform found");
    }
 
    {
      std::lock_guard<std::mutex> guard(m_mutex_);
      tf2::toMsg(transform, this->pose_.pose);
      this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
      this->pose_.header.frame_id = referenceFrame_;
      this->isInitialized = true;
    }
  }
  catch (tf2::TransformException & ex)
  {
    // RCLCPP_DEBUG(getLogger(), "[pose] EXCEPTION");
    RCLCPP_ERROR_STREAM_THROTTLE(
      getLogger(), *(getNode()->get_clock()), 1000,
      "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
                           << "] ) is failing on pose update : " << ex.what());
  }
}

References frozenReferenceFrameTime, 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.

{
  bool found = false;
  RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate");
  while (rclcpp::ok() && !found)
  {
    tf2::Stamped<tf2::Transform> transform;
    try
    {
      {
        RCLCPP_INFO_THROTTLE(
          getLogger(), *(getNode()->get_clock()), 1000,
          "[Pose Component] waiting transform %s -> %s", referenceFrame_.c_str(),
          poseFrameName_.c_str());
        std::lock_guard<std::mutex> lock(listenerMutex_);
        auto transformstamped =
          tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, getNode()->now());
        tf2::fromMsg(transformstamped, transform);
      }
 
      {
        std::lock_guard<std::mutex> guard(m_mutex_);
        tf2::toMsg(transform, this->pose_.pose);
        this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
        this->pose_.header.frame_id = referenceFrame_;
        found = true;
        this->isInitialized = true;
      }
    }
    catch (tf2::TransformException & ex)
    {
      RCLCPP_ERROR_STREAM_THROTTLE(
        getLogger(), *(getNode()->get_clock()), 1000,
        "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
                             << "] ) is failing on pose update : " << ex.what());
    }
 
    r.sleep();
  }
  RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate -> pose found!");
}

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

Here is the call graph for this function:

Member Data Documentation

◆ frozenReferenceFrameTime

std::optional<rclcpp::Time> cl_nav2z::Pose::frozenReferenceFrameTime

Definition at line 85 of file cp_pose.hpp.

Referenced by freezeReferenceFrame(), unfreezeReferenceFrame(), and update().

◆ isInitialized

bool cl_nav2z::Pose::isInitialized

Definition at line 83 of file cp_pose.hpp.

Referenced by update(), and waitTransformUpdate().

◆ listenerMutex_

std::mutex cl_nav2z::Pose::listenerMutex_

staticprivate

Definition at line 99 of file cp_pose.hpp.

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

◆ m_mutex_

std::mutex cl_nav2z::Pose::m_mutex_

private

Definition at line 104 of file cp_pose.hpp.

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

◆ pose_

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

private

Definition at line 94 of file cp_pose.hpp.

Referenced by getX(), getY(), getYaw(), getZ(), Pose(), toPoseMsg(), toPoseStampedMsg(), update(), and waitTransformUpdate().

◆ poseFrameName_

std::string cl_nav2z::Pose::poseFrameName_

private

Definition at line 101 of file cp_pose.hpp.

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

◆ referenceFrame_

std::string cl_nav2z::Pose::referenceFrame_

private

Definition at line 102 of file cp_pose.hpp.

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

◆ tfBuffer_

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

staticprivate

Definition at line 96 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 97 of file cp_pose.hpp.

Referenced by onInitialize().

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

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/pose/cp_pose.hpp
smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/pose/cp_pose.cpp

Public Member Functions

Public Attributes

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ Pose() [1/2]

◆ Pose() [2/2]

Member Function Documentation

◆ freezeReferenceFrame()

◆ getFrameId()

◆ getReferenceFrame()

◆ getX()

◆ getY()

◆ getYaw()

◆ getZ()

◆ onInitialize()

◆ setReferenceFrame()

◆ toPoseMsg()

◆ toPoseStampedMsg()

◆ unfreezeReferenceFrame()

◆ update()

◆ waitTransformUpdate()

Member Data Documentation

◆ frozenReferenceFrameTime

◆ isInitialized

◆ listenerMutex_

◆ m_mutex_

◆ pose_

◆ poseFrameName_

◆ referenceFrame_

◆ tfBuffer_

◆ tfListener_