def main():
    global NUM_SENSORS, BAUD_RATE, DEVICE_ADDRESS
    rospy.loginfo("Launching arduino analog reader")
    rospy.init_node("arduinoReader")

    ser = serial.Serial(DEVICE_ADDRESS)
    ser.baudrate = BAUD_RATE

    pubs = []
    for index in range(NUM_SENSORS):
        pubs.append(rospy.Publisher(("/sonar/" + str(index)), Range))

    while not rospy.is_shutdown():
        data = ser.readline().split("\t")
        data.pop() # last element is garbage
        rospy.loginfo("Read line: " + str(data))
        for reading in data:
            #construct message
            msg = Range()
            try:
                msg.range = float(reading)
            except ValueError:
                msg.range = -1.0

            pubIndex = data.index(reading)
            if pubIndex < NUM_SENSORS :
                pubs[pubIndex].publish(msg)

    def __init__(self):
        #初始化节点
        rospy.init_node("talkerUltraSound", anonymous = True)        
        #超声波数据发布节点
        ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20)
        
        ran_broadcaster = tf.TransformBroadcaster()
        
        rate=rospy.Rate(1)
        
        while not rospy.is_shutdown():
			ran_quat = Quaternion()
			ran_quat = quaternion_from_euler(0, 0, 0)	
			#发布TF关系
			ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link")
			#定义一个超声波消息
			ran = Range()
			ran.header.stamp = rospy.Time.now()
			ran.header.frame_id = "/ultrasound"
			ran.field_of_view = 1;
			ran.min_range = 0;
			ran.max_range = 5;
			ran.range = 0.5;
			ultrasound_pub.publish(ran)
			rate.sleep()

    def Publish(self):
		#初始化节点
        rospy.init_node("talkerUltraSound", anonymous = True)        
        #超声波数据发布节点
        self.ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20)
        
        ran_broadcaster = tf.TransformBroadcaster()
        
        rate=rospy.Rate(10)
        #print "Input distance('+' is the end of the number):"
        while not rospy.is_shutdown():
			#ch=self.getKey()
			#self.addChar(ch)
			ran_quat = Quaternion()
			ran_quat = quaternion_from_euler(0, 0, 0)	
			#发布TF关系
			ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link")
			#定义一个超声波消息
			ran = Range()
			ran.header.stamp = rospy.Time.now()
			ran.header.frame_id = "/ultrasound"
			ran.field_of_view = 1;
			ran.min_range = 0;
			ran.max_range = 5;
			ran.range = self.dist;
			#ultrasound_pub.publish(ran)
			rate.sleep()		

 def altitude_pub(self, alt):
     rng = Range()
     rng.field_of_view = math.pi * 0.1
     rng.max_range = 300
     rng.header.frame_id = "sonar_link"
     rng.header.stamp = rospy.Time.now()
     rng.range = alt
     return rng

 def prepareArray(self):
     for i in range(0, len(self.sensors)):
         r = Range()
         r.header.frame_id = "ir{}_link".format(i + 1)
         r.min_range = self.sensors[i].min_range
         r.max_range = self.sensors[i].max_range
         r.radiation_type = r.INFRARED
         self.msg.array += [r]

def sendMessage(sonarId, distance):
	sonarDictionary = {'1':"frontLeftSonar", '2':"diagLeftSonar", '3':"sideLeftSonar", '4':"frontRightSonar", '5':"diagRightSonar", '6':"sideRightSonar"}
	sonarName = sonarDictionary[sonarId]
	sonar = Range()
	sonar.header.stamp = rospy.Time.now()
	sonar.header.frame_id = sonarName
	sonar.range = float(distance)
	sonar.field_of_view = .3489
	sonar.max_range = 6.45
	sonar.min_range = .1524
	return sonar

