jsk_footstep_controller: stance_phase.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_footstep_controller.msg import GroundContactState
00005 from jsk_rviz_plugins.msg import OverlayText
00006 
00007 def callback(msg):
00008     text = OverlayText()
00009     if msg.contact_state == GroundContactState.CONTACT_BOTH_GROUND:
00010         text.text = "Double Stance Phase"
00011         text.fg_color.a = 1.0
00012         text.fg_color.r = 25 / 255.0
00013         text.fg_color.g = 1
00014         text.fg_color.b = 1
00015     elif msg.contact_state == GroundContactState.CONTACT_LLEG_GROUND or msg.contact_state == GroundContactState.CONTACT_RLEG_GROUND:
00016         text.text = "Single Stance Phase"
00017         text.fg_color.a = 1.0
00018         text.fg_color.r = 1
00019         text.fg_color.g = 0
00020         text.fg_color.b = 0
00021     elif msg.contact_state == GroundContactState.CONTACT_UNSTABLE:
00022         text.text = "Unstable"
00023         text.fg_color.a = 1.0
00024         text.fg_color.r = 1
00025         text.fg_color.g = 0
00026         text.fg_color.b = 0
00027     elif msg.contact_state == GroundContactState.CONTACT_AIR:
00028         text.text = "Air"
00029         text.fg_color.a = 1.0
00030         text.fg_color.r = 1
00031         text.fg_color.g = 0
00032         text.fg_color.b = 0
00033     pub.publish(text)
00034 
00035 def main():
00036     global pub
00037     pub = rospy.Publisher("stance_phase_text", OverlayText)
00038     sub = rospy.Subscriber("/footcoords/contact_state", GroundContactState, callback)
00039     rospy.spin()
00040 
00041 
00042 if __name__ == "__main__":
00043     rospy.init_node("stance_phase_text")
00044     main()