1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
|
from py_trees.behaviour import Behaviour
from py_trees.common import Status
from constants import TIMEOUT, JAR_OFFSETS
from setup import blackboard, robot, gps, compass, camera
from utilities import robot_to_world_coords, grasp_point
from numpy import arctan2
# Detect the jar using the camera and store its position in the blackboard.
class JarFinder(Behaviour):
def __init__(self, name, target_jar, jar_offset):
super().__init__(name)
self.target_jar = target_jar
self.offset = jar_offset
def initialise(self):
self.initial_time = robot.getTime()
print("%s.initialise(), targeting %s" % (self.name, self.target_jar))
def update(self):
if robot.getTime() - self.initial_time > TIMEOUT:
print(
"%s.update()[%s->%s], Timed out"
% (self.name, self.status, Status.FAILURE)
)
return Status.FAILURE
gps_pos = gps.getValues()
theta = arctan2(*compass.getValues()[:2])
for jar in camera.getRecognitionObjects():
print("%s.update(), %s detected." % (self.name, jar.getModel()))
if jar.getModel() == self.target_jar:
jar_pos = robot_to_world_coords(jar.getPosition(), gps_pos, theta)
blackboard["target_pos"] = grasp_point(jar_pos, gps_pos, self.offset)
print(
"%s.update()[%s->%s], jar at [%.3f, %.3f]"
% (self.name, self.status, Status.SUCCESS, *jar_pos)
)
return Status.SUCCESS
print(
"%s.update()[%s->%s], %s not found yet."
% (self.name, self.status, self.target_jar, Status.RUNNING)
)
return Status.RUNNING
|
