clean up and integrate pose estimators (#336)

* clean up and integrate pose estimators

* format PoseEstimatorModule

* update CHANGELOG.md

* add documentation

* add documentation

* fix the issues on comments in PR#336

CHANGELOG.md

@@ -21,6 +21,10 @@
   * Introduced uniform and dart namespaces: [#342](https://github.com/personalrobotics/aikido/pull/342)
   * Removed Barrett-specific hand executors: [#380](https://github.com/personalrobotics/aikido/pull/380)
 
+* Perception
+
+  * Added integrated PoseEstimatorModule: [#336](https://github.com/personalrobotics/aikido/pull/336)
+
 * Planner
 
   * Changed interface for TrajectoryPostProcessor: [#341](https://github.com/personalrobotics/aikido/pull/341)

include/aikido/perception/ObjectDatabase.hpp

@@ -4,8 +4,8 @@
 #include <stdexcept>
 #include <dart/common/LocalResourceRetriever.hpp>
 #include <dart/dart.hpp>
-#include <aikido/io/CatkinResourceRetriever.hpp>
-#include <aikido/io/yaml.hpp>
+#include "aikido/io/CatkinResourceRetriever.hpp"
+#include "aikido/io/yaml.hpp"
 
 namespace aikido {
 namespace perception {
@@ -24,7 +24,13 @@ namespace perception {
 /// \code
 /// "obj_key": {
 ///    "resource": "package://pr_ordata/data/objects/obj_filename.urdf",
-///    "name": "obj_name"
+///    "name": "obj_name",
+///    "offset": [
+///      [1.0, 0.0, 0.0, 0.0],
+///      [0.0, 1.0, 0.0, 0.0],
+///      [0.0, 0.0, 1.0, 0.0],
+///      [0.0, 0.0, 0.0, 1.0]
+///    ]
 ///  }
 /// \endcode
 
@@ -41,10 +47,17 @@ class ObjectDatabase
 
   virtual ~ObjectDatabase() = 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) const;
+      dart::common::Uri& objectResource,
+      Eigen::Isometry3d& objectOffset) const;
 
 private:
   /// The map of object keys to object names and resources for models

include/aikido/perception/PerceptionModule.hpp

@@ -1,7 +1,7 @@
 #ifndef AIKIDO_PERCEPTION_PERCEPTIONMODULE_HPP_
 #define AIKIDO_PERCEPTION_PERCEPTIONMODULE_HPP_
 
-#include <aikido/planner/World.hpp>
+#include "aikido/planner/World.hpp"
 
 namespace aikido {
 namespace perception {

include/aikido/perception/RcnnPoseModule.hpp → include/aikido/perception/PoseEstimatorModule.hpp

@@ -1,28 +1,29 @@
-#ifndef AIKIDO_PERCEPTION_RCNNPOSEMODULE_HPP_
-#define AIKIDO_PERCEPTION_RCNNPOSEMODULE_HPP_
+#ifndef AIKIDO_PERCEPTION_POSEESTIMATORMODULE_HPP_
+#define AIKIDO_PERCEPTION_POSEESTIMATORMODULE_HPP_
 
 #include <string>
 #include <dart/dart.hpp>
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
-#include <aikido/io/CatkinResourceRetriever.hpp>
-#include <aikido/perception/ObjectDatabase.hpp>
-#include <aikido/perception/PerceptionModule.hpp>
+#include "aikido/io/CatkinResourceRetriever.hpp"
+#include "aikido/perception/ObjectDatabase.hpp"
+#include "aikido/perception/PerceptionModule.hpp"
 
 namespace aikido {
 namespace perception {
 
-/// Instantiates the \c PerceptionModule specifically for RcnnPose.
+/// Instantiates the \c PerceptionModule specifically for PoseEstimator.
 ///
-/// It receives input from the RcnnPose running in a separate process,
-/// by subscribing to a \c visualization_msg::MarkerArray ROS topic published by
-/// the RcnnPose node. It uses a \c ObjectDatabase to resolve each marker to a
+/// 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
 /// \c dart::common::Uri, and updates the environment which is an
 /// \c aikido::planner::World.
-class RcnnPoseModule : public PerceptionModule
+class PoseEstimatorModule : public PerceptionModule
 {
 public:
-  /// Construct an RcnnPose receiver that subscribes to the specified topic
+  /// Construct a PoseEstimator receiver that subscribes to the specified topic
   /// where objects' pose information is being published as a
   /// \c visualization_msg::MarkerArray, uses a database loader for
   /// configuration information related to tags.
@@ -37,15 +38,15 @@ class RcnnPoseModule : public PerceptionModule
   /// object pose
   ///	\param[in] referenceLink A link on the robot with respect to which the
   /// pose is transformed
-  RcnnPoseModule(
+  PoseEstimatorModule(
       ros::NodeHandle nodeHandle,
       std::string markerTopic,
       std::shared_ptr<ObjectDatabase> configData,
       std::shared_ptr<aikido::io::CatkinResourceRetriever> resourceRetriever,
       std::string referenceFrameId,
       dart::dynamics::Frame* referenceLink);
 
-  virtual ~RcnnPoseModule() = default;
+  virtual ~PoseEstimatorModule() = default;
 
   // Documentation inherited
   bool detectObjects(
@@ -79,4 +80,4 @@ class RcnnPoseModule : public PerceptionModule
 } // namespace perception
 } // namespace aikido
 
-#endif // AIKIDO_PERCEPTION_RCNNPOSEMODULE_HPP_
+#endif // AIKIDO_PERCEPTION_POSEESTIMATORMODULE_HPP_