00001
00002
00003 PKG = 'pr2_power_board'
00004 NAME = 'fake_powerboard'
00005
00006 import roslib; roslib.load_manifest(PKG)
00007
00008 import sys
00009 import time
00010
00011 import rospy
00012 from pr2_msgs.msg import *
00013
00014 from optparse import OptionParser
00015
00016 import threading
00017 from pr2_power_board.srv import PowerBoardCommand
00018
00019 class PowerBoard(threading.Thread):
00020 def __init__(self, options):
00021 threading.Thread.__init__(self)
00022 self.setDaemon(True)
00023
00024 self.options = options
00025
00026 self.pb = PowerBoardState()
00027 self.pb.header.stamp = rospy.rostime.get_rostime()
00028 self.pb.serial_num = 9999
00029 self.pb.input_voltage = 80.123456
00030 self.pb.circuit_state = [3,3,3]
00031 self.pb.circuit_voltage = [70.123456,70.234567,70.345678]
00032 if self.options.run_stop: self.pb.run_stop = 1
00033 else: self.pb.run_stop = 0
00034 if self.options.wireless_stop: self.pb.wireless_stop = 1
00035 else: self.pb.wireless_stop = 0
00036
00037 self.pub = rospy.Publisher("power_board/state", PowerBoardState)
00038
00039 def power_board_control(self, msg):
00040 if msg.command == "disable":
00041 self.pb.circuit_state[msg.breaker_number] = 0
00042 self.pb.circuit_voltage[msg.breaker_number] = 1
00043 if msg.command == "stop":
00044 if self.pb.circuit_state[msg.breaker_number] != 0:
00045 self.pb.circuit_state[msg.breaker_number] = 1
00046 self.pb.circuit_voltage[msg.breaker_number] = 19
00047 if msg.command == "reset":
00048 if self.pb.circuit_state[msg.breaker_number] == 0:
00049 self.pb.circuit_state[msg.breaker_number] = 1
00050 if msg.command == "start":
00051 if self.pb.circuit_state[msg.breaker_number] != 0:
00052 self.pb.circuit_state[msg.breaker_number] = 3
00053 self.pb.circuit_voltage[msg.breaker_number] = 70
00054
00055 def run(self):
00056 while 1:
00057 self.pub.publish(self.pb)
00058 time.sleep(1)
00059
00060 def talker(options):
00061 rospy.init_node(NAME, anonymous=True)
00062 pb = PowerBoard(options)
00063 s = rospy.Service('power_board/control', PowerBoardCommand, pb.power_board_control)
00064
00065 pb.start()
00066 rospy.spin()
00067
00068
00069 if __name__ == '__main__':
00070 parser = OptionParser()
00071 parser.add_option('-r', '--run_stop', action='store_false', default=True)
00072 parser.add_option('-w', '--wireless_stop', action='store_false', default=True)
00073 (options, args) = parser.parse_args()
00074
00075 try:
00076 talker(options)
00077 except KeyboardInterrupt, e:
00078 pass
00079 print "exiting"
00080
