Go to the documentation of this file.00001
00002
00003 import rospy
00004 from sensor_msgs.msg import LaserScan
00005
00006 pub = None
00007
00008 def callback(msg):
00009 """
00010 Convert laser scans to REP 117 standard:
00011 http://www.ros.org/reps/rep-0117.html
00012 """
00013 ranges_out = []
00014 for dist in msg.ranges:
00015 if dist < msg.range_min:
00016
00017
00018 ranges_out.append(float("-inf"))
00019
00020 elif dist > msg.range_max:
00021
00022
00023 ranges_out.append(float("inf"))
00024 else:
00025 ranges_out.append(dist)
00026
00027 msg.ranges = ranges_out
00028 pub.publish(msg)
00029
00030 def main():
00031 global pub
00032 rospy.init_node('rep117_filter')
00033
00034 pub = rospy.Publisher('scan_filtered', LaserScan, queue_size=10)
00035 rospy.Subscriber('scan', LaserScan, callback)
00036 rospy.spin()
00037
00038 if __name__ == '__main__':
00039 try:
00040 main()
00041 except rospy.ROSInterruptException:
00042 pass