def test_dist_control_sign(self):
p = wp.RobotPose(xy=[0, 0], theta=0)
t = wp.RobotPose(xy=[1, 0], theta=0)
waypoint = wp.WayPoint()
vl, vr = waypoint.process(pose=p, target=t)
self.assertTrue(vl > 0)
self.assertTrue(vr > 0)
def test_forward_distance_to(self):
p = wp.RobotPose(xy=[1, 1], theta=math.pi / 2)
t = wp.RobotPose(xy=[3, 2], theta=0)
self.assertAlmostEqual(p.forward_distance_to(t), 1)
p = wp.RobotPose(xy=[1, 1], theta=-math.pi / 2)
t = wp.RobotPose(xy=[3, 2], theta=0)
self.assertAlmostEqual(p.forward_distance_to(t), -1)
def test_rel_bearing_to(self):
p = wp.RobotPose(xy=[0, 0], theta=0)
t = wp.RobotPose(xy=[0, 1], theta=0)
self.assertAlmostEqual(p.rel_bearing_to(t), math.pi / 2)
p = wp.RobotPose(xy=[0, 0], theta=math.pi / 2)
t = wp.RobotPose(xy=[0, 1], theta=0)
self.assertAlmostEqual(p.rel_bearing_to(t), 0)
def test_obstacle_avoidance_robot_shoud_stop(self):
p = wp.RobotPose(xy=[0, 0], theta=0)
t = wp.RobotPose(xy=[1, 0], theta=0)
self.assertFalse(wp.obstacle_avoidance_robot_should_stop(p, t, []))
self.assertFalse(
wp.obstacle_avoidance_robot_should_stop(p, t, [[0.5, 0.5]]))
self.assertTrue(
wp.obstacle_avoidance_robot_should_stop(p, t, [[0.1, 0.1]]))
self.assertFalse(
wp.obstacle_avoidance_robot_should_stop(p, t, [[-0.1, 0.1]]))
def test_dont_advance_if_heading_error_is_too_large(self):
p = wp.RobotPose(xy=[0, 0], theta=0)
t = wp.RobotPose(xy=[0, 1], theta=0)
waypoint = wp.WayPoint()
dist, head = waypoint.error(pose=p, target=t)
self.assertTrue(dist == 0)
def test_distance_to(self):
p = wp.RobotPose(xy=[1, 2], theta=0)
t = wp.RobotPose(xy=[0, 0], theta=0)
self.assertAlmostEqual(p.distance_to(t), math.sqrt(5))