Aikido
aikido::perception::PoseEstimatorModule Class Reference

Instantiates the PerceptionModule specifically for PoseEstimator. More...

#include <aikido/perception/PoseEstimatorModule.hpp>

Inheritance diagram for aikido::perception::PoseEstimatorModule:
aikido::perception::PerceptionModule

Public Member Functions

 PoseEstimatorModule (ros::NodeHandle nodeHandle, const std::string &markerTopic, std::shared_ptr< AssetDatabase > assetData, std::shared_ptr< aikido::io::CatkinResourceRetriever > resourceRetriever, const std::string &referenceFrameId, dart::dynamics::Frame *referenceLink)
 Construct a PoseEstimator receiver that subscribes to the specified topic where objects' pose information is being published as a visualization_msg::MarkerArray, uses a database loader for configuration information related to tags. More...
 
virtual ~PoseEstimatorModule ()=default
 
bool detectObjects (const aikido::planner::WorldPtr &env, ros::Duration timeout=ros::Duration(), ros::Time timestamp=ros::Time(0.0), std::vector< DetectedObject > *detectedObjects=nullptr) override
 Run the specific detector via a service call or ROS message reception, lookup the transform between the specified frames, load object models for new objects, compute their pose, and update the list of DART Skeletons that represents the world. More...
 
- Public Member Functions inherited from aikido::perception::PerceptionModule
virtual ~PerceptionModule ()=default
 

Private Attributes

ros::NodeHandle mNodeHandle
 For the ROS node that will work with the April Tags module. More...
 
std::string mMarkerTopic
 The name of the ROS topic to read marker info from. More...
 
std::shared_ptr< AssetDatabasemAssetData
 The pointer to the loader of visual asset data. More...
 
std::shared_ptr< aikido::io::CatkinResourceRetrievermResourceRetriever
 To retrieve resources from disk and from packages. More...
 
std::string mReferenceFrameId
 The desired reference frame for the object pose. More...
 
dart::dynamics::Frame * mReferenceLink
 The reference frame of HERB to transform with respect to. More...
 
tf::TransformListener mTfListener
 Listens to the transform attached to the node. More...
 

Detailed Description

Instantiates the PerceptionModule specifically for PoseEstimator.

It receives input from the PoseEstimator running in a separate process, by subscribing to a visualization_msg::MarkerArray ROS topic published by the PoseEstimator node. It uses a AssetDatabase to resolve each marker to a dart::common::Uri, and updates the environment which is an aikido::planner::World.

Constructor & Destructor Documentation

◆ PoseEstimatorModule()

aikido::perception::PoseEstimatorModule::PoseEstimatorModule ( ros::NodeHandle  nodeHandle,
const std::string &  markerTopic,
std::shared_ptr< AssetDatabase assetData,
std::shared_ptr< aikido::io::CatkinResourceRetriever resourceRetriever,
const std::string &  referenceFrameId,
dart::dynamics::Frame *  referenceLink 
)

Construct a PoseEstimator receiver that subscribes to the specified topic where objects' pose information is being published as a visualization_msg::MarkerArray, uses a database loader for configuration information related to tags.

Parameters
[in]nodeHandleThe node handle to be passed to the detector
[in]markerTopicThe name of the topic on which objects' pose information is being published
[in]assetDataThe pointer to the loader of visual asset data (
See also
AssetDatabase)
Parameters
[in]resourceRetrieverA CatkinResourceRetriever for resources related to config files and models
[in]referenceFrameIdThe desired reference frame for the object pose
[in]referenceLinkA link on the robot with respect to which the pose is transformed

◆ ~PoseEstimatorModule()

virtual aikido::perception::PoseEstimatorModule::~PoseEstimatorModule ( )
virtualdefault

Member Function Documentation

◆ detectObjects()

bool aikido::perception::PoseEstimatorModule::detectObjects ( const aikido::planner::WorldPtr env,
ros::Duration  timeout = ros::Duration(),
ros::Time  timestamp = ros::Time(0.0),
std::vector< DetectedObject > *  detectedObjects = nullptr 
)
overridevirtual

Run the specific detector via a service call or ROS message reception, lookup the transform between the specified frames, load object models for new objects, compute their pose, and update the list of DART Skeletons that represents the world.

Parameters
[in]envWorld to add perceived objects to.
[in]timeoutThe duration up to which to wait for the transform. Returns false if none of the markers get correctly transformed
[in]timestampOnly detections more recent than this timestamp will be accepted. A timestamp of 0 greedily takes the first available message, and is the default behaviour.
[out]detectedObjectsAn output vector for detected objects before timeout.
Returns
Returns false if no detection observed before timeout. Returns true otherwise. Looks for the following information sent via ROS (see DetectedObject): visualization_msgs/Marker message like the following: Marker.header.frame_id -> detectionFrameID Marker.ns + "_" + Marker.id -> objUid (identity in planner::World) Marker.ns -> objAssetKey (determines visual asset, see AssetDatabase)

Implements aikido::perception::PerceptionModule.

Member Data Documentation

◆ mAssetData

std::shared_ptr<AssetDatabase> aikido::perception::PoseEstimatorModule::mAssetData
private

The pointer to the loader of visual asset data.

◆ mMarkerTopic

std::string aikido::perception::PoseEstimatorModule::mMarkerTopic
private

The name of the ROS topic to read marker info from.

◆ mNodeHandle

ros::NodeHandle aikido::perception::PoseEstimatorModule::mNodeHandle
private

For the ROS node that will work with the April Tags module.

◆ mReferenceFrameId

std::string aikido::perception::PoseEstimatorModule::mReferenceFrameId
private

The desired reference frame for the object pose.

◆ mReferenceLink

dart::dynamics::Frame* aikido::perception::PoseEstimatorModule::mReferenceLink
private

The reference frame of HERB to transform with respect to.

◆ mResourceRetriever

std::shared_ptr<aikido::io::CatkinResourceRetriever> aikido::perception::PoseEstimatorModule::mResourceRetriever
private

To retrieve resources from disk and from packages.

◆ mTfListener

tf::TransformListener aikido::perception::PoseEstimatorModule::mTfListener
private

Listens to the transform attached to the node.