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 from hrpsys_ros_bridge.msg import ContactStatesStamped, ContactStateStamped, ContactState
00010 from hrpsys_ros_bridge.msg import MotorStates
00011 
00012 # global variable
00013 g_previous_st_controller_mode = None
00014 is_servo_on = False
00015 
00016 def controllerModeToString(msg):
00017     is_lleg_contact = None
00018     is_rleg_contact = None
00019     for state in msg.states:
00020         if state.header.frame_id == "lfsensor":
00021             if state.state.state == ContactState.ON:
00022                 is_lleg_contact = True
00023             else:
00024                 is_lleg_contact = False
00025         if state.header.frame_id == "rfsensor":
00026             if state.state.state == ContactState.ON:
00027                 is_rleg_contact = True
00028             else:
00029                 is_rleg_contact = False
00030     if is_lleg_contact or is_rleg_contact:
00031         return "MODE_ST"
00032     else:
00033         return "MODE_AIR"
00034 
00035 def isChangedControllerMode(actual_from, actual_to, expected_from, expected_to):
00036     if (actual_from in expected_from and
00037         actual_to == expected_to):
00038         return True
00039     else:
00040         return False
00041 
00042 def trig():
00043     global is_servo_on
00044     g_odom_init_trigger_pub.publish(Empty())
00045     if is_servo_on is False:
00046         return
00047     # Say something
00048     sound = SoundRequest()
00049     sound.sound = SoundRequest.SAY
00050     sound.command = SoundRequest.PLAY_ONCE
00051     sound.arg = "Robot stands on the ground."
00052     g_robotsound_pub.publish(sound)
00053     
00054 def contactStatesCallback(msg):
00055     global g_previous_st_controller_mode
00056     global g_odom_init_trigger_pub, g_robotsound_pub
00057     controller_mode = controllerModeToString(msg)
00058     if (controller_mode == "MODE_AIR" or 
00059         controller_mode == "MODE_IDLE" or 
00060         controller_mode == "MODE_ST"):
00061         if g_previous_st_controller_mode == None:
00062             g_previous_st_controller_mode = controller_mode
00063             if controller_mode == "MODE_ST":
00064                 trig()
00065         else:
00066             if isChangedControllerMode(g_previous_st_controller_mode,
00067                                        controller_mode,
00068                                        ["MODE_AIR", "MODE_IDLE"],
00069                                        "MODE_ST"):
00070                 trig()
00071             g_previous_st_controller_mode = controller_mode
00072 
00073 def motorStatesCallback(msg):
00074     global is_servo_on
00075     is_servo_on = any(msg.servo_state)
00076 
00077 if __name__ == "__main__":
00078     rospy.init_node("stabilizer_watcher")
00079     contact_states_sub = rospy.Subscriber("/act_contact_states", ContactStatesStamped, contactStatesCallback, queue_size=1)
00080     motor_states_sub = rospy.Subscriber("/motor_states", MotorStates, motorStatesCallback, queue_size=1)
00081     g_odom_init_trigger_pub = rospy.Publisher("/odom_init_trigger", Empty)
00082     g_robotsound_pub = rospy.Publisher("/robotsound", SoundRequest)
00083     rospy.spin()
00084