Added Chisel to the perception module

src/prpy/perception/chisel.py

@@ -0,0 +1,139 @@
+import logging
+from base import PerceptionModule, PerceptionMethod
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+class ChiselModule(PerceptionModule):
+
+    def __init__(self, env, mesh_client=None, service_namespace=None,
+               detection_frame=None, destination_frame=None, reference_link=None):
+        """
+        @param env The OpenRAVE environment
+        @param mesh_client An instance of the Chisel sensor system (if None, one is created)
+        @param service_namespace The namespace to use for Chisel service calls (default: Chisel)
+        @param destination_frame The tf frame the detected chisel kinbody should be transformed to
+        (default map)
+        @param reference_link A link on an OpenRAVE kinbody that corresponds to the tf frame
+        specified by the destination_frame parameter. If not provided it is assumed that the 
+        transform from the destination_frame tf frame to the OpenRAVE environment is the identity
+        """
+        import rospy
+
+        try:
+            rospy.init_node('chisel_detector',anonymous=True)
+        except rospy.exceptions.ROSException:
+            pass
+
+        if service_namespace is None:
+            service_namespace = 'Chisel'
+
+        if mesh_client is None:
+            import openravepy
+            mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker')
+
+        self.mesh_client = mesh_client
+        self.serv_ns = service_namespace
+
+        # Create a ROS camera used to generate a synthetic point cloud
+        #  to mask known kinbodies from the chisel mesh
+        from offscreen_render import ros_camera_sim
+        self.camera = ros_camera_sim.RosCameraSim(env)
+        self.camera.start('/head/kinect2/qhd')
+        self.camera_bodies = []
+
+        self.reset_detection_srv = None
+
+        if detection_frame is None:
+            # By default, the Chisel kinbody is put in map frame
+            #  not camera frame
+            detection_frame='/map' 
+        self.detection_frame = detection_frame
+
+        if destination_frame is None:
+            destination_frame = '/map'
+        self.destination_frame = destination_frame
+        self.reference_link = reference_link
+
+    @PerceptionMethod
+    def DetectObject(self, robot, ignored_bodies=None, **kw_args):
+        """
+        @param robot Required by the PerceptionMethod interface
+        @param ignored_bodies A list of known kinbodies to be masked out
+        of the chisel mesh
+        """
+        env = robot.GetEnv()
+
+        if ignored_bodies is None:
+            ignored_bodies = []
+
+        # Dictionary mapping ignored bodies to start transform
+        orig_bodies = {}
+
+        #Check if chisel kinbody in env
+        already_in_env = False
+        for body in env.GetBodies():
+            if body.GetName() == 'Chisel/full_mesh':
+                already_in_env = True
+                break
+
+        #Remove chisel kinbody if present
+        if already_in_env:
+            chisel_kb = env.GetKinBody('Chisel/full_mesh')
+            env.Remove(chisel_kb)
+
+        try:
+            from kinbody_helper import transform_from_or
+
+            # Add any ignored bodies to the camera
+            # We need to transform all these bodies from their pose
+            #   in OpenRAVE to the equivalent pose in TF so that
+            #   chisel can perform the appropriate masking
+            #   Then we will transform them all back after the server
+            #   performs the detection
+            for b in ignored_bodies:
+                if b not in self.camera_bodies:
+                    with env:
+                        orig_bodies[b] = b.GetTransform()
+                    transform_from_or(kinbody=b, 
+                                      detection_frame=self.detection_frame,
+                                      destination_frame=self.destination_frame,
+                                      reference_link=self.reference_link)
+                    self.camera.add_body(b)
+                    self.camera_bodies.append(b)
+
+            #Reset Chisel
+            if self.reset_detection_srv is None:
+                import rospy, chisel_msgs.srv
+                srv_nm = self.serv_ns+'/Reset'
+                rospy.wait_for_service(srv_nm)
+                self.reset_detection_srv = rospy.ServiceProxy(srv_nm,
+                                                              chisel_msgs.srv.ResetService)
+            self.reset_detection_srv()
+
+            #Get Kinbody and load into env
+            self.mesh_client.SendCommand('GetMesh Chisel/full_mesh')
+            chisel_mesh = env.GetKinBody('Chisel/full_mesh')
+
+        finally:
+
+            # Remove any previous bodies in the camera
+            for b in self.camera_bodies:
+                self.camera.remove_body(b)
+            self.camera_bodies = []
+
+            with env:
+                # Restore all bodies to their original pose
+                for b, orig_pose in orig_bodies.iteritems():
+                    b.SetTransform(orig_pose)
+
+        if chisel_mesh is not None and self.reference_link is not None:
+            # Apply a transform to the chisel kinbody to put it in the correct
+            # location in the OpenRAVE environment
+            from kinbody_helper import transform_to_or
+            transform_to_or(kinbody=chisel_mesh,
+                            detection_frame=self.detection_frame,
+                            destination_frame=self.destination_frame,
+                            reference_link=self.reference_link)
+
+        return chisel_mesh

