cl_foundation_pose::CpObjectTrackerTf Class Reference

#include <cp_object_tracker_tf.hpp>

Inheritance diagram for cl_foundation_pose::CpObjectTrackerTf:

Collaboration diagram for cl_foundation_pose::CpObjectTrackerTf:

Public Member Functions
	CpObjectTrackerTf (std::string global_frame_id="map")
void	setEnabled (bool enabled)
bool	isEnabled ()
void	resetPoseEstimation ()
void	update () override
void	onInitialize ()
std::optional< geometry_msgs::msg::PoseStamped >	updateAndGetGlobalPose (const std::string &child_frame_id, const std::string &frame_id)
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)

Private Attributes
std::shared_ptr< tf2_ros::Buffer >	tfBuffer_
std::shared_ptr< tf2_ros::TransformListener >	tfListener_
std::shared_ptr< tf2_ros::TransformBroadcaster >	tfBroadcaster_
std::string	global_frame_id_
bool	enabled_ = false
std::map< std::string, DetectedObject >	detectedObjects
std::mutex	m_mutex_

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	onStateOrthogonalAllocation ()
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 30 of file cp_object_tracker_tf.hpp.

Constructor & Destructor Documentation

CpObjectTrackerTf()

cl_foundation_pose::CpObjectTrackerTf::CpObjectTrackerTf ( std::string global_frame_id = "map" )

inline

Definition at line 45 of file cp_object_tracker_tf.hpp.

: global_frame_id_(global_frame_id) {}

Member Function Documentation

isEnabled()

bool cl_foundation_pose::CpObjectTrackerTf::isEnabled ( )

inline

Definition at line 53 of file cp_object_tracker_tf.hpp.

{ return enabled_; }

References enabled_.

Referenced by cl_foundation_pose::CbTrackObjectPose::update().

Here is the caller graph for this function:

onInitialize()

void cl_foundation_pose::CpObjectTrackerTf::onInitialize ( )

inlinevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 102 of file cp_object_tracker_tf.hpp.

  {
    tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->getNode()->get_clock());
    tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
    tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this->getNode());
  }

References smacc2::ISmaccComponent::getNode(), tfBroadcaster_, tfBuffer_, and tfListener_.

Here is the call graph for this function:

resetPoseEstimation()

void cl_foundation_pose::CpObjectTrackerTf::resetPoseEstimation ( )

inline

Definition at line 55 of file cp_object_tracker_tf.hpp.

  {
    std::lock_guard<std::mutex> lock(m_mutex_);
 
    RCLCPP_INFO(
      getLogger(), "CpObjectTrackerTf::resetPoseEstimation() tracked objects: %ld",
      detectedObjects.size());
    for (auto & detectedObject : detectedObjects)
    {
      auto & detectedObjectInfo = detectedObject.second;
      RCLCPP_INFO(
        getLogger(), "CpObjectTrackerTf::resetPoseEstimation() tracking object: %s",
        detectedObject.first.c_str());
 
      detectedObjectInfo.filtered_pose.reset();
      detectedObjectInfo.historicalPoses_.clear();
    }
    detectedObjects.clear();
  }

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

Referenced by setEnabled().

Here is the call graph for this function:

Here is the caller graph for this function:

setEnabled()

void cl_foundation_pose::CpObjectTrackerTf::setEnabled ( bool enabled )

inline

Definition at line 47 of file cp_object_tracker_tf.hpp.

  {
    resetPoseEstimation();
    enabled_ = enabled;
  }

References enabled_, and resetPoseEstimation().

Referenced by cl_foundation_pose::CbTrackObjectPause::onEntry(), cl_foundation_pose::CbTrackObjectPose::onEntry(), cl_foundation_pose::CbTrackObjectPause::onExit(), and cl_foundation_pose::CbTrackObjectPose::onExit().

Here is the call graph for this function:

Here is the caller graph for this function:

update()

void cl_foundation_pose::CpObjectTrackerTf::update ( )

inlineoverridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 75 of file cp_object_tracker_tf.hpp.

  {
    RCLCPP_INFO(
      getLogger(), "CpObjectTrackerTf::update() tracked objects: %ld", detectedObjects.size());
    std::lock_guard<std::mutex> lock(m_mutex_);
 
    if (!enabled_) return;
 
    RCLCPP_INFO(
      getLogger(), "CpObjectTrackerTf::update() heartbeat, tracked objects: %ld",
      detectedObjects.size());
 
    // refresh tracked object poses
    for (auto & detectedObject : detectedObjects)
    {
      RCLCPP_INFO(
        getLogger(), "CpObjectTrackerTf::update() tracking object: %s",
        detectedObject.first.c_str());
 
      auto globalObjectPose = this->updateAndGetGlobalPose(detectedObject.first, global_frame_id_);
      if (globalObjectPose)
      {
        detectedObject.second.filtered_pose = *globalObjectPose;
      }
    }
  }

References detectedObjects, enabled_, smacc2::ISmaccComponent::getLogger(), global_frame_id_, m_mutex_, and updateAndGetGlobalPose().

Here is the call graph for this function:

updateAndGetGlobalPose()

std::optional< geometry_msgs::msg::PoseStamped > cl_foundation_pose::CpObjectTrackerTf::updateAndGetGlobalPose ( const std::string & child_frame_id, const std::string & frame_id )

inline

Definition at line 109 of file cp_object_tracker_tf.hpp.

  {
    RCLCPP_INFO(
      getLogger(), "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s')", child_frame_id.c_str(),
      frame_id.c_str());
    // check in database if the object is already tracked
    auto object = detectedObjects.find(child_frame_id);
    DetectedObject * detectedObject = nullptr;
    if (object != detectedObjects.end())  // already tracked
    {
      detectedObject = &object->second;
      RCLCPP_INFO(
        getLogger(), "[CpObjectTrackerTf] tracking existing object: %s", child_frame_id.c_str());
    }
    else
    {
      detectedObjects[child_frame_id] = DetectedObject();
      detectedObject = &detectedObjects[child_frame_id];
      RCLCPP_INFO(
        getLogger(), "[CpObjectTrackerTf] tracking new object: %s", child_frame_id.c_str());
    }
 
    if (tfBuffer_->canTransform(frame_id, child_frame_id, rclcpp::Time(0)))
    {
      geometry_msgs::msg::PoseStamped pose;
 
      auto transformStamped = tfBuffer_->lookupTransform(frame_id, child_frame_id, rclcpp::Time(0));
      pose.header = transformStamped.header;
      pose.pose.position.x = transformStamped.transform.translation.x;
      pose.pose.position.y = transformStamped.transform.translation.y;
      pose.pose.position.z = transformStamped.transform.translation.z;
      pose.pose.orientation = transformStamped.transform.rotation;
 
      RCLCPP_INFO(
        getLogger(), "[CpObjectTrackerTf] *updateAndGetGlobalPose('%s', '%s') pose: %f, %f, %f",
        child_frame_id.c_str(), frame_id.c_str(), pose.pose.position.x, pose.pose.position.y,
        tf2::getYaw(pose.pose.orientation));
 
      auto & historicalPoses_ = detectedObject->historicalPoses_;
      RCLCPP_INFO(
        getLogger(), "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s') historical poses: %ld",
        child_frame_id.c_str(), frame_id.c_str(), historicalPoses_.size());
 
      const size_t MAX_HISTORY = 512;
      if (historicalPoses_.size() > MAX_HISTORY)
      {
        RCLCPP_INFO(
          getLogger(),
          "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s') historical poses, popping oldest "
          "pose",
          child_frame_id.c_str(), frame_id.c_str());
        historicalPoses_.erase(historicalPoses_.begin());
      }
      RCLCPP_INFO(
        getLogger(),
        "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s') historical poses, pushing new pose",
        child_frame_id.c_str(), frame_id.c_str());
      historicalPoses_.push_back(pose);
 
      RCLCPP_INFO(
        getLogger(),
        "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s') -presort- historical poses: %ld",
        child_frame_id.c_str(), frame_id.c_str(), historicalPoses_.size());
 
      // compute median position in x
      std::sort(
        historicalPoses_.begin(), historicalPoses_.end(),
        [](const geometry_msgs::msg::PoseStamped & a, const geometry_msgs::msg::PoseStamped & b)
        { return a.pose.position.x < b.pose.position.x; });
 
      RCLCPP_INFO(
        getLogger(),
        "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s')- presort- historical poses: %ld",
        child_frame_id.c_str(), frame_id.c_str(), historicalPoses_.size());
 
      geometry_msgs::msg::PoseStamped medianPose;
      medianPose.pose.position.x = historicalPoses_[historicalPoses_.size() / 2].pose.position.x;
 
      // compute median position in y
      std::sort(
        historicalPoses_.begin(), historicalPoses_.end(),
        [](const geometry_msgs::msg::PoseStamped & a, const geometry_msgs::msg::PoseStamped & b)
        { return a.pose.position.y < b.pose.position.y; });
 
      medianPose.pose.position.y = historicalPoses_[historicalPoses_.size() / 2].pose.position.y;
      medianPose.header = pose.header;
 
      detectedObject->filtered_pose = medianPose;
 
      RCLCPP_INFO(
        getLogger(),
        "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s') filtered pose [%ld samples]: %f, "
        "%f, %f",
        child_frame_id.c_str(), frame_id.c_str(), detectedObject->historicalPoses_.size(),
        detectedObject->filtered_pose->pose.position.x,
        detectedObject->filtered_pose->pose.position.y,
        tf2::getYaw(detectedObject->filtered_pose->pose.orientation));
      this->postEvent<EvObjectDetected>();
 
      return detectedObject->filtered_pose;
    }
 
    RCLCPP_INFO(
      getLogger(), "[CpObjectTrackerTf] updateAndGetGlobalPose('%s', '%s') failed",
      child_frame_id.c_str(), frame_id.c_str());
 
    return std::nullopt;
  }

