|
From: <wa...@us...> - 2009-07-08 22:03:43
|
Revision: 18502
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18502&view=rev
Author: wattsk
Date: 2009-07-08 22:03:40 +0000 (Wed, 08 Jul 2009)
Log Message:
-----------
Changed runtime test to publish on diagnositics when batteries aren't there
Modified Paths:
--------------
pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py
Modified: pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test 2009-07-08 22:03:40 UTC (rev 18502)
@@ -47,6 +47,7 @@
latest_messages = {}
test_name = 'uninitialized'
+status_name = 'Runtime Test'
package = 'uninitialized'
last_runtime = rospy.get_time()
startup_delay = 5.0
@@ -60,31 +61,33 @@
str_map[val.label] = val.value;
str_map["name"]= status.name
str_map["message"] = status.message
+
+ # Store last time message was recorded
+ str_map["last_time"] = rospy.get_time()
return str_map
-def analyze(package, test_impl, params):
- # run the test
- return test_impl.test(latest_messages, params)
-def error_response(error):
- rospy.logerr( "Error: %s"% error)
-def warning_response(warning):
- rospy.logwarn("Warning: %s"% warning)
+def analyze(test_impl, params):
+ # run the test
+ return test_impl.test(latest_messages, params, status_name)
def callback(message, args):
for s in message.status:
latest_messages[s.name] = status_to_map(s)
- sys.stdout.flush()
+ #sys.stdout.flush()
execute_test(args)
def execute_test(args):
- global startup_delay
global last_runtime
+ global publisher
+
if rospy.get_time() < start_time + startup_delay:
rospy.logdebug("Waiting to for startup delay")
return
+
+ # Don't execute at greater than max frequency
time_step = rospy.get_time() - last_runtime
if 1.0 /time_step > options.max_freq:
return
@@ -92,17 +95,11 @@
last_runtime = rospy.get_time()
test_impl, params = args
- results = analyze(package, test_impl, params)
- if 'error' in results:
- for err in results['error']:
- error_response(err)
-
- if 'warn' in results:
- for warn in results['warn']:
- warning_response(warn)
+ msg = DiagnosticMessage()
+ msg.status = [analyze(test_impl, params)]
+ publisher.publish(msg)
-
def runtime_test(package, test_name):
# retrieve the test implementation
roslib.load_manifest(package)
@@ -118,12 +115,16 @@
# must be inited before reading parameters
rospy.init_node(NAME, anonymous=True)
-
# get it's parameters
params = rospy.get_param("~")
rospy.Subscriber("/diagnostics", DiagnosticMessage, callback, (test_impl, params))
+ # Publish in diagnostics the test results
+ global publisher
+ publisher = rospy.Publisher('/diagnostics', DiagnosticMessage)
+
+ # Always executes at greater than the min frequency
while not rospy.is_shutdown():
if rospy.get_time() - last_runtime > 1/options.min_freq:
execute_test((test_impl, params))
@@ -135,6 +136,9 @@
parser.add_option("--test", metavar="TEST_NAME",
dest="test_name", default='',
type="string", help="test name")
+ parser.add_option("--name", metavar="STATUS_NAME",
+ dest="status_name", default='Runtime Test',
+ type="string", help="Name of status message")
parser.add_option("--package", metavar="ROS_PACKAGE",
dest="package", default='runtime_monitor',
type="string", help="package test is in")
@@ -147,16 +151,17 @@
parser.add_option("--startup_delay", metavar="startup_delay",
dest="startup_delay", default='5.0',
type="float", help="Time to wait before Polling(Seconds)")
+
options, args = parser.parse_args()
-
# expected or default
package = options.package
- global startup_delay
startup_delay = options.startup_delay
+ status_name = options.status_name
+
if options.test_name:
test_name = options.test_name
else:
Modified: pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py 2009-07-08 22:03:40 UTC (rev 18502)
@@ -39,35 +39,43 @@
import rospy
from diagnostic_msgs.msg import *
+stat_dict = { 0: 'OK', 2: 'Error', 1: 'Warning' }
+def test(latest_msgs, parameters, test_name):
+ status = DiagnosticStatus()
+ status.name = 'Expected %s' % test_name
+ status.level = 0
+ status.message = 'OK'
+ status.strings = []
+ status.values = []
-
-def test(latest_status, parameters):
- #print latest_status
- results = {}
if "expected_present" in parameters:
for name in parameters["expected_present"]:
- if name in latest_status:
- #print "OK"
- pass
+ if name in latest_msgs and latest_msgs[name]["last_time"] - rospy.get_time() < 3:
+ msg = 'OK'
+ elif name in latest_msgs:
+ msg = 'Stale - Error'
+ status.level = max(status.level, 2)
else:
- #print name, "expected but not observed"
- if 'error' in results:
- results['error'].append("%s expected but not observed"%name)
- else:
- results['error'] = ["%s expected but not observed"%name]
+ msg = 'Missing - Error'
+ status.level = max(status.level, 2)
+ status.strings.append(DiagnosticString(label = name, value = msg))
+
if "desired_present" in parameters:
for name in parameters["desired_present"]:
- if name in latest_status:
- #print "OK"
- pass
+ if name in latest_msgs and latest_msgs[name]["last_time"] - rospy.get_time() < 3:
+ msg = 'OK'
+ elif name in latest_msgs:
+ msg = 'Stale - Warning'
+ status.level = max(status.level, 1)
else:
- #print name, "expected but not observed"
- if 'warn' in results:
- results['warn'].append("%s desired but not observed"%name)
- else:
- results['warn'] = ["%s desired but not observed"%name]
- return results
+ msg = 'Missing - Warning'
+ status.level = max(status.level, 1)
+ status.strings.append(DiagnosticString(label = name, value = msg))
+ status.message = stat_dict[status.level]
+
+ return status
+
Modified: pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py 2009-07-08 22:03:40 UTC (rev 18502)
@@ -37,19 +37,27 @@
import sys
import rospy
-from robot_msgs.msg import *
+from diagnostic_msgs.msg import *
-def test(latest_status, parameters):
- #print latest_status
+def test(latest_status, parameters, test_name):
+ status = DiagnosticStatus()
+ status.name = test_name
+ status.level = 0
+ status.message = 'OK'
+ status.strings = []
+ status.values = []
for name in parameters["expected_present"]:
if name in latest_status:
- print "OK"
+ msg = 'OK'
else:
- print name, "expected but not observed"
+ msg = 'Error'
+ status.strings.append(DiagnosticString(label = name, value = msg))
+ return status
+
Modified: pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch
===================================================================
--- pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch 2009-07-08 22:03:40 UTC (rev 18502)
@@ -1,6 +1,6 @@
<launch>
-<group ns="/runtime">
- <node pkg="runtime_monitor" type="runtime_test" args="--test=expected --min_freq=1.0 --max_freq=2.0 --startup_delay=10.0" output="screen" name="battery_presence">
+<group ns="runtime">
+ <node pkg="runtime_monitor" type="runtime_test" args="--name=Batteries --test=expected --min_freq=1.0 --max_freq=2.0 --startup_delay=10.0" output="screen" name="battery_presence">
<rosparam command="load" file="$(find ocean_battery_driver)/battery_presence.yaml"/>
</node>
</group>
Modified: pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py
===================================================================
--- pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py 2009-07-08 22:03:40 UTC (rev 18502)
@@ -52,7 +52,7 @@
self.mail_sent = False
def update(self, state):
- if((state.energy_remaining / state.energy_capacity) <= self.notify_limit):
+ if(state.energy_capacity == 0 or (state.energy_remaining / state.energy_capacity) <= self.notify_limit):
if not self.mail_sent and state.power_consumption < 0.0:
self.sendEmail()
self.mail_sent = True
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|