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/ObjectDatabase.hpp

@@ -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/PoseEstimatorModule.hpp

@@ -6,6 +6,7 @@
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
 #include "aikido/io/CatkinResourceRetriever.hpp"
+#include "aikido/io/yaml.hpp"
 #include "aikido/perception/ObjectDatabase.hpp"
 #include "aikido/perception/PerceptionModule.hpp"
 
@@ -54,6 +55,17 @@ class PoseEstimatorModule : public PerceptionModule
       ros::Duration timeout = ros::Duration(0.0),
       ros::Time timestamp = ros::Time(0.0)) override;
 
+  /// Getter for \c mObjInfo. \c mObjInfo is saving additional information
+  /// from the text field in the MarkerArray message in json format.
+  /// This getter returns a YAML::Node of an object by id.
+  ///
+  /// \param[in] obj_id The key of an object to retrieve its additional
+  /// information
+  YAML::Node getObjInfo(const std::string& obj_id);
+
+  /// If this is set, all objects are projected to this height.
+  void setObjectProjectionHeight(double projectionHeight);
+
 private:
   /// For the ROS node that will work with the April Tags module
   ros::NodeHandle mNodeHandle;
@@ -75,6 +87,12 @@ class PoseEstimatorModule : public PerceptionModule
 
   /// Listens to the transform attached to the node
   tf::TransformListener mTfListener;
+
+  /// For additional information for each object
+  std::unordered_map<std::string, YAML::Node> mObjInfo;
+
+  bool mProjectObjectToFixedHeight;
+  double mProjectionHeight;
 };
 
 } // namespace perception

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

src/perception/PoseEstimatorModule.cpp

@@ -6,6 +6,7 @@
 #include <ros/topic.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
+#include <yaml-cpp/exceptions.h>
 #include "aikido/io/CatkinResourceRetriever.hpp"
 #include "aikido/perception/shape_conversions.hpp"
 
@@ -27,6 +28,8 @@ PoseEstimatorModule::PoseEstimatorModule(
   , mReferenceFrameId(std::move(referenceFrameId))
   , mReferenceLink(std::move(referenceLink))
   , mTfListener(mNodeHandle)
+  , mProjectObjectToFixedHeight(false)
+  , mProjectionHeight(0.0)
 {
   // Do nothing
 }
@@ -64,14 +67,18 @@ bool PoseEstimatorModule::detectObjects(
   for (const auto& marker_transform : marker_message->markers)
   {
     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_key = marker_transform.ns;
     const auto& detection_frame = marker_transform.header.frame_id;
     if (!timestamp.isValid() || marker_stamp < timestamp)
     {
       continue;
     }
 
+    YAML::Node info_json = YAML::Load(marker_transform.text);
+    const std::string obj_id = info_json["id"].as<std::string>();
+
+    mObjInfo.insert(std::make_pair(obj_id, info_json));
+
     // If obj_key is "None",
     // remove a skeleton with obj_id from env
     // and move to next marker
@@ -122,6 +129,11 @@ bool PoseEstimatorModule::detectObjects(
     Eigen::Isometry3d link_offset = mReferenceLink->getWorldTransform();
     obj_pose = link_offset * obj_pose;
 
+    if (mProjectObjectToFixedHeight)
+    {
+      obj_pose.translation()[2] = mProjectionHeight;
+    }
+
     bool is_new_obj;
     dart::dynamics::SkeletonPtr obj_skeleton;
 
@@ -195,5 +207,23 @@ bool PoseEstimatorModule::detectObjects(
   return true;
 }
 
+YAML::Node PoseEstimatorModule::getObjInfo(const std::string& obj_id)
+{
+  YAML::Node targetNode = mObjInfo[obj_id];
+  if (!targetNode)
+  {
+    throw std::runtime_error(
+        "[PoseEstimatorModule::getObjInfo] Error: invalid obj_id");
+  }
+
+  return targetNode;
+}
+
+void PoseEstimatorModule::setObjectProjectionHeight(double projectionHeight)
+{
+  mProjectObjectToFixedHeight = true;
+  mProjectionHeight = projectionHeight;
+}
+
 } // namespace perception
 } // namespace aikido