def test_set_pose(self):
"""Tests the _set_pose function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
pose = np.array([4.3, 8.7, -0.3])
controller._set_pose(pose)
self.assertTrue((controller._pos == np.array([4.3, 8.7])).all())
self.assertEqual(controller._x, 4.3)
self.assertEqual(controller._y, 8.7)
self.assertEqual(controller._psi, -0.3)
self.assertEqual(controller._i, 4)
self.assertEqual(controller._j, 8)
def test_goal_is_visible(self):
"""Tests the _goal_is_visible function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
# there is an obstacle between the position and the goal:
controller._occupancy_grid[6, 5] = 1
pose = np.array([8.3, 5.6, 0.3])
controller._set_pose(pose)
self.assertFalse(controller._goal_is_visible())
# there is no obstacle between the position and the goal:
pose = np.array([5.6, 8.3, 0.3])
controller._set_pose(pose)
def test_get_cmd_vel_end(self):
"""Tests the _get_cmd_vel_end function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
controller._max_ang_vel = 0.1
controller._ang_tolerance = 0.05
pose = np.array([0, 0, 0.16])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, -0.08])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, -0.16])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0.08])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, 1.6])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, -0.1])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, -1.6])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0.1])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, 0.04])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0])).all())
self.assertTrue(controller._goal_ang_is_reached)
pose = np.array([0, 0, -0.04])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0])).all())
self.assertTrue(controller._goal_ang_is_reached)