Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('map_laser')
00004 import rospy
00005 import tf
00006 from sensor_msgs.msg import LaserScan
00007 from nav_msgs.msg import OccupancyGrid
00008 from tf.transformations import euler_from_quaternion
00009 from std_msgs.msg import ColorRGBA
00010 import math
00011
00012 class MapLaser:
00013 def __init__(self):
00014 rospy.init_node('map_laser_filter')
00015 self.pub = rospy.Publisher('/base_scan_filter', LaserScan)
00016 self.listener = tf.TransformListener()
00017 self.map = None
00018 self.save = None
00019 self.sub = rospy.Subscriber('/scan_filtered', LaserScan, self.laser_cb, queue_size=1)
00020 self.sub2 = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
00021
00022
00023 def map_cb(self, msg):
00024 self.map = msg
00025
00026 def get_laser_frame(self, msg):
00027 now = msg.header.stamp
00028 f2 = msg.header.frame_id
00029 f1 = '/map'
00030 self.listener.waitForTransform(f1, f2, now, rospy.Duration(.15))
00031
00032 return self.listener.lookupTransform(f1, f2, now)
00033
00034 def is_occupied(self, x, y):
00035 N = 2
00036 for dx in range(-N, N+1):
00037 for dy in range(-N, N+1):
00038 index = (x + dx) + (y+dy)*self.map.info.width
00039 if index < 0 or index > len(self.map.data):
00040 continue
00041 value = self.map.data[index]
00042 if value > 50 or value<0:
00043 return True
00044 return False
00045
00046
00047 def laser_cb(self, msg):
00048 if self.map is None:
00049 return
00050 try:
00051 (trans,rot) = self.get_laser_frame(msg)
00052 self.save = (trans, rot)
00053 except:
00054 if self.save is None:
00055 return
00056 (trans, rot) = self.save
00057
00058 yaw = euler_from_quaternion(rot)[2]
00059
00060 nr = []
00061
00062 for (i, d) in enumerate(msg.ranges):
00063 if d>msg.range_max or d<msg.range_min:
00064 nr.append( msg.range_max )
00065 continue
00066 angle = yaw + msg.angle_min + msg.angle_increment*i
00067 dx = math.cos(angle) * d
00068 dy = math.sin(angle) * d
00069 map_x = trans[0] + dx
00070 map_y = trans[1] + dy
00071
00072 grid_x = int((map_x - self.map.info.origin.position.x) / self.map.info.resolution)
00073 grid_y = int((map_y - self.map.info.origin.position.y) / self.map.info.resolution)
00074
00075 if self.is_occupied(grid_x, grid_y):
00076 nr.append( msg.range_max + 1.0 )
00077 else:
00078 nr.append( d )
00079
00080 msg.ranges = nr
00081 self.pub.publish(msg)
00082
00083 if __name__=='__main__':
00084 x = MapLaser()
00085 rospy.spin()