← Back to branch summary

~csiro-asl/csiro-asl-ros-drivers/trunk

« back to all changes in this revision

Viewing changes to csiro_asl_lib_wrappers/mrol_mapping/test/Kinect_republish.py

  • Committer: Brett Grandbois
  • Date: 2011-09-19 01:21:53 UTC
  • Revision ID: brett.grandbois@csiro.au-20110919012153-9jd3hwkyvd2iwm03
Tags: release-1.0
initial setup and stack repo for csiro-asl-ros-drivers

Show diffs side-by-side

added added

removed removed

Lines of Context:
csiro_asl_lib_wrappers/mrol_mapping/test/Kinect_republish.py
1
 
from __future__ import division
2
 
import roslib; roslib.load_manifest('mrol_mapping')
3
 
import rospy
4
 
 
5
 
import mrol_mapping.poseutil as poseutil
6
 
import mrol_mapping.mrol as mrol
7
 
import mrol_mapping.util as util
8
 
import mrol_mapping.occupiedlist as occupiedlist
9
 
import mrol_mapping.mapper as mapper
10
 
import mrol_mapping.pointcloud as pointcloud
11
 
import numpy as np
12
 
import rosutils_mrol.rosutils_mrol as rosutils
13
 
from sensor_msgs.msg import PointCloud2
14
 
from thread import allocate_lock
15
 
import tf
16
 
 
17
 
 
18
 
def PCcallback(msg):
19
 
    global xyzs
20
 
    global pub
21
 
    global i
22
 
    i += 1
23
 
    xyzs = rosutils.pc22xyzs(msg, max_range=4.)
24
 
    xyzs = pointcloud.PointCloud(xyzs)
25
 
    xyzs.points[:,:3] = poseutil.transformPointsTuple(xyzs.points[:,:3], [0,0,0,-np.pi/2.,0,-np.pi/2.])
26
 
    points = xyzs.points
27
 
    #pc = rosutils.xyzs2pc2(points[:,:3], msg.header.stamp, i, frame_id='/world')
28
 
    pc = rosutils.xyzs2pc2(points, msg.header.stamp, i, frame_id='/world')
29
 
    pub.publish(pc)
30
 
    print "done"
31
 
 
32
 
global xyzs
33
 
global pub
34
 
global i
35
 
i = 0
36
 
pub = rospy.Publisher("/kinect_mrol", PointCloud2)
37
 
rospy.init_node('kinect_republish',log_level=rospy.DEBUG )
38
 
rospy.Subscriber("/camera/rgb/points", PointCloud2, PCcallback, queue_size=5) 
39
 
print "running"
40
 
rospy.spin()