00001 from occupancy_grid_utils import *
00002 import math
00003
00004 def cell_str (c):
00005 return str((c.x, c.y))
00006
00007 def header_str (h):
00008 return "[Frame: {0}, Stamp: {1}]".format(h.frame_id, h.stamp)
00009
00010 def time_str (t):
00011 return str(t.sec + t.nsec*1e-9)
00012
00013 def info_str (i):
00014 return "[{0}x{1} map at {2} m/cell]".format(i.width, i.height, i.resolution)
00015
00016 def grid_str (g):
00017 if len(g.data)==g.info.height*g.info.width:
00018 return "[Grid with dims: {0}]".format(g.info)
00019 elif len(g.data)==0:
00020 return "[Info: {0}, data uninitialized]".format(s)
00021 else:
00022 return "[Info: {0}, data size {1} instead of {2}]".format(g.info, len(g.data), g.info.height*g.info.width)
00023
00024 def scan_str (s):
00025 lr = len(s.ranges)
00026 nz = list(s.ranges).count(0)
00027 if nz<lr:
00028 return "[Scan with header {3}, bounds [{0}, {1}] and {2} readings]".\
00029 format(s.angle_min, s.angle_max, len(s.ranges), s.header)
00030 else:
00031 return "[Scan with header {2}, bounds [{0}, {1}]. Uninitialized.]".\
00032 format(s.angle_min, s.angle_max, s.header)
00033
00034 assume_2d = True
00035
00036 def pt_str(p):
00037 if assume_2d:
00038 return str((p.x, p.y))
00039 else:
00040 return str((p.x, p.y, p.z))
00041
00042 def quaternion_str(q):
00043 if assume_2d:
00044 return str(2*math.acos(q.w))
00045 else:
00046 return str((q.x, q.y, q.z, q.w))
00047
00048 def pose_str(p):
00049 return "({0}, {1})".format(p.position, p.orientation)
00050
00051 def polygon_str(p):
00052 s = ', '.join(pt_str(p) for p in p.points)
00053 return "[Polygon with points {0}]".format(s)
00054
00055
00056 Cell.__str__ = cell_str
00057 Header.__str__ = header_str
00058 Time.__str__ = time_str
00059 MapMetaData.__str__ = info_str
00060 OccupancyGrid.__str__ = grid_str
00061
00062 Pose.__str__ = pose_str
00063 Point32.__str__ = pt_str
00064 Point.__str__ = pt_str
00065 Polygon.__str__ = polygon_str
00066
00067
00068 use_repr = True
00069
00070
00071 if use_repr:
00072 Cell.__repr__ = cell_str
00073 Header.__repr__ = header_str
00074 Time.__repr__ = time_str
00075 MapMetaData.__repr__ = info_str
00076 OccupancyGrid.__repr__ = grid_str
00077
00078 Pose.__repr__ = pose_str
00079 Point.__repr__ = pt_str
00080 Point32.__repr__ = pt_str
00081 Polygon.__repr__ = polygon_str
00082 Quaternion.__repr__ = quaternion_str
00083
