#include <cp_pose.h>

Inheritance diagram for cl_move_base_z::Pose:

Collaboration diagram for cl_move_base_z::Pose:

Public Member Functions
	Pose (std::string poseFrameName="base_link", std::string referenceFrame="odom")

virtual void	update () override

void	waitTransformUpdate (ros::Rate r=ros::Rate(20))

geometry_msgs::Pose	toPoseMsg ()

geometry_msgs::PoseStamped	toPoseStampedMsg ()

const std::string &	getReferenceFrame () const

const std::string &	getFrameId () const

Public Member Functions inherited from smacc::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Public Member Functions inherited from smacc::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (ros::Duration duration)

void	executeUpdate ()

void	setUpdatePeriod (ros::Duration duration)

Public Attributes
bool	isInitialized

Private Attributes
geometry_msgs::PoseStamped	pose_

std::string	poseFrameName_

std::string	referenceFrame_

std::mutex	m_mutex_

Static Private Attributes
static std::shared_ptr< tf::TransformListener >	tfListener_ = nullptr

static std::mutex	listenerMutex_

Additional Inherited Members
Protected Member Functions inherited from smacc::ISmaccComponent
virtual void	initialize (ISmaccClient *owner)

void	setStateMachine (ISmaccStateMachine *stateMachine)

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)

virtual void	onInitialize ()

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)

virtual void	update ()=0

Protected Attributes inherited from smacc::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Definition at line 18 of file cp_pose.h.

Constructor & Destructor Documentation

◆ Pose()

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

Definition at line 15 of file cp_pose.cpp.

        : poseFrameName_(targetFrame),
          referenceFrame_(referenceFrame),
          isInitialized(false),
          m_mutex_()
    {
        this->pose_.header.frame_id = referenceFrame_;
        ROS_INFO("[Pose] Creating Pose tracker component to track %s in the reference frame %s", targetFrame.c_str(), referenceFrame.c_str());
 
        {
            //singleton
            std::lock_guard<std::mutex> guard(listenerMutex_);
            if (tfListener_ == nullptr)
                tfListener_ = std::make_shared<tf::TransformListener>();
        }
    }

References listenerMutex_, pose_, referenceFrame_, and tfListener_.

Member Function Documentation

◆ getFrameId()

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

inline

Definition at line 44 of file cp_pose.h.

    {
        return poseFrameName_;
    }

References poseFrameName_.

◆ getReferenceFrame()

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

inline

Definition at line 39 of file cp_pose.h.

    {
        return referenceFrame_;
    }

References referenceFrame_.

Referenced by cl_move_base_z::CbNavigateGlobalPosition::execute(), cl_move_base_z::CbAbsoluteRotate::onEntry(), cl_move_base_z::CbNavigateBackwards::onEntry(), cl_move_base_z::CbNavigateForward::onEntry(), cl_move_base_z::CbRotate::onEntry(), and cl_move_base_z::CbUndoPathBackwards2::publishMarkers().

Here is the caller graph for this function:

◆ toPoseMsg()

geometry_msgs::Pose cl_move_base_z::Pose::toPoseMsg ( )

inline

Definition at line 27 of file cp_pose.h.

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

References m_mutex_, and pose_.

Referenced by cl_move_base_z::CbUndoPathBackwards2::onEntry(), and cl_move_base_z::CbUndoPathBackwards2::update().

Here is the caller graph for this function:

◆ toPoseStampedMsg()

geometry_msgs::PoseStamped cl_move_base_z::Pose::toPoseStampedMsg ( )

inline

Definition at line 33 of file cp_pose.h.

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

References m_mutex_, and pose_.

◆ update()

void cl_move_base_z::Pose::update ( )

overridevirtual

Implements smacc::ISmaccUpdatable.

Definition at line 64 of file cp_pose.cpp.

    {
        tf::StampedTransform transform;
        try
        {
            {
                std::lock_guard<std::mutex> lock(listenerMutex_);
                tfListener_->lookupTransform(referenceFrame_, poseFrameName_,
                                                  ros::Time(0), transform);
            }
 
            {
                std::lock_guard<std::mutex> guard(m_mutex_);
                tf::poseTFToMsg(transform, this->pose_.pose);
                this->pose_.header.stamp = transform.stamp_;
                this->isInitialized = true;
            }
        }
        catch (tf::TransformException ex)
        {
            ROS_ERROR_STREAM_THROTTLE(1, "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_ << "] ) is failing on pose update : " << ex.what());
        }
    }

References isInitialized, listenerMutex_, m_mutex_, pose_, poseFrameName_, referenceFrame_, and tfListener_.

◆ waitTransformUpdate()

void cl_move_base_z::Pose::waitTransformUpdate ( ros::Rate r = ros::Rate(20) )

Definition at line 32 of file cp_pose.cpp.

    {
        bool found = false;
        while (ros::ok() && !found)
        {
            tf::StampedTransform transform;
            try
            {
                {
                    std::lock_guard<std::mutex> lock(listenerMutex_);
                    tfListener_->lookupTransform(referenceFrame_, poseFrameName_,
                                                      ros::Time(0), transform);
                }
 
                {
                    std::lock_guard<std::mutex> guard(m_mutex_);
                    tf::poseTFToMsg(transform, this->pose_.pose);
                    this->pose_.header.stamp = transform.stamp_;
                    found = true;
                    this->isInitialized = true;
                }
            }
            catch (tf::TransformException ex)
            {
                ROS_ERROR_STREAM_THROTTLE(1, "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_ << "] ) is failing on pose update : " << ex.what());
            }
 
            r.sleep();
            ros::spinOnce();
        }
    }

References isInitialized, listenerMutex_, m_mutex_, pose_, poseFrameName_, referenceFrame_, and tfListener_.

Member Data Documentation

◆ isInitialized

bool cl_move_base_z::Pose::isInitialized

Definition at line 49 of file cp_pose.h.

Referenced by update(), and waitTransformUpdate().

◆ listenerMutex_

std::mutex cl_move_base_z::Pose::listenerMutex_

staticprivate

Definition at line 55 of file cp_pose.h.

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

◆ m_mutex_

std::mutex cl_move_base_z::Pose::m_mutex_

private

Definition at line 60 of file cp_pose.h.

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

◆ pose_

geometry_msgs::PoseStamped cl_move_base_z::Pose::pose_

private

Definition at line 52 of file cp_pose.h.

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

◆ poseFrameName_

std::string cl_move_base_z::Pose::poseFrameName_

private

Definition at line 57 of file cp_pose.h.

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

◆ referenceFrame_

std::string cl_move_base_z::Pose::referenceFrame_

private

Definition at line 58 of file cp_pose.h.

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

◆ tfListener_

std::shared_ptr< tf::TransformListener > cl_move_base_z::Pose::tfListener_ = nullptr

staticprivate

Definition at line 54 of file cp_pose.h.

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

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

smacc_client_library/move_base_z_client/move_base_z_client_plugin/include/move_base_z_client_plugin/components/pose/cp_pose.h
smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/components/pose/cp_pose.cpp

Public Member Functions

Public Attributes

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ Pose()

Member Function Documentation

◆ getFrameId()

◆ getReferenceFrame()

◆ toPoseMsg()

◆ toPoseStampedMsg()

◆ update()

◆ waitTransformUpdate()

Member Data Documentation

◆ isInitialized

◆ listenerMutex_

◆ m_mutex_

◆ pose_

◆ poseFrameName_

◆ referenceFrame_

◆ tfListener_