relayboard_sim.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('cob_relayboard')
00003 import rospy
00004 import time
00005 from cob_relayboard.msg import EmergencyStopState
00006 from pr2_msgs.msg import PowerBoardState
00007 from std_msgs.msg import Float64
00008 
00009 def relayboard_sim():
00010         rospy.init_node('cob_relayboard_sim')
00011 
00012         # emergency_stop topic
00013         pub_em_stop = rospy.Publisher('/emergency_stop_state', EmergencyStopState)
00014         msg_em = EmergencyStopState()
00015         msg_em.emergency_button_stop = False
00016         msg_em.scanner_stop = False
00017         msg_em.emergency_state = 0
00018 
00019         # power_board/state topic
00020         pub_power_board = rospy.Publisher('/power_board/state', PowerBoardState)
00021         msg_power_board = PowerBoardState()
00022         msg_power_board.header.stamp = rospy.Time.now()
00023         msg_power_board.run_stop = True
00024         msg_power_board.wireless_stop = True #for cob the wireless stop field is misused as laser stop field
00025 
00026         # power_board/voltage topic
00027         pub_voltage = rospy.Publisher('/power_board/voltage', Float64)
00028         msg_voltage = Float64()
00029         msg_voltage.data = 48.0 # in simulation battery is always full
00030 
00031         while not rospy.is_shutdown():
00032                 pub_em_stop.publish(msg_em)
00033                 pub_power_board.publish(msg_power_board)
00034                 pub_voltage.publish(msg_voltage)
00035                 rospy.sleep(1.0)
00036 
00037 if __name__ == '__main__':
00038         try:
00039                 relayboard_sim()
00040         except rospy.ROSInterruptException:
00041                 print "Interupted"
00042                 pass


cob_relayboard
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:34