def __init__(self, debug=False, name="TrafficJamChecker"):
super(TrafficJamChecker, self).__init__(name)
self.debug = debug
self.blackboard = Blackboard()
self.world = CarlaDataProvider.get_world()
self.map = CarlaDataProvider.get_map()
self.list_intersection_waypoints = []
# remove initial collisions during setup
list_actors = list(CarlaActorPool.get_actors())
for _, actor in list_actors:
if actor.attributes['role_name'] == 'autopilot':
if detect_lane_obstacle(actor, margin=0.2):
CarlaActorPool.remove_actor_by_id(actor.id)
# prepare a table to check for stalled vehicles during the execution of the scenario
self.table_blocked_actors = {}
current_game_time = GameTime.get_time()
for actor_id, actor in CarlaActorPool.get_actors():
if actor.attributes['role_name'] == 'autopilot':
actor.set_autopilot(True)
self.table_blocked_actors[actor_id] = {
'location': actor.get_location(),
'time': current_game_time
}
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
class BallFound(py_trees.behaviour.Behaviour):
def __init__(self, name="BallFound"):
super(BallFound, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def setup(self, unused_timeout=15):
self.logger.debug("%s.setup()" % (self.__class__.__name__))
return True
def initialise(self):
self.logger.debug("%s.initialise()" % (self.__class__.__name__))
def update(self):
self.logger.debug("%s.update()" % (self.__class__.__name__))
robot = self.blackboard.get("robot")
ball = self.blackboard.get("ball")
th = direction(robot, ball)
d = distance(robot, ball)
if math.fabs(th) > 1 and d > 10:
new_status = py_trees.common.Status.FAILURE
else:
new_status = py_trees.common.Status.SUCCESS
return new_status
def terminate(self, new_status):
self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
class BinClose(py_trees.behaviour.Behaviour):
def __init__(self, name="BinClose"):
super(BinClose, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def setup(self, unused_timeout=15):
self.logger.debug("%s.setup()" % (self.__class__.__name__))
return True
def initialise(self):
self.logger.debug("%s.initialise()" % (self.__class__.__name__))
def update(self):
self.logger.debug("%s.update()" % (self.__class__.__name__))
robot = self.blackboard.get("robot")
bin = self.blackboard.get("bin")
d = distance(robot, bin)
if d < 10:
new_status = py_trees.common.Status.SUCCESS
else:
new_status = py_trees.common.Status.FAILURE
return new_status
def terminate(self, new_status):
self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
def __init__(self,
actor,
target_speed=None,
plan=None,
blackboard_queue_name=None,
avoid_collision=False,
name="FollowWaypoints"):
"""
Set up actor and local planner
"""
super(WaypointFollower, self).__init__(name)
self._actor_list = []
self._actor_list.append(actor)
self._target_speed = target_speed
self._local_planner_list = []
self._plan = plan
self._blackboard_queue_name = blackboard_queue_name
if blackboard_queue_name is not None:
self._queue = Blackboard().get(blackboard_queue_name)
self._args_lateral_dict = {
'K_P': 1.0,
'K_D': 0.01,
'K_I': 0.0,
'dt': 0.05
}
self._avoid_collision = avoid_collision
def test_relative_name():
console.banner("Relative Names")
# should use Blackboard.separator here, but it's a pita - long and unreadable
# just update this if the separator ever changes
test_tuples = [
# namespace, name, relative name
("/", "foo", "foo"),
("/", "/foo", "foo"),
("/foo", "bar", "bar"),
("/foo/", "bar", "bar"),
("/foo", "/foo/bar", "bar"),
("/foo/", "/foo/bar", "bar"),
]
for (namespace, name, absolute_name) in test_tuples:
print("[{}][{}]..........[{}][{}]".format(
namespace,
name,
absolute_name,
Blackboard.absolute_name(namespace, name)
))
assert(absolute_name == Blackboard.relative_name(namespace, name))
namespace = "/bar"
name = "/foo/bar"
print("[{}][{}]..........ValueError".format(namespace, name))
nose.tools.assert_raises(
ValueError, Blackboard.relative_name, namespace, name
)
with nose.tools.assert_raises(ValueError):
print("[{}][{}]..........Expecting ValueError".format(namespace, name))
Blackboard.relative_name(namespace, name)
def __init__(self,
actor_type_list,
transform,
threshold,
blackboard_queue_name,
actor_limit=999,
name="ActorSource",
# ======== additional args ========
init_speed=0., # initial speed of spawned vehicle
randomize=False,
):
"""
Setup class members
"""
super(ActorSource, self).__init__(name)
self._world = CarlaDataProvider.get_world()
self._actor_types = actor_type_list
self._spawn_point = transform
self._threshold = threshold + 5. # todo fix this with accurate vehicle length
self._queue = Blackboard().get(blackboard_queue_name)
self._actor_limit = actor_limit
self._last_blocking_actor = None
# ========== additional attributes ==========
self.init_speed = init_speed
self.randomize = randomize
self.distance_gap = None
def update(self):
blackboard = Blackboard()
num = sum(blackboard.pos[:3])
for idx,item in enumerate(blackboard.candi):
if idx < 3:
blackboard.protocol_flag[item] = (True,blackboard.pos[idx]/num)
return py_trees.Status.SUCCESS
def setup(self, unused_timeout = 15):
blackboard = Blackboard()
PC = dict()
pro_df = pd.read_excel(self.protocols)
for line in pro_df.iterrows():
line_ss = [(i.strip().lower()[:-1],i.strip().lower()[-1]) for i in line[1]['Signs&Symptoms'].split(';')]
if not pd.isnull(line[1]['Possible signs&symptoms additions']):
line_ssr = [(i.strip().lower()[:-1],i.strip().lower()[-1]) for i in line[1]['Possible signs&symptoms additions'].split(';')]
name = line[1]['Protocol']
PC[name] = line_ss + line_ssr
self.PV = dict()
for item in PC:
vec = list()
su = 0.
for i in blackboard.Signs:
res = 0.
for j in PC[item]:
if i == j[0]:
res = 8.**int(j[1])
break;
su += res
vec.append(res)
for i in xrange(len(vec)):
vec[i] = vec[i] / su
self.PV[item] = vec
blackboard.PV = self.PV
return True
class ApproachBin(py_trees.behaviour.Behaviour):
def __init__(self, name="ApproachBin"):
super(ApproachBin, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def setup(self, unused_timeout=15):
self.logger.debug("%s.setup()" % (self.__class__.__name__))
return True
def initialise(self):
self.logger.debug("%s.initialise()" % (self.__class__.__name__))
def update(self):
self.logger.debug("%s.update()" % (self.__class__.__name__))
robot = self.blackboard.get("robot")
ball = self.blackboard.get("ball")
bin = self.blackboard.get("bin")
dx = bin.x - robot.x
dy = bin.y - robot.y
d = math.sqrt(dx*dx+dy*dy)
robot.x += int(10/d*dx)
robot.y += int(10/d*dy)
ball.x += int(10/d*dx)
ball.y += int(10/d*dy)
new_status = py_trees.common.Status.RUNNING
return new_status
def terminate(self, new_status):
self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
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 __init__(self, ego_vehicle, debug=False, name="TrafficLightManipulator"):
super(TrafficLightManipulator, self).__init__(name)
self.ego_vehicle = ego_vehicle
self.debug = debug
self.blackboard = Blackboard()
self.target_traffic_light = None
self.annotations = None
self.reset_annotations = None
self.intervention = False
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
def setup(self, unused_timeout = 15):
level = 'I/P'
blackboard = Blackboard()
#blackboard.action = []
blackboard.level = level
blackboard.tick_num = 0
blackboard.protocol = "Universal Patient Care"
self.text = ''
self.sent_text = []
return True
class ActorSource(AtomicBehavior):
"""
Implementation for a behavior that will indefinitely create actors
at a given transform if no other actor exists in a given radius
from the transform.
Important parameters:
- actor_type_list: Type of CARLA actors to be spawned
- transform: Spawn location
- threshold: Min available free distance between other actors and the spawn location
- blackboard_queue_name: Name of the blackboard used to control this behavior
- actor_limit [optional]: Maximum number of actors to be spawned (default=7)
A parallel termination behavior has to be used.
"""
def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name,
actor_limit=7, name="ActorSource"):
"""
Setup class members
"""
super(ActorSource, self).__init__(name)
self._world = CarlaDataProvider.get_world()
self._actor_types = actor_type_list
self._spawn_point = transform
self._threshold = threshold
self._queue = Blackboard().get(blackboard_queue_name)
self._actor_limit = actor_limit
self._last_blocking_actor = None
def update(self):
new_status = py_trees.common.Status.RUNNING
if self._actor_limit > 0:
world_actors = self._world.get_actors()
spawn_point_blocked = False
if (self._last_blocking_actor and
self._spawn_point.location.distance(self._last_blocking_actor.get_location()) < self._threshold):
spawn_point_blocked = True
if not spawn_point_blocked:
for actor in world_actors:
if self._spawn_point.location.distance(actor.get_location()) < self._threshold:
spawn_point_blocked = True
self._last_blocking_actor = actor
break
if not spawn_point_blocked:
try:
new_actor = CarlaActorPool.request_new_actor(np.random.choice(self._actor_types), self._spawn_point)
self._actor_limit -= 1
self._queue.put(new_actor)
except:
print("ActorSource unable to spawn actor")
return new_status
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 setup(self, unused_timeout=15):
#level = raw_input("Please type in your certification(EMT,A,I/P): \n")
level = 'I/P'
blackboard = Blackboard()
#blackboard.action = []
blackboard.level = level
blackboard.tick_num = 0
blackboard.protocol = "Universal Patient Care"
self.text = ''
self.sent_text = []
return True
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 __init__(self, actor_type_list, transform, threshold, blackboard_queue_name,
actor_limit=7, name="ActorSource"):
"""
Setup class members
"""
super(ActorSource, self).__init__(name)
self._world = CarlaDataProvider.get_world()
self._actor_types = actor_type_list
self._spawn_point = transform
self._threshold = threshold
self._queue = Blackboard().get(blackboard_queue_name)
self._actor_limit = actor_limit
self._last_blocking_actor = None
def test_activity_stream():
console.banner("Activity Stream")
Blackboard.enable_activity_stream(100)
blackboard = py_trees.blackboard.Client(name="Client")
for key in {'foo', 'dude'}:
blackboard.register_key(key=key, access=py_trees.common.Access.READ)
for key in {"spaghetti", "motley"}:
blackboard.register_key(key=key, access=py_trees.common.Access.WRITE)
try:
unused = blackboard.dude
except KeyError: # NO_KEY
pass
try:
unused = blackboard.dudette
except AttributeError: # ACCESS_DENIED
pass
try:
blackboard.dudette = "Jane"
except AttributeError: # ACCESS_DENIED
pass
blackboard.spaghetti = {"type": "Carbonara", "quantity": 1} # INITIALISED
blackboard.spaghetti = {"type": "Gnocchi", "quantity": 2} # WRITE
blackboard.motley = Motley()
blackboard.motley.nested = "mutt"
unused_result = blackboard.motley.nested
try:
# NO_OVERWRITE
blackboard.set("spaghetti", {
"type": "Bolognese",
"quantity": 3
},
overwrite=False)
except AttributeError:
pass
blackboard.unset("spaghetti") # UNSET
print(py_trees.display.unicode_blackboard_activity_stream())
expected_types = [
py_trees.blackboard.ActivityType.NO_KEY,
py_trees.blackboard.ActivityType.ACCESS_DENIED,
py_trees.blackboard.ActivityType.ACCESS_DENIED,
py_trees.blackboard.ActivityType.INITIALISED,
py_trees.blackboard.ActivityType.WRITE,
py_trees.blackboard.ActivityType.INITIALISED,
py_trees.blackboard.ActivityType.ACCESSED,
py_trees.blackboard.ActivityType.ACCESSED,
py_trees.blackboard.ActivityType.NO_OVERWRITE,
py_trees.blackboard.ActivityType.UNSET,
]
for item, expected in zip(Blackboard.activity_stream.data, expected_types):
assert (item.activity_type == expected.value)
blackboard.unregister(clear=True)
def test_unregister_keys_with_clear():
console.banner("Unregister Keys with Clear")
Blackboard.clear()
print(py_trees.display.unicode_blackboard())
print(py_trees.display.unicode_blackboard(display_only_key_metadata=True))
print("--------------------------")
with create_namespaced_blackboards() as (unused_foo, unused_bar, unused_namespace):
print(py_trees.display.unicode_blackboard())
print(py_trees.display.unicode_blackboard(display_only_key_metadata=True))
print("--------------------------")
print(py_trees.display.unicode_blackboard())
print(py_trees.display.unicode_blackboard(display_only_key_metadata=True))
assert(not Blackboard.storage)
assert(not Blackboard.metadata)
def test_nested_exists():
console.banner("Nested Read")
for create in [create_blackboards, create_namespaced_blackboards]:
with create() as (foo, unused_bar, namespace):
print("foo.exists('motley.nested') [{}][{}]".format(
foo.exists("motley.nested"), True))
assert (foo.exists("motley.nested"))
print("foo.exists('motley.not_here') [{}][{}]".format(
foo.exists("motley.not_here"), False))
assert (not foo.exists("motley.not_here"))
namespaced_name = "{}/motley.nested".format(namespace)
print("Blackboard.exists({}) [{}][{}]".format(
namespaced_name, Blackboard.exists(namespaced_name), True))
assert (Blackboard.exists(namespaced_name))
def update(self):
blackboard = Blackboard()
self.posi = blackboard.protocol_flag[self.key][1]
if self.posi == 0:
return py_trees.Status.SUCCESS
if blackboard.Signs['combative'].binary:
s = blackboard.Signs['combative'].score / 1000.
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs['hypoxemia'].score / 1000. * s
blackboard.feedback['physical restraint'] += self.posi * s
if blackboard.Signs['agitation'].binary:
blackboard.feedback['midazolam'] += self.posi * blackboard.Signs['agitation'].score / 1000. * s
# blackboard.feedback['diazepam'] += self.posi * blackboard.Signs['agitation'].score / 1000. * s
# blackboard.feedback['geodon'] += self.posi * blackboard.Signs['agitation'].score / 1000. * s
blackboard.feedback['transport'] += self.posi * s
elif blackboard.Signs['violent'].binary:
s = blackboard.Signs['violent'].score / 1000.
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs['hypoxemia'].score / 1000. * s
blackboard.feedback['physical restraint'] += self.posi * s
if not blackboard.Signs['hypotension'].binary and blackboard.Signs['agitation'].binary:
blackboard.feedback['midazolam'] += self.posi * blackboard.Signs['agitation'].score / 1000. *\
blackboard.Signs['hypotension'].score / 1000. * s
# blackboard.feedback['diazepam'] += self.posi * blackboard.Signs['agitation'].score / 1000. * s
# blackboard.feedback['geodon'] += self.posi * blackboard.Signs['agitation'].score / 1000. * s
blackboard.feedback['transport'] += self.posi * s
else:
# blackboard.feedback['encourage patient to relax'] += self.posi
blackboard.feedback['transport'] += self.posi
return py_trees.Status.SUCCESS
def update(self):
blackboard = Blackboard()
self.posi = blackboard.protocol_flag[self.key][1]
if self.posi == 0:
return py_trees.Status.SUCCESS
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs['hypoxemia'].score / 1000.
blackboard.feedback['cardiac monitor'] += self.posi
blackboard.feedback['transport'] += self.posi
if blackboard.candi[0] == self.key and blackboard.Vitals['gcs'].value == '15':
blackboard.feedback['aspirin'] += self.posi
# consider add dependency on iv
if blackboard.Signs['substance abuse history'].binary:
blackboard.feedback['midazolam'] += self.posi * blackboard.Signs['substance abuse history'].score / 1000.
# blackboard.feedback['diazepam'] += self.posi * blackboard.Signs['substance abuse history'].score / 1000.
elif blackboard.Signs['abuse of substance'].binary:
blackboard.feedback['midazolam'] += self.posi * blackboard.Signs['abuse of substance'].score / 1000.
# blackboard.feedback['diazepam'] += self.posi * blackboard.Signs['abuse of substance'].score / 1000.
if 'STEMI' in blackboard.Vitals['ekg'].value and blackboard.Vitals['pain'].binary \
and blackboard.Vitals['bp'].binary and int(blackboard.Vitals['bp'].value.strip().split('/')[0]) > 100:
blackboard.feedback['nitroglycerin'] += self.posi * blackboard.Vitals['pain'].score / 1000. * blackboard.Vitals['bp'].score / 1000.
if blackboard.Vitals['pain'].binary and blackboard.Inters['nitroglycerin'].binary:
# blackboard.feedback['morphine'] += self.posi * blackboard.Vitals['pain'].score / 1000. * blackboard.Inters['nitroglycerin'].score / 1000.
blackboard.feedback['fentanyl'] += self.posi * blackboard.Vitals['pain'].score / 1000. * blackboard.Inters['nitroglycerin'].score / 1000.
return py_trees.Status.SUCCESS
def update(self):
blackboard = Blackboard()
self.posi = blackboard.protocol_flag[self.key][1]
if self.posi == 0:
return py_trees.Status.SUCCESS
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs[
'hypoxemia'].score / 1000.
if blackboard.Vitals['BP'].binary and int(
blackboard.Vitals['BP'].value.strip().split('/')[0]) <= 90:
blackboard.feedback[
'normal saline'] += self.posi * blackboard.Vitals[
'BP'].score / 1000.
if blackboard.Signs['abuse of substance'].binary:
blackboard.feedback['narcan'] += self.posi * blackboard.Signs[
'abuse of substance'].score / 1000.
elif blackboard.Signs['pin point pupils'].binary:
blackboard.feedback['narcan'] += self.posi * blackboard.Signs[
'pin point pupils'].score / 1000.
elif blackboard.Signs['decreased mental status'].binary:
blackboard.feedback['narcan'] += self.posi * blackboard.Signs[
'decreased mental status'].score / 1000.
elif blackboard.Signs['hypotension'].binary:
blackboard.feedback['narcan'] += self.posi * blackboard.Signs[
'hypotension'].score / 1000.
elif blackboard.Signs['bradypnea'].binary:
blackboard.feedback['narcan'] += self.posi * blackboard.Signs[
'bradypnea'].score / 1000.
blackboard.feedback['cardiac monitor'] += self.posi
blackboard.feedback['transport'] += self.posi
return py_trees.Status.SUCCESS
def update(self):
blackboard = Blackboard()
# posi = Normalized confidence score (dervied from cosine similarity) for protocol indicated by key
self.posi = blackboard.protocol_flag[self.key][1]
if self.posi == 0:
return py_trees.Status.SUCCESS #might be better to return FAILURE?
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs['hypoxemia'].score / 1000.
blackboard.feedback['cardiac monitor'] += self.posi
blackboard.feedback['transport'] += self.posi
if blackboard.candi[0] == self.key and blackboard.Vitals['GCS'].value == '15':
blackboard.feedback['aspirin'] += self.posi
# consider add dependency on iv
if blackboard.Signs['substance abuse history'].binary:
blackboard.feedback['midazolam'] += self.posi * blackboard.Signs['substance abuse history'].score / 1000.
# blackboard.feedback['diazepam'] += self.posi * blackboard.Signs['substance abuse history'].score / 1000.
elif blackboard.Signs['abuse of substance'].binary:
blackboard.feedback['midazolam'] += self.posi * blackboard.Signs['abuse of substance'].score / 1000.
# blackboard.feedback['diazepam'] += self.posi * blackboard.Signs['abuse of substance'].score / 1000.
if 'STEMI' in blackboard.Vitals['EKG'].value and blackboard.Vitals['Pain'].binary \
and blackboard.Vitals['BP'].binary and int(blackboard.Vitals['BP'].value.strip().split('/')[0]) > 100:
blackboard.feedback['nitroglycerin'] += self.posi * blackboard.Vitals['Pain'].score / 1000. * blackboard.Vitals['BP'].score / 1000.
if blackboard.Vitals['Pain'].binary and blackboard.Inters['nitroglycerin'].binary:
# blackboard.feedback['morphine'] += self.posi * blackboard.Vitals['Pain'].score / 1000. * blackboard.Inters['nitroglycerin'].score / 1000.
blackboard.feedback['fentanyl'] += self.posi * blackboard.Vitals['Pain'].score / 1000. * blackboard.Inters['nitroglycerin'].score / 1000.
return py_trees.Status.SUCCESS
def update(self):
blackboard = Blackboard()
self.posi = blackboard.protocol_flag[self.key][1]
if self.posi == 0:
return py_trees.Status.SUCCESS
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs['hypoxemia'].score / 1000.
# have to do?
# blackboard.feedback['capnography'] += self.posi
# blackboard.feedback['bronchodilator'] += self.posi
# blackboard.feedback['metered dose inhaler'] += self.posi
# if respiratory distress
if blackboard.Signs['shortness of breath'].binary and (blackboard.Signs['hypoxemia'].binary or blackboard.Signs['rhonchi'].binary):
s = blackboard.Signs['shortness of breath'].score / 1000. *\
((blackboard.Signs['hypoxemia'].binary * blackboard.Signs['hypoxemia'].score +\
blackboard.Signs['rhonchi'].binary * blackboard.Signs['rhonchi'].score) / \
(blackboard.Signs['hypoxemia'].binary +blackboard.Signs['rhonchi'].binary) / 1000.)
blackboard.feedback['bag valve mask ventilation'] += self.posi * s
# bvm ventilation have been done
if blackboard.Inters['bag valve mask ventilation'].binary:
blackboard.feedback['endotracheal tube'] += self.posi * s * blackboard.Inters['bag valve mask ventilation'].score / 1000.
if not blackboard.Signs['hypertension'].binary:
blackboard.feedback['albuterol'] += self.posi * s * blackboard.Signs['hypertension'].score / 1000.
blackboard.feedback['ipratropium'] += self.posi * s * blackboard.Signs['hypertension'].score / 1000.
if blackboard.Signs['wheezing'].binary:
blackboard.feedback['dexamethasone'] += self.posi * s * blackboard.Signs['wheezing'].score / 1000.
if blackboard.Signs['hypoxemia'].binary and blackboard.Signs['tachypnea'].binary \
and blackboard.Vitals['bp'].binary and blackboard.Vitals['bp'].value.strip().split('/')[0].isdigit() and int(blackboard.Vitals['bp'].value.strip().split('/')[0]) > 90:
blackboard.feedback['cpap'] += self.posi * s * blackboard.Signs['tachypnea'].score / 1000.
blackboard.feedback['cardiac monitor'] += self.posi
blackboard.feedback['transport'] += self.posi
return py_trees.Status.SUCCESS
def update(self):
'''
mild pain: ps <= 5
moderate: ps 6,7
severe: ps >= 8
'''
blackboard = Blackboard()
self.posi = blackboard.protocol_flag[self.key][1]
if self.posi == 0:
return py_trees.Status.SUCCESS
if blackboard.Signs['hypoxemia'].binary:
blackboard.feedback['oxygen'] += self.posi * blackboard.Signs[
'hypoxemia'].score / 1000.
if blackboard.Signs['pain severity'].binary:
s = blackboard.Signs['pain severity'].score / 1000.
ps = int(blackboard.Signs['pain severity'].value)
if ps <= 5:
blackboard.feedback['nitronox'] += self.posi * s
blackboard.feedback['acetaminophen'] += self.posi * s
blackboard.feedback['ibuprofen'] += self.posi * s
elif ps == 6 or ps == 7:
blackboard.feedback['toradol'] += self.posi * s
else:
blackboard.feedback['normal saline'] += self.posi * s
blackboard.feedback['fentanyl'] += self.posi * s
# blackboard.feedback['morphine sulfate'] += self.posi * s
if blackboard.Signs['nausea'].binary:
blackboard.feedback['ondansetron'] += self.posi * blackboard.Signs[
'nausea'].score / 1000.
elif blackboard.Signs['vomiting'].binary:
blackboard.feedback['ondansetron'] += self.posi * blackboard.Signs[
'vomiting'].score / 1000.
blackboard.feedback['cardiac monitor'] += self.posi
blackboard.feedback['transport'] += self.posi
return py_trees.Status.SUCCESS
def test_key_exists():
console.banner("Key Exists")
for create in [create_blackboards, create_namespaced_blackboards]:
with create() as (foo, unused_bar, namespace):
print("foo.exists('dude') [{}][{}]".format(foo.exists("dude"), True))
assert(foo.exists("dude"))
if namespace:
print("Blackboard.exists('{}/dude') [{}][{}]".format(
namespace,
Blackboard.exists(name="{}/dude".format(namespace)),
True)
)
assert(Blackboard.exists(name="{}/dude".format(namespace)))
with nose.tools.assert_raises_regexp(AttributeError, "does not have read/write access"):
print("Expecting Attribute Error with substring 'does not have read/write access'")
print("foo.exists('dude_not_here') [{}][{}]".format(foo.exists("dude_not_here"), False))
assert(not foo.exists("dude_not_here"))
class ActorSource(AtomicBehavior):
"""
Implementation for a behavior that will indefinitely create actors
at a given transform if no other actor exists in a given radius
from the transform.
"""
def __init__(self, world, actor_type_list, transform, threshold, blackboard_queue_name,
actor_limit=7, name="ActorSource"):
"""
Setup class members
"""
super(ActorSource, self).__init__(name)
self._world = world
self._actor_types = actor_type_list
self._spawn_point = transform
self._threshold = threshold
self._queue = Blackboard().get(blackboard_queue_name)
self._actor_limit = actor_limit
self._last_blocking_actor = None
def update(self):
new_status = py_trees.common.Status.RUNNING
if self._actor_limit > 0:
world_actors = self._world.get_actors()
spawn_point_blocked = False
if (self._last_blocking_actor and
self._spawn_point.location.distance(self._last_blocking_actor.get_location()) < self._threshold):
spawn_point_blocked = True
if not spawn_point_blocked:
for actor in world_actors:
if self._spawn_point.location.distance(actor.get_location()) < self._threshold:
spawn_point_blocked = True
self._last_blocking_actor = actor
break
if not spawn_point_blocked:
try:
new_actor = CarlaActorPool.request_new_actor(np.random.choice(self._actor_types), self._spawn_point)
self._actor_limit -= 1
self._queue.put(new_actor)
except:
print("ActorSource unable to spawn actor")
return new_status
def test_key_filters():
console.banner("Key Accessors")
for create in [create_blackboards, create_namespaced_blackboards]:
with create() as (foo, bar, unused_namespace):
no_of_keys = len(Blackboard.keys())
print("{}".format(Blackboard.keys()))
print("# Registered keys: {} [{}]".format(no_of_keys, 5))
assert (no_of_keys == 5)
no_of_keys = len(Blackboard.keys_filtered_by_regex("dud"))
print("# Keys by regex 'dud': {} [{}]".format(no_of_keys, 2))
assert (no_of_keys == 2)
no_of_keys = len(
Blackboard.keys_filtered_by_clients({foo.unique_identifier}))
print("# Keys by id [foo.id] {} [{}]".format(no_of_keys, 4))
assert (no_of_keys == 4)
no_of_keys = len(
Blackboard.keys_filtered_by_clients({bar.unique_identifier}))
print("# Keys by id [bar.id] {} [{}]".format(no_of_keys, 4))
assert (no_of_keys == 4)
no_of_keys = len(
Blackboard.keys_filtered_by_clients(
{foo.unique_identifier, bar.unique_identifier}))
print("# Keys by id [foo.id, bar.id] {} [{}]".format(
no_of_keys, 5))
assert (no_of_keys == 5)
# show the convenience list -> set helper is ok
no_of_keys = len(
Blackboard.keys_filtered_by_clients([foo.unique_identifier]))
print("# Can pass in a list instead of a set: True")
assert (no_of_keys == 4)
def create_blackboard():
"""
Create a blackboard with a few variables.
Fill with as many different types as we need to get full coverage on
pretty printing blackboard tests.
"""
Blackboard.clear()
blackboard = Client(name="Tester")
for key in {"foo", "some_tuple", "nested", "nothing"}:
blackboard.register_key(key=key, access=py_trees.common.Access.READ)
for key in {"foo", "some_tuple", "nested", "nothing"}:
blackboard.register_key(key=key, access=py_trees.common.Access.WRITE)
blackboard.foo = "bar"
blackboard.some_tuple = (1, "bar")
blackboard.nested = Nested()
blackboard.nothing = None
return blackboard
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
class FindBin(py_trees.behaviour.Behaviour):
def __init__(self, name="FindBin"):
super(FindBin, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def setup(self, unused_timeout=15):
self.logger.debug("%s.setup()" % (self.__class__.__name__))
return True
def initialise(self):
self.logger.debug("%s.initialise()" % (self.__class__.__name__))
def update(self):
self.logger.debug("%s.update()" % (self.__class__.__name__))
robot = self.blackboard.get("robot")
robot.th += 0.5
new_status = py_trees.common.Status.RUNNING
return new_status
def terminate(self, new_status):
self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
def __init__(self, name="ApproachBin"):
super(ApproachBin, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def __init__(self, name="BinClose"):
super(BinClose, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def __init__(self, name="FindBall"):
super(FindBall, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
def __init__(self, name="BallFound"):
super(BallFound, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self.blackboard = Blackboard()
