monitor_rmp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Battery monitor for the Segway RMP platform.
00005 
00006 Author:  Chris Dunkers, Worcester Polytechnic Institute
00007 Version: June 23, 2014
00008 """
00009 from rmp_msgs.msg import RMPFeedback, RMPBatteryStatus
00010 from python_ethernet_rmp.system_defines import *
00011 import rospy
00012 import math
00013 import time
00014 import os
00015 
00016 class BatteryMonitor:
00017 
00018         def __init__(self):
00019                 """
00020                 Initialize the subscriptions and publishers of the node.
00021                 """
00022                 self.battStatusPub = rospy.Publisher('battery_status_rmp', RMPBatteryStatus, queue_size = 'None')
00023 
00024                 rospy.Subscriber("rmp_feedback", RMPFeedback, self.get_batt_state)
00025                 
00026                 """
00027                 Get the battery parameters
00028                 """
00029                 self.has_front_batt_1 = rospy.get_param('~front_base_batt_1',False)
00030                 self.has_front_batt_2 = rospy.get_param('~front_base_batt_2',False)
00031                 self.has_rear_batt_1 = rospy.get_param('~rear_base_batt_1',False)
00032                 self.has_rear_batt_2 = rospy.get_param('~rear_base_batt_2',False)
00033                 self.has_aux_batt = rospy.get_param('~aux_batt',False)
00034                 
00035                 self.next_check = rospy.Time.now()
00036                 
00037         def get_batt_state(self, rmp):
00038                 """
00039                 Read in the current RMP feedback and publish the pose
00040                 :param rmp: the RMP feedback message
00041                 """
00042                 rmp_items = rmp.sensor_items
00043                 rmp_values = rmp.sensor_values
00044                 
00045                 batt_stat = RMPBatteryStatus()
00046                 
00047                 """
00048                 get the values for the feedback items needed
00049                 """
00050                 for x in range(0, len(rmp_items)):
00051                         if rmp_items[x] == 'front_base_batt_1_soc' and self.has_front_batt_1 == True:
00052                                 batt_stat.soc_items.append("front_base_batt_1_soc")
00053                                 batt_stat.soc_values.append(rmp_values[x])
00054                         elif rmp_items[x] == 'front_base_batt_2_soc' and self.has_front_batt_2 == True:
00055                                 batt_stat.soc_items.append("front_base_batt_2_soc")
00056                                 batt_stat.soc_values.append(rmp_values[x])
00057                         elif rmp_items[x] == 'rear_base_batt_1_soc' and self.has_rear_batt_1 == True:
00058                                 batt_stat.soc_items.append("rear_base_batt_1_soc")
00059                                 batt_stat.soc_values.append(rmp_values[x])
00060                         elif rmp_items[x] == 'rear_base_batt_2_soc' and self.has_rear_batt_2 == True:
00061                                 batt_stat.soc_items.append("rear_base_batt_2_soc")
00062                                 batt_stat.soc_values.append(rmp_values[x])
00063                         elif rmp_items[x] == 'aux_batt_soc' and self.has_aux_batt == True:
00064                                 batt_stat.soc_items.append("aux_batt_soc")
00065                                 batt_stat.soc_values.append(rmp_values[x])
00066                         
00067                 """
00068                 Publish the state of charges of the batteries present
00069                 """     
00070                 batt_stat.header.stamp = rospy.Time.now()
00071                 self.battStatusPub.publish(batt_stat)
00072                 
00073                 if rospy.Time.now() > self.next_check:
00074                         for x in range(0,len(batt_stat.soc_items)):
00075                                 if batt_stat.soc_items[x] == 'front_base_batt_1_soc':
00076                                         self.send_warning("Front battery one", batt_stat.soc_values[x])
00077                                 elif batt_stat.soc_items[x] == 'front_base_batt_2_soc':
00078                                         self.send_warning("Front battery two", batt_stat.soc_values[x])
00079                                 elif batt_stat.soc_items[x] == 'rear_base_batt_1_soc':
00080                                         self.send_warning("Rear battery one", batt_stat.soc_values[x])
00081                                 elif batt_stat.soc_items[x] == 'rear_base_batt_2_soc':
00082                                         self.send_warning("Rear battery two", batt_stat.soc_values[x])
00083                                 elif batt_stat.soc_items[x] == 'aux_batt_soc':
00084                                         self.send_warning("Auxiliary battery", batt_stat.soc_values[x])
00085                                 
00086         def send_warning(self, batt, soc):
00087                 if soc < 5:
00088                         self.next_check = rospy.Time.now() + rospy.Duration.from_sec(5*60) #5min
00089                         message = "espeak \"%s has 5 percent remaining\"" % (batt)
00090                         os.system(message)
00091                 if soc < 10:
00092                         self.next_check = rospy.Time.now() + rospy.Duration.from_sec(10*60)     #10min
00093                         message = "espeak \"%s has 10 percent remaining\"" % (batt)
00094                         os.system(message)
00095                 if soc < 20:
00096                         self.next_check = rospy.Time.now() + rospy.Duration.from_sec(15*60) #15min
00097                         message = "espeak \"%s has 20 percent remaining\"" % (batt)
00098                         os.system(message)
00099                 else:
00100                         self.next_check = rospy.Time.now() + rospy.Duration.from_sec(20*60) #20min
00101 
00102 if __name__ == "__main__":
00103         rospy.init_node("rmp_battery_monitor")
00104         battMonitor = BatteryMonitor()
00105         rospy.loginfo("RMP Battery Monitor Started")
00106         rospy.spin()


battery_monitor_rmp
Author(s): Chris Dunkers
autogenerated on Thu Jun 6 2019 18:52:45