src/prpy/perception/kinbody_helper.py

@@ -0,0 +1,87 @@
+
+def transform_to_or(kinbody, detection_frame, destination_frame, 
+                    reference_link=None):
+    """
+    Transform the pose of a kinbody from a pose determined using TF to 
+    the correct relative pose in OpenRAVE. This transformation is performed
+    by providing a link in OpenRAVE that corresponds directly to a frame in TF.
+
+    @param kinbody The kinbody to transform    
+    @param detection_frame The tf frame the kinbody was originally detected in
+    @param destination_frame A tf frame that has a direct correspondence with
+      a link on an OpenRAVE Kinbody
+    @param reference_link The OpenRAVE link that corresponds to the tf frame
+      given by the destination_frame parameter (if None it is assumed
+      that the transform between the OpenRAVE world and the destination_frame 
+      tf frame is the identity)
+    """
+
+    import numpy, tf, rospy
+    listener = tf.TransformListener()
+
+    listener.waitForTransform(
+        detection_frame, destination_frame,
+        rospy.Time(),
+        rospy.Duration(10))
+
+    frame_trans, frame_rot = listener.lookupTransform(
+        destination_frame, detection_frame,
+        rospy.Time(0))
+
+    from tf.transformations import quaternion_matrix
+    detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot)))
+    detection_in_destination[:3,3] = frame_trans
+
+    body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform())
+
+    if reference_link is not None:
+        destination_in_or = reference_link.GetTransform()
+    else:        
+        destination_in_or = numpy.eye(4)
+
+    body_in_or= numpy.dot(destination_in_or, body_in_destination)
+    kinbody.SetTransform(body_in_or)
+
+def transform_from_or(kinbody, detection_frame, destination_frame, 
+                      reference_link=None):
+    """
+    Transform the pose of a kinbody from a OpenRAVE pose to the correct
+    relative pose in TF.This transformation is performed
+    by providing a link in OpenRAVE that corresponds directly to a frame in TF.
+
+    @param kinbody The kinbody to transform    
+    @param detection_frame The tf frame the kinbody was originally detected in
+    @param destination_frame A tf frame that has a direct correspondence with
+      a link on an OpenRAVE Kinbody
+    @param reference_link The OpenRAVE link that corresponds to the tf frame
+      given by the destination_frame parameter (if None it is assumed
+      that the transform between the OpenRAVE world and the destination_frame 
+      tf frame is the identity)
+    """
+
+    import numpy, tf, rospy
+    listener = tf.TransformListener()
+
+    listener.waitForTransform(
+        detection_frame, destination_frame,
+        rospy.Time(),
+        rospy.Duration(10))
+
+    frame_trans, frame_rot = listener.lookupTransform(
+        detection_frame, destination_frame,
+        rospy.Time(0))
+
+    from tf.transformations import quaternion_matrix
+    destination_in_detection = numpy.array(numpy.matrix(quaternion_matrix(frame_rot)))
+    destination_in_detection[:3,3] = frame_trans
+
+    with kinbody.GetEnv():
+        body_in_or = kinbody.GetTransform()
+
+    or_in_destination = numpy.linalg.inv(reference_link.GetTransform())
+    body_in_destination = numpy.dot(or_in_destination, body_in_or)
+
+    body_in_detection = numpy.dot(destination_in_detection, body_in_destination)
+
+    with kinbody.GetEnv():
+        kinbody.SetTransform(body_in_detection)

src/prpy/perception/simtrack.py

@@ -7,14 +7,16 @@
 
 class SimtrackModule(PerceptionModule):
 
