00001
00002 import roslib; roslib.load_manifest('KheperaNavigation')
00003 import sensor_msgs.msg._Range
00004 import rospy
00005 from KheperaController import KheperaController
00006
00007 import matplotlib.pyplot as plt
00008 import numpy as np
00009
00010 class ir_sensor:
00011 def __init__(self, x, y, degree, min_val=0, max_val=1023):
00012 self.x = x
00013 self.y = y
00014 self.degree = degree
00015
00016 self.min_val = min_val
00017 self.max_val = max_val
00018
00019 self.z = np.polyfit(self.x, self.y, degree)
00020 self.poly = np.poly1d(self.z)
00021
00022 def getDistance(self, v):
00023
00024 if v == self.max_val:
00025 return 2.0
00026 elif v ==self.min_val:
00027 return -1
00028 else:
00029 return self.poly(v)
00030
00031
00032 x = np.array([4., 5., 6., 7., 8., 9., 10., 11., 12., 13., 14., 15., 16., 17., 18., 19., 20., 21., 22., 23., 24., 25., 26., 27., 28., 29., 30.])
00033 y = np.array([1023., 860.6, 742.26, 649.964, 572.588, 521.5, 468.816, 424.624, 387.74, 360.64, 333.072, 308.99, 286.208, 263.312, 247.024, 231.604, 216.42, 201.2, 193.668, 186.236, 178.62, 171.032, 163.38, 155.776, 153.576, 148.228, 140.548])
00034 ir = ir_sensor(x, y, 12)
00035
00036 y = np.arange(2, 30, 0.1)
00037 x = ir.poly(y)
00038 ir_long = ir_sensor(x, y, 12)
00039
00040 khepera = KheperaController()
00041 khepera.connect()
00042
00043 rospy.init_node("Khepera")
00044
00045 pub = rospy.Publisher('ir_distance', sensor_msgs.msg.Range)
00046 msg = sensor_msgs.msg._Range.Range()
00047
00048 msg.radiation_type = 1
00049 msg.min_range = 2
00050 msg.max_range = 50
00051 msg.header.frame_id = 'ir_distance'
00052
00053 while not rospy.is_shutdown():
00054 msg.range = ir_long.getDistance(khepera.getADInput(3))
00055 msg.header.stamp = rospy.Time.now()
00056 pub.publish(msg)
00057
00058 khepera.disconnect()
00059