def _initialize_behaviour_model(self):
"""
This function initialises the RHBP behaviour/goal model.
"""
# Random Move/Exploration
random_move = RandomMove(name="random_move",
agent_name=self._agent_name)
self.behaviours.append(random_move)
random_move.add_effect(
Effect(self.perception_provider.dispenser_visible_sensor.name,
indicator=True))
# Moving to a dispenser if in vision range
move_to_dispenser = MoveToDispenser(
name="move_to_dispense",
perception_provider=self.perception_provider,
agent_name=self._agent_name)
self.behaviours.append(move_to_dispenser)
move_to_dispenser.add_effect(
Effect(self.perception_provider.closest_dispenser_distance_sensor.
name,
indicator=-1,
sensor_type=float))
move_to_dispenser.add_precondition(
Condition(self.perception_provider.dispenser_visible_sensor,
BooleanActivator(desiredValue=True)))
move_to_dispenser.add_precondition(
Condition(
self.perception_provider.closest_dispenser_distance_sensor,
ThresholdActivator(isMinimum=True, thresholdValue=2)))
# Dispense a block if close enough
dispense = Dispense(name="dispense",
perception_provider=self.perception_provider,
agent_name=self._agent_name)
self.behaviours.append(dispense)
dispense.add_effect(
Effect(self.perception_provider.number_of_blocks_sensor.name,
indicator=+1,
sensor_type=float))
dispense.add_precondition(
Condition(
self.perception_provider.closest_dispenser_distance_sensor,
ThresholdActivator(isMinimum=False, thresholdValue=1)))
# Our simple goal is to create more and more blocks
dispense_goal = GoalBase(
"dispensing",
permanent=True,
conditions=[
Condition(self.perception_provider.number_of_blocks_sensor,
GreedyActivator())
],
planner_prefix=self._agent_name)
self.goals.append(dispense_goal)
def test_conditions_in_multiple_levels(self):
"""
Testing conditions that are used as well on the highest manager hierarchy level as well as in a sub manager of
a NetworkBehaviour. In particular one conditions is used as precondition, the other one as goal condition.
"""
method_prefix = self.__message_prefix + "/test_conditions_in_multiple_levels"
pre_con_sensor = Sensor(name="Precon_sensor", initial_value=False)
pre_con = Condition(pre_con_sensor, BooleanActivator(desiredValue=True))
topic_name = method_prefix + '/Topic'
sensor = TopicSensor(topic=topic_name, message_type=Int32, initial_value=0)
condition = Condition(sensor, ThresholdActivator(thresholdValue=3))
planner_prefix = method_prefix + "/Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
goal = OfflineGoal('CentralGoal', planner_prefix=planner_prefix)
goal.add_condition(condition)
m.add_goal(goal)
effect = Effect(sensor_name=sensor.name, indicator=1, sensor_type=int, activator_name=condition.activator.name)
first_level_network = NetworkBehaviour(name=method_prefix + '/FirstLevel', planner_prefix=planner_prefix,
createLogFiles=True)
first_level_network.add_effects([effect])
first_level_network.add_precondition(pre_con)
goal_with_same_cond = OfflineGoal('CentralGoal2', planner_prefix=planner_prefix)
goal_with_same_cond.add_condition(condition)
first_level_network.add_goal(goal_with_same_cond)
increaser_behavior = IncreaserBehavior(effect_name=sensor.name, topic_name=topic_name,
name=method_prefix + "TopicIncreaser",
planner_prefix=first_level_network.get_manager_prefix())
increaser_behavior.add_precondition(pre_con)
# activate the first_level_network increaser_Behavior
for x in range(0, 3, 1):
self.assertFalse(first_level_network._isExecuting)
m.step()
pre_con_sensor.update(True)
rospy.sleep(0.1)
self.assertTrue(first_level_network._isExecuting)
for x in range(0, 4, 1):
m.step()
rospy.sleep(0.1)
self.assertTrue(increaser_behavior._isExecuting)
def test_rl_basic(self):
method_prefix = self.__message_prefix + "test_rl_basic"
planner_prefix = method_prefix + "Manager"
register_in_factory(ActivationAlgorithmFactory)
m = Manager(activationThreshold=7,
prefix=planner_prefix,
activation_algorithm="reinforcement")
behaviour_a = BehaviourA(name=method_prefix + "A",
planner_prefix=planner_prefix)
behaviour_b = BehaviourB(name=method_prefix + "B",
planner_prefix=planner_prefix)
sensor = Sensor(name="bool_sensor", initial_value=False)
# same effect so planner cannot distinguish
behaviour_a.add_effect(Effect(sensor_name=sensor.name, indicator=1.0))
behaviour_b.add_effect(Effect(sensor_name=sensor.name, indicator=1.0))
goal = GoalBase(method_prefix + '_goal', planner_prefix=planner_prefix)
goal.add_condition(Condition(sensor, BooleanActivator()))
for x in range(0, 10, 1):
m.step()
rospy.sleep(0.1)
def test_activator(self):
method_prefix = self.__message_prefix + "TestOfflineGoal"
planner_prefix = method_prefix + "Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
topic_name = 'IncreaseTopicTest/Topic'
sensor = TopicSensor(topic=topic_name, name='IncreaseTopicTestSensor', message_type=Int32,
initial_value=0)
activator = GreedyActivator()
condition = Condition(sensor=sensor, activator=activator)
IncreaserBehavior(topic_name=topic_name, effect_name=sensor.name,
createLogFiles=True, planner_prefix=planner_prefix)
goal = OfflineGoal(name=self.__message_prefix + 'CentralGoal', planner_prefix=planner_prefix)
goal.add_condition(condition)
m.add_goal(goal)
number_of_steps = 15
for x in range(0, number_of_steps + 1 , 1):
m.step()
rospy.sleep(0.1)
# it takes 2 steps until the activation has increased
expected_behaviour_steps = number_of_steps - 2
self.assertEquals(expected_behaviour_steps, sensor.latestValue)
def test_guarantee_decision(self):
"""
Testing guarantee_decision parameter of manager.step()
"""
method_prefix = self.__message_prefix + "test_guarantee_decision"
planner_prefix = method_prefix + "Manager"
manager = Manager(activationThreshold=7.0, prefix=planner_prefix)
sensor_3 = Sensor(name="Sensor3", initial_value=False)
behaviour_1 = BehaviourBase(name="Behaviour1",
planner_prefix=planner_prefix)
goal1 = GoalBase(name="Test_Goal1",
conditions=[Condition(sensor_3, BooleanActivator())],
planner_prefix=planner_prefix)
manager.step()
self.assertFalse(
behaviour_1._isExecuting,
"Behaviour 1 is executed even though it should not have enough activation"
)
manager.step(guarantee_decision=True)
self.assertTrue(
behaviour_1._isExecuting,
"Behaviour 1 is not executed even though a decision was forced")
def test_offline_goal(self):
method_prefix = self.__message_prefix + "TestOfflineGoal"
planner_prefix = method_prefix + "/Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
topic_name = method_prefix + '/Topic'
sensor = TopicSensor(topic=topic_name,
message_type=Bool,
initial_value=False)
condition = Condition(sensor, BooleanActivator())
SetTrueBehavior(effect_name=sensor.name,
topic_name=topic_name,
name=method_prefix + "SetTrue",
planner_prefix=planner_prefix)
goal = OfflineGoal('CentralGoal', planner_prefix=planner_prefix)
goal.add_condition(condition)
m.add_goal(goal)
for x in range(0, 3, 1):
m.step()
rospy.sleep(0.1)
goal.fetchStatus(3)
self.assertTrue(goal.satisfied, 'Goal is not satisfied')
goal.unregister()
def test_independent_behaviour(self):
"""
Test behaviour property independentFromPlanner
"""
method_prefix = self.__message_prefix + "test_independent_behaviour"
planner_prefix = method_prefix + "Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
topic_name_1 = method_prefix + '/sensor_1'
sensor = TopicSensor(topic=topic_name_1,
message_type=Bool,
initial_value=False)
condition = Condition(sensor, BooleanActivator())
#independentFromPlanner and effects
independent_behaviour = SetTrueBehavior(effect_name=sensor.name,
topic_name=topic_name_1,
name=method_prefix + "SetTrue",
planner_prefix=planner_prefix,
independentFromPlanner=True)
#independentFromPlanner and no effects
independent_behaviour2 = SetTrueBehavior(effect_name=None,
topic_name=topic_name_1,
name=method_prefix +
"SetTrue2",
planner_prefix=planner_prefix,
independentFromPlanner=True)
# not independentFromPlanner and no effects
independent_behaviour3 = SetTrueBehavior(effect_name=None,
topic_name=topic_name_1,
name=method_prefix +
"SetTrue3",
planner_prefix=planner_prefix,
independentFromPlanner=False)
goal = GoalBase(method_prefix + 'CentralGoal',
planner_prefix=planner_prefix)
goal.add_condition(condition)
for x in range(0, 3, 1):
m.step()
rospy.sleep(0.1)
goal_proxy = m.goals[0]
goal_proxy.fetchStatus(3)
self.assertTrue(goal_proxy.satisfied, 'Goal is not satisfied')
self.assertTrue(independent_behaviour._isExecuting,
"independent_behaviour is not executed")
self.assertFalse(independent_behaviour2.was_executed,
"independent_behaviour2 was executed")
self.assertFalse(independent_behaviour3.was_executed,
"independent_behaviour2 was executed")
def setUp(self):
rospy.init_node("TestNode")
sensor = TopicSensor(name="test_sensor",
topic="sensor_topic",
message_type=Bool,
initial_value=False)
self.conditions = [Condition(sensor, BooleanActivator())]
self.manager_name = "Test_manager"
self.manager = Manager(prefix=self.manager_name)
self.goal_name = "test_goal"
self.satisfaction_threshold = 1.0
def setUp(self):
rospy.init_node("TestNode")
self.manager = Manager(prefix="test_manager")
self.uut = TestDelegationBehaviour(name="test_behaviour",
planner_prefix="test_manager")
self.sensor = TopicSensor(name="test_sensor",
topic="/sensor_topic",
message_type=Bool,
initial_value=False)
self.test_condition = Condition(self.sensor, BooleanActivator())
self.effect = Effect(sensor_name="test_sensor", indicator=1.0)
self.client = self.uut.delegation_client
def setUpClass(cls):
print("Needs a running roscore")
super(GoalWrapperTest, cls).setUpClass()
rospy.init_node("test_node")
cls.manager = Manager(prefix="Test_manager")
cls.sensor = TopicSensor(name="test_sensor",
topic="/sensor_topic",
message_type=Bool,
initial_value=False)
cls.test_condition = Condition(cls.sensor, BooleanActivator())
cls.comparison_goal = OfflineGoal(name="comparison_goal",
planner_prefix="Test_manager")
cls.manager_name = "Test_manager"
def create_condition(self):
"""
Creating a new condition object based on the positive goal enabled state --> full activation on enabled goal
:returns new condition object
"""
if not self.__condition:
condition_name = self._name + "_condition"
sensor_name = self._name + "_sensor"
sensor = TopicSensor(name=sensor_name, topic=self.__topic_name, message_type=Bool,
initial_value=self._enabled)
activator = BooleanActivator(desiredValue=True)
self.__condition = Condition(name=condition_name, sensor=sensor, activator=activator)
return self.__condition
def test_register_unregister(self):
method_prefix = self.__message_prefix + "TestRegisterUnregisterGoal"
planner_prefix = method_prefix + "Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
topic_name = method_prefix + '/Topic'
sensor = TopicSensor(topic=topic_name,
message_type=Bool,
initial_value=False)
condition = Condition(sensor, BooleanActivator())
goal = GoalBase(method_prefix + 'Goal', planner_prefix=planner_prefix)
goal.add_condition(condition)
rospy.sleep(0.1)
m.step()
self.assertEquals(len(m.goals), 1, 'goal not registered properly')
goal.unregister()
rospy.sleep(0.1)
m.step()
self.assertEquals(len(m.goals), 0, 'goal not unregistered properly')
# try to register again
goal.register()
rospy.sleep(0.1)
m.step()
self.assertEquals(len(m.goals), 1, 'goal not registered properly')
goal.unregister(terminate_services=False)
rospy.sleep(0.1)
m.step()
self.assertEquals(len(m.goals), 0, 'goal not unregistered properly')
# try to register again
goal.register()
rospy.sleep(0.1)
m.step()
self.assertEquals(len(m.goals), 1, 'goal not registered properly')
def setUp(self):
rospy.init_node("TestNode")
sensor = TopicSensor(name="test_sensor",
topic="sensor_topic",
message_type=Bool,
initial_value=False)
self.conditions = [Condition(sensor, BooleanActivator())]
self.manager_name = "Test_manager"
self.manager = Manager(prefix=self.manager_name)
self.goal_name = "test_goal"
self.satisfaction_threshold = 1.0
self.mockedDM = MockedDelegationCommunicator(
manager_name=self.manager_name, name=self.manager_name)
self.goal_repr = " ".join(
[x.getPreconditionPDDL(1.0).statement for x in self.conditions])
def test_publisher_goal(self):
method_prefix = self.__message_prefix + "TestPublisherGoal"
planner_prefix = method_prefix + "Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
topic_name = method_prefix + '/Topic'
sensor = TopicSensor(topic=topic_name,
message_type=Bool,
initial_value=False)
condition = Condition(sensor, BooleanActivator())
SetTrueBehavior(effect_name=sensor.name,
topic_name=topic_name,
name=method_prefix + "SetTrue",
planner_prefix=planner_prefix)
goal = PublisherGoal(method_prefix + 'PublisherGoal',
planner_prefix=planner_prefix)
goal.add_condition(condition)
goal_activated_condition = goal.create_condition()
m.step()
rospy.sleep(0.1)
sensor.update(True)
m.step()
rospy.sleep(0.1)
goal_proxy = m.goals[0]
goal_proxy.fetchStatus(2)
self.assertFalse(goal_proxy.enabled, 'Goal still enabled')
# manually updating this conditions because it is not registered somewhere
goal_activated_condition.sync()
goal_activated_condition.updateComputation()
# Satisfaction should become 0.0 if the goal is deactivated, this also tests the publishing part indirectly
self.assertEquals(goal_activated_condition.satisfaction, 0.0,
'goal_condition not properly updated')
def test_interruptable_behaviour(self):
"""
Test behaviour interruptable property
"""
method_prefix = self.__message_prefix + "test_interruptable_behaviour"
planner_prefix = method_prefix + "Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
topic_name_1 = method_prefix + '/sensor_1'
non_interruptable_sensor = TopicSensor(topic=topic_name_1,
message_type=Int32,
initial_value=False)
non_interruptable_condition = Condition(non_interruptable_sensor,
GreedyActivator())
non_interruptable_behaviour = IncreaserBehavior(
effect_name=non_interruptable_sensor.name,
topic_name=topic_name_1,
name=method_prefix + "TopicIncreaser",
planner_prefix=planner_prefix,
interruptable=False)
topic_name_2 = method_prefix + '/sensor_2'
interruptable_sensor = TopicSensor(topic=topic_name_2,
message_type=Int32,
initial_value=False)
interruptable_condition = Condition(interruptable_sensor,
GreedyActivator())
interruptable_behaviour = IncreaserBehavior(
effect_name=interruptable_sensor.name,
topic_name=topic_name_2,
name=method_prefix + "TopicIncreaser2",
planner_prefix=planner_prefix,
interruptable=True)
enable_sensor = Sensor(name='enable_sensor', initial_value=True)
enable_cond = Condition(enable_sensor, BooleanActivator())
non_interruptable_behaviour.add_precondition(enable_cond)
goal = GoalBase(method_prefix + 'CentralGoal',
planner_prefix=planner_prefix,
permanent=True)
goal.add_condition(non_interruptable_condition)
goal.add_condition(interruptable_condition)
# first normal operation: every behaviour runs as expected
for x in range(0, 4, 1):
m.step()
rospy.sleep(0.1)
self.assertTrue(non_interruptable_behaviour._isExecuting,
"Non-Interruptable Behaviour is not executed")
self.assertTrue(interruptable_behaviour._isExecuting,
"Interruptable Behaviour is not executed")
#disable non_interruptable_behaviour precondition and check if it is affected
enable_sensor.update(False)
for x in range(0, 1, 1):
m.step()
rospy.sleep(0.1)
self.assertTrue(non_interruptable_behaviour._isExecuting,
"Non-Interruptable Behaviour is not executed")
self.assertTrue(interruptable_behaviour._isExecuting,
"Interruptable Behaviour is not executed")
#disable precondition of interruptable behaviour and check if it is disabled as well
interruptable_behaviour.add_precondition(enable_cond)
for x in range(0, 1, 1):
m.step()
rospy.sleep(0.1)
self.assertTrue(non_interruptable_behaviour._isExecuting,
"Non-Interruptable Behaviour is not executed")
self.assertFalse(interruptable_behaviour._isExecuting,
"Interruptable Behaviour is executed")
def test_plan_monitoring_float_sensors(self):
"""
Testing plan monitoring and replanning management
"""
method_prefix = self.__message_prefix + "test_plan_monitoring_float_sensors"
planner_prefix = method_prefix + "Manager"
manager = Manager(activationThreshold=7.0,
prefix=planner_prefix,
max_parallel_behaviours=1)
sensor_1 = Sensor(name="Sensor1", initial_value=0)
behaviour_1 = BehaviourBase(name="Behaviour1",
planner_prefix=planner_prefix)
behaviour_1.add_effect(Effect(sensor_1.name, 1, sensor_type=float))
sensor_2 = Sensor(name="Sensor2", initial_value=0)
behaviour_2 = BehaviourBase(name="Behaviour2",
planner_prefix=planner_prefix)
behaviour_2.add_effect(Effect(sensor_2.name, 1, sensor_type=float))
behaviour_3 = BehaviourBase(name="Behaviour3",
planner_prefix=planner_prefix)
# adding precondition to get a reference to the sensor in the manager
behaviour_3.add_precondition(Condition(sensor_2, BooleanActivator()))
behaviour_3.add_effect(Effect(sensor_2.name, 1, sensor_type=float))
goal1 = GoalBase(name="Test_Goal1",
conditions=[
Conjunction(
Condition(sensor_1, ThresholdActivator(1)),
Condition(
sensor_2,
ThresholdActivator(0),
))
],
planner_prefix=planner_prefix,
permanent=True)
manager.step(guarantee_decision=True)
self.assertTrue(
behaviour_1._isExecuting,
"Behaviour 1 is not executed even though a decision was forced and it would fulfill the plan"
)
sensor_1.update(
0.5
) # faking partial effect of behaviour1, all changes are induced by behaviour, no replanning
manager.step(guarantee_decision=True)
self.assertTrue(
behaviour_1._isExecuting,
"Behaviour 1 is not executed even though a decision was forced and it would fulfill the plan"
)
self.assertTrue(
manager._are_all_sensor_changes_from_executed_behaviours())
self.assertFalse(manager._are_effects_of_planned_behaviour_realised()
) # we did not yet reach the full effect
self.assertTrue(manager._executed_behaviours_influenced_as_expected())
sensor_1.update(
1.0
) # faking partial effect of behaviour1, all changes are induced by behaviour, no replanning
manager.step(guarantee_decision=True)
self.assertTrue(
behaviour_1._isExecuting,
"Behaviour 1 is not executed even though a decision was forced and it would fulfill the plan"
)
self.assertTrue(
manager._are_all_sensor_changes_from_executed_behaviours())
self.assertTrue(manager._executed_behaviours_influenced_as_expected())
manager._planExecutionIndex -= 1 # we have to manipulate here because it was incremented
self.assertTrue(manager._are_effects_of_planned_behaviour_realised())
manager._planExecutionIndex += 1
def test_inverted_activation(self):
planner_prefix = "condition_elements_test"
m = Manager(activationThreshold=7,
prefix=planner_prefix,
createLogFiles=True)
satisfaction_threshold = 0.8
sensor1 = Sensor()
activator_increasing = LinearActivator(zeroActivationValue=0,
fullActivationValue=1)
activator_decreasing = LinearActivator(zeroActivationValue=1,
fullActivationValue=0)
condition_increasing = Condition(sensor1, activator_increasing)
condition_decreasing = Condition(sensor1, activator_decreasing)
sensor1.update(newValue=0.8)
condition_increasing.sync()
condition_increasing.updateComputation()
condition_decreasing.sync()
condition_decreasing.updateComputation()
wish_increasing = condition_increasing.getWishes()[0]
wish_decreasing = condition_decreasing.getWishes()[0]
self.assertAlmostEqual(0.2, wish_increasing.indicator, delta=0.001)
self.assertAlmostEqual(-0.8, wish_decreasing.indicator, delta=0.001)
increasing_precondition_precon_pddl = condition_increasing.getPreconditionPDDL(
satisfaction_threshold=satisfaction_threshold)
decreasing_precondition_precon_pddl = condition_decreasing.getPreconditionPDDL(
satisfaction_threshold=satisfaction_threshold)
increasing_precondition_state_pddl = condition_increasing.getStatePDDL(
)[0]
decreasing_precondition_state_pddl = condition_decreasing.getStatePDDL(
)[0]
sensor2 = Sensor()
sensor2.update(newValue=0.4)
behaviour1 = BehaviourBase("behaviour_1",
planner_prefix=planner_prefix)
behaviour1.add_effect(
Effect(sensor_name=sensor1.name, indicator=-0.1,
sensor_type=float))
behaviour1.add_precondition(condition_increasing)
behaviour2 = BehaviourBase("behaviour_2",
planner_prefix=planner_prefix)
behaviour2.add_effect(
Effect(sensor_name=sensor1.name, indicator=0.1, sensor_type=float))
behaviour2.add_precondition(condition_decreasing)
m.step()
m.step()
b1_state_pddl = behaviour1.getStatePDDL()
b2_state_pddl = behaviour2.getStatePDDL()
b1_action_pddl = behaviour1.getActionPDDL()
b2_action_pddl = behaviour2.getActionPDDL()
for x in range(0, 3, 1):
m.step()
rospy.sleep(0.1)
def _initialize_behaviour_model(self):
"""
This function initialises the RHBP behaviour/goal model.
"""
# RANDOM MOVE
random_move = RandomMove(name="random_move",
agent_name=self._agent_name)
self.behaviours.append(random_move)
random_move.add_effect(
Effect(self.sensor_manager.dispenser_distance_sensor.name,
indicator=True))
random_move.add_precondition(
Condition(self.sensor_manager.dispenser_in_map_sensor,
BooleanActivator(desiredValue=False)))
# Exploration
# explore_move = ExploreMove(name="explore_move", agent_name=self._agent_name, agent_map=self.map)
# self.behaviours.append(explore_move)
# explore_move.add_effect(Effect(self.perception_provider.dispenser_visible_sensor.name, indicator=True))
# MOVE TO DISPENSER
move_to_dispenser = MoveToDispenser(
name="move_to_dispenser",
perception_provider=self.perception_provider,
agent_name=self._agent_name,
agent_map=self.map)
self.behaviours.append(move_to_dispenser)
move_to_dispenser.add_effect(
Effect(self.sensor_manager.next_to_dispenser_sensor.name,
indicator=True))
# move_to_dispenser.add_effect(
# Effect(self.sensor_manager.pseudo_sensor.name, indicator=1, sensor_type=float))
move_to_dispenser.add_precondition(
Condition(self.sensor_manager.dispenser_in_map_sensor,
BooleanActivator(desiredValue=True)))
move_to_dispenser.add_precondition(
Condition(self.sensor_manager.next_to_dispenser_sensor,
BooleanActivator(desiredValue=False)))
# # Our simple goal is to move to the dispenser
# move_to_dispenser_goal = GoalBase("move_to_dispenser_goal", permanent=True,
# conditions=[Condition(self.sensor_manager.pseudo_sensor, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(move_to_dispenser_goal)
# Request a block if close enough
request = Request(name="request",
perception_provider=self.perception_provider,
agent_name=self._agent_name,
agent_map=self.map)
self.behaviours.append(request)
# request.add_effect(
# Effect(self.sensor_manager.pseudo_sensor.name, indicator=+1, sensor_type=float))
request.add_effect(
Effect(self.sensor_manager.next_to_block_sensor.name,
indicator=True))
request.add_precondition(
Condition(self.sensor_manager.next_to_dispenser_sensor,
BooleanActivator(desiredValue=True)))
# # Our simple goal is to request a block
# request_goal = GoalBase("request_goal", permanent=True,
# conditions=[Condition(self.sensor_manager.pseudo_sensor, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(request_goal)
# # Move to nearby block
# move_to_block = MoveToBlock(name="move_to_block", perception_provider=self.perception_provider,
# agent_name=self._agent_name, agent_map=self.map)
# self.behaviours.append(move_to_block)
# move_to_block.add_effect(
# Effect(self.sensor_manager.block_distance_sensor.name, indicator=-1, sensor_type=float))
# # move_to_block.add_effect(
# # Effect(self.sensor_manager.pseudo_sensor.name, indicator=1, sensor_type=float))
# move_to_block.add_precondition(
# Condition(self.sensor_manager.block_in_map_sensor, BooleanActivator(desiredValue=True)))
# move_to_block.add_precondition(Condition(self.sensor_manager.block_distance_sensor,
# ThresholdActivator(isMinimum=True, thresholdValue=2)))
# # Our simple goal is to move to a block
# move_to_block_goal = GoalBase("move_to_block_goal", permanent=True,
# conditions=[Condition(self.sensor_manager.pseudo_sensor, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(move_to_block_goal)
# ATTACH
attach = Attach(name="attach",
perception_provider=self.perception_provider,
agent_name=self._agent_name,
agent_map=self.map)
self.behaviours.append(attach)
# attach.add_effect(Effect(self.sensor_manager.pseudo_sensor.name, indicator=+1, sensor_type=float))
attach.add_effect(
Effect(self.sensor_manager.block_attached_sensor.name,
indicator=True))
attach.add_precondition(
Condition(self.sensor_manager.next_to_block_sensor,
BooleanActivator(desiredValue=True)))
# # Our simple goal is to attach to a block
# attach_goal = GoalBase("attach_goal", permanent=True,
# conditions=[Condition(self.sensor_manager.pseudo_sensor, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(attach_goal)
# MOVE TO GOAL
move_to_goal = MoveToGoal(name="move_to_goal",
perception_provider=self.perception_provider,
agent_name=self._agent_name,
agent_map=self.map)
self.behaviours.append(move_to_goal)
# move_to_goal.add_effect(Effect(self.sensor_manager.pseudo_sensor.name, indicator=+1, sensor_type=float))
move_to_goal.add_effect(
Effect(self.sensor_manager.on_goal_sensor.name, indicator=True))
move_to_goal.add_precondition(
Condition(self.sensor_manager.block_attached_sensor,
BooleanActivator(desiredValue=True)))
# # Our simple goal is to attach to a block
# move_to_goal_goal = GoalBase("move_to_goal_goal", permanent=True,
# conditions=[Condition(self.sensor_manager.pseudo_sensor, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(move_to_goal_goal)
# PREPARE SUBMISSION
prepare_submission = PrepareSubmission(
name="prepare_submission",
perception_provider=self.perception_provider,
agent_name=self._agent_name)
self.behaviours.append(prepare_submission)
prepare_submission.add_effect(
Effect(self.sensor_manager.correct_pattern_position_sensor.name,
indicator=True))
# prepare_submission.add_effect(Effect(self.sensor_manager.pseudo_sensor.name, indicator=+1, sensor_type=float))
prepare_submission.add_precondition(
Condition(self.sensor_manager.on_goal_sensor,
BooleanActivator(desiredValue=True)))
prepare_submission.add_precondition(
Condition(self.sensor_manager.block_attached_sensor,
BooleanActivator(desiredValue=True)))
# # Our simple goal is to rotate the pattern into the correct position
# prepare_submission_goal = GoalBase("prepare_submission_goal", permanent=True,
# conditions=[Condition(self.sensor_manager.pseudo_sensor, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(prepare_submission_goal)
# SUBMIT
submit = Submit(name="submit",
perception_provider=self.perception_provider,
agent_name=self._agent_name,
agent=self)
self.behaviours.append(submit)
submit.add_effect(
Effect(self.sensor_manager.pseudo_sensor.name,
indicator=+1,
sensor_type=float))
submit.add_precondition(
Condition(self.sensor_manager.on_goal_sensor,
BooleanActivator(desiredValue=True)))
submit.add_precondition(
Condition(self.sensor_manager.correct_pattern_position_sensor,
BooleanActivator(desiredValue=True)))
# Our simple goal is to submit a task
submit_goal = GoalBase("submit_goal",
permanent=True,
conditions=[
Condition(self.sensor_manager.pseudo_sensor,
GreedyActivator())
],
planner_prefix=self._agent_name)
self.goals.append(submit_goal)
def test_plan_with_registered_goals(self):
method_prefix = self.__message_prefix + "test_handle_interfering_correlations"
planner_prefix = method_prefix + "Manager"
manager = Manager(activationThreshold=7.0, prefix=planner_prefix)
sensor_1 = Sensor(name="Sensor1", initial_value=False)
sensor_2 = Sensor(name="Sensor2", initial_value=False)
sensor_3 = Sensor(name="Sensor3", initial_value=False)
behaviour_1 = BehaviourBase(name="Behaviour1",
planner_prefix=planner_prefix)
behaviour_1.add_effect(Effect(sensor_name=sensor_1.name, indicator=1))
behaviour_2 = BehaviourBase(name="Behaviour2",
planner_prefix=planner_prefix)
behaviour_2.add_effect(Effect(sensor_name=sensor_2.name, indicator=1))
behaviour_2.add_precondition(Condition(sensor_1, BooleanActivator()))
behaviour_3 = BehaviourBase(name="Behaviour3",
planner_prefix=planner_prefix)
behaviour_3.add_effect(Effect(sensor_name=sensor_3.name, indicator=1))
behaviour_3.add_precondition(Condition(sensor_2, BooleanActivator()))
goal1 = GoalBase(name="Test_Goal1",
conditions=[Condition(sensor_3, BooleanActivator())],
planner_prefix=planner_prefix)
goal2 = GoalBase(name="Test_Goal2",
conditions=[Condition(sensor_2, BooleanActivator())],
planner_prefix=planner_prefix)
# test plannning without prior decision step
plan_seq = manager.plan_with_registered_goals([manager.goals[0]])
expected_plan_seq = ["Behaviour1", "Behaviour2", "Behaviour3"]
self.assertEquals(len(plan_seq), len(expected_plan_seq))
self.assertListEqual(plan_seq, expected_plan_seq)
# plan again using a service
service_name = planner_prefix + '/' + 'PlanWithGoal'
# do not wait forever here, manager might be already closed
rospy.wait_for_service(service_name, timeout=1)
plan_service = rospy.ServiceProxy(service_name, PlanWithGoal)
# use all registered goals
res = plan_service()
self.assertEquals(len(res.plan_sequence), len(expected_plan_seq))
self.assertListEqual(res.plan_sequence, expected_plan_seq)
# use other named goal
expected_plan_seq = ["Behaviour1", "Behaviour2"]
res = plan_service(goal_names=[goal2.name])
self.assertEquals(len(res.plan_sequence), len(expected_plan_seq))
self.assertListEqual(res.plan_sequence, expected_plan_seq)
# test infeasible plan
sensor_4 = Sensor(name="Sensor4", initial_value=False)
goal3 = GoalBase(name="Test_Goal3",
conditions=[Condition(sensor_4, BooleanActivator())],
planner_prefix=planner_prefix)
expected_plan_seq = []
# force an update to make the new goal available
res = plan_service(goal_names=[goal3.name], force_state_update=True)
self.assertEquals(len(res.plan_sequence), len(expected_plan_seq))
self.assertListEqual(res.plan_sequence, expected_plan_seq)
def test_handle_interfering_correlations(self):
"""
Test manager interference resolving capabilities
"""
method_prefix = self.__message_prefix + "test_handle_interfering_correlations"
planner_prefix = method_prefix + "Manager"
m = Manager(activationThreshold=7.0, prefix=planner_prefix)
topic_name_1 = method_prefix + '/sensor_1'
sensor1 = TopicSensor(topic=topic_name_1,
message_type=Int32,
initial_value=False)
condition1 = Condition(sensor1, GreedyActivator())
behaviour1 = IncreaserBehavior(effect_name=sensor1.name,
topic_name=topic_name_1,
name=method_prefix + "TopicIncreaser",
planner_prefix=planner_prefix,
interruptable=True)
topic_name_2 = method_prefix + '/sensor_2'
sensor2 = TopicSensor(topic=topic_name_2,
message_type=Int32,
initial_value=False)
condition2 = Condition(sensor2, GreedyActivator())
behaviour2 = IncreaserBehavior(effect_name=sensor2.name,
topic_name=topic_name_2,
name=method_prefix + "TopicIncreaser2",
planner_prefix=planner_prefix,
interruptable=True)
# add a conflict here "-1"
behaviour1.add_effect(
Effect(sensor_name=sensor2.name, indicator=-1, sensor_type=int))
goal = GoalBase(method_prefix + 'CentralGoal',
planner_prefix=planner_prefix,
permanent=True)
goal.add_condition(condition1)
goal.add_condition(condition2)
# first one of the behaviours can not be executed due to a conflict
for x in range(0, 4, 1):
m.step()
rospy.sleep(0.1)
# behaviour 2 should be executed because it does not conflict
self.assertFalse(behaviour1._isExecuting,
"Behaviour 1 is executed in spite of a conflict")
self.assertTrue(behaviour2._isExecuting, "Behaviour 2 is not executed")
behaviour1.priority = 1 # increase priority
for x in range(0, 1, 1):
m.step()
rospy.sleep(0.1)
# now behaviour 1 should be executed
self.assertTrue(
behaviour1._isExecuting,
"Behaviour 1 is not executed in spite of higher priority")
self.assertFalse(behaviour2._isExecuting,
"Behaviour 2 is executed in spite of lower priority")
behaviour1.priority = 0 # reset priority
# Manipulate activation of behaviour 2 with an extra goal, wish is positively influencing
goal2 = GoalBase(method_prefix + 'ExtraGoal',
planner_prefix=planner_prefix,
permanent=True)
goal2.add_condition(
condition2) # this condition is targeted by behaviour 2
rospy.sleep(0.1)
for x in range(
0, 5, 1
): # it takes some time with configured decay factor to integrate the changed activation
m.step()
rospy.sleep(0.1)
self.assertFalse(behaviour1._isExecuting, "Behaviour 1 is executed")
self.assertTrue(behaviour2._isExecuting, "Behaviour 2 is not executed")
def _initialize_behaviour_model(self):
"""
This function initialises the RHBP behaviour/goal model.
"""
### Exploration ##
exploration_move = ExplorationBehaviour(name="exploration_move", agent_name=self._agent_name, rhbp_agent=self)
self.behaviours.append(exploration_move)
# assigned to a task
exploration_move.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=True)))
exploration_move.add_effect(Effect(self.perception_provider.dispenser_visible_sensor.name, indicator=True))
exploration_move.add_effect(Effect(self.sensor_manager.assigned_task_list_empty.name, indicator=True))
### Move to Dispenser ###
move_to_dispenser = MoveToDispenserBehaviour(name="move_to_dispenser", agent_name=self._agent_name,
rhbp_agent=self)
self.behaviours.append(move_to_dispenser)
# assigned to a task precondition
move_to_dispenser.add_precondition(
Condition(self.sensor_manager.assigned_task_list_empty,
BooleanActivator(desiredValue=False))
)
# block not attached precondition
move_to_dispenser.add_precondition(
Condition(self.sensor_manager.attached_to_block,
BooleanActivator(desiredValue=False))
)
# not at the dispenser precondition
move_to_dispenser.add_precondition(
Condition(self.sensor_manager.at_the_dispenser,
BooleanActivator(desiredValue=False))
)
move_to_dispenser.add_effect(Effect(self.sensor_manager.at_the_dispenser.name, indicator=True))
# # Our simple goal is to create more and more blocks
# dispense_goal = GoalBase("dispensing", permanent=True,
# conditions=[
# Condition(self.sensor_manager.at_the_dispenser, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(dispense_goal)
### Requeste block - Dispense ###
dispense = DispenseBehaviour(name="dispense", agent_name=self._agent_name,
rhbp_agent=self)
self.behaviours.append(dispense)
# assigned to a task precondition
dispense.add_precondition(
Condition(self.sensor_manager.assigned_task_list_empty,
BooleanActivator(desiredValue=False))
)
# block not attached precondition
dispense.add_precondition(
Condition(self.sensor_manager.attached_to_block,
BooleanActivator(desiredValue=False))
)
# not at the dispenser precondition
dispense.add_precondition(
Condition(self.sensor_manager.at_the_dispenser,
BooleanActivator(desiredValue=True))
)
# not next to block
dispense.add_precondition(Condition(self.sensor_manager.next_to_block, BooleanActivator(desiredValue=False)))
# effect of dispense is that agent is next to block
dispense.add_effect(Effect(self.sensor_manager.next_to_block.name, indicator=True))
# Our simple goal is to create more and more blocks
# dispense_goal = GoalBase("dispensing", permanent=True,
# conditions=[
# Condition(self.sensor_manager.attached_to_block, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(dispense_goal)
#### Attach to Block ###
attach = AttachBehaviour(name="attach", agent_name=self._agent_name, rhbp_agent=self)
self.behaviours.append(attach)
# Preconditions
# assigned to a task
attach.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=False)))
# is not yet attached to a block of type of the current task
attach.add_precondition(Condition(sensor=self.sensor_manager.attached_to_block, activator=BooleanActivator(desiredValue=False)))
# has already dispensed the block, temporary solution for lack of communication
attach.add_precondition(
Condition(sensor=self.sensor_manager.is_dispensed, activator=BooleanActivator(desiredValue=True)))
# is next to a block
attach.add_precondition(
Condition(sensor=self.sensor_manager.next_to_block, activator=BooleanActivator(desiredValue=True)))
# has free capacity to attach
attach.add_precondition(Condition(sensor=self.sensor_manager.fully_attached, activator=BooleanActivator(desiredValue=False)))
# effect of attach is that agent is attached to a block
attach.add_effect(Effect(self.sensor_manager.attached_to_block.name, indicator=True))
# attach_goal = GoalBase("attaching", permanent=True,
# conditions=[
# Condition(self.sensor_manager.attached_to_block, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(attach_goal)
#### Reach meeting point ###
reach_meeting_point = ReachMeetingPointBehaviour(name="reach_meeting_point", agent_name=self._agent_name, rhbp_agent=self)
self.behaviours.append(reach_meeting_point)
# assigned to a task
reach_meeting_point.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=False)))
# have the block attached
reach_meeting_point.add_precondition(Condition(sensor=self.sensor_manager.attached_to_block,
activator=BooleanActivator(desiredValue=True)))
# the shape is not complete
reach_meeting_point.add_precondition(Condition(sensor=self.sensor_manager.shape_complete,
activator=BooleanActivator(desiredValue=False)))
# has not reached the meeting point already
reach_meeting_point.add_precondition(Condition(sensor=self.sensor_manager.at_meeting_point,
activator=BooleanActivator(desiredValue=False)))
# effect is moving till the agent reaches the meeting point
reach_meeting_point.add_effect(Effect(self.sensor_manager.at_meeting_point.name, indicator=True))
# at_meeting_point_goal = GoalBase("reach_meeting_point_goal", permanent=True,
# conditions=[
# Condition(self.sensor_manager.at_meeting_point, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(at_meeting_point_goal)
#### Connect ###
connect = ConnectBehaviour(name="connect", agent_name=self._agent_name,
rhbp_agent=self)
self.behaviours.append(connect)
# assigned to a task
connect.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=False)))
# have the block attached
connect.add_precondition(Condition(sensor=self.sensor_manager.attached_to_block,
activator=BooleanActivator(desiredValue=True)))
# connection not completed
connect.add_precondition(Condition(sensor=self.sensor_manager.connect_successful,
activator=BooleanActivator(desiredValue=False)))
# has reached the meeting point
connect.add_precondition(Condition(sensor=self.sensor_manager.at_meeting_point,
activator=BooleanActivator(desiredValue=True)))
# effect is creating the shape
connect.add_effect(Effect(self.sensor_manager.connect_successful.name, indicator=True))
connect.add_effect(Effect(self.sensor_manager.shape_complete.name, indicator=True))
# connect_successful_goal = GoalBase("connect_successful_goal", permanent=True,
# conditions=[
# Condition(self.sensor_manager.connect_successful, GreedyActivator())],
# planner_prefix=self._agent_name)
# self.goals.append(connect_successful_goal)
#### Detach to Block ###
detach = DetachBehaviour(name="detach", agent_name=self._agent_name, rhbp_agent=self)
self.behaviours.append(detach)
# Preconditions
# assigned to a task
detach.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=False)))
# connect action was succesful
detach.add_precondition(
Condition(sensor=self.sensor_manager.connect_successful, activator=BooleanActivator(desiredValue=True)))
# is NOT the agent assigned to submit
detach.add_precondition(
Condition(sensor=self.sensor_manager.can_submit, activator=BooleanActivator(desiredValue=False)))
detach.add_effect(Effect(self.sensor_manager.points.name, indicator=True))
#### Go to goal area ###
go_to_goal_area = ReachGoalAreaBehaviour(name="go_to_goal_area", agent_name=self._agent_name, rhbp_agent=self)
self.behaviours.append(go_to_goal_area)
# Preconditions
# assigned to a task
go_to_goal_area.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=False)))
# connect action was succesful
go_to_goal_area.add_precondition(
Condition(sensor=self.sensor_manager.shape_complete, activator=BooleanActivator(desiredValue=True)))
# is the agent assigned to submit
go_to_goal_area.add_precondition(
Condition(sensor=self.sensor_manager.can_submit, activator=BooleanActivator(desiredValue=True)))
# is the agent assigned to submit
go_to_goal_area.add_precondition(
Condition(sensor=self.sensor_manager.at_goal_area, activator=BooleanActivator(desiredValue=False)))
# effect is that agent is at goal area
go_to_goal_area.add_effect(Effect(self.sensor_manager.at_goal_area.name, indicator=True))
#### Submit ###
submit = SubmitBehaviour(name="submit", agent_name=self._agent_name, rhbp_agent=self)
self.behaviours.append(submit)
# Preconditions
# assigned to a task
submit.add_precondition(Condition(sensor=self.sensor_manager.assigned_task_list_empty,
activator=BooleanActivator(desiredValue=False)))
# have the block attached
connect.add_precondition(Condition(sensor=self.sensor_manager.attached_to_block,
activator=BooleanActivator(desiredValue=True)))
# connect action was succesful
submit.add_precondition(
Condition(sensor=self.sensor_manager.shape_complete, activator=BooleanActivator(desiredValue=True)))
# is the agent assigned to submit
submit.add_precondition(
Condition(sensor=self.sensor_manager.can_submit, activator=BooleanActivator(desiredValue=True)))
# is the agent at the goal area
submit.add_precondition(
Condition(sensor=self.sensor_manager.at_goal_area, activator=BooleanActivator(desiredValue=True)))
# effect of is that points are earned
submit.add_effect(Effect(self.sensor_manager.points.name, indicator=True))
# our unique goal is to make points
make_points_goal = GoalBase("make_points_goal", permanent=True,
conditions=[
Condition(self.sensor_manager.points, GreedyActivator())],
planner_prefix=self._agent_name)
self.goals.append(make_points_goal)
def _initialize_behaviour_model(self):
"""
This function initialises the RHBP behaviour/goal model.
"""
if self.manual_player:
control_player()
control = AgentControl(name="manual_control", perception_provider=self.perception_provider,
agent_name=self._agent_name)
self.behaviours.append(control)
else:
# Exploration targeted at locating goal
explorer = Explore_astar(name="explore", perception_provider=self.perception_provider, agent_name=self._agent_name,priority=1)
self.behaviours.append(explorer)
explorer.add_effect(
Effect(self.perception_provider.count_goal_cells.name, indicator=+1, sensor_type=float))
#Reach dispenser once origin has been obtained
reach_dispenser = MoveToDispenser(name="reach_dispenser", perception_provider=self.perception_provider,
agent_name=self._agent_name,priority=2)
self.behaviours.append(reach_dispenser)
reach_dispenser.add_effect(
Effect(self.perception_provider.closest_dispenser_distance_sensor.name, indicator=-1, sensor_type=float))
pre_cond2 = Condition(self.perception_provider.target_dispenser_selected_sensor,
BooleanActivator(desiredValue=True))
reach_dispenser.add_precondition(pre_cond2)
# #Dispense from dispenser if near
dispense = Dispense(name="dispense", perception_provider=self.perception_provider,
agent_name=self._agent_name,priority=3)
self.behaviours.append(dispense)
dispense.add_effect(
Effect(self.perception_provider.sensor_dispensed_blocks.name, indicator=+1, sensor_type=float))
pre_cond3 = Condition(self.perception_provider.closest_dispenser_distance_sensor,
ThresholdActivator(isMinimum=False, thresholdValue=1))
dispense.add_precondition(Conjunction(pre_cond2,pre_cond3))
# # Attach a block if close enough
attach = Attach(name="attach", perception_provider=self.perception_provider, agent_name=self._agent_name,priority=4)
self.behaviours.append(attach)
pre_cond4 = Condition(self.perception_provider.sensor_dispensed_blocks,
ThresholdActivator(isMinimum=True,thresholdValue=1))
pre_cond5 = Condition(self.perception_provider.closest_block_distance_sensor,
ThresholdActivator(isMinimum=False, thresholdValue=1))
attach.add_effect(
Effect(self.perception_provider.sensor_attached_blocks.name, indicator=+1, sensor_type=float))
attach.add_precondition(Conjunction(pre_cond4,pre_cond5))
#Move to goal once attached
reach_goal = MoveToGoal(name="reach_goal", perception_provider=self.perception_provider,
agent_name=self._agent_name,priority=10)
self.behaviours.append(reach_goal)
reach_goal.add_effect(
Effect(self.perception_provider.submit_count.name, indicator=+1, sensor_type=float))
pre_cond6 = Condition(self.perception_provider.sensor_attached_blocks,
ThresholdActivator(isMinimum=True,thresholdValue=1))
reach_goal.add_precondition(pre_cond6)
#Submit Task if agent has reached location
submit_task = Submit(name="submit_task",perception_provider=self.perception_provider,
agent_name=self._agent_name,priority=20)
self.behaviours.append(submit_task)
submit_task.add_effect(Effect(self.perception_provider.score_sensor.name,indicator=+1, sensor_type=float))
pre_cond7 = Condition(self.perception_provider.submit_sensor,
ThresholdActivator(isMinimum=True,thresholdValue=1))
submit_task.add_precondition(pre_cond7)
#Keep dispensing if possible
dispense_goal = GoalBase("dispense_goal",permanent=True,
conditions=[Condition(self.perception_provider.sensor_dispensed_blocks, GreedyActivator())],
planner_prefix=self._agent_name,
priority=1)
self.goals.append(dispense_goal)
#Attach to dispensed blocks
attach_goal = GoalBase("attach_goal",permanent=True,
conditions=[Condition(self.perception_provider.sensor_attached_blocks, GreedyActivator())],
planner_prefix=self._agent_name,
priority=2)
self.goals.append(attach_goal)
#Primary goal to get more points
submit_goal = GoalBase("submit_goal",permanent=True,
conditions=[Condition(self.perception_provider.score_sensor, GreedyActivator())],
planner_prefix=self._agent_name,
priority=4)
def test_multiple_embedded_network_behaviors(self):
"""
Tests the case, that one network behavior is embedded into another network behavior.
The goal requires to receive an int (3) in a topic.
"""
method_prefix = self.__message_prefix + "/test_multiple_embedded_network_behaviors"
topic_name = method_prefix + '/Topic'
sensor = TopicSensor(topic=topic_name, message_type=Int32, initial_value=0)
condition = Condition(sensor, ThresholdActivator(thresholdValue=3))
planner_prefix = method_prefix + "/Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
goal = OfflineGoal('CentralGoal', planner_prefix=planner_prefix)
goal.add_condition(condition)
m.add_goal(goal)
effect = Effect(sensor_name=sensor.name, indicator=1, sensor_type=int, activator_name=condition.activator.name)
first_level_network = NetworkBehaviour(name=method_prefix + '/FirstLevel', planner_prefix=planner_prefix, createLogFiles=True)
first_level_network.add_effects_and_goals([(sensor, effect)])
second_level_network = NetworkBehaviour(name=method_prefix + '/SecondLevel',
planner_prefix=first_level_network.get_manager_prefix(), createLogFiles=True)
# Doesnt matter, whether the effects are added via the constructor or the add method.
# Both methods are used here, to demonstrate both ways.
second_level_network.add_effects_and_goals([(sensor, effect)])
increaser_behavior = IncreaserBehavior(effect_name=sensor.name, topic_name=topic_name,
name=method_prefix + "TopicIncreaser",
planner_prefix=second_level_network.get_manager_prefix())
# activate the first_level_network, second_level_network and increaser_Behavior
for x in range(0, 3, 1):
self.assertFalse(first_level_network._isExecuting)
m.step()
rospy.sleep(0.1)
self.assertTrue(first_level_network._isExecuting)
for x in range(0, 3, 1):
self.assertFalse(second_level_network._isExecuting)
first_level_network.do_step()
rospy.sleep(0.1)
self.assertTrue(second_level_network._isExecuting)
for x in range(0, 3, 1):
self.assertFalse(increaser_behavior._isExecuting)
second_level_network.do_step()
rospy.sleep(0.1)
self.assertTrue(increaser_behavior._isExecuting)
# Satisfy goal
for step in range(0, 3, 1):
sensor.sync()
self.assertEqual(step, sensor.value)
second_level_network.do_step()
rospy.sleep(0.1)
goal.fetchStatus(3)
self.assertTrue(goal.satisfied, 'Goal is not satisfied')