temperature_monitor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 import rospy
00020 from diagnostic_msgs.msg import DiagnosticArray
00021 
00022 import curses
00023 import traceback
00024 
00025 CASE_WIDTH = 20
00026 CASE_HEIGHT = 1
00027 JOINT_NAMES = ["FFJ0", "FFJ3", "FFJ4",
00028                "MFJ0", "MFJ3", "MFJ4",
00029                "RFJ0", "RFJ3", "RFJ4",
00030                "LFJ0", "LFJ3", "LFJ4",
00031                "THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
00032                "WRJ1", "WRJ2"]
00033 COOL = 50
00034 WARM = 55
00035 
00036 class Joint(object):
00037     def __init__(self, screen, joint_name, x, y):
00038         self.screen = screen
00039         self.x = x
00040         self.y = y
00041         self.joint_name = joint_name
00042         self.temperature = -1
00043 
00044         #self.window = self.screen.subwin(1, 15, self.y+1, self.x + 1)
00045         self.refresh()
00046 
00047 
00048     def set_temperature(self, temperature):
00049         self.temperature = temperature
00050         self.refresh()
00051 
00052     def refresh(self):
00053         self.screen.addstr(self.y+1, self.x+1, self.joint_name)
00054 
00055         if self.temperature == -1: #joint not found
00056             self.screen.addstr(self.y + 1, self.x + 6, "X", curses.color_pair(4) )
00057         elif self.temperature < COOL:
00058             self.screen.addstr(self.y + 1, self.x+6, str(self.temperature), curses.color_pair(1) )
00059         elif self.temperature < WARM:
00060             self.screen.addstr(self.y + 1, self.x + 6, str(self.temperature), curses.color_pair(2) )
00061         else:
00062             self.screen.addstr(self.y + 1, self.x + 6, str(self.temperature), curses.color_pair(3) )
00063         #self.window.refresh()#0,0,0,0,1,15)
00064 
00065 class TemperatureMonitor(object):
00066     MAX_X = 17
00067     MAX_Y = 21
00068 
00069     def __init__(self, screen = None):
00070         try:
00071             curses.curs_set(0)
00072         except:
00073             pass
00074         self.screen = screen
00075         self.pad = curses.newpad(self.MAX_Y,self.MAX_X)
00076         self.pad_pos_x_ = 0
00077         self.pad_pos_y_ = 0
00078         self.pad.border(0)
00079         self.joint_monitors = {}
00080 
00081         curses.init_pair(4, curses.COLOR_WHITE, curses.COLOR_BLACK)
00082         curses.init_pair(1, curses.COLOR_BLACK, curses.COLOR_GREEN)
00083         curses.init_pair(2, curses.COLOR_BLACK, curses.COLOR_YELLOW)
00084         curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_RED)
00085 
00086         for index,joint_name in enumerate(JOINT_NAMES):
00087             begin_x = 0
00088             begin_y = CASE_HEIGHT*index
00089 
00090             self.joint_monitors[joint_name] = Joint(self.pad, joint_name, begin_x, begin_y)
00091 
00092         self.diag_sub_ = rospy.Subscriber("/diagnostics", DiagnosticArray, self.diag_cb_)
00093         self.resize_()
00094 
00095         while True:
00096             event = self.pad.getch()
00097             if event == ord("q"): break
00098             elif event == curses.KEY_RESIZE:
00099                 self.resize_()
00100 
00101             elif event == ord("s"):
00102                 self.pad_pos_y_ += 1
00103                 self.refresh_()
00104             elif event == ord("w"):
00105                 self.pad_pos_y_ -= 1
00106                 self.refresh_()
00107             elif event == ord("a"):
00108                 self.pad_pos_x_ -= 1
00109                 self.refresh_()
00110             elif event == ord("d"):
00111                 self.pad_pos_x_ += 1
00112                 self.refresh_()
00113 
00114     def diag_cb_(self, msg):
00115         for status in msg.status:
00116             for joint in JOINT_NAMES:
00117                 if joint in status.name:
00118                     for value in status.values:
00119                         if value.key == "Temperature":
00120                             self.joint_monitors[joint].set_temperature(round(float(value.value), 1))
00121                             break
00122                     break
00123         self.resize_()
00124 
00125     def resize_(self):
00126         self.pad_pos_x_ = 0
00127         self.pad_pos_y_ = 0
00128         self.refresh_()
00129 
00130     def refresh_(self):
00131         y,x = self.screen.getmaxyx()
00132         self.pad_pos_x_ = min(max(self.pad_pos_x_, 0), self.MAX_X - 1)
00133         self.pad_pos_y_ = min(max(self.pad_pos_y_, 0), self.MAX_Y - 1)
00134         self.pad.refresh(self.pad_pos_y_, self.pad_pos_x_, 0,0, y - 1, x -1)
00135         self.pad.border(0)
00136         for monitor in self.joint_monitors.values():
00137             monitor.refresh()
00138 
00139 
00140 if __name__ == '__main__':
00141     rospy.init_node("temperature_monitor", anonymous=True)
00142     try:
00143         curses.wrapper(TemperatureMonitor)
00144     except:
00145         traceback.print_exc()


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Fri Aug 21 2015 12:24:47