def random_sonar(delay=1):
    print('Starting random sonar...')
    pub = Publisher('/sensors/range', Range)
    msg = Range()
    while not rospy.is_shutdown():
        msg.header = rospy.Header(frame_id='right_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)
        sleep(delay)
        msg.header = rospy.Header(frame_id='left_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)

def talker():
    pub = rospy.Publisher("sensor1", Range, queue_size=50)
    rospy.init_node("robot", anonymous=True)
    rate = rospy.Rate(1) # 10hz
    while not rospy.is_shutdown():
        msg = Range()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "/sensor1"
        msg.min_range = 0
        msg.max_range = 2
        msg.radiation_type = Range.ULTRASOUND

        rospy.loginfo(msg)
        pub.publish(msg)
        rate.sleep()

    def getDistance(self):
		while True:
			self.dist=input("plase input dis:") 
			try:
				print "you have input:"+str(self.dist)
			except:
				print "you have input nothing"
			ran = Range()
			ran.header.stamp = rospy.Time.now()
			ran.header.frame_id = "/ultrasound"
			ran.field_of_view = 1;
			ran.min_range = 0;
			ran.max_range = 5;
			ran.range = self.dist;
			self.ultrasound_pub.publish(ran)

def range_test():
	from sensor_msgs.msg import Range 
	sensor = RangeSensor()
	print sensor

	m = Range()
	m.range = 2000.0
	m.max_range = 3000.0
	m.min_range = 2.01

	sensor.min_safe = 500

	sensor.measure(m)

	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	m.range = 4000
	sensor.measure(m)
	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	m.range = -2000
	sensor.measure(m)
	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	m.range = 300
	sensor.measure(m)
	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	def test_range(self):
		sensor = Sensors.Range(min_safe=10)
		msg = Range()
		msg.max_range = 3000
		msg.min_range = 5
		msg.range = 2500

		sensor.measure(msg)

		self.assertEquals(sensor.max_range, msg.max_range)
		self.assertEquals(sensor.min_range, msg.min_range)
		self.assertEquals(sensor.range , msg.range)
		self.assertFalse(sensor.isFar())
		self.assertFalse(sensor.isNear())

		msg.range = 4001
		sensor.measure(msg)
		self.assertTrue(sensor.isFar())
		self.assertFalse(sensor.isNear())

		msg.range = 3
		sensor.measure(msg)
		self.assertFalse(sensor.isFar())
		self.assertTrue(sensor.isNear())

		msg.range = 8
		sensor.measure(msg)
		self.assertFalse(sensor.isFar())
		self.assertTrue(sensor.isNear())

def talker():
    pub = rospy.Publisher('lidar_data', Range, queue_size=10)
    rospy.init_node('lidar_node')
    rate = rospy.Rate(1000) # 10hz
    ser=serial.Serial('/dev/ttyUSB1',115200)
    msg=Range()
    
    while not rospy.is_shutdown():
	p=ser.readline()
	if p!='\n' and p!='':
	   try:
	      msg.range=int(p)
	      msg.header.stamp = rospy.Time.now()
	      print msg.range
	      pub.publish(msg)
	   except:
	      pass   
	rate.sleep()

 def publish(self, data):
     msg = Range()
     msg.header.stamp = rospy.get_rostime()
     msg.header.frame_id = self._frameId
     if self._urfType == URF_HRLV:
         msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
         msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
         msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
     else:
         msg.min_range = MIN_RANGE_URF_LV_MaxSonar
         msg.max_range = MAX_RANGE_URF_LV_MaxSonar
         msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
     msg.radiation_type = Range.ULTRASOUND
     msg.range = data
     self._pub.publish(msg)

def node():
    rospy.init_node('sonar_node', anonymous=True)
    range_msg = Range()
    range_msg.radiation_type = range_msg.ULTRASOUND
    pub = rospy.Publisher("/msg_from_sonar", Range, queue_size=10)
    r = rospy.Rate(20)
    rospy.loginfo("reading sonar range")
    while not rospy.is_shutdown():
        #trigger sonar to measure
        cmd = chr(0x22)+chr(0x00)+chr(0x00)+chr(0x22)
        ser.write(cmd)
        data = ser.read(4)
        if len(data)==4 and (ord(data[0])+ord(data[1])+ord(data[2]))&255 == ord(data[3]):
            sonar_range = ord(data[1])*256+ord(data[2])
            range_msg.range = sonar_range
            pub.publish(range_msg)
            rospy.loginfo("distance : %d", sonar_range)
        else:
            rospy.logwarn("sonar error!")
        r.sleep()

def callback(event):
	value = ADC.read("P9_39")
	outvoltage = 3.3 * value
	#minimum range
	if outvoltage > 2.75:
		rangevalue = 15
	elif outvoltage < .4:
		rangevalue = 150
	else:
		#exponential fit function, derived from data sheet
		rangevalue = 61.7 * math.pow(outvoltage, -1.2)

	#assign values and publish
	pub = rospy.Publisher('/range', Range, queue_size=10)
	rangeout = Range()
	rangeout.min_range = 15
	rangeout.max_range = 150
	rangeout.range = rangevalue
	rospy.loginfo(rangeout)
	pub.publish(rangeout)

def callDist(data):

		#reads voltage value
        voltage = ADC.read("P9_40")
		#converts voltage values into distance(meters)
        distance = (voltage**-.8271732796)
        distance = distance*.1679936709
    	#checks and discards data outside of accurate range
        if distance>2:
            distance = 2
        elif distance<.15:
            distance = .15

    	#Writes data to Range message
        msg = Range()
        header = Header()
        #creates header
        msg.header.stamp.secs = rospy.get_time()
        msg.header.frame_id = "/base_link"
        msg.radiation_type = 1
        msg.field_of_view = .15	#about 8.5 degrees
        msg.min_range = .15
        msg.max_range = 2
        msg.range = distance
        pub.publish(msg)

def talker():
    signal.signal(signal.SIGINT, signal_handler)
    time.sleep(1)
    setup_servo()
    pub = rospy.Publisher('servo_ulta', Range, queue_size=10)
    rospy.init_node('servo_ultra', anonymous=True, disable_signals=False )
    rate = rospy.Rate(10) # 10hz
    ultrasonic_number = 1
    
    pause_time = 0.1
    delay_ultrasonic = 0.11
    STEP_ANGLE = 10

    CURR_ANGLE = (90.0 * math.pi) / 180.0

    mindt = 0.65
    maxdt = 2.48
    dt = [mindt, maxdt]
    extremes = [int(x * 1024.0 / 20.0) for x in dt]

    dt_range = extremes[1] - extremes[0]
    dt_step = int((dt_range / 180.0) * STEP_ANGLE)

    br = tf.TransformBroadcaster()

    while not rospy.is_shutdown():
        #broadcast_transforms()

        # Move servo to counter-clockwise extreme.
        wiringpi.pwmWrite(18, extremes[1])
        sleep(0.2)
        CURR_ANGLE = (90.0 * math.pi) / 180.0

        for i in range(extremes[1],extremes[0],-dt_step):         # 1025 because it stops at 1024
            wiringpi.pwmWrite(18,i)
            sleep(pause_time)
            br.sendTransform((0.15, 0.0, 0.0),tf.transformations.quaternion_about_axis(CURR_ANGLE, (0, 0, 1)), rospy.Time.now(), "1", "base_link")
            data = Range()
            data.header.frame_id = "1"
            data.header.stamp = rospy.Time.now()
            data.radiation_type = 0
            data.min_range = 0.05 
            data.max_range = 2.0
            data.field_of_view = 0.164
            try :
                data.range = float(get_us_data_from(ultrasonic_number)) /100.0
            except IOError:
                subprocess.call(['i2cdetect', '-y', '1'])
                data.range = 0.0
            pub.publish(data)
            CURR_ANGLE -= (STEP_ANGLE * math.pi) / 180.0

    def default(self, ci='unused'):
        msg = Range()
        msg.header = self.get_ros_header()
        msg.radiation_type = Range.INFRARED
        msg.field_of_view = 20
        msg.min_range = 0
        msg.max_range = self.component_instance.bge_object['laser_range']
        tmp = msg.max_range
        for ray in self.data['range_list']:
            if tmp > ray:
                tmp = ray
        msg.range = tmp

        self.publish(msg)

	def send_range(self, msg, current_time):
		# ultra sonic range finder
		scan = Range()
		scan.header.stamp = current_time
		scan.header.frame_id = "forward_sensor"
		scan.radiation_type = 0
		scan.field_of_view = 60*pi/180
		scan.min_range = 0.0
		scan.max_range = 4.0
		scan.range = msg.d1/100.0
		self.pub_sonar.publish(scan)

    def _msg(self, frame_id, distance):
        msg = Range()
        msg.header.frame_id = frame_id
        msg.header.stamp = rospy.Time.now()
        msg.radiation_type = self._type
        msg.field_of_view = self._fov
        msg.min_range = self._min
        msg.max_range = self._max
        msg.range = min(max(distance, msg.min_range), msg.max_range)

        return msg