plane_reflect_cloud.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('jsk_pr2_startup')
00003 import rospy
00004 import math
00005 import tf
00006 from numpy import array,matrix
00007 from sensor_msgs.msg import *
00008 from geometry_msgs.msg import Point32
00009 
00010 def abs_ray_trace (msg):
00011     global tf_listener, sub, pub, thre
00012     dummy_height = 0.2
00013     floor_frame_id = '/base_footprint'
00014     laser_frame_id = '/laser_tilt_link'
00015 
00016     tf_listener.waitForTransform(floor_frame_id, laser_frame_id,
00017                                  msg.header.stamp, rospy.Duration(0.5))
00018     (trans,rot) = tf_listener.lookupTransform(floor_frame_id, laser_frame_id,
00019                                               msg.header.stamp)
00020     for pt in msg.points:
00021         if pt.z < thre:
00022             alpha = - pt.z / (trans[2] - pt.z)
00023             pt.x = alpha * trans[0] + (1 - alpha) * pt.x
00024             pt.y = alpha * trans[1] + (1 - alpha) * pt.y
00025             pt.z = dummy_height
00026     pub.publish(msg)
00027 
00028 def abs_cloud (msg):
00029     global pub, thre
00030     for p in msg.points:
00031         p.z = p.z if thre < p.z else abs(p.z)
00032     pub.publish(msg)
00033 
00034 if __name__ == '__main__':
00035     global tf_listener, sub, pub, thre
00036 
00037     rospy.init_node('reflect_scan_publisher')
00038     tf_listener = tf.TransformListener()
00039     sub = rospy.Subscriber('input_cloud', PointCloud, abs_ray_trace)
00040     pub = rospy.Publisher('output_cloud', PointCloud)
00041     thre = rospy.get_param('~z_threshold',-0.05)
00042 
00043     rospy.spin()


jsk_pr2_startup
Author(s):
autogenerated on Wed Sep 16 2015 10:32:17