def test_get_ang_vel(self):
"""Tests the _get_ang_vel 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
out = controller._get_ang_vel(0.16, 0.5)
self.assertEqual(out, -0.08)
self.assertFalse(controller._goal_ang_is_reached)
out = controller._get_ang_vel(-0.16, 0.5)
self.assertEqual(out, 0.08)
self.assertFalse(controller._goal_ang_is_reached)
out = controller._get_ang_vel(0.3, 0.5)
self.assertEqual(out, -0.1)
self.assertFalse(controller._goal_ang_is_reached)
out = controller._get_ang_vel(-0.3, 0.5)
self.assertEqual(out, 0.1)
self.assertFalse(controller._goal_ang_is_reached)
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)