jsk_footstep_controller: stabilizer

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # A script to watch stabilizer status
00004 
00005 from hrpsys_ros_bridge.srv import OpenHRP_StabilizerService_getParameter as getParameter
00006 import rospy
00007 from std_msgs.msg import Empty
00008 from sound_play.msg import SoundRequest
00009 
00010 # global variable
00011 g_previous_st_controller_mode = None
00012 
00013 def controllerModeToString(mode):
00014     if mode == 0:
00015         return "MODE_IDLE"
00016     elif mode == 1:
00017         return "MODE_AIR"
00018     elif mode == 2:
00019         return "MODE_ST"
00020     elif mode == 3:
00021         return "MODE_SYNC_TO_IDLE"
00022     elif mode == 4:
00023         return "MODE_SYNC_TO_AIR"
00024 
00025 def isChangedControllerMode(actual_from, actual_to, expected_from, expected_to):
00026     if (actual_from in expected_from and
00027         actual_to == expected_to):
00028         return True
00029     else:
00030         return False
00031 
00032 def trig():
00033     g_odom_init_trigger_pub.publish(Empty())
00034     # Say something
00035     sound = SoundRequest()
00036     sound.sound = SoundRequest.SAY
00037     sound.command = SoundRequest.PLAY_ONCE
00038     sound.arg = "Robot stans on the ground."
00039     g_robotsound_pub.publish(sound)
00040     
00041     
00042 def watch(event):
00043     global g_get_parameter_srv, g_previous_st_controller_mode
00044     global g_odom_init_trigger_pub, g_robotsound_pub
00045     st_param = g_get_parameter_srv().i_param
00046     controller_mode = controllerModeToString(st_param.controller_mode)
00047     if (controller_mode == "MODE_AIR" or 
00048         controller_mode == "MODE_IDLE" or 
00049         controller_mode == "MODE_ST"):
00050         if g_previous_st_controller_mode == None:
00051             g_previous_st_controller_mode = controller_mode
00052             if controller_mode == "MODE_ST":
00053                 trig()
00054         else:
00055             if isChangedControllerMode(g_previous_st_controller_mode,
00056                                        controller_mode,
00057                                        ["MODE_AIR", "MODE_IDLE"],
00058                                        "MODE_ST"):
00059                 trig()
00060             g_previous_st_controller_mode = controller_mode
00061 
00062 if __name__ == "__main__":
00063     rospy.init_node("stabilizer_watcher")
00064     rate = rospy.get_param("~rate", 1.0) # 10Hz
00065     g_get_parameter_srv = rospy.ServiceProxy("/StabilizerServiceROSBridge/getParameter", getParameter)
00066     g_odom_init_trigger_pub = rospy.Publisher("/odom_init_trigger", Empty)
00067     g_robotsound_pub = rospy.Publisher("/robotsound", SoundRequest)
00068     timer = rospy.Timer(rospy.Duration(1.0 / rate), watch)
00069     rospy.spin()
00070