cl_foundation_pose::CpObjectTracker1 Class Reference

#include <cp_object_tracker_1.hpp>

Inheritance diagram for cl_foundation_pose::CpObjectTracker1:

Collaboration diagram for cl_foundation_pose::CpObjectTracker1:

Public Member Functions
void	onInitialize ()
void	onDetection3DArrayReceived (const vision_msgs::msg::Detection3DArray &msg)
std::optional< geometry_msgs::msg::PoseStamped >	getPose (const std::string &object_id)
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Private Attributes
CpTopicSubscriber< vision_msgs::msg::Detection3DArray > *	fondationPoseTopic_
std::map< std::string, DetectedObject >	detectedObjects

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 Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 26 of file cp_object_tracker_1.hpp.

Member Function Documentation

getPose()

std::optional< geometry_msgs::msg::PoseStamped > cl_foundation_pose::CpObjectTracker1::getPose ( const std::string & object_id )

inline

Definition at line 64 of file cp_object_tracker_1.hpp.

  {
    auto object = detectedObjects.find(object_id);
    if (object != detectedObjects.end())
    {
      geometry_msgs::msg::PoseStamped pose;
      pose.header = object->second.msg.header;
      pose.pose = object->second.msg.results[0].pose.pose;
      return pose;
    }
    return std::nullopt;
  }

References detectedObjects.

onDetection3DArrayReceived()

void cl_foundation_pose::CpObjectTracker1::onDetection3DArrayReceived ( const vision_msgs::msg::Detection3DArray & msg )

inline

Definition at line 39 of file cp_object_tracker_1.hpp.

  {
    RCLCPP_INFO(getLogger(), "Received %ld detections", msg.detections.size());
 
    for (auto & detection : msg.detections)
    {
      auto previouslyExistingObjectEntry = detectedObjects.find(detection.id);
 
      // if we have seen this object before...
      if (previouslyExistingObjectEntry != detectedObjects.end())
      {
        auto & previouslyExistingObject = previouslyExistingObjectEntry->second;
        previouslyExistingObject.msg = detection;
      }
      else
      {
        DetectedObject detectedObject;
        detectedObject.msg = detection;
        detectedObjects[detection.id] = detectedObject;
      }
 
      this->postEvent<EvObjectDetected>();
    }
  }

References detectedObjects, smacc2::ISmaccComponent::getLogger(), cl_foundation_pose::DetectedObject::msg, and smacc2::ISmaccComponent::postEvent().

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_foundation_pose::CpObjectTracker1::onInitialize ( )

inlinevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 30 of file cp_object_tracker_1.hpp.

  {
    // Gain access to the foundationpose subscriber component.
    requiresComponent(fondationPoseTopic_);
 
    // Then hook each received message to store it into our little map/database.
    fondationPoseTopic_->onMessageReceived(&CpObjectTracker1::onDetection3DArrayReceived, this);
  }

References fondationPoseTopic_, onDetection3DArrayReceived(), smacc2::client_core_components::CpTopicSubscriber< MessageType >::onMessageReceived(), and smacc2::ISmaccComponent::requiresComponent().

Here is the call graph for this function:

Member Data Documentation

detectedObjects

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

private

Definition at line 82 of file cp_object_tracker_1.hpp.

Referenced by getPose(), and onDetection3DArrayReceived().

fondationPoseTopic_

CpTopicSubscriber<vision_msgs::msg::Detection3DArray>* cl_foundation_pose::CpObjectTracker1::fondationPoseTopic_

private

Definition at line 79 of file cp_object_tracker_1.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_1.hpp