Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [52] pkg/trunk/IBPSBatteryInterface
|
From: <tf...@us...> - 2008-03-28 01:20:44
|
Revision: 52
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=52&view=rev
Author: tfoote
Date: 2008-03-27 18:20:50 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
basic ros node calling parseIBPS to read from batteries
Modified Paths:
--------------
pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py
Added Paths:
-----------
pkg/trunk/IBPSBatteryInterface/nodes/
pkg/trunk/IBPSBatteryInterface/nodes/monitorBatteries
pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/monitorBatteries.py
Added: pkg/trunk/IBPSBatteryInterface/nodes/monitorBatteries
===================================================================
--- pkg/trunk/IBPSBatteryInterface/nodes/monitorBatteries (rev 0)
+++ pkg/trunk/IBPSBatteryInterface/nodes/monitorBatteries 2008-03-28 01:20:50 UTC (rev 52)
@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+
+PKG = 'IBPSBatteryInterface'
+MODULE = 'IBPSBatteryInterface.monitorBatteries' #this is the module that will be invoked by python
+
+######################################################
+# BOILERPLATE: Should not have to modify anything
+# below except for very last line
+######################################################
+# Bootstrap ourselves into latest rospy install
+import sys, os
+BOOTSTRAP_VERSION = "0.1"
+
+# Read in ROS_ROOT
+if not os.environ.has_key('ROS_ROOT'):
+ print """\nCannot run ROS: ROS_ROOT is not set.\nPlease set the ROS_ROOT environment variable to the
+location of your ROS install.\n"""
+ sys.exit(-1)
+rosRoot = os.environ['ROS_ROOT']
+
+# Read in the rospy directory location from the 'rospack latest rospy' command
+rospackLatest = os.popen(os.path.join(rosRoot,'rospack')+' latest rospy', 'r')
+rospyDir = rospackLatest.read()
+rospackLatest.close()
+if rospyDir is None or not os.path.isdir(rospyDir.strip()):
+ print "\nERROR: Cannot locate rospy installation.\n"
+ sys.exit(-1)
+
+# Run launcher bootstrapper
+sys.path.append(os.path.join(rospyDir.strip(),'scripts'))
+import launcher
+
+manifestFile = launcher.getManifestFile(sys.argv[0], PKG)
+launcher.init(BOOTSTRAP_VERSION)
+launchCommand, launchArgs, launchEnv = \
+ launcher.getLaunchCommands(manifestFile, MODULE)
+launcher.ready(launchCommand, launchArgs, launchEnv, BOOTSTRAP_VERSION)
+
+######################################################
+# END BOILERPLATE:
+# You may wish to modify the exec command below to
+# customize the behavior of your node, e.g.:
+# * env['FOO'] = bar
+# * launchArgs.append('--test')
+######################################################
+
+os.execvpe(launchCommand, launchArgs, launchEnv)
Property changes on: pkg/trunk/IBPSBatteryInterface/nodes/monitorBatteries
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/monitorBatteries.py
===================================================================
--- pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/monitorBatteries.py (rev 0)
+++ pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/monitorBatteries.py 2008-03-28 01:20:50 UTC (rev 52)
@@ -0,0 +1,93 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+"""
+monitorBatteries
+
+To run, invoke nodes/monitorBatteries
+"""
+
+import os, sys, getopt, traceback, logging
+import rospy
+import time
+import IBPSBatteryInterface.parseIBPS
+
+NAME = 'monitorBatteries'
+
+def usage(stdout, progname):
+ print >>stdout, """%s [-h]
+
+"""%progname+rospy.USAGE_ENV
+
+def monitorBatteriesMain(argv, stdout, env):
+ # default arguments
+ server = "http://localhost"
+ server_port = rospy.DEFAULT_TEST_PORT
+
+ #check arguments for a help flag
+ optlist, args = getopt.getopt(argv[1:], "h?p:s:", ["help","port=","server=","test"])
+ for o, a in optlist:
+ if o in ("-h","-?","--help"):
+ usage(stdout, argv[0])
+ return
+ elif o in ("--test"):
+ server_port = rospy.DEFAULT_TEST_PORT
+ elif o in ("-p", "--port"):
+ server_port = a
+ elif o in ("-s", "--server"):
+ server = a
+
+ serverUri = '%s:%s/'%(server,server_port)
+ print "Looking for server at %s"%serverUri
+ os.environ[rospy.ROS_MASTER_URI] = serverUri
+ os.environ[rospy.ROS_NODE] = "viewGraph"
+ os.environ[rospy.ROS_PORT] = str(0) # any
+
+
+ master = rospy.getMaster()
+
+
+ IBPSBatteryInterface.parseIBPS.main()
+
+if __name__ == '__main__':
+ try:
+ monitorBatteriesMain(sys.argv, sys.stdout, os.environ)
+ except Exception, e:
+ traceback.print_exc()
+ #attempt to log, may fail if logger is not properly initialized
+ logger = logging.getLogger(NAME)
+ logger.error(str(e)+"\n"+traceback.format_exc())
+ print "Exception is causing %s exit, check log for details"%NAME
+
+ print "exiting"
+
+
Modified: pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py
===================================================================
--- pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py 2008-03-27 23:56:14 UTC (rev 51)
+++ pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py 2008-03-28 01:20:50 UTC (rev 52)
@@ -104,139 +104,140 @@
+def main():
-os.system('stty 19200 </dev/ttyUSB0')
-os.system('stty 19200 </dev/ttyUSB1')
-os.system('stty 19200 </dev/ttyUSB2')
-os.system('stty 19200 </dev/ttyUSB3')
+ os.system('stty 19200 </dev/ttyUSB0')
+ os.system('stty 19200 </dev/ttyUSB1')
+ os.system('stty 19200 </dev/ttyUSB2')
+ os.system('stty 19200 </dev/ttyUSB3')
-
+
#f= open('testfile.txt','r')
-f0 = open('/dev/ttyUSB0','r')
-f1 = open('/dev/ttyUSB1','r')
-f2 = open('/dev/ttyUSB2','r')
-f3 = open('/dev/ttyUSB3','r')
+ f0 = open('/dev/ttyUSB0','r')
+ f1 = open('/dev/ttyUSB1','r')
+ f2 = open('/dev/ttyUSB2','r')
+ f3 = open('/dev/ttyUSB3','r')
# split on % to string and checksum
# check checksum
-myPow = robotPower()
+ myPow = robotPower()
-start_time = time.time()
-last_time = start_time
-while True:
- current, blah, blah2 = select.select([f0,f1,f2,f3],[],[],1)
- for f in current:
- if f == f0:
- port = 0;
- port_string = 'f0'
- if f == f1:
- port = 1;
- port_string = 'f1'
- if f == f2:
- port = 2;
- port_string = 'f2'
- if f == f3:
- port = 3;
- port_string = 'f3'
- line = f.readline()
- halves = line.split('%')
- if len(halves) != 2:
- # print 'I did not split on \% correctly'
- continue
- halves[1]=halves[1].strip()
- halves[0]=halves[0].lstrip('$')
+ start_time = time.time()
+ last_time = start_time
+ while True:
+ current, blah, blah2 = select.select([f0,f1,f2,f3],[],[],1)
+ for f in current:
+ if f == f0:
+ port = 0;
+ port_string = 'f0'
+ if f == f1:
+ port = 1;
+ port_string = 'f1'
+ if f == f2:
+ port = 2;
+ port_string = 'f2'
+ if f == f3:
+ port = 3;
+ port_string = 'f3'
+ line = f.readline()
+ halves = line.split('%')
+ if len(halves) != 2:
+ # print 'I did not split on \% correctly'
+ continue
+ halves[1]=halves[1].strip()
+ halves[0]=halves[0].lstrip('$')
# print line
# print halves
# print len(halves)
# print checksum256(halves[0])
# print hex2dec(halves[1])
- # print hex(255)
+ # print hex(255)
- # read first char and switch
- message = halves[0]
+ # read first char and switch
+ message = halves[0]
- # case C controller
+ # case C controller
- # split on commas
- # first element is controller number
- # for each pair
- # read index 01-07
- # record value
- if len(message) < 2:
- print "error message too short: \"%s\" from \"%s\""%(message, line)
- print "This often indicates a misconfigured serial port check port:"
- print f
- continue
- if message[0] == 'C':
- controller_number = int(message[1])
- #print 'Controller on port %s says:'%(port_string)
- splitmessage = message.split(',')
+ # split on commas
+ # first element is controller number
+ # for each pair
+ # read index 01-07
+ # record value
+ if len(message) < 2:
+ print "error message too short: \"%s\" from \"%s\""%(message, line)
+ print "This often indicates a misconfigured serial port check port:"
+ print f
+ continue
+ if message[0] == 'C':
+ controller_number = int(message[1])
+ #print 'Controller on port %s says:'%(port_string)
+ splitmessage = message.split(',')
- #print 'batteries present'
- #print readmask(splitmessage[2])
- mask = readmask(splitmessage[2])
- for i in range(0,8):
- myPow.controllers[port].batteries[i].present = mask[i]
- #print 'batteries charging'
- #print readmask(splitmessage[4])
- mask = readmask(splitmessage[2])
- for i in range(0,8):
- myPow.controllers[port].batteries[i].charging = mask[i]
- #print 'batteries supplying power to system'
- #print readmask(splitmessage[6])
- mask = readmask(splitmessage[6])
- for i in range(0,8):
- myPow.controllers[port].batteries[i].supplying_power = mask[i]
- #print 'batteries which have charge power present'
- #print readmask(splitmessage[10])
- mask = readmask(splitmessage[10])
- for i in range(0,8):
- myPow.controllers[port].batteries[i].w_charge_power = mask[i]
- #print 'batteries with "Power no good"'
- #print readmask(splitmessage[12])
- mask = readmask(splitmessage[12])
- for i in range(0,8):
- myPow.controllers[port].batteries[i].power_no_good = mask[i]
- #print 'batteries charge inhibited'
- #print readmask(splitmessage[14])
- mask = readmask(splitmessage[14])
- for i in range(0,8):
- myPow.controllers[port].batteries[i].charge_inhibited = mask[i]
+ #print 'batteries present'
+ #print readmask(splitmessage[2])
+ mask = readmask(splitmessage[2])
+ for i in range(0,8):
+ myPow.controllers[port].batteries[i].present = mask[i]
+ #print 'batteries charging'
+ #print readmask(splitmessage[4])
+ mask = readmask(splitmessage[2])
+ for i in range(0,8):
+ myPow.controllers[port].batteries[i].charging = mask[i]
+ #print 'batteries supplying power to system'
+ #print readmask(splitmessage[6])
+ mask = readmask(splitmessage[6])
+ for i in range(0,8):
+ myPow.controllers[port].batteries[i].supplying_power = mask[i]
+ #print 'batteries which have charge power present'
+ #print readmask(splitmessage[10])
+ mask = readmask(splitmessage[10])
+ for i in range(0,8):
+ myPow.controllers[port].batteries[i].w_charge_power = mask[i]
+ #print 'batteries with "Power no good"'
+ #print readmask(splitmessage[12])
+ mask = readmask(splitmessage[12])
+ for i in range(0,8):
+ myPow.controllers[port].batteries[i].power_no_good = mask[i]
+ #print 'batteries charge inhibited'
+ #print readmask(splitmessage[14])
+ mask = readmask(splitmessage[14])
+ for i in range(0,8):
+ myPow.controllers[port].batteries[i].charge_inhibited = mask[i]
- else:
- pass
+ else:
+ pass
- if message[0] == 'B':
- controller_number = int(message[1])
- battery_number = int(message[2])
+ if message[0] == 'B':
+ controller_number = int(message[1])
+ battery_number = int(message[2])
- #print 'Battery %d on Controller %d'%(battery_number, controller_number)
+ #print 'Battery %d on Controller %d'%(battery_number, controller_number)
- splitmessage = message.split(',')
+ splitmessage = message.split(',')
- for i in range(0,(len(splitmessage)-1)/2):
- key = splitmessage[i*2+1]
- value = splitmessage[i*2+2]
+ for i in range(0,(len(splitmessage)-1)/2):
+ key = splitmessage[i*2+1]
+ value = splitmessage[i*2+2]
# print 'key %s , value %s'%(splitmessage[i*2+1],splitmessage[i*2+2])
# print 'key %s , value dec %s'%(splitmessage[i*2+1],hex2dec(splitmessage[i*2+2]))
- if key == '09':
- #print 'voltage is %f'%(float(hex2dec(value))/1000.0)
- myPow.controllers[port].batteries[battery_number].voltage = float(hex2dec(value))/1000.0
+ if key == '09':
+ #print 'voltage is %f'%(float(hex2dec(value))/1000.0)
+ myPow.controllers[port].batteries[battery_number].voltage = float(hex2dec(value))/1000.0
- if key == '0A':
- intval = float(hex2dec(value))
- if intval < 32767:
- val = intval
- else:
- val = (intval - 65636.0)
- #print 'current is %f'%(val/1000.0)
- myPow.controllers[port].batteries[battery_number].current = val/1000.0
+ if key == '0A':
+ intval = float(hex2dec(value))
+ if intval < 32767:
+ val = intval
+ else:
+ val = (intval - 65636.0)
+ #print 'current is %f'%(val/1000.0)
+ myPow.controllers[port].batteries[battery_number].current = val/1000.0
# case B battery
@@ -247,32 +248,35 @@
# record value cast properly so pages 13 to 35
#case S system data
- if message[0] == 'S':
- splitmessage = message.split(',')
- #print 'System Message'
- for i in range(0,(len(splitmessage)-1)/2):
- key = splitmessage[i*2+1]
- value = splitmessage[i*2+2]
+ if message[0] == 'S':
+ splitmessage = message.split(',')
+ #print 'System Message'
+ for i in range(0,(len(splitmessage)-1)/2):
+ key = splitmessage[i*2+1]
+ value = splitmessage[i*2+2]
# print 'key %s , value %s'%(key,value)
# print 'key %s , value dec %s'%(key,hex2dec(value))
- if key == '01':
- #print 'time until exhaustion = %s minutes'%hex2dec(value)
- myPow.controllers[port].time_remaining = int(hex2dec(value))
- if key == '02':
- #reserved
- pass
+ if key == '01':
+ #print 'time until exhaustion = %s minutes'%hex2dec(value)
+ myPow.controllers[port].time_remaining = int(hex2dec(value))
+ if key == '02':
+ #reserved
+ pass
- if key == '03':
- #print 'text message to the system %s'%value
- myPow.controllers[port].new_system_message(value)
+ if key == '03':
+ #print 'text message to the system %s'%value
+ myPow.controllers[port].new_system_message(value)
- if key == '04':
- #print 'average charge percent %d'% hex2dec(value)
- myPow.controllers[port].average_charge = int(hex2dec(value))
+ if key == '04':
+ #print 'average charge percent %d'% hex2dec(value)
+ myPow.controllers[port].average_charge = int(hex2dec(value))
- #print time.time()
- increment = 1.0
- if time.time() - last_time > increment:
- last_time = last_time + increment
- myPow.print_remaining()
+ #print time.time()
+ increment = 1.0
+ if time.time() - last_time > increment:
+ last_time = last_time + increment
+ myPow.print_remaining()
+
+
+#main()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
