def test_conditional_region_merge_2(self):
""" Two blocks besides and below each other. Shouldn't be merged.
xx
xx
xx
xx
"""
r = Region(1)
r2 = Region(2)
r.cells = [Cell() for x in xrange(4)]
r.cell_coords.append([0, 0])
r.cell_coords.append([0, 1])
r.cell_coords.append([1, 0])
r.cell_coords.append([1, 1])
r.min_x = 0
r.min_y = 0
r.max_x = 1
r.max_y = 1
r2.cells = [Cell() for x in xrange(4)]
r2.cell_coords.append([2, 2])
r2.cell_coords.append([2, 3])
r2.cell_coords.append([3, 2])
r2.cell_coords.append([3, 3])
r2.min_x = 2
r2.min_y = 2
r2.max_x = 3
r2.max_y = 3
self.assertFalse(r.merge_region_w_criteria(1, 1, r2))
# Check the size of cell lists
self.assertTrue(len(r.cells) == 4)
self.assertTrue(len(r.cell_coords) == 4)
def init_field(self):
ce = Cell()
ce.set_free()
self.field = [[ce for x in xrange(self.map_width)]
for x in xrange(self.map_width)]
self.segmented_field = [[
self.SEGMENT_MAP_NOT_COLORED for x in xrange(self.map_width)
] for x in xrange(self.map_width)]
def test2_3(self):
c = Cell()
c.set_obstacle()
c.points[Cell.UNDEF_ID] = 94
c.points[Cell.RED_ID] = 6
self.assertTrue(c.is_red(), c.get_color_id())
print c.get_color_hex()
self.assertEqual(c.get_color_hex(), 'ff0000')
self.assertFalse(c.is_undef(), c.get_color_id())
def test2_2(self):
c = Cell()
c.set_obstacle()
c.points[Cell.UNDEF_ID] = 95
c.points[Cell.RED_ID] = 5
self.assertFalse(c.is_red(), c.get_color_id())
self.assertTrue(c.is_undef(), c.get_color_id())
def test_line_fragment_to_region(self):
lf = RegionLineFragment()
lf.cells = [Cell(), Cell()]
lf.cell_coords = [[0, 0], [0, 1]]
lf.min_y = 0
lf.max_y = 1
lf.x = 0
r = lf.to_region(2)
self.assertTrue(len(r.cells) == 2)
self.assertTrue(len(r.cell_coords) == 2)
self.assertTrue([0, 0] in r.cell_coords)
self.assertTrue([0, 1] in r.cell_coords)
self.assertEquals(r.id, 2)
def test_conditional_region_merge(self):
""" Two blocks below each other. Merge should work
xx
xx
xx
xx
"""
r = Region(1)
r2 = Region(2)
r.cells = [Cell() for x in xrange(4)]
r.cell_coords.append([0, 0])
r.cell_coords.append([0, 1])
r.cell_coords.append([1, 0])
r.cell_coords.append([1, 1])
r.min_x = 0
r.min_y = 0
r.max_x = 1
r.max_y = 1
r2.cells = [Cell() for x in xrange(4)]
r.cell_coords.append([2, 0])
r.cell_coords.append([2, 1])
r.cell_coords.append([3, 0])
r.cell_coords.append([3, 1])
r2.min_x = 2
r2.min_y = 0
r2.max_x = 3
r2.max_y = 1
self.assertTrue(r.merge_region_w_criteria(1, 1, r2))
# Check the size of cell lists
self.assertTrue(len(r.cells) == 8)
self.assertTrue(len(r.cell_coords) == 8)
self.assertTrue([0, 0] in r.cell_coords)
self.assertTrue([0, 1] in r.cell_coords)
self.assertTrue([1, 0] in r.cell_coords)
self.assertTrue([1, 1] in r.cell_coords)
self.assertTrue([2, 0] in r.cell_coords)
self.assertTrue([2, 1] in r.cell_coords)
self.assertTrue([3, 0] in r.cell_coords)
self.assertTrue([3, 1] in r.cell_coords)
self.assertEqual(r.min_x, 0)
self.assertEqual(r.min_y, 0)
self.assertEqual(r.max_x, 3)
self.assertEqual(r.max_y, 1)
def test_field2():
size = 50
field = [[Cell() for x in xrange(size)] for x in xrange(size)]
for row in field:
for c in row:
c.set_unknown()
return field
def test2_2(self):
c = Cell()
c.set_obstacle()
c.points[Cell.UNDEF_ID] = 95
c.points[Cell.RED_ID] = 5
self.assertFalse(c.is_red(), c.get_color_id())
self.assertTrue(c.is_undef(), c.get_color_id())
def test_field1():
c = Cell()
c.set_free()
field = [[c for x in xrange(15)] for x in xrange(15)]
field[0][6] = Cell()
field[0][6].set_unknown()
field[0][7] = Cell()
field[0][7].set_unknown()
field[0][8] = Cell()
field[0][8].set_unknown()
field[2][6] = Cell()
field[2][6].set_unknown()
field[2][7] = Cell()
field[2][7].set_unknown()
field[2][8] = Cell()
field[2][8].set_unknown()
return field
def test_field1():
c = Cell()
c.set_free()
field = [[c for x in xrange(15)] for x in xrange(15)]
field[0][6] = Cell()
field[0][6].set_unknown()
field[0][7] = Cell()
field[0][7].set_unknown()
field[0][8] = Cell()
field[0][8].set_unknown()
field[2][6] = Cell()
field[2][6].set_unknown()
field[2][7] = Cell()
field[2][7].set_unknown()
field[2][8] = Cell()
field[2][8].set_unknown()
return field
def __init__(self, size):
self.field = [[Cell() for i in range(self.num_of_cells)]
for j in range(self.num_of_cells)]
self.cell_size = 1.0 * size / self.num_of_cells
self.size = size
self.max_coord = self.size / 2
self.obstacle_regions = []
self.unknown_regions = []
self.__get_point_array = rospy.ServiceProxy('/suturo/GetPointArray',
GetPointArray)
def test_conditional_region_merge4(self):
""" Two blocks diagonal of each other. But this time, no left/right tolerance in merge is given.
Merge should not work
xx
xx
"""
r = Region(1)
r2 = Region(2)
r.cells = [Cell() for x in xrange(4)]
r.cell_coords.append([0, 0])
r.cell_coords.append([0, 1])
r.cell_coords.append([1, 0])
r.cell_coords.append([1, 1])
r.min_x = 0
r.min_y = 0
r.max_x = 1
r.max_y = 1
r2.cells = [Cell() for x in xrange(4)]
r2.cell_coords.append([2, 1])
r2.cell_coords.append([2, 2])
r2.cell_coords.append([3, 1])
r2.cell_coords.append([3, 2])
r2.min_x = 2
r2.min_y = 1
r2.max_x = 3
r2.max_y = 2
self.assertFalse(r.merge_region_w_criteria(0, 0, r2))
# Check the size of cell lists
self.assertEquals(len(r.cells), 4)
self.assertEquals(len(r.cell_coords), 4)
self.assertTrue([0, 0] in r.cell_coords)
self.assertTrue([0, 1] in r.cell_coords)
self.assertTrue([1, 0] in r.cell_coords)
self.assertTrue([1, 1] in r.cell_coords)
def test_region_merge(self):
r = Region(1)
r2 = Region(2)
r_c1 = Cell()
r_c1.set_free()
r.cells.append(r_c1)
r.cell_coords.append([0, 0])
r.min_x = 0
r.min_y = 0
r.max_x = 0
r.max_y = 0
r2_c1 = Cell()
r2_c1.set_unknown()
r2.cells.append(r2_c1)
r2.cell_coords.append([1, 1])
r2.min_x = 0
r2.min_y = 0
r2.max_x = 1
r2.max_y = 1
r.merge_region(r2)
# Check the size of cell lists
self.assertTrue(len(r.cells) == 2)
self.assertTrue(len(r.cell_coords) == 2)
# Check the actual cells
self.assertTrue(r2_c1 in r.cells)
self.assertTrue(r_c1 in r.cells)
self.assertTrue([0, 0] in r.cell_coords)
self.assertTrue([1, 1] in r.cell_coords)
self.assertEqual(r.min_x, 0)
self.assertEqual(r.min_y, 0)
self.assertEqual(r.max_x, 1)
self.assertEqual(r.max_y, 1)
def test_region_merge(self):
r = Region(1)
r2 = Region(2)
r_c1 = Cell()
r_c1.set_free()
r.cells.append(r_c1)
r.cell_coords.append([0,0])
r.min_x = 0
r.min_y = 0
r.max_x = 0
r.max_y = 0
r2_c1 = Cell()
r2_c1.set_unknown()
r2.cells.append(r2_c1)
r2.cell_coords.append([1,1])
r2.min_x = 0
r2.min_y = 0
r2.max_x = 1
r2.max_y = 1
r.merge_region(r2)
# Check the size of cell lists
self.assertTrue(len(r.cells) == 2)
self.assertTrue(len(r.cell_coords) == 2)
# Check the actual cells
self.assertTrue(r2_c1 in r.cells)
self.assertTrue(r_c1 in r.cells)
self.assertTrue([0,0] in r.cell_coords)
self.assertTrue([1,1] in r.cell_coords)
self.assertEqual(r.min_x,0)
self.assertEqual(r.min_y,0)
self.assertEqual(r.max_x,1)
self.assertEqual(r.max_y,1)
def init_field(self):
ce = Cell()
ce.set_free()
self.field = [[ce for x in xrange(self.map_width)] for x in xrange(self.map_width)]
self.segmented_field = [[self.SEGMENT_MAP_NOT_COLORED for x in xrange(self.map_width)] for x in
xrange(self.map_width)]
def make_free_map(self):
c = Cell()
c.set_free()
test_field = [[deepcopy(c) for x in xrange(15)] for y in xrange(15)]
return test_field
def test1(self):
c = Cell()
self.assertTrue(c.is_unknown())
self.assertFalse(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
c.set_obstacle()
self.assertFalse(c.is_unknown())
self.assertFalse(c.is_object())
self.assertTrue(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
self.assertFalse(c.is_blue())
self.assertFalse(c.is_red())
self.assertTrue(c.is_undef(), c.get_color_id())
self.assertTrue(c.get_color() == c.UNDEF)
c.set_blue()
self.assertFalse(c.is_unknown())
self.assertFalse(c.is_object())
self.assertTrue(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
self.assertTrue(c.is_blue())
self.assertFalse(c.is_red())
self.assertFalse(c.is_undef())
c.set_free()
self.assertFalse(c.is_unknown())
self.assertFalse(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertTrue(c.is_free())
self.assertFalse(c.is_blue())
self.assertFalse(c.is_red())
c.set_object()
self.assertFalse(c.is_unknown())
self.assertTrue(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
self.assertTrue(c.is_blue())
self.assertFalse(c.is_red())
self.assertFalse(c.is_undef())
c.set_mark()
self.assertFalse(c.is_unknown())
self.assertTrue(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertTrue(c.is_marked())
self.assertFalse(c.is_free())
self.assertTrue(c.is_blue())
self.assertFalse(c.is_red())
def test2_3(self):
c = Cell()
c.set_obstacle()
c.points[Cell.UNDEF_ID] = 94
c.points[Cell.RED_ID] = 6
self.assertTrue(c.is_red(), c.get_color_id())
print c.get_color_hex()
self.assertEqual(c.get_color_hex(), 'ff0000')
self.assertFalse(c.is_undef(), c.get_color_id())
def test1(self):
c = Cell()
self.assertTrue(c.is_unknown())
self.assertFalse(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
c.set_obstacle()
self.assertFalse(c.is_unknown())
self.assertFalse(c.is_object())
self.assertTrue(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
self.assertFalse(c.is_blue())
self.assertFalse(c.is_red())
self.assertTrue(c.is_undef(), c.get_color_id())
self.assertTrue(c.get_color() == c.UNDEF)
c.set_blue()
self.assertFalse(c.is_unknown())
self.assertFalse(c.is_object())
self.assertTrue(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
self.assertTrue(c.is_blue())
self.assertFalse(c.is_red())
self.assertFalse(c.is_undef())
c.set_free()
self.assertFalse(c.is_unknown())
self.assertFalse(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertTrue(c.is_free())
self.assertFalse(c.is_blue())
self.assertFalse(c.is_red())
c.set_object()
self.assertFalse(c.is_unknown())
self.assertTrue(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertFalse(c.is_marked())
self.assertFalse(c.is_free())
self.assertTrue(c.is_blue())
self.assertFalse(c.is_red())
self.assertFalse(c.is_undef())
c.set_mark()
self.assertFalse(c.is_unknown())
self.assertTrue(c.is_object())
self.assertFalse(c.is_obstacle())
self.assertTrue(c.is_marked())
self.assertFalse(c.is_free())
self.assertTrue(c.is_blue())
self.assertFalse(c.is_red())
def reset(self):
self.field = [[Cell() for i in range(self.num_of_cells)]
for j in range(self.num_of_cells)]
def make_free_map(self):
c = Cell()
c.set_free()
test_field = [[deepcopy(c) for x in xrange(15)] for y in xrange(15)]
return test_field
