jsk_rviz_plugins: landing_time_detector.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Display the difference between reference and actual swing/support time according to hrpsys_ros_bridge/ContactState using
00005 jsk_rviz_plugins/PictogramArray
00006 """
00007 
00008 import rospy
00009 from jsk_rviz_plugins.msg import Pictogram, PictogramArray
00010 from geometry_msgs.msg import Quaternion
00011 from hrpsys_ros_bridge.msg import ContactState, ContactStateStamped, ContactStatesStamped
00012 from std_msgs.msg  import ColorRGBA
00013 import message_filters
00014 
00015 def callback(ref, act):
00016     "msgs = ContactStatesStamped"
00017     if len(ref_contact_states_queue) > buffer_size - 1:
00018         arr = PictogramArray()
00019         arr.header.frame_id = "/odom"
00020         arr.header.stamp = rospy.Time.now()
00021         for i, (ref_st, act_st) in enumerate(zip(ref.states, act.states)):
00022             picto = Pictogram()
00023             if ref_st.state.state == act_st.state.state:
00024                 continue
00025             else:
00026                 if [ref_st.state.state, act_st.state.state] == [ContactState.OFF, ContactState.ON]:
00027                     if [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.OFF] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.OFF] * buffer_size:
00028                         picto.character = "fa-long-arrow-down"
00029                         picto.size = 1
00030                         picto.pose.orientation = Quaternion(0, -1, 0, 1)
00031                         picto.action = Pictogram.ADD
00032                         picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8)
00033                         rospy.loginfo("%s early landing %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time))
00034                     elif [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.ON] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.ON] * buffer_size:
00035                         picto.character = "up-bold"
00036                         picto.size = 1
00037                         picto.pose.orientation = Quaternion(0, -1, 0, 1)
00038                         picto.action = Pictogram.ADD
00039                         picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
00040                         rospy.loginfo("%s late taking off", ref_st.header.frame_id)
00041                         print "oso hanare"
00042                     else:
00043                         continue
00044                 elif [ref_st.state.state, act_st.state.state] == [ContactState.ON, ContactState.OFF]:
00045                     if [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.OFF] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.OFF] * buffer_size:
00046                         picto.character = "fa-long-arrow-down"
00047                         picto.size = 1
00048                         picto.pose.orientation = Quaternion(0, -1, 0, 1)
00049                         picto.action = Pictogram.ADD
00050                         picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
00051                         rospy.loginfo("%s late landing", ref_st.header.frame_id)
00052                     elif [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.ON] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.ON] * buffer_size:
00053                         picto.character = "up-bold"
00054                         picto.size = 1
00055                         picto.pose.orientation = Quaternion(0, -1, 0, 1)
00056                         picto.action = Pictogram.ADD
00057                         picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8)
00058                         rospy.loginfo("%s early taking off %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time))
00059                     else:
00060                         continue
00061             picto.header.frame_id = ref_st.header.frame_id
00062             picto.header.stamp = ref_st.header.stamp
00063             arr.pictograms.append(picto)
00064         if len(arr.pictograms) > 0:
00065             pub.publish(arr)
00066         ref_contact_states_queue.pop(0)
00067         act_contact_states_queue.pop(0)
00068     ref_contact_states_queue.append(ref)
00069     act_contact_states_queue.append(act)
00070 
00071 if __name__ == "__main__":
00072     rospy.init_node("landing_time_detector")
00073 
00074     pub = rospy.Publisher("~pictogram_array", PictogramArray)
00075     ref_contact_states_sub = message_filters.Subscriber('~input_ref', ContactStatesStamped)
00076     act_contact_states_sub = message_filters.Subscriber('~input_act', ContactStatesStamped)
00077 
00078     buffer_size = 5
00079     ref_contact_states_queue = []
00080     act_contact_states_queue = []
00081 
00082     ts = message_filters.TimeSynchronizer([ref_contact_states_sub, act_contact_states_sub], 10)
00083     ts.registerCallback(callback)
00084 
00085     rospy.spin()