From a1ef21ae359664ad761a7e82e733d3e02113dc92 Mon Sep 17 00:00:00 2001 From: Youngsun Kim Date: Wed, 13 Feb 2019 23:51:59 -0800 Subject: [PATCH] Enhancement/pose estimator (#497) * Pose estimator enhancement * Add cpp classes in perception * clean up hancks for demo * update CHANGELOG.md * add a new class DetectedObject and update PoseEstimatorModule * minor fixes * remove the projection height from PoseEstimatorModule * document DetectedObject class * change 'id' -> 'uid' * Update include/aikido/perception/DetectedObject.hpp Co-Authored-By: yskim041 * Update include/aikido/perception/DetectedObject.hpp Co-Authored-By: yskim041 * Update include/aikido/perception/DetectedObject.hpp Co-Authored-By: yskim041 * change the way to parse a Marker message * Allow nullptr for detectedObjects argument * Added robustness against bad YAML strings, plus more easily-parse-able UIDs * Fixed documentation per comments, tested with simple_perception in ada_demos * Added default argument to detectedObjects. * Fixed comments on pull request * Indigo compatibility (no DELETEALL) * Apply suggestions from code review Added non-refactor changes from Brian Co-Authored-By: egordon * Applied comments from PR review: refactored ObjectDatabase to AssetDatabase * Clang formatted * Fixed PR comments. * Move vector push to end of for loop * Final PR nits. --- CHANGELOG.md | 1 + include/aikido/perception.hpp | 7 +- .../{ObjectDatabase.hpp => AssetDatabase.hpp} | 46 +++++----- include/aikido/perception/DetectedObject.hpp | 91 +++++++++++++++++++ .../aikido/perception/PerceptionModule.hpp | 12 ++- .../aikido/perception/PoseEstimatorModule.hpp | 31 ++++--- .../{ObjectDatabase.cpp => AssetDatabase.cpp} | 47 ++++++---- src/perception/CMakeLists.txt | 3 +- src/perception/DetectedObject.cpp | 74 +++++++++++++++ src/perception/PoseEstimatorModule.cpp | 83 ++++++++++++----- 10 files changed, 312 insertions(+), 83 deletions(-) rename include/aikido/perception/{ObjectDatabase.hpp => AssetDatabase.hpp} (53%) create mode 100644 include/aikido/perception/DetectedObject.hpp rename src/perception/{ObjectDatabase.cpp => AssetDatabase.cpp} (51%) create mode 100644 src/perception/DetectedObject.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c7e25e09d..b6db27b8b5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -43,6 +43,7 @@ * Added integrated PoseEstimatorModule: [#336](https://github.com/personalrobotics/aikido/pull/336) * Added voxel grid perception module: [#448](https://github.com/personalrobotics/aikido/pull/448) + * Added YAML communication between PoseEstimatorModule and third-party perception algorithms: [#497](https://github.com/personalrobotics/aikido/pull/497) * Trajectory diff --git a/include/aikido/perception.hpp b/include/aikido/perception.hpp index 61dcd2f0f1..1eed8f2888 100644 --- a/include/aikido/perception.hpp +++ b/include/aikido/perception.hpp @@ -1,5 +1,6 @@ -#include "perception/AprilTagsDatabase.hpp" -#include "perception/AprilTagsModule.hpp" +#include "perception/AssetDatabase.hpp" +#include "perception/DetectedObject.hpp" #include "perception/PerceptionModule.hpp" -#include "perception/YamlAprilTagsDatabase.hpp" +#include "perception/PoseEstimatorModule.hpp" +#include "perception/VoxelGridModule.hpp" #include "perception/shape_conversions.hpp" diff --git a/include/aikido/perception/ObjectDatabase.hpp b/include/aikido/perception/AssetDatabase.hpp similarity index 53% rename from include/aikido/perception/ObjectDatabase.hpp rename to include/aikido/perception/AssetDatabase.hpp index 17413c7275..db4076371c 100644 --- a/include/aikido/perception/ObjectDatabase.hpp +++ b/include/aikido/perception/AssetDatabase.hpp @@ -1,5 +1,5 @@ -#ifndef AIKIDO_PERCEPTION_OBJECT_DATABASE_HPP_ -#define AIKIDO_PERCEPTION_OBJECT_DATABASE_HPP_ +#ifndef AIKIDO_PERCEPTION_ASSET_DATABASE_HPP_ +#define AIKIDO_PERCEPTION_ASSET_DATABASE_HPP_ #include #include @@ -10,8 +10,8 @@ namespace aikido { namespace perception { -/// Instantiation of ObjectDatabase that reads of JSON file containing the -/// information that maps object keys to the object names and resources. +/// Instantiation of AssetDatabase that reads of JSON file containing the +/// information that maps object keys to visual assets and resources. /// The JSON file should have a map with object keys. /// Each such key points to a nested map, where the keys are @@ -22,9 +22,9 @@ namespace perception { /// /// Here is an example entry in a JSON file: /// \code -/// "obj_key": { -/// "resource": "package://pr_ordata/data/objects/obj_filename.urdf", -/// "name": "obj_name", +/// "asset_key": { +/// "resource": "package://pr_assets/data/objects/obj_filename.urdf", +/// "name": "asset_name", /// "offset": [ /// [1.0, 0.0, 0.0, 0.0], /// [0.0, 1.0, 0.0, 0.0], @@ -34,34 +34,34 @@ namespace perception { /// } /// \endcode -class ObjectDatabase +class AssetDatabase { public: - /// Construct a \c ObjectDatabase that uses \c ResourceRetriever to + /// Construct a \c AssetDatabase that uses \c ResourceRetriever to /// load configuration data from a JSON file at URI \c configDataURI. /// \param[in] resourceRetriever The pointer to obtain the configuration file /// \param[in] configDataURI The URI for the configuration information file - ObjectDatabase( + AssetDatabase( const dart::common::ResourceRetrieverPtr& resourceRetriever, const dart::common::Uri& configDataURI); - virtual ~ObjectDatabase() = default; + virtual ~AssetDatabase() = default; /// Get the object name, resource, and offset from database by objectKey - /// \param[in] objectKey The key (string) of an object in ObjectDatabase - /// \param[out] objectName The retrieved object name from ObjectDatabase - /// \param[out] objectResource The retrieved uri of the object - /// \param[out] objectOffset The retrieved offset matrix of the object - /// i.e. the offset between a tag and the actual origin of an object - void getObjectByKey( - const std::string& objectKey, - std::string& objectName, - dart::common::Uri& objectResource, - Eigen::Isometry3d& objectOffset) const; + /// \param[in] assetKey The key (string) of an object in AssetDatabase + /// \param[out] assetName The retrieved object name from AssetDatabase + /// \param[out] assetResource The retrieved uri of the object + /// \param[out] assetOffset The retrieved offset matrix of the object + /// e.g. the offset between a tag and the actual origin of an object + void getAssetByKey( + const std::string& assetKey, + std::string& assetName, + dart::common::Uri& assetResource, + Eigen::Isometry3d& assetOffset) const; private: - /// The map of object keys to object names and resources for models - YAML::Node mObjData; + /// The map of asset keys to object names and resources for models + YAML::Node mAssetData; }; } // namespace perception diff --git a/include/aikido/perception/DetectedObject.hpp b/include/aikido/perception/DetectedObject.hpp new file mode 100644 index 0000000000..fe0a2868c2 --- /dev/null +++ b/include/aikido/perception/DetectedObject.hpp @@ -0,0 +1,91 @@ +#ifndef AIKIDO_PERCEPTION_DETECTEDOBJECT_HPP_ +#define AIKIDO_PERCEPTION_DETECTEDOBJECT_HPP_ + +#include +#include "aikido/io/yaml.hpp" + +namespace aikido { +namespace perception { + +/// DetectedObject delegates a detected object from a third-party +/// perception algorithm. + +/// A perception algorithm should send information for an object via ROS +/// visualization_msgs/Marker message like the following: +/// Marker.ns + "_" + Marker.id -> uid (identity in planner::World) +/// Marker.ns -> assetKey (determines visual asset, \see AssetDatabase) +/// Marker.header.frame_id -> detectionFrameID +/// Marker.text -> yamlStr + +/// yamlStr contains any additional information in YAML format. +/// It also can overwrite the Asset Database Key +/// using "db_key". (\see AssetDatabase for details) +/// Here is an example: +/// \code +/// { +/// "db_key": "food_item", +/// "score": 0.9, +/// "success_rates": [0.4, 0.9, 0.1, 0.2], +/// "strategies": ["vertical_0", "vertical_90", "tilted_0", "tilted_90"], +/// } +/// \endcode + +class DetectedObject +{ +public: + /// Construct a \c DetectedObject + /// \param[in] uid Unique ID for object in Aikido world. Same UID -> Same + /// Object + /// \param[in] assetKey Key for AssetDatabase passed into constructor of + /// PoseEstimatorModule. Defines visuals / assets. + /// \param[in] detectionFrameID Frame ID from ROS Marker + /// \param[in] yamlStr String of additional parameters for object. Can + /// override objAssetDBKey by specifying "db_key". + DetectedObject( + const std::string& uid, + const std::string& assetKey, + const std::string& detectionFrameID, + const std::string& yamlStr); + + virtual ~DetectedObject() = default; + + /// Get the unique id of the object + std::string getUid(); + + /// Get the object key for \c AssetDatabase + std::string getAssetKey(); + + /// Get the detection frame id that refers the origin of this object's pose + std::string getDetectionFrameID(); + + /// Get the map of keys to additional informations + YAML::Node getYamlNode(); + + /// Get a specific value from the information map by a key and the typename + /// of the field + /// \param[in] key The key (string) of a field in the information map + /// Sequence types (e.g. [1, 2]) can be read into standard containers (e.g. + /// std::vector) + /// Map types are not supported with this function. Please get the manually + /// with getYamlNode(). + template + T getInfoByKey(const std::string& key); + +private: + /// The unique id of the object + std::string mUid; + + /// The object key for \c AssetDatabase + std::string mAssetKey; + + /// The detection frame id that refers the origin of this object's pose + std::string mDetectionFrameID; + + /// The information map with additional information of this object + YAML::Node mYamlNode; +}; + +} // namespace perception +} // namespace aikido + +#endif // AIKIDO_PERCEPTION_DETECTEDOBJECT_HPP_ diff --git a/include/aikido/perception/PerceptionModule.hpp b/include/aikido/perception/PerceptionModule.hpp index adbe1aacee..aeff2beeb2 100644 --- a/include/aikido/perception/PerceptionModule.hpp +++ b/include/aikido/perception/PerceptionModule.hpp @@ -1,6 +1,7 @@ #ifndef AIKIDO_PERCEPTION_PERCEPTIONMODULE_HPP_ #define AIKIDO_PERCEPTION_PERCEPTIONMODULE_HPP_ +#include "aikido/perception/DetectedObject.hpp" #include "aikido/planner/World.hpp" namespace aikido { @@ -26,13 +27,16 @@ class PerceptionModule /// \param[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. - /// \return Returns \c false if no detection observed, or if none of the - /// detections has a more recent timestamp than the parameter. Returns \c true + /// \param[out] detectedObjects An output vector for detected objects before + /// timeout. + /// \return Returns \c false if no detection observed before timeout. Returns + /// \c true /// otherwise. virtual bool detectObjects( const aikido::planner::WorldPtr& env, - ros::Duration timeout, - ros::Time timestamp) + ros::Duration timeout = ros::Duration(), + ros::Time timestamp = ros::Time(0.0), + std::vector* detectedObjects = nullptr) = 0; }; diff --git a/include/aikido/perception/PoseEstimatorModule.hpp b/include/aikido/perception/PoseEstimatorModule.hpp index 1e2c6a4b68..c223c516c5 100644 --- a/include/aikido/perception/PoseEstimatorModule.hpp +++ b/include/aikido/perception/PoseEstimatorModule.hpp @@ -6,7 +6,9 @@ #include #include #include "aikido/io/CatkinResourceRetriever.hpp" -#include "aikido/perception/ObjectDatabase.hpp" +#include "aikido/io/yaml.hpp" +#include "aikido/perception/AssetDatabase.hpp" +#include "aikido/perception/DetectedObject.hpp" #include "aikido/perception/PerceptionModule.hpp" namespace aikido { @@ -17,7 +19,7 @@ namespace perception { /// It receives input from the PoseEstimator running in a separate process, /// by subscribing to a \c visualization_msg::MarkerArray ROS topic /// published by the PoseEstimator node. -/// It uses a \c ObjectDatabase to resolve each marker to a +/// It uses a \c AssetDatabase to resolve each marker to a /// \c dart::common::Uri, and updates the environment which is an /// \c aikido::planner::World. class PoseEstimatorModule : public PerceptionModule @@ -31,7 +33,8 @@ class PoseEstimatorModule : public PerceptionModule /// \param[in] nodeHandle The node handle to be passed to the detector /// \param[in] markerTopic The name of the topic on which objects' pose /// information is being published - /// \param[in] configData The pointer to the loader of configuration data + /// \param[in] assetData The pointer to the loader of visual asset data (\see + /// AssetDatabase) /// \param[in] resourceRetriever A CatkinResourceRetriever for resources /// related to config files and models /// \param[in] referenceFrameId The desired reference frame for the @@ -40,19 +43,25 @@ class PoseEstimatorModule : public PerceptionModule /// pose is transformed PoseEstimatorModule( ros::NodeHandle nodeHandle, - std::string markerTopic, - std::shared_ptr configData, + const std::string& markerTopic, + std::shared_ptr assetData, std::shared_ptr resourceRetriever, - std::string referenceFrameId, + const std::string& referenceFrameId, dart::dynamics::Frame* referenceLink); virtual ~PoseEstimatorModule() = default; - // Documentation inherited + /// \copydoc PerceptionModule::detectObjects() + /// Looks for the following information sent via ROS (see \c 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) bool detectObjects( const aikido::planner::WorldPtr& env, - ros::Duration timeout = ros::Duration(0.0), - ros::Time timestamp = ros::Time(0.0)) override; + ros::Duration timeout = ros::Duration(), + ros::Time timestamp = ros::Time(0.0), + std::vector* detectedObjects = nullptr) override; private: /// For the ROS node that will work with the April Tags module @@ -61,8 +70,8 @@ class PoseEstimatorModule : public PerceptionModule /// The name of the ROS topic to read marker info from std::string mMarkerTopic; - /// The pointer to the loader of configuration data - std::shared_ptr mConfigData; + /// The pointer to the loader of visual asset data + std::shared_ptr mAssetData; /// To retrieve resources from disk and from packages std::shared_ptr mResourceRetriever; diff --git a/src/perception/ObjectDatabase.cpp b/src/perception/AssetDatabase.cpp similarity index 51% rename from src/perception/ObjectDatabase.cpp rename to src/perception/AssetDatabase.cpp index 812f9fa478..bf1b2146d4 100644 --- a/src/perception/ObjectDatabase.cpp +++ b/src/perception/AssetDatabase.cpp @@ -1,4 +1,4 @@ -#include "aikido/perception/ObjectDatabase.hpp" +#include "aikido/perception/AssetDatabase.hpp" #include #include @@ -8,7 +8,7 @@ namespace aikido { namespace perception { //============================================================================== -ObjectDatabase::ObjectDatabase( +AssetDatabase::AssetDatabase( const dart::common::ResourceRetrieverPtr& resourceRetriever, const dart::common::Uri& configDataURI) { @@ -26,54 +26,65 @@ ObjectDatabase::ObjectDatabase( } // Load from string - mObjData = YAML::Load(content); + try + { + mAssetData = YAML::Load(content); + } + catch (YAML::Exception& e) + { + dtwarn << "[AssetDatabase::AssetDatabase] JSON File Exception: " << e.what() + << std::endl + << "Loading empty asset database." << std::endl; + mAssetData = YAML::Load(""); // Create Null Node + } } //============================================================================== -void ObjectDatabase::getObjectByKey( - const std::string& objectKey, - std::string& objectName, - dart::common::Uri& objectResource, - Eigen::Isometry3d& objectOffset) const +void AssetDatabase::getAssetByKey( + const std::string& assetKey, + std::string& assetName, + dart::common::Uri& assetResource, + Eigen::Isometry3d& assetOffset) const { - // Get name of object and pose for a given tag ID - YAML::Node objectNode = mObjData[objectKey]; - if (!objectNode) + // Get name of asset and pose for a given tag ID + YAML::Node assetNode = mAssetData[assetKey]; + if (!assetNode) { - throw std::runtime_error("[ObjectDatabase] Error: invalid object key"); + throw std::runtime_error( + "[AssetDatabase] Error: invalid asset key: " + assetKey); } // Convert resource field try { - objectResource.fromString(objectNode["resource"].as()); + assetResource.fromString(assetNode["resource"].as()); } catch (const YAML::ParserException& ex) { throw std::runtime_error( - "[ObjectDatabase] Error in converting [resource] field"); + "[AssetDatabase] Error in converting [resource] field"); } // Convert name field try { - objectName = objectNode["name"].as(); + assetName = assetNode["name"].as(); } catch (const YAML::ParserException& ex) { throw std::runtime_error( - "[ObjectDatabase] Error in converting [name] field"); + "[AssetDatabase] Error in converting [name] field"); } // Convert offset field try { - objectOffset = objectNode["offset"].as(); + assetOffset = assetNode["offset"].as(); } catch (const YAML::ParserException& ex) { throw std::runtime_error( - "[ObjectDatabase] Error in converting [offset] field"); + "[AssetDatabase] Error in converting [offset] field"); } } diff --git a/src/perception/CMakeLists.txt b/src/perception/CMakeLists.txt index ac73429dbb..156860513d 100644 --- a/src/perception/CMakeLists.txt +++ b/src/perception/CMakeLists.txt @@ -45,8 +45,9 @@ endif() set(sources PoseEstimatorModule.cpp shape_conversions.cpp - ObjectDatabase.cpp + AssetDatabase.cpp VoxelGridModule.cpp + DetectedObject.cpp ) add_library("${PROJECT_NAME}_perception" SHARED ${sources}) diff --git a/src/perception/DetectedObject.cpp b/src/perception/DetectedObject.cpp new file mode 100644 index 0000000000..d798672d38 --- /dev/null +++ b/src/perception/DetectedObject.cpp @@ -0,0 +1,74 @@ +#include "aikido/perception/DetectedObject.hpp" + +#include + +namespace aikido { +namespace perception { + +//============================================================================== +DetectedObject::DetectedObject( + const std::string& uid, + const std::string& assetKey, + const std::string& detectionFrameID, + const std::string& yamlStr) + : mUid(std::move(uid)) + , mAssetKey(std::move(assetKey)) + , mDetectionFrameID(std::move(detectionFrameID)) +{ + // Load YAML nodes from string + try + { + mYamlNode = YAML::Load(yamlStr); + mAssetKey = mYamlNode["db_key"].as(mAssetKey); + } + catch (const YAML::Exception& e) + { + dtwarn << "[DetectedObject::DetectedObject] YAML String Exception: " + << e.what() << std::endl; + mYamlNode = YAML::Load(""); // Create Null Node + } +} + +//============================================================================== +std::string DetectedObject::getUid() +{ + return mUid; +} + +//============================================================================== +std::string DetectedObject::getAssetKey() +{ + return mAssetKey; +} + +//============================================================================== +std::string DetectedObject::getDetectionFrameID() +{ + return mDetectionFrameID; +} + +//============================================================================== +YAML::Node DetectedObject::getYamlNode() +{ + return mYamlNode; +} + +//============================================================================== +template +T DetectedObject::getInfoByKey(const std::string& key) +{ + T value; + try + { + value = mYamlNode[key].as(); + } + catch (const YAML::ParserException& ex) + { + throw std::runtime_error( + "[DetectedObject] Error in converting [" + key + "] field"); + } + return value; +} + +} // namespace perception +} // namespace aikido diff --git a/src/perception/PoseEstimatorModule.cpp b/src/perception/PoseEstimatorModule.cpp index 9cf1c356aa..ba96b7f43b 100644 --- a/src/perception/PoseEstimatorModule.cpp +++ b/src/perception/PoseEstimatorModule.cpp @@ -2,11 +2,9 @@ #include #include -#include #include #include #include -#include "aikido/io/CatkinResourceRetriever.hpp" #include "aikido/perception/shape_conversions.hpp" namespace aikido { @@ -15,14 +13,14 @@ namespace perception { //============================================================================= PoseEstimatorModule::PoseEstimatorModule( ros::NodeHandle nodeHandle, - std::string markerTopic, - std::shared_ptr configData, + const std::string& markerTopic, + std::shared_ptr assetData, std::shared_ptr resourceRetriever, - std::string referenceFrameId, + const std::string& referenceFrameId, dart::dynamics::Frame* referenceLink) : mNodeHandle(std::move(nodeHandle)) , mMarkerTopic(std::move(markerTopic)) - , mConfigData(std::move(configData)) + , mAssetData(std::move(assetData)) , mResourceRetriever(std::move(resourceRetriever)) , mReferenceFrameId(std::move(referenceFrameId)) , mReferenceLink(std::move(referenceLink)) @@ -35,7 +33,8 @@ PoseEstimatorModule::PoseEstimatorModule( bool PoseEstimatorModule::detectObjects( const aikido::planner::WorldPtr& env, ros::Duration timeout, - ros::Time timestamp) + ros::Time timestamp, + std::vector* detectedObjects) { // Checks detected objects, looks up the database, // and adds new skeletons to the world env @@ -63,25 +62,39 @@ bool PoseEstimatorModule::detectObjects( for (const auto& marker_transform : marker_message->markers) { + // TODO: Add DELETE_ALL Functionality + // TODO: Update when we move over to ROS Kinetic. Indigo doesn't + // have the enum value. + // if (marker_transform.action == visualization_msgs::Marker::DELETEALL) { + if (marker_transform.action == 3) + { + dtwarn << "[PoseEstimatorModule::detectObjects] We cannot currently " + "handle DELETE_ALL markers." + << std::endl; + continue; + } const auto& marker_stamp = marker_transform.header.stamp; - const auto& obj_key = marker_transform.text; - const auto& obj_id = marker_transform.ns; + const auto& obj_id = marker_transform.id; + const auto& obj_ns = marker_transform.ns; const auto& detection_frame = marker_transform.header.frame_id; if (!timestamp.isValid() || marker_stamp < timestamp) { continue; } - // If obj_key is "None", - // remove a skeleton with obj_id from env - // and move to next marker - dart::dynamics::SkeletonPtr env_skeleton = env->getSkeleton(obj_id); - if (!obj_key.compare("None")) + const std::string obj_uid = obj_ns + "_" + std::to_string(obj_id); + + // Initialize a DetectedObject class for this object + DetectedObject this_object( + obj_uid, obj_ns, detection_frame, marker_transform.text); + + const std::string asset_key = this_object.getAssetKey(); + + if (asset_key.empty()) { - if (env_skeleton != nullptr) - { - env->removeSkeleton(env_skeleton); - } + dtwarn << "[PoseEstimatorModule::detectObjects] Invalid YAML String in " + "Marker: " + << obj_uid << std::endl; continue; } @@ -90,8 +103,28 @@ bool PoseEstimatorModule::detectObjects( Eigen::Isometry3d obj_offset; ros::Time t0 = ros::Time(0); - // get the object name, resource, and offset from database by objectKey - mConfigData->getObjectByKey(obj_key, obj_name, obj_resource, obj_offset); + // Get the object name, resource, and offset from database by assetKey + try + { + mAssetData->getAssetByKey(asset_key, obj_name, obj_resource, obj_offset); + } + catch (std::runtime_error& e) + { + dtwarn << e.what() << std::endl; + continue; + } + + // If marker_transform.action is "DELETE", + // remove a skeleton with obj_uid from env + dart::dynamics::SkeletonPtr env_skeleton = env->getSkeleton(obj_uid); + if (marker_transform.action == visualization_msgs::Marker::DELETE) + { + if (env_skeleton != nullptr) + { + env->removeSkeleton(env_skeleton); + } + continue; + } tf::StampedTransform transform; try @@ -128,8 +161,7 @@ bool PoseEstimatorModule::detectObjects( // Check if skel in World // If there is, update its pose // If not, add skeleton to env - // A pose estimator module should provide the unique obj_id per object - if (env_skeleton == nullptr) + if (!env_skeleton) { is_new_obj = true; obj_skeleton @@ -142,7 +174,7 @@ bool PoseEstimatorModule::detectObjects( << "for URI " << obj_resource.toString() << std::endl; continue; } - obj_skeleton->setName(obj_id); + obj_skeleton->setName(obj_uid); } else { @@ -181,6 +213,11 @@ bool PoseEstimatorModule::detectObjects( env->addSkeleton(obj_skeleton); } + // Add object to output vector, if available + if (detectedObjects) + { + detectedObjects->push_back(this_object); + } any_detected = true; }