← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_ros/nodes/Kinect_mapper_demo.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-16 19:59:46 UTC
  • Revision ID: vikasdhi@buffalo.edu-20121116195946-fwfdqevt3h3eqoyg
Manually added julians changes after merge failed. "Added function to save point cloud in the .ply format"; "Got speed_test working again after API changes.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_ros/nodes/Kinect_mapper_demo.py
106
106
        self.pc_frame_id_lock = allocate_lock()
107
107
        self.pc_frame_id = None
108
108
 
109
 
        self.odometry_map = True
110
 
 
111
109
        #self.mapper = mapper.VoxelMap(resolution=self.res, levels=self.levels)
112
110
        self.lock = allocate_lock() # lock to have thread-safety
113
111
        self.num_base_scans = num_base_scans
124
122
        Thread(target=self.publish_map_thread).start()
125
123
        # Transform broadcast should be at a higher frequency then the map
126
124
        # scan building, so it's better to have a separate thread for this.
127
 
        if not self.odometry_map:
128
 
            Thread(target=self.broadcast_transform_thread).start()
 
125
        Thread(target=self.broadcast_transform_thread).start()
129
126
        # Let the mapping be in another thread
130
127
        Thread(target=self.mapping_thread).start()
131
128
        if self.use_odom:
188
185
            mapper.buildmap(self.pointcloud_queue,
189
186
                            self.res,
190
187
                            visualise=False, # visualise using rviz
191
 
                            odometry_map=self.odometry_map, # odometry is unreliable
 
188
                            odometry_map=False, # odometry is unreliable
192
189
                            bestpose_queue=self.bestpose_queue,
193
 
                            voxel_queue=self.voxel_queue,
 
190
                            voxel_queue=self.voxel_queue
194
191
                           )
195
192
        except Queue.Empty:
196
193
            sys.exit(0)
324
321
            #         ODOM_FRAME, frame_id, timestamp, rospy.Duration(1.0))
325
322
            #     (trans, quat) = self.listener.lookupTransform(
326
323
            #         ODOM_FRAME, frame_id, timestamp)
327
 
            reference_frame = GLOBAL_FRAME if self.odometry_map else ODOM_FRAME
328
324
            (trans, quat) = self.listener.lookupTransform(
329
 
                reference_frame, frame_id, rospy.Time(0))
 
325
                ODOM_FRAME, frame_id, rospy.Time(0))
330
326
            now = rospy.Time.now()
331
327
            if timestamp and now != timestamp:
332
328
                rospy.loginfo("Now:{0}; Timestamp:{1}".format(now, timestamp))
425
421
    #                                     CameraInfo)
426
422
    # ts = message_filters.TimeSynchronizer([depth_sub, info_sub], 10)
427
423
    # ts.registerCallback(orm.DepthImageCallback)
428
 
    # image_raw is OpenNI depth image representation which is not what we
429
 
    # want.
430
 
    rospy.Subscriber("/camera/depth/image", Image, orm.DepthImageCallback, queue_size=5) 
 
424
    rospy.Subscriber("/camera/depth/image_raw", Image, orm.DepthImageCallback, queue_size=5) 
431
425
    rospy.spin()
432
426
    
433
427
def profile_yappi(func):