Instantiates the PerceptionModule
specifically for PoseEstimator.
More...
#include <aikido/perception/PoseEstimatorModule.hpp>
|
| 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...
|
|
virtual | ~PerceptionModule ()=default |
|
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
.
◆ 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] | nodeHandle | The node handle to be passed to the detector |
[in] | markerTopic | The name of the topic on which objects' pose information is being published |
[in] | assetData | The pointer to the loader of visual asset data ( |
- See also
- AssetDatabase)
- Parameters
-
[in] | resourceRetriever | A CatkinResourceRetriever for resources related to config files and models |
[in] | referenceFrameId | The desired reference frame for the object pose |
[in] | referenceLink | A link on the robot with respect to which the pose is transformed |
◆ ~PoseEstimatorModule()
virtual aikido::perception::PoseEstimatorModule::~PoseEstimatorModule |
( |
| ) |
|
|
virtualdefault |
◆ 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] | env | World to add perceived objects to. |
[in] | timeout | The duration up to which to wait for the transform. Returns false if none of the markers get correctly transformed |
[in] | timestamp | Only 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] | detectedObjects | An 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.
◆ 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
To retrieve resources from disk and from packages.
◆ mTfListener
tf::TransformListener aikido::perception::PoseEstimatorModule::mTfListener |
|
private |
Listens to the transform attached to the node.