wt_node: battery capacity
[ros_wild_thumper.git] / scripts / sensor_board.py
index 337830ddb4407c9fad4c6b1511fdb81fb9cdd697..c688ff9616c019d5f7b5c5c3b30cb1d8a54a6d3f 100755 (executable)
@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: iso-8859-15 -*-
 
+import sys
 import rospy
 import struct
 import prctl
@@ -11,8 +12,8 @@ from pyshared.humidity import *
 from wild_thumper.msg import Sensor
 
 # Board warming offset
-TEMP_ERROR = -2 # -5 # degree celsius
-PRESSURE_ERROR = -4.5
+TEMP_ERROR = 0 # -2.5 # -5 # degree celsius
+PRESSURE_ERROR = -2.5
 
 """
 LDR:
@@ -43,9 +44,13 @@ def get(addr=0x58):
        pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
 
        # fix temperature/humidity
-       temp_real = temp_mess + TEMP_ERROR 
-       humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
-       humidity_real = calc_humidity_rel(temp_real, humidity_abs)
+       if TEMP_ERROR:
+               temp_real = temp_mess + TEMP_ERROR
+               humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
+               humidity_real = calc_humidity_rel(temp_real, humidity_abs)
+       else:
+               temp_real = temp_mess
+               humidity_real = humidity_mess
 
        return lux, temp_real, humidity_real, pressure_kpa, co
 
@@ -53,6 +58,11 @@ class SensorBoard:
        def __init__(self):
                rospy.init_node('sensor_board')
                prctl.set_name("sensor_board")
+               try:
+                       get()
+               except:
+                       print >>sys.stderr, "No sensor board, shutting down"
+                       exit(1)
                self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
                self.run()
 
@@ -66,8 +76,6 @@ class SensorBoard:
                                if (datetime.now() - t_last_check).seconds > 900:
                                        ventilate = check_ventilate(temp, humidity)
                                        t_last_check = datetime.now()
-                               if (ventilate > 1.10):
-                                       print "L├╝ften!"
                                
                                msg = Sensor()
                                msg.header.stamp = rospy.Time.now()
@@ -76,7 +84,7 @@ class SensorBoard:
                                msg.humidity = humidity
                                msg.pressure = pressure
                                msg.co = co
-                               msg.ventilate = ventilate
+                               msg.ventilate = True if ventilate > 1.10 else False
 
                                self.pub.publish(msg)