class MoveUntilNewBiome(Behaviour):
def __init__(self, name="MoveUntilNewBiome"):
super(MoveUntilNewBiome, self).__init__(name)
self.bb = Blackboard()
def setup(self, timeout, robot=None, speeds=None):
if robot:
self.robot = robot
if speeds:
self.speeds = speeds
return True
def initialise(self):
pass
# Move forward until the color of the floor changes enough to be
# recognized as a new floor color
def update(self):
if new_biome(self.robot.get_color(), self.bb.get("color_floor")):
if self.status == Status.RUNNING:
return Status.SUCCESS
else:
if self.status != Status.RUNNING:
speed_left, speed_right = self.speeds
self.robot.move(speed_left, speed_right)
return Status.RUNNING
return Status.FAILURE
# Stops the robot and signals the analyzer to analyze the floor color
def terminate(self, new_status):
if new_status == Status.SUCCESS:
self.bb.set("biome_check", True)
self.robot.stop()
def terminate(self, new_status):
if new_status == Status.SUCCESS:
bb = Blackboard()
bb.set(
"biome_type",
analyze_type(bb.get("biome_type"), self.color_original,
self.color_left, self.color_right))
bb.set("biome_check", False)
def test_blackboard_static_names():
console.banner("Test Absolute Names with Static Methods")
print("Set 'foo'")
Blackboard.set("foo", "foo")
print("Get 'foo' [{}]".format(Blackboard.get("foo")))
assert (Blackboard.get("foo") == "foo")
assert (Blackboard.get("/foo") == "foo")
print("Set '/bar'")
Blackboard.set("/bar", "bar")
print("Get 'bar' [{}]".format(Blackboard.get("bar")))
assert (Blackboard.get("bar") == "bar")
assert (Blackboard.get("/bar") == "bar")
def test_static_get_set():
console.banner("Blackboard get/set")
print("Set foo: 5")
Blackboard.set("foo", 5)
print("Unset foo")
Blackboard.unset("foo")
with nose.tools.assert_raises_regexp(KeyError, "foo"):
print(" - Expecting a KeyError")
unused_value = Blackboard.get("foo")
print("Get bar")
with nose.tools.assert_raises_regexp(KeyError, "bar"):
print(" - Expecting a KeyError")
unused_value = Blackboard.get("bar")
print("Set motley: Motley()")
Blackboard.set("motley", Motley())
print("Set motley.nested: nooo")
Blackboard.set("motley.nested", "nooo")
assert (Blackboard.get("motley.nested") == "nooo")
print("Get motley.foo")
with nose.tools.assert_raises_regexp(KeyError, "nested attributes"):
print(" - Expecting a KeyError")
unused_value = Blackboard.get("motley.foo")
print("Set motley.other: floosie")
Blackboard.set("motley.other", "floosie")
assert (Blackboard.get("motley.other") == "floosie")
print("Get missing")
with nose.tools.assert_raises_regexp(KeyError, "missing"):
print(" - Expecting a KeyError")
unused_value = Blackboard.get("missing")
def update(self):
bb = Blackboard()
if bb.get("color_floor"):
return Status.SUCCESS
color = self.robot.get_color()
result = bb.set("color_floor", color)
if result:
return Status.RUNNING
else:
return Status.FAILURE
def main():
"""
Entry point for the demo script.
"""
command_line_argument_parser().parse_args()
print(description())
py_trees.logging.level = py_trees.logging.Level.DEBUG
tree = create_tree()
tree.setup(timeout=15)
robot = Pose2D(250,250);
ball = Pose2D(int(random.random()*500),
int(random.random()*500))
bin = Pose2D(int(random.random()*500),
int(random.random()*500))
blackboard = Blackboard()
blackboard.set("robot", robot)
blackboard.set("ball", ball)
blackboard.set("bin", bin)
try:
while tree.status != py_trees.common.Status.SUCCESS:
print("tick")
tree.tick_once()
img = np.full((500,500,3), 0, dtype=np.uint8)
cv2.circle(img, (robot.x,robot.y), 10, (0,0,255), thickness=-1)
cv2.line(img, (robot.x, robot.y),
(robot.x + int(50*math.cos(robot.th+1.0)),
robot.y + int(50*math.sin(robot.th+1.0))),
(0,0,255), 1, cv2.LINE_AA)
cv2.line(img, (robot.x, robot.y),
(robot.x + int(50*math.cos(robot.th-1.0)),
robot.y + int(50*math.sin(robot.th-1.0))),
(0,0,255), 1, cv2.LINE_AA)
cv2.circle(img, (ball.x,ball.y), 10, (0,255,0), thickness=-1)
cv2.circle(img, (bin.x,bin.y), 10, (255,0,0), thickness=-1)
cv2.imshow('image', img)
cv2.waitKey(100)
if random.random() < 0.05:
ball.x = int(random.random()*500)
ball.y = int(random.random()*500)
print("\n")
except KeyboardInterrupt:
print("")
pass
