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