fake_powerboard.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 PKG = 'pr2_power_board' # this package name
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 


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Thu Jun 6 2019 21:11:02