-    def __init__(self, kinbody_path, detection_frame, world_frame,
+    def __init__(self, kinbody_path, detection_frame, 
+                 destination_frame=None, reference_link=None,
                  service_namespace=None):
         """
         This initializes a simtrack detector.
 
         @param kinbody_path The path to the folder where kinbodies are stored
         @param detection_frame The TF frame of the camera
-        @param world_frame The desired world TF frame
+        @param destination_frame The tf frame that the kinbody should be transformed to
+        @param reference_link The OpenRAVE link that corresponds to the tf destination_frame
         @param service_namespace The namespace for the simtrack service (default: /simtrack)
         """
         import rospy
@@ -34,10 +36,13 @@ def __init__(self, kinbody_path, detection_frame, world_frame,
 
         if service_namespace is None:
             service_namespace='/simtrack'
+        self.service_namespace = service_namespace
 
         self.detection_frame = detection_frame
-        self.world_frame = world_frame
-        self.service_namespace = service_namespace
+        if destination_frame is None:
+            destination_frame='/map'
+        self.destination_frame = destination_frame
+        self.reference_link = reference_link
 
         self.kinbody_path = kinbody_path
 
@@ -65,31 +70,6 @@ def _MsgToPose(msg):
 
 	return pose
 
-    def _LocalToWorld(self,pose):
-        """
-        Transform a pose from local frame to world frame
-        @param pose The 4x4 transformation matrix containing the pose to transform
-        @return The 4x4 transformation matrix describing the pose in world frame
-        """
-        import rospy
-        #Get pose w.r.t world frame
-        self.listener.waitForTransform(self.world_frame,self.detection_frame,
-                                       rospy.Time(),rospy.Duration(10))
-        t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame,
-                                             rospy.Time(0))
-
-        #Get relative transform between frames
-        offset_to_world = numpy.matrix(transformations.quaternion_matrix(r))
-        offset_to_world[0,3] = t[0]
-        offset_to_world[1,3] = t[1]
-        offset_to_world[2,3] = t[2]
-
-        #Compose with pose to get pose in world frame
-        result = numpy.array(numpy.dot(offset_to_world, pose))
-
-        return result
-
-
     def _GetDetections(self, obj_names):
         import simtrack_msgs.srv
         """
@@ -111,8 +91,6 @@ def _GetDetections(self, obj_names):
             obj_name = detect_resp.detected_models[i];
             obj_pose = detect_resp.detected_poses[i];
             obj_pose_tf = self._MsgToPose(obj_pose);
-            if (self.detection_frame is not None and self.world_frame is not None):
-                obj_pose_tf = self._LocalToWorld(obj_pose_tf)
             detections.append((obj_name, obj_pose_tf));
 
         return detections
@@ -157,7 +135,15 @@ def DetectObject(self, robot, obj_name, **kw_args):
                 os.path.join(self.kinbody_path, kinbody_file))
 
         body = env.GetKinBody(obj_name)
-        body.SetTransform(obj_pose)
+
+        # Apply a transform to the kinbody to put it in the 
+        #  desired location in OpenRAVE
+        from kinbody_helper import transform_to_or
+        transform_to_or(kinbody=body,
+                        detection_frame=self.detection_frame,
+                        destination_frame=self.destination_frame,
+                        reference_link=self.reference_link)
+
         return body
 
     @PerceptionMethod 
@@ -166,6 +152,8 @@ def DetectObjects(self, robot, **kw_args):
         Overriden method for detection_frame
         """
         from prpy.perception.base import PerceptionException
+        from kinbody_helper import transform_to_or
+
         env = robot.GetEnv()
         # Detecting empty list will detect all possible objects
         detections = self._GetDetections([])
@@ -175,16 +163,19 @@ def DetectObjects(self, robot, **kw_args):
                 continue
 
             kinbody_name = self.query_to_kinbody_map[obj_name]
-
             if env.GetKinBody(kinbody_name) is None:
                 from prpy.rave import add_object
                 kinbody_file = '%s.kinbody.xml' % kinbody_name
                 new_body = add_object(
                     env,
                     kinbody_name,
                     os.path.join(self.kinbody_path, kinbody_file))
-            print kinbody_name
+
+
             body = env.GetKinBody(kinbody_name)
-            body.SetTransform(obj_pose)
+            transform_to_or(kinbody=body,
+                            detection_frame=self.detection_frame,
+                            destination_frame=self.destination_frame,
+                            reference_link=self.reference_link)