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

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)

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

ISmaccStateMachine *	getStateMachine ()

virtual void	update ()=0

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Definition at line 44 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

◆ getFrameId()

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

inline

Definition at line 79 of file cp_pose.hpp.

79{ return poseFrameName_; }

References poseFrameName_.

◆ getReferenceFrame()

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

inline

Definition at line 77 of file cp_pose.hpp.

77{ return referenceFrame_; }

References referenceFrame_.

Referenced by cl_nav2z::CbNavigateGlobalPosition::execute(), cl_nav2z::CbAbsoluteRotate::onEntry(), cl_nav2z::CbNavigateBackwards::onEntry(), cl_nav2z::CbNavigateForward::onEntry(), and cl_nav2z::CbRotate::onEntry().

Here is the caller graph for this function:

◆ 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_.

Referenced by sm_dance_bot_warehouse::cl_nav2z::CpSquareShapeBoundary::getForwardDistance(), sm_dance_bot_warehouse_2::cl_nav2z::CpSquareShapeBoundary::getForwardDistance(), and sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary::getForwardDistance().

Here is the caller graph for this function:

◆ 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.

{
  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:

◆ toPoseMsg()

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

inline

Definition at line 58 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::WaypointNavigator::sendNextGoal().

Here is the caller graph for this function:

◆ toPoseStampedMsg()

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

inline

Definition at line 64 of file cp_pose.hpp.

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

References m_mutex_, and pose_.

◆ update()

void cl_nav2z::Pose::update ( )

overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 124 of file cp_pose.cpp.

{
  tf2::Stamped<tf2::Transform> transform;
  try
  {
    {
      std::lock_guard<std::mutex> lock(listenerMutex_);
      RCLCPP_DEBUG(getLogger(), "[pose] looking up transform");
      auto transformstamped =
        tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, rclcpp::Time(0));
      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->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 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_);
        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_.

Referenced by 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 81 of file cp_pose.hpp.

Referenced by update(), and waitTransformUpdate().

◆ listenerMutex_

std::mutex cl_nav2z::Pose::listenerMutex_

staticprivate

Definition at line 89 of file cp_pose.hpp.

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

◆ m_mutex_

std::mutex cl_nav2z::Pose::m_mutex_

private

Definition at line 94 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 84 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 91 of file cp_pose.hpp.

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

◆ referenceFrame_

std::string cl_nav2z::Pose::referenceFrame_

private

Definition at line 92 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 86 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 87 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

◆ getFrameId()

◆ getReferenceFrame()

◆ getX()

◆ getY()

◆ getYaw()

◆ getZ()

◆ onInitialize()

◆ toPoseMsg()

◆ toPoseStampedMsg()

◆ update()

◆ waitTransformUpdate()

Member Data Documentation

◆ isInitialized

◆ listenerMutex_

◆ m_mutex_

◆ pose_

◆ poseFrameName_

◆ referenceFrame_

◆ tfBuffer_

◆ tfListener_