Enhancement/pose estimator

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
 

include/aikido/perception.hpp

@@ -1,5 +1,6 @@
-#include "perception/AprilTagsDatabase.hpp"
-#include "perception/AprilTagsModule.hpp"
+#include "perception/DetectedObject.hpp"
+#include "perception/ObjectDatabase.hpp"
 #include "perception/PerceptionModule.hpp"
-#include "perception/YamlAprilTagsDatabase.hpp"
+#include "perception/PoseEstimatorModule.hpp"
+#include "perception/VoxelGridModule.hpp"
 #include "perception/shape_conversions.hpp"

include/aikido/perception/DetectedObject.hpp

@@ -0,0 +1,89 @@
+#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.header.frame_id -> detectionFrameID
+/// Marker.ns + "_" + Marker.id -> objUid (identity in DART world)
+/// Marker.ns -> objAssetDBKey (determines visual asset, see ObjectDatabase)
+/// Marker.text -> yamlStr
+
+/// yamlStr contains any types of additional
+/// information of the object in YAML format.
+/// It also can overwrite the Asset Database Key
+/// using "db_key". (see ObjectDatabase 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
+
+/// Clarification: objUid = DART world ID; objDbKey = visual asset key in ObjectDatabase.
+
+class DetectedObject
+{
+public:
+  /// Construct a \c DetectedObject
+  /// \param[in] objUid Unique ID for object in DART world. Same UID -> Same Object
+  /// \param[in] objAssetDBKey Key for ObjectDatabase 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 with "db_key".
+  DetectedObject(
+      const std::string& objUID,
+      const std::string& objAssetDBKey,
+      const std::string& detectionFrameID,
+      const std::string& yamlStr);
+
+  virtual ~DetectedObject() = default;
+
+  /// Get the unique id of the object
+  std::string getObjUID();
+
+  /// Get the object key for \c ObjectDatabase
+  std::string getObjDBKey();
+
+  /// 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 mObjUID;
+
+  /// The object key for \c ObjectDatabase
+  std::string mObjDBKey;
+
+  /// 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_

include/aikido/perception/ObjectDatabase.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 ObjectDatabase (a.k.a. Asset Database) that reads of JSON file containing the
+/// information that maps object keys to the object 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
@@ -23,7 +23,7 @@ namespace perception {
 /// Here is an example entry in a JSON file:
 /// \code
 /// "obj_key": {
-///    "resource": "package://pr_ordata/data/objects/obj_filename.urdf",
+///    "resource": "package://pr_assets/data/objects/obj_filename.urdf",
 ///    "name": "obj_name",
 ///    "offset": [
 ///      [1.0, 0.0, 0.0, 0.0],

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,15 @@ 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.
+  /// \param[out] detectedObjects An output vector for detected objects.
   /// \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
   /// otherwise.
   virtual bool detectObjects(
       const aikido::planner::WorldPtr& env,
       ros::Duration timeout,
-      ros::Time timestamp)
+      ros::Time timestamp,
+      std::vector<DetectedObject>* detectedObjects)
       = 0;
 };
 

include/aikido/perception/PoseEstimatorModule.hpp

@@ -6,6 +6,8 @@
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
 #include "aikido/io/CatkinResourceRetriever.hpp"
+#include "aikido/io/yaml.hpp"
+#include "aikido/perception/DetectedObject.hpp"
 #include "aikido/perception/ObjectDatabase.hpp"
 #include "aikido/perception/PerceptionModule.hpp"
 
@@ -40,19 +42,25 @@ class PoseEstimatorModule : public PerceptionModule
   /// pose is transformed
   PoseEstimatorModule(
       ros::NodeHandle nodeHandle,
-      std::string markerTopic,
+      const std::string& markerTopic,
       std::shared_ptr<ObjectDatabase> configData,
       std::shared_ptr<aikido::io::CatkinResourceRetriever> resourceRetriever,
-      std::string referenceFrameId,
+      const std::string& referenceFrameId,
       dart::dynamics::Frame* referenceLink);
 
   virtual ~PoseEstimatorModule() = default;
 
   // Documentation inherited
+  /// 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 DART world)
+  /// Marker.ns -> objAssetDBKey (determines visual asset, see ObjectDatabase)
   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

src/perception/CMakeLists.txt

@@ -47,6 +47,7 @@ set(sources
   shape_conversions.cpp
   ObjectDatabase.cpp
   VoxelGridModule.cpp
+  DetectedObject.cpp
 )
 
 add_library("${PROJECT_NAME}_perception" SHARED ${sources})

src/perception/DetectedObject.cpp

@@ -0,0 +1,70 @@
+#include "aikido/perception/DetectedObject.hpp"
+
+#include <yaml-cpp/exceptions.h>
+
+namespace aikido {
+namespace perception {
+
+//==============================================================================
+DetectedObject::DetectedObject(
+    const std::string& objUID,
+    const std::string& objAssetDBKey,
+    const std::string& detectionFrameID,
+    const std::string& yamlStr)
+  : mObjUID(std::move(objUID))
+  , mObjDBKey(std::move(objAssetDBKey))
+  , mDetectionFrameID(std::move(detectionFrameID))
+{
+  // Load YAML nodes from string
+  try {
+    mYamlNode = YAML::Load(yamlStr);
+    mObjDBKey = mYamlNode["db_key"].as<std::string>(mObjDBKey);
+  } catch(const YAML::Exception &e) {
+    dtwarn << "[DetectedObject::DetectedObject] YAML String Exception: " << e.what() << std::endl;
+    mYamlNode = YAML::Load(""); // Create Null Node
+  }
+}
+
+//==============================================================================
+std::string DetectedObject::getObjUID()
+{
+  return mObjUID;
+}
+
+//==============================================================================
+std::string DetectedObject::getObjDBKey()
+{
+  return mObjDBKey;
+}
+
+//==============================================================================
+std::string DetectedObject::getDetectionFrameID()
+{
+  return mDetectionFrameID;
+}
+
+//==============================================================================
+YAML::Node DetectedObject::getYamlNode()
+{
+  return mYamlNode;
+}
+
+//==============================================================================
+template <typename T>
+T DetectedObject::getInfoByKey(const std::string& key)
+{
+  T value;
+  try
+  {
+    value = mYamlNode[key].as<T>();
+  }
+  catch (const YAML::ParserException& ex)
+  {
+    throw std::runtime_error(
+        "[DetectedObject] Error in converting [" + key + "] field");
+  }
+  return value;
+}
+
+} // namespace perception
+} // namespace aikido

src/perception/ObjectDatabase.cpp

@@ -40,7 +40,8 @@ void ObjectDatabase::getObjectByKey(
   YAML::Node objectNode = mObjData[objectKey];
   if (!objectNode)
   {
-    throw std::runtime_error("[ObjectDatabase] Error: invalid object key");
+    throw std::runtime_error(
+        "[ObjectDatabase] Error: invalid object key: " + objectKey);
   }
 
   // Convert resource field