Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 from __future__ import division
00023 import rospy
00024 from sensor_msgs.msg import LaserScan
00025
00026
00027 class LaserPlay(object):
00028 def __init__(self):
00029 self.last_scan = None
00030 self.laser_sub = rospy.Subscriber('/scan',
00031 LaserScan,
00032 self.scan_cb,
00033 queue_size=1)
00034 rospy.loginfo(
00035 "Subscribed to: '" + str(self.laser_sub.resolved_name) + "' topic.")
00036 self.run()
00037
00038 def scan_cb(self, msg):
00039 """
00040 :type msg: LaserScan
00041 """
00042 self.last_scan = msg
00043
00044 def run(self):
00045 """Show information on what was found in the laser scans"""
00046 rospy.loginfo("Waiting for first LaserScan message...")
00047 while not rospy.is_shutdown() and self.last_scan is None:
00048 rospy.sleep(0.2)
00049
00050
00051 r = rospy.Rate(5)
00052 while not rospy.is_shutdown():
00053 self.do_stuff_with_last_scan()
00054 r.sleep()
00055
00056 def do_stuff_with_last_scan(self):
00057 """Print funny sentences about what we can guess of our surroundings by the laser"""
00058
00059 close_reads = 0
00060 far_reads = 0
00061 for r in self.last_scan.ranges:
00062
00063 if r < 1.0:
00064 close_reads += 1
00065
00066 elif r > 3.0:
00067 far_reads += 1
00068
00069
00070 if close_reads / len(self.last_scan.ranges) > 0.5:
00071 rospy.loginfo("Looks like I'm surrounded of friends :D")
00072
00073
00074 if far_reads / len(self.last_scan.ranges) > 0.5:
00075 rospy.loginfo("Looks like I'm alone :(")
00076
00077
00078 if __name__ == '__main__':
00079 rospy.init_node('laser_play')
00080 lp = LaserPlay()
00081 rospy.spin()