Merge branch 'block_sorting' of https://github.com/personalrobotics/prpy into block_sorting

Author: jeking04

src/prpy/perception/apriltags.py

@@ -58,14 +58,17 @@ def __init__(self, marker_topic, marker_data_path, kinbody_path,
 
 
     def __str__(self):
-        return 'Apriltags'
+        return self.__class__.__name__
 
     def DetectObjects(self, env, object_names, **kw_args):
         """
         This hack detects only the objects in object_names. Updates existing
         objects, but only adds objects in the object_names list.
+
+        @param env: The current OpenRAVE environment
+        @param object_names: The list of names of objects to detect
         """
-        added_kinbodies, updated_kinbodies = self.DetectObjects(robot, **kw_args);
+        added_kinbodies, updated_kinbodies = self._DetectObjects(env, **kw_args);
         detected = [];
         for body in added_kinbodies:
             if not (body.GetName() in object_names):
@@ -81,37 +84,38 @@ def DetectObject(self, env, object_name, **kw_args):
         return (self._DetectObjects( env, object_names=[object_name], **kw_args))[0][0];
 
 
-    def _DetectObjects(self, env, **kw_args):
+    def _DetectObjects(self, env, marker_topic, marker_data_path, kinbody_path,
+                 detection_frame, destination_frame,**kw_args):
         """
         Use the apriltags service to detect objects and add them to the
-        environment
+        environment. Params are as in __init__.
+
+        @param env: The current OpenRAVE environment
+        @param marker_topic The ROS topic to read markers from. Typically the output topic for April Tags
+        @param marker_data_path The json file where the association between tag and object is stored
+        @param kinbody_path The path to the folder where kinbodies are stored
+        @param detection_frame The TF frame of the camera
+        @param destination_frame The desired world TF frame
+
+        @return The list of kinbodies associated with the detected apriltags
         """
         try:
             # Allow caller to override any of the initial parameters
             # loaded into the module
-            if 'marker_data_path' in kw_args:
-                marker_data_path = kw_args['marker_data_path']
-            else:
-                marker_data_path = self.marker_data_path
+            if marker_topic is None:
+                marker_topic = self.marker_topic
 
-            if 'kinbody_path' in kw_args:
-                kinbody_path = kw_args['kinbody_path']
-            else:
+            if marker_data_path is None:
+                marker_data_path = self.marker_data_path
+
+            if kinbody_path is None:
                 kinbody_path = self.kinbody_path
-                
-            if 'marker_topic' in kw_args:
-                marker_topic = kw_args['marker_topic']
-            else:
-                marker_topic = self.marker_topic
+            
 
-            if 'detection_frame' in kw_args:
-                detection_frame = kw_args['detection_frame']
-            else:
+            if detection_frame is None:
                 detection_frame = self.detection_frame
 
-            if 'destination_frame' in kw_args:
-                destination_frame = kw_args['destination_frame']
-            else:
+            if destination_frame is None:
                 destination_frame = self.destination_frame
 
             # TODO: Creating detector is not instant...might want

src/prpy/perception/block_detector.py

@@ -63,6 +63,8 @@ def find_blocks(self,service_name="tools_server/find_blocks",
         @param segment_box: flag to discard points outside of (box_min, box_max)
         @param box_min: minimum coordinsates of search area in camera frame
         @param box_max: maximum coordinsates of search area in camera frame
+
+        @return list of blocks found, if any
         """
 
         logging.info("waiting for service...")
@@ -89,7 +91,13 @@ def find_blocks(self,service_name="tools_server/find_blocks",
     @PerceptionMethod
     def DetectBlocks(self, robot, table, blocks=None,timeout=10, **kw_args):
         """
-        Place blocks on the table
+        Calls detector for blocks and places them on the table
+
+        @param robot: The robot instance using the detector
+        @param table: The kinbody for the table on which the blocks are placed
+        @blocks: List of blocks currently in the environment; if present, redetection not done
+
+        @return The list of blocks detected
         """
         if blocks is not None and len(blocks) == 0:
             # Add all blocks