References detectedObjects, cl_foundation_pose::DetectedObject::filtered_pose, smacc2::ISmaccComponent::getLogger(), cl_foundation_pose::DetectedObject::historicalPoses_, smacc2::ISmaccComponent::postEvent(), and tfBuffer_.

Referenced by cl_foundation_pose::CbTrackObjectPose::onEntry(), cl_foundation_pose::CbTrackObjectPose::update(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

detectedObjects

std::map<std::string, DetectedObject> cl_foundation_pose::CpObjectTrackerTf::detectedObjects

private

Definition at line 41 of file cp_object_tracker_tf.hpp.

Referenced by resetPoseEstimation(), update(), and updateAndGetGlobalPose().

enabled_

bool cl_foundation_pose::CpObjectTrackerTf::enabled_ = false

private

Definition at line 38 of file cp_object_tracker_tf.hpp.

Referenced by isEnabled(), setEnabled(), and update().

global_frame_id_

std::string cl_foundation_pose::CpObjectTrackerTf::global_frame_id_

private

Definition at line 37 of file cp_object_tracker_tf.hpp.

Referenced by update().

m_mutex_

std::mutex cl_foundation_pose::CpObjectTrackerTf::m_mutex_

private

Definition at line 42 of file cp_object_tracker_tf.hpp.

Referenced by resetPoseEstimation(), and update().

tfBroadcaster_

std::shared_ptr<tf2_ros::TransformBroadcaster> cl_foundation_pose::CpObjectTrackerTf::tfBroadcaster_

private

Definition at line 35 of file cp_object_tracker_tf.hpp.

Referenced by onInitialize().

tfBuffer_

std::shared_ptr<tf2_ros::Buffer> cl_foundation_pose::CpObjectTrackerTf::tfBuffer_

private

Definition at line 33 of file cp_object_tracker_tf.hpp.

Referenced by onInitialize(), and updateAndGetGlobalPose().

tfListener_

std::shared_ptr<tf2_ros::TransformListener> cl_foundation_pose::CpObjectTrackerTf::tfListener_

private

Definition at line 34 of file cp_object_tracker_tf.hpp.

Referenced by onInitialize().

---

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

• smacc2_client_library/cl_foundation_pose/include/cl_foundation_pose/components/cp_object_tracker_tf.hpp