# Source code for pyfrc.physics.visionsim

"""
The 'vision simulator' provides objects that assist in modeling inputs
from a camera processing system.
"""

import collections
import math

inf = float("inf")
twopi = math.pi * 2.0

def _in_angle_interval(x, a, b):
return (x - a) % twopi <= (b - a) % twopi

[docs]class VisionSimTarget:
"""
Target object that you pass the to the constructor of :class:.VisionSim
"""

def __init__(self, x, y, view_angle_start, view_angle_end):
"""
:param x: Target x position
:param y: Target y position

:param view_angle_start:
:param view_angle_end: clockwise from start angle

View angle is defined in degrees from 0 to 360, with 0 = east,
increasing clockwise. So, if the robot could only see
the target from the south east side, you would use a view angle
of start=0, end=90.
"""
self.x = x
self.y = y

def compute(self, now, x, y, angle):

# incoming angle must be normalized to -180,180 (done in VisionSim)

# note: the position tracking of the robot has Y incrementing
# positively downwards, presumably because it's convenient for
# drawing stuff. Not what I had expected.

# compute target distance
dx = self.x - x
dy = self.y - y

distance = math.hypot(dx, dy)

# at the right distance?
if distance < self.view_dst_start or distance > self.view_dst_end:
return

# determine the absolute angle of the target relative to the robot
target_angle = math.atan2(dy, dx)

# in the view range?
if not _in_angle_interval(
target_angle + math.pi, self.view_angle_start, self.view_angle_end
):
return

# is the robot facing the target?
a = angle - self.fov2
b = angle + self.fov2

if _in_angle_interval(target_angle + math.pi, a + math.pi, b + math.pi):
offset = math.degrees(
(((target_angle - angle) + math.pi) % (math.pi * 2)) - math.pi
)
return (1, now, offset, distance)

[docs]class VisionSim:
"""
This helper object is designed to help you simulate input from a
vision system. The algorithm is a very simple approximation and
has some weaknesses, but it should be a good start and general
enough to work for many different usages.

There are a few assumptions that this makes:

- Your camera code sends new data at a constant frequency
- The data from the camera lags behind at a fixed latency
- If the camera is too close, the target cannot be seen
- If the camera is too far, the target cannot be seen
- You can only 'see' the target when the 'front' of the robot is
around particular angles to the target
- The camera is in the center of your robot (this simplifies some
things, maybe fix this in the future...)

To use this, create an instance in your physics simulator::

targets = [
VisionSim.Target(...)
]

Then call the :meth:compute method from your update_sim method
whenever your camera processing is enabled::

# in physics engine update_sim()
x, y, angle = self.physics_controller.get_position()

if self.camera_enabled:
data = self.vision_sim.compute(now, x, y, angle)
if data is not None:
self.nt.putNumberArray('/camera/target', data[0])
else:
self.vision_sim.dont_compute()

.. note:: There is a working example in the examples repository
you can use to try this functionality out
"""

Target = VisionSimTarget

def __init__(
self,
targets,
camera_fov,
view_dst_start,
view_dst_end,
data_frequency=15,
data_lag=0.050,
physics_controller=None,
):
"""
There are a lot of constructor parameters:

:param targets:          List of target positions (x, y) on field in feet
:param view_angle_start: Center angle that the robot can 'see' the target from (in degrees)
:param camera_fov:       Field of view of camera (in degrees)
:param view_dst_start:   If the robot is closer than this, the target cannot be seen
:param view_dst_end:     If the robot is farther than this, the target cannot be seen
:param data_frequency:   How often the camera transmits new coordinates
:param data_lag:         How long it takes for the camera data to be processed
and make it to the robot
:param physics_controller: If set, will draw target information in UI
"""

self.distance = 0
self.update_period = 1.0 / data_frequency
self.data_lag = data_lag

self.last_compute_time = -10
self.send_queue = collections.deque()

self.targets = targets

assert view_dst_start < view_dst_end
assert self.data_lag > 0.001

objects = []
if physics_controller:
objects = physics_controller.config_obj["pyfrc"]["field"]["objects"]

for target in targets:
target.camera_fov = camera_fov
target.view_dst_start = view_dst_start
target.view_dst_end = view_dst_end
target.fov2 = fov2

objects.append(
{"color": "red", "rect": [target.x - 0.1, target.y - 0.1, 0.4, 0.4]}
)

[docs]    def dont_compute(self):
"""
Call this when vision processing should be disabled
"""
self.send_queue.clear()

[docs]    def get_immediate_distance(self):
"""
Use this data to feed to a sensor that is mostly instantaneous
(such as an ultrasonic sensor).

.. note:: You must call :meth:compute first.
"""
return self.distance

[docs]    def compute(self, now, x, y, angle):
"""
Call this when vision processing should be enabled

:param now:   The value passed to update_sim
:param x:     Returned from physics_controller.get_position
:param y:     Returned from physics_controller.get_position
:param angle: Returned from physics_controller.get_position

:returns: None or list of tuples of (found=0 or 1, capture_time, offset_degrees, distance).
The tuples are ordered by absolute offset from the
target. If a list is returned, it is guaranteed to have at
least one element in it.

Note: If your vision targeting doesn't have the ability
to focus on multiple targets, then you should ignore
the other elements.
"""

# Normalize angle to [-180,180]
output = []
angle = ((angle + math.pi) % (math.pi * 2)) - math.pi

for target in self.targets:
proposed = target.compute(now, x, y, angle)
if proposed:
output.append(proposed)

if not output:
output.append((0, now, inf, 0))
self.distance = None
else:
# order by absolute offset
output.sort(key=lambda i: abs(i[2]))
self.distance = output[-1][3]

# Only store stuff every once in awhile
if now - self.last_compute_time > self.update_period:

self.last_compute_time = now
self.send_queue.appendleft(output)

# simulate latency by delaying camera output
if self.send_queue:
output = self.send_queue[-1]
if now - output[0][1] > self.data_lag:
return self.send_queue.pop()