|
| 1 | +#!/usr/bin/env python |
| 2 | + |
| 3 | +# Copyright (c) 2015, Carnegie Mellon University |
| 4 | +# All rights reserved. |
| 5 | +# Authors: Jennifer King <jeking@cs.cmu.edu> |
| 6 | +# |
| 7 | +# Redistribution and use in source and binary forms, with or without |
| 8 | +# modification, are permitted provided that the following conditions are met: |
| 9 | +# |
| 10 | +# - Redistributions of source code must retain the above copyright notice, this |
| 11 | +# list of conditions and the following disclaimer. |
| 12 | +# - Redistributions in binary form must reproduce the above copyright notice, |
| 13 | +# this list of conditions and the following disclaimer in the documentation |
| 14 | +# and/or other materials provided with the distribution. |
| 15 | +# - Neither the name of Carnegie Mellon University nor the names of its |
| 16 | +# contributors may be used to endorse or promote products derived from this |
| 17 | +# software without specific prior written permission. |
| 18 | +# |
| 19 | +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 20 | +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 21 | +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 22 | +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 23 | +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 24 | +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 25 | +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 26 | +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 27 | +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 28 | +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 29 | +# POSSIBILITY OF SUCH DAMAGE. |
| 30 | + |
| 31 | +import logging |
| 32 | +from base import PerceptionModule, PerceptionMethod |
| 33 | + |
| 34 | +logger = logging.getLogger(__name__) |
| 35 | +logger.setLevel(logging.INFO) |
| 36 | + |
| 37 | +class ApriltagsModule(PerceptionModule): |
| 38 | + |
| 39 | + def __init__(self, marker_topic, marker_data_path, kinbody_path, |
| 40 | + detection_frame, destination_frame): |
| 41 | + """ |
| 42 | + This initializes an April Tags detector. |
| 43 | + |
| 44 | + @param marker_topic The ROS topic to read markers from. Typically the output topic for April Tags |
| 45 | + @param marker_data_path The json file where the association between tag and object is stored |
| 46 | + @param kinbody_path The path to the folder where kinbodies are stored |
| 47 | + @param detection_frame The TF frame of the camera |
| 48 | + @param destination_frame The desired world TF frame |
| 49 | + """ |
| 50 | + |
| 51 | + super(ApriltagsModule, self).__init__() |
| 52 | + |
| 53 | + self.marker_topic = marker_topic |
| 54 | + self.marker_data_path = marker_data_path |
| 55 | + self.kinbody_path = kinbody_path |
| 56 | + self.detection_frame = detection_frame |
| 57 | + self.destination_frame = destination_frame |
| 58 | + |
| 59 | + |
| 60 | + def __str__(self): |
| 61 | + return self.__class__.__name__ |
| 62 | + |
| 63 | + def DetectObjects(self, env, object_names, **kw_args): |
| 64 | + """ |
| 65 | + This hack detects only the objects in object_names. Updates existing |
| 66 | + objects, but only adds objects in the object_names list. |
| 67 | +
|
| 68 | + @param env: The current OpenRAVE environment |
| 69 | + @param object_names: The list of names of objects to detect |
| 70 | + """ |
| 71 | + added_kinbodies, updated_kinbodies = self._DetectObjects(env, **kw_args); |
| 72 | + detected = []; |
| 73 | + for body in added_kinbodies: |
| 74 | + if not (body.GetName() in object_names): |
| 75 | + env.RemoveKinbody(body); |
| 76 | + else: |
| 77 | + detected.append(body); |
| 78 | + return detected; |
| 79 | + |
| 80 | + def DetectObject(self, env, object_name, **kw_args): |
| 81 | + """ |
| 82 | + Detects a single named object. |
| 83 | + """ |
| 84 | + return (self._DetectObjects( env, object_names=[object_name], **kw_args))[0][0]; |
| 85 | + |
| 86 | + |
| 87 | + def _DetectObjects(self, env, marker_topic, marker_data_path, kinbody_path, |
| 88 | + detection_frame, destination_frame,**kw_args): |
| 89 | + """ |
| 90 | + Use the apriltags service to detect objects and add them to the |
| 91 | + environment. Params are as in __init__. |
| 92 | +
|
| 93 | + @param env: The current OpenRAVE environment |
| 94 | + @param marker_topic The ROS topic to read markers from. Typically the output topic for April Tags |
| 95 | + @param marker_data_path The json file where the association between tag and object is stored |
| 96 | + @param kinbody_path The path to the folder where kinbodies are stored |
| 97 | + @param detection_frame The TF frame of the camera |
| 98 | + @param destination_frame The desired world TF frame |
| 99 | +
|
| 100 | + @return The list of kinbodies associated with the detected apriltags |
| 101 | + """ |
| 102 | + try: |
| 103 | + # Allow caller to override any of the initial parameters |
| 104 | + # loaded into the module |
| 105 | + if marker_topic is None: |
| 106 | + marker_topic = self.marker_topic |
| 107 | + |
| 108 | + if marker_data_path is None: |
| 109 | + marker_data_path = self.marker_data_path |
| 110 | + |
| 111 | + if kinbody_path is None: |
| 112 | + kinbody_path = self.kinbody_path |
| 113 | + |
| 114 | + |
| 115 | + if detection_frame is None: |
| 116 | + detection_frame = self.detection_frame |
| 117 | + |
| 118 | + if destination_frame is None: |
| 119 | + destination_frame = self.destination_frame |
| 120 | + |
| 121 | + # TODO: Creating detector is not instant...might want |
| 122 | + # to just do this once in the constructor |
| 123 | + import kinbody_detector.kinbody_detector as kd |
| 124 | + detector = kd.KinBodyDetector(env, |
| 125 | + marker_data_path, |
| 126 | + kinbody_path, |
| 127 | + marker_topic, |
| 128 | + detection_frame, |
| 129 | + destination_frame) |
| 130 | + |
| 131 | + logger.warn('Waiting to detect objects...') |
| 132 | + return detector.Update() |
| 133 | + except Exception, e: |
| 134 | + logger.error('Detecton failed update: %s' % str(e)) |
| 135 | + raise |
| 136 | + |
| 137 | + @PerceptionMethod |
| 138 | + def DetectObjects(self, robot, **kw_args): |
| 139 | + """ |
| 140 | + Overriden method for detection_frame |
| 141 | + """ |
| 142 | + return self._DetectObjects(robot.GetEnv(),**kwargs) |
| 143 | + |
0 commit comments