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 <[email protected]> * Update include/aikido/perception/DetectedObject.hpp Co-Authored-By: yskim041 <[email protected]> * Update include/aikido/perception/DetectedObject.hpp Co-Authored-By: yskim041 <[email protected]> * 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 <[email protected]> * 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.
personalrobotics · Feb 14, 2019 · a1ef21a · a1ef21a
1 parent 67ab2db
commit a1ef21a
Show file tree

Hide file tree

Showing 10 changed files with 312 additions and 83 deletions.
diff --git 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
@@ -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 → include/aikido/perception/AssetDatabase.hpp b/include/aikido/perception/ObjectDatabase.hpp → 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 <stdexcept>
 #include <dart/common/LocalResourceRetriever.hpp>
@@ -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
@@ -0,0 +1,91 @@
+#ifndef AIKIDO_PERCEPTION_DETECTEDOBJECT_HPP_
+#define AIKIDO_PERCEPTION_DETECTEDOBJECT_HPP_
+
+#include <string>
+#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<double>)
+  /// Map types are not supported with this function. Please get the manually
+  /// with getYamlNode().
+  template <typename T>
+  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
@@ -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<DetectedObject>* detectedObjects = nullptr)
       = 0;
 };
 

diff --git a/include/aikido/perception/PoseEstimatorModule.hpp b/include/aikido/perception/PoseEstimatorModule.hpp
@@ -6,7 +6,9 @@
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
 #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<ObjectDatabase> configData,
+      const std::string& markerTopic,
+      std::shared_ptr<AssetDatabase> assetData,
       std::shared_ptr<aikido::io::CatkinResourceRetriever> 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<DetectedObject>* 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<ObjectDatabase> mConfigData;
+  /// The pointer to the loader of visual asset data
+  std::shared_ptr<AssetDatabase> mAssetData;
 
   /// To retrieve resources from disk and from packages
   std::shared_ptr<aikido::io::CatkinResourceRetriever> mResourceRetriever;

diff --git a/src/perception/ObjectDatabase.cpp → src/perception/AssetDatabase.cpp b/src/perception/ObjectDatabase.cpp → src/perception/AssetDatabase.cpp
@@ -1,4 +1,4 @@
-#include "aikido/perception/ObjectDatabase.hpp"
+#include "aikido/perception/AssetDatabase.hpp"
 
 #include <dart/common/Console.hpp>
 #include <dart/common/LocalResourceRetriever.hpp>
@@ -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<std::string>());
+    assetResource.fromString(assetNode["resource"].as<std::string>());
   }
   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<std::string>();
+    assetName = assetNode["name"].as<std::string>();
   }
   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<Eigen::Isometry3d>();
+    assetOffset = assetNode["offset"].as<Eigen::Isometry3d>();
   }
   catch (const YAML::ParserException& ex)
   {
     throw std::runtime_error(
-        "[ObjectDatabase] Error in converting [offset] field");
+        "[AssetDatabase] Error in converting [offset] field");
   }
 }