def test_no_angle_normal_goal_top(self):
"""Test the normal case with the goal on top and no angle.
o := goal
o
*=============*
= =
= =
*=============*
"""
box_length = 4
box_width = 2
box_current_pose = [5.0, 5.0, 0.0]
box_goal_pose = [7.0, 10.0]
docking = find_docking_points(box_current_pose, box_goal_pose, box_length, box_width)
# Points
points = []
normals = []
for item in docking:
points.append(item['point'])
normals.append(item['normal'])
# Expected
expected_points = [[3, 5], [4, 4], [6, 4]]
expected_normals = [[1, 0], [0, 1], [0, 1]]
outcome = True
for p in expected_points:
if not p in points:
outcome = False
break
if not np.allclose(normals, expected_normals):
outcome = False
err_msg = 'wrong result is: '
for i in range(3):
err_msg += '[' + `points[i][0]` + ', ' + `points[i][1]` + ']'
for i in range(3):
err_msg += '[' + `normals[i][0]` + ', ' + `normals[i][1]` + ']'
self.assertTrue(outcome, err_msg)
def test_no_angle_singular_goal_top(self):
"""Test the singular case with the goal on top and no angle.
o := goal
o
*=============*
= =
= =
*=============*
"""
box_length = 2
box_width = 1
box_current_pose = [5.0, 5.0, 0.0]
box_goal_pose = [5.0, 10.0]
docking = find_docking_points(box_current_pose, box_goal_pose, box_length, box_width)
# Points
points = []
normals = []
for item in docking:
points.append(item['point'])
normals.append(item['normal'])
# Expected
expected_points = [[5, 4.5], [4, 5], [6, 5]]
expected_normals = [[0, 1], [1, 0], [-1, 0]]
outcome = True
for p in expected_points:
if not p in points:
outcome = False
break
if not np.allclose(normals, expected_normals):
outcome = False
self.assertTrue(outcome)
