getDockIR.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #########AUTHOR: Younghun Ju <yhju@yujinrobot.comm>, <yhju83@gmail.com>
00003 
00004 import roslib; roslib.load_manifest('kobuki_node')
00005 import rospy
00006 
00007 from kobuki_msgs.msg import DockInfraRed
00008 from kobuki_msgs.msg import SensorState
00009 
00010 class Converter(object):
00011 
00012   def __init__(self):
00013     rospy.init_node("dock_ir_intepreter", anonymous=True)
00014     self.sub_dock_ir = rospy.Subscriber("/mobile_base/sensors/dock_ir", DockInfraRed, self.DockIRCallback)
00015     self.sub_core    = rospy.Subscriber("/mobile_base/sensors/core" , SensorState, self.SensorStateCallback)
00016     self.window = rospy.get_param('~window',10)
00017     self.oneline = rospy.get_param('~oneline',False)
00018     self.stack = []
00019     self.bumper = 0
00020     self.charger = 0
00021 
00022 
00023   def SensorStateCallback(self,data):
00024     self.bumper = data.bumper
00025     self.charger = data.charger
00026 
00027   def DockIRCallback(self,data):
00028     #v1 not filtering
00029     array = [ ord(x) for x in data.data]
00030     array.reverse()
00031     #v2 filtering
00032     self.stack.append(array)
00033     while len(self.stack) > self.window:
00034       del self.stack[0]
00035     if not self.oneline: print 
00036     head = "window: {0:d}/{1:d} ".format(len(self.stack),self.window)
00037     head += "charger: {0:d} ".format(self.charger)
00038     head += "[ON] " if self.charger else "[  ] "
00039     head += "bumper: ["
00040     head += "L" if self.bumper&4 else "~"
00041     head += "C" if self.bumper&2 else "~"
00042     head += "R" if self.bumper&1 else "~"
00043     head += "]"
00044 
00045     array = [0,0,0]
00046     for i in range(len(self.stack)):
00047       for j in range(3):
00048         array[j] |= self.stack[i][j]
00049 
00050 
00051     delimiter = " " if self.oneline else "\n"
00052     ostr = ""
00053     ostr_top = "[far ] "; ostr_bot = "[near] "; ostr_bin="[bin ] "; ostr_dec="[dec ] "
00054     for ir in array:
00055       #ver1: just digits: 
00056       #ostr += "{0:3d}".format(ord(array[i])) + " "
00057       #ver2: just binary number: 
00058       #ostr += "{0:#08b}".format(ord(array[i]))[2:] + " "
00059       #ver3: [far]/[near] = [R|C|L]/[R|C|L] for each channel
00060       #ostr_top = ""; ostr_bot = ""
00061       top = ir>>3; bot =ir
00062       ostr_top += " L" if top&2 else " ~" # far left is 16
00063       ostr_top += "|C" if top&1 else "|~" # far center is 8
00064       ostr_top += "|R" if top&4 else "|~" # far right is 32
00065       ostr_bot += " L" if bot&1 else " ~" # near left is 1
00066       ostr_bot += "|C" if bot&2 else "|~" # near center is 2
00067       ostr_bot += "|R" if bot&4 else "|~" # near right is 4
00068       ostr_top += " "; ostr_bot += " "
00069       ostr_bin += "{0:#08b}".format(ir)[2:] + " "
00070       #ostr_dec += "{0:3d}".format(ir) + " "
00071     ostr = head + delimiter + ostr_top + delimiter + ostr_bot + delimiter + ostr_bin# + "\n" + ostr_dec
00072       #verN: average
00073       #verN: 
00074     print ostr
00075 
00076 if __name__ == '__main__':
00077   try:
00078     instance = Converter()
00079     print 
00080     print "It converts dock_ir data to human friendly format."
00081     print
00082     rospy.spin()
00083   except rospy.ROSInterruptException: pass


kobuki_auto_docking
Author(s): Younghun Ju
autogenerated on Thu Jun 6 2019 17:37:33