Merge pull request #261 from personalrobotics/feature/perception/relative_transform

Perception README + Feature/perception/relative transform

Author: mkoval

README.md

@@ -177,6 +177,81 @@ path1 = planner.PlanToConfiguration(robot, goal)
 path2 = planner.PlanToBasePose(robot, goal_pose)
 ```
 
+## Perception Pipeline
+
+Recently, support has been added for a few perception routines. The general structure is intended
+to mirror that of the planning pipeline, but it is somewhat less encapsulated than 
+planning, from the user's perspective.
+
+There is a `prpy.perception.base.PerceptionModule` class which is extended by every perception
+routine. Every routine has some common methods for perception, which are annotated with 
+`@PerceptionMethod`. Here is an example call (should happen in a typical herbpy console):
+
+```python
+from prpy.perception.apriltags import ApriltagsModule
+
+adetector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array',
+                           marker_data_path=FindCatkinResource('pr_ordata','data/objects/tag_data.json'), 
+                           kinbody_path=FindCatkinResource('pr_ordata','data/objects'), 
+                           destination_frame='/map', 
+                           detection_frame='/head/kinect2_rgb_optical_frame')
+detected_objects = adetector.DetectObjects(robot)
+```
+IMPORTANT - Most of these methods require some underlying CPP server to be running, before calls can be
+made to the PrPy detector. 
+
+### Perception Modules
+
+Currently, the following perception routines are supported:
+
+- `AprilTags`
+- `VNCC`: Vectorized Normalized Cross Correlation
+- `SimTrack`
+- `BlockDetector`
+- `ROCK`: Robust Object Constellation and Kinematic Pose
+
+### Underlying Servers
+
+- `AprilTags`: Started via `apriltags.launch` in [herb_launch](https://github.com/personalrobotics/herb_launch). Publishes to `/apriltags_kinect2/detections` and `/apriltags_kinect2/marker_array`.
+- `VNCC`: Have [vncc_msgs](https://github.com/personalrobotics/vncc_msgs) and [vncc](https://github.com/personalrobotics/vncc) in your workspace. Run `roslaunch vncc vncc_estimator.launch`. This provides the `/vncc/get_vncc_detections` service.
+- `SimTrack` - See Caveats section below
+- `BlockDetector` - Have [tabletop_perception_tools](https://github.com/personalrobotics/tabletop_perception_tools) in your workspace. Run `rosrun tabletop_perception_tools tools_server`. This provides the `/tools_server/find_blocks` service.
+- `ROCK` - To be updated later.
+
+
+### Common Perception Methods
+
+At this point, two methods are common to all perception routines. However, some 
+routine-specific knowledge may be required to make them work. This is particularly reflected
+in the constructor for the perception module.
+
+- `DetectObjects(self, robot, **kw_args)`: This runs the perception method for all
+objects that the particular routine knows about. Typically, this information is specified
+either as a config file (in the case of AprilTags) or in the constructor of the respective
+module.
+- `DetectObject(self,robot,obj_name)`: This runs the perception routine to detect a particular object,
+based on the known names in the database.
+
+The return type for both is typically one or more OpenRAVE kinbodies, with the correct
+transformation relative to the current environment, if the input `tf`s have been 
+correctly provided.
+
+
+### Caveats
+
+As mentioned above, running the perception routines require a bit of routine-specific knowledge,
+because of differences in the way some of them operate. Some of those caveats, for each routine
+are mentioned here.
+
+- `AprilTags`: This method involves detection of visual fiducial markers. There is a database that maps
+april tag IDs to the objects to which they are attached, along with the relative transform
+of the tag with respect to the object kinbody, in `pr_ordata/data/objects/tag_data.json`.
+- `VNCC`: This is a single-query method and so currently does not support `DetectObjects`, but just
+`DetectObject`, where the object names are obtained from the map in the module's constructor.
+- `SimTrack`: See https://github.com/personalrobotics/simtrack for more details. You will need the `personalrobotics` fork. This can track/detect any kind of textured object stored as an `.obj` file. The perception module only calls the detector, but the tracker can also be integrated pretty easily. It supports `DetectObjects`, and requires the simtrack `multi_rigid_node` to be running on the robot to work. Inside the module, there is a map of `simtrack` objects to kinbodies.
+- `BlockDetector`: This is specifically for detecting blocks on a table in front of the camera. Therefore,
+it only has a `DetectBlocks` method.
+- `ROCK`: This is still under development and so does not exactly conform to the underlying API.
 
 ## Environment Cloning
 

src/prpy/perception/apriltags.py

@@ -37,7 +37,7 @@
 class ApriltagsModule(PerceptionModule):
 
     def __init__(self, marker_topic, marker_data_path, kinbody_path,
-                 detection_frame, destination_frame):
+                 detection_frame, destination_frame,reference_link,):
         """
         This initializes an April Tags detector.
         
@@ -55,6 +55,7 @@ def __init__(self, marker_topic, marker_data_path, kinbody_path,
         self.kinbody_path = kinbody_path
         self.detection_frame = detection_frame
         self.destination_frame = destination_frame
+        self.reference_link = reference_link
 
 
     def __str__(self):
@@ -63,7 +64,7 @@ def __str__(self):
 
     def _DetectObjects(self, env, marker_topic=None, marker_data_path=None, 
                        kinbody_path=None, detection_frame=None, 
-                       destination_frame=None, **kw_args):
+                       destination_frame=None, reference_link=None,**kw_args):
         """
         Use the apriltags service to detect objects and add them to the
         environment. Params are as in __init__.
@@ -88,14 +89,16 @@ def _DetectObjects(self, env, marker_topic=None, marker_data_path=None,
 
             if kinbody_path is None:
                 kinbody_path = self.kinbody_path
-            
 
             if detection_frame is None:
                 detection_frame = self.detection_frame
 
             if destination_frame is None:
                 destination_frame = self.destination_frame
 
+            if reference_link is None:
+                reference_link = self.reference_link
+
             # TODO: Creating detector is not instant...might want
             #  to just do this once in the constructor
             import kinbody_detector.kinbody_detector as kd
@@ -104,7 +107,8 @@ def _DetectObjects(self, env, marker_topic=None, marker_data_path=None,
                                           kinbody_path,
                                           marker_topic,
                                           detection_frame,
-                                          destination_frame)
+                                          destination_frame,
+                                          reference_link)
 
             logger.warn('Waiting to detect objects...')
             return detector.Update()