asr_vosk: Allow to handle keyword and command in one sentence
[ros_wild_thumper.git] / scripts / sensor_board.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import sys
5 import rospy
6 import struct
7 import prctl
8 import logging
9 from datetime import datetime
10 from time import sleep
11 from math import *
12 from pyshared.i2c import i2c
13 from pyshared.humidity import *
14 from wild_thumper.msg import Sensor
15
16 # Board warming offset
17 TEMP_ERROR = -3.0 # -5 # degree celsius
18 PRESSURE_ERROR = -2.5
19 A_dew_point = 17.271
20 B_dew_point = 237.7
21
22 """
23 LDR:
24 val = [1023, 396.8, 110.75, 28.1]
25 lux = [0, 13.8, 164.9, 1341]
26 """
27
28 logger = logging.getLogger(__name__)
29
30 def get_i2c(addr):
31         dev = i2c(addr)
32         dev.write(struct.pack(">BB", 0x00, 0x0f))
33         dev.close()
34
35         dev = i2c(addr)
36         dev.write(chr(0x00))
37         msg = dev.read(11) # for unknown reason need to read one more byte
38         dev.close()
39         return msg[:10]
40
41
42 def get(addr=0x58):
43         msg = get_i2c(addr)
44         ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
45
46         temp_mess/=10.0
47         humidity_mess/=10.0
48         lux = exp(-8.1065e-08*ldr**3 +8.8678e-05*ldr**2 -0.03636*ldr +8.1547)
49         pressure_v = pressure*2.56/1024
50         pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
51
52         # fix temperature/humidity
53         if TEMP_ERROR:
54                 temp_real = temp_mess + TEMP_ERROR
55                 humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
56                 humidity_real = calc_humidity_rel(temp_real, humidity_abs)
57         else:
58                 temp_real = temp_mess
59                 humidity_real = humidity_mess
60
61         return lux, temp_real, humidity_real, pressure_kpa, co
62
63 class SensorBoard:
64         def __init__(self):
65                 rospy.init_node('sensor_board')
66                 prctl.set_name("sensor_board")
67                 try:
68                         get()
69                 except:
70                         print >>sys.stderr, "No sensor board, shutting down"
71                         exit(1)
72                 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
73                 self.run()
74
75         def run(self):
76                 rate = rospy.Rate(1.0)
77                 t_last_check = datetime.min
78                 ventilate = 0
79                 while not rospy.is_shutdown():
80                         if self.pub.get_num_connections() > 0:
81                                 ldr, temp, humidity, pressure, co = get()
82                                 ventilate = None
83                                 try:
84                                         if (datetime.now() - t_last_check).seconds > 900:
85                                                 ventilate = check_ventilate(temp, humidity)
86                                                 t_last_check = datetime.now()
87                                 except Exception as e:
88                                         logger.error(e)
89                                 
90                                 msg = Sensor()
91                                 msg.header.stamp = rospy.Time.now()
92                                 msg.light = ldr
93                                 msg.temp = temp
94                                 msg.humidity = humidity
95                                 msg.pressure = pressure
96                                 msg.co = co
97                                 msg.ventilate = True if ventilate > 1.10 else False
98                                 tmp = ((A_dew_point * temp) / (B_dew_point + temp)) + log(humidity/100.0)
99                                 msg.dew_point = (B_dew_point * tmp) / (A_dew_point - tmp)
100
101                                 self.pub.publish(msg)
102
103                         rate.sleep()
104
105 if __name__ == "__main__":
106         SensorBoard()