4 from jsk_footstep_msgs.msg
import ExecFootstepsAction
5 from jsk_footstep_controller.msg
import GroundContactState
6 from jsk_footstep_controller.srv
import RequireMonitorStatus, RequireMonitorStatusResponse
7 from threading
import Lock
12 g_contact_state =
None 15 requested_stamp = req.header.stamp
18 while not rospy.is_shutdown():
20 if g_contact_state
and requested_stamp < g_contact_state.header.stamp:
23 rospy.loginfo(
"Waiting for newer message")
26 if abs(g_contact_state.error_pitch_angle) > req.threshold:
27 rospy.loginfo(
"error: %f > %f" % (g_contact_state.error_pitch_angle,
29 return RequireMonitorStatusResponse(
False)
31 rospy.loginfo(
"error: %f < %f" % (g_contact_state.error_pitch_angle,
33 return RequireMonitorStatusResponse(
True)
36 global g_contact_state
41 global pitch_threshold, controller_ac
43 pitch_threshold = rospy.get_param(
"~pitch_threshold", 0.02)
46 sub = rospy.Subscriber(
"/footcoords/contact_state", GroundContactState, contactGroundCallback)
47 srv = rospy.Service(
"require_foot_contact_monitor", RequireMonitorStatus, requireMonitorCallback)
50 if __name__ ==
"__main__":
51 rospy.init_node(
"foot_contact_monitor")