Merge pull request #1 from personalrobotics/feature/ComputeEnabledAABB

Added ComputeEnabledAABB function

src/prpy/perception/block_detector.py

@@ -37,24 +37,40 @@ def find_blocks(self,service_name="tools_server/find_blocks",
                      segment_box=True,
                      box_min = [-0.5, 0.1, 0.5],
                      box_max = [0.5, 0.5, 1.5]):
-	    print "waiting for service..."
-	    rospy.wait_for_service(service_name);
-	    print "Calling service..."
-	    try:
-	        box_min_pt = geometry_msgs.msg.Point();
-	        box_min_pt.x = box_min[0];
-	        box_min_pt.y = box_min[1];
-	        box_min_pt.z = box_min[2];
-	        box_max_pt = geometry_msgs.msg.Point();
-	        box_max_pt.x = box_max[0];
-	        box_max_pt.y = box_max[1];
-	        box_max_pt.z = box_max[2];
-
-	        service = rospy.ServiceProxy(service_name, FindBlocks);
-	        response = service(cloud_topic, segment_planes, num_planes, plane_distance, segment_depth, min_depth, max_depth, cluster_tolerance, min_cluster_size, max_cluster_size, segment_box, box_min_pt, box_max_pt);
-	        return response.blocks;
-	    except rospy.ServiceException, e:
-	        print "Service call failed: %s"%e
+        """
+        @param service_name: name of the ROS service for tabletop_perception_tools
+        @param cloud_topic: name of the ROS topic for the colored PointCloud2
+        @param segment_planes: discard the largest num_plane planes
+        @param num_planes: number of planes to discard, if segment_planes is True
+        @param plane_distance: minimum distance from largest plane for points to be accepted
+        @param min_depth: minimum depth from the camera
+        @param max_depth: minimum depth from the camera
+        @param cluster_tolerance: maximum distance between any two points in a cluster
+        @param min_cluster_size: minimum number of points in a cluster
+        @param max_cluster_size: maximum number of points in a cluster
+        @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
+        """
+
+        print "waiting for service..."
+        rospy.wait_for_service(service_name);
+        print "Calling service..."
+        try:
+            box_min_pt = geometry_msgs.msg.Point();
+            box_min_pt.x = box_min[0];
+            box_min_pt.y = box_min[1];
+            box_min_pt.z = box_min[2];
+            box_max_pt = geometry_msgs.msg.Point();
+            box_max_pt.x = box_max[0];
+            box_max_pt.y = box_max[1];
+            box_max_pt.z = box_max[2];
+
+            service = rospy.ServiceProxy(service_name, FindBlocks);
+            response = service(cloud_topic, segment_planes, num_planes, plane_distance, segment_depth, min_depth, max_depth, cluster_tolerance, min_cluster_size, max_cluster_size, segment_box, box_min_pt, box_max_pt);
+            return response.blocks;
+        except rospy.ServiceException, e:
+            print "Service call failed: %s"%e
 
 
     @PerceptionMethod
@@ -125,8 +141,8 @@ def DetectBlocks(self, robot, table, blocks=[], **kw_args):
                 block = env.ReadKinBodyXMLFile(block_path)
 
                 for link in block.GetLinks():
-                	for geometry in link.GetGeometries():
-                		geometry.SetDiffuseColor(numpy.array([b.avg_color.r,b.avg_color.g,b.avg_color.b,b.avg_color.a]))
+                    for geometry in link.GetGeometries():
+                        geometry.SetDiffuseColor(numpy.array([b.avg_color.r,b.avg_color.g,b.avg_color.b,b.avg_color.a]))
 
                 block_pose = numpy.array(quaternion_matrix([
                         b.pose.orientation.x,
@@ -167,4 +183,4 @@ def DetectBlocks(self, robot, table, blocks=[], **kw_args):
                 env.Add(block)
             blocks.append(block)
 
-        return blocks
+        return blocks

src/prpy/util.py

@@ -749,3 +749,29 @@ def IsTimedTrajectory(trajectory):
     """
     cspec = trajectory.GetConfigurationSpecification()
     return cspec.ExtractDeltaTime(trajectory.GetWaypoint(0)) is not None
+
+
+def ComputeEnabledAABB(kinbody):
+    """
+    Returns the AABB of the enabled links of a KinBody.
+
+    @param kinbody: an OpenRAVE KinBody
+    @returns: AABB of the enabled links of the KinBody
+    """
+    from numpy import NINF, PINF
+    from openravepy import AABB
+
+    min_corner = numpy.array([PINF] * 3)
+    max_corner = numpy.array([NINF] * 3)
+
+    for link in kinbody.GetLinks():
+        if link.IsEnabled():
+            link_aabb = link.ComputeAABB()
+            center = link_aabb.pos()
+            half_extents = link_aabb.extents()
+            min_corner = numpy.minimum(center - half_extents, min_corner)
+            max_corner = numpy.maximum(center + half_extents, max_corner)
+
+    center = (min_corner + max_corner) / 2.
+    half_extents = (max_corner - min_corner) / 2.
+    return AABB(center, half_extents)