def __init__(self):
rospy.logdebug("RhbpAgent::init")
rospy.init_node('agent_node', anonymous=True, log_level=rospy.INFO)
self._agent_name = rospy.get_param('~agent_name', 'agentA1') # default for debugging 'agentA1'
self._agent_topic_prefix = get_bridge_topic_prefix(agent_name=self._agent_name)
# ensure also max_parallel_behaviours during debugging
self._manager = Manager(prefix=self._agent_name, max_parallel_behaviours=1)
self.behaviours = []
self.goals = []
self.perception_provider = PerceptionProvider()
self._sim_started = False
#if manual player is called, do not call other behaviours
self.manual_player = False
# subscribe to MAPC bridge core simulation topics
rospy.Subscriber(self._agent_topic_prefix + "request_action", RequestAction, self._action_request_callback)
rospy.Subscriber(self._agent_topic_prefix + "start", SimStart, self._sim_start_callback)
rospy.Subscriber(self._agent_topic_prefix + "end", SimEnd, self._sim_end_callback)
rospy.Subscriber(self._agent_topic_prefix + "bye", Bye, self._bye_callback)
rospy.Subscriber(self._agent_topic_prefix + "generic_action", GenericAction, self._callback_generic_action)
self._received_action_response = False
def test_remote_goal(self):
method_prefix = self.__message_prefix + "TestRemoteGoal"
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 = 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')
def __init__(self,
name,
requires_execution_steps=True,
only_running_for_deciding_interruptible=Manager.
USE_ONLY_RUNNING_BEHAVIOURS_FOR_INTERRUPTIBLE_DEFAULT_VALUE,
correlations=None,
always_update_activation=False,
**kwargs):
"""
:param correlations: tuple <Effect>
:param name: name of the behaviour that is also used to create the sub manager name together with the NetworkBehavior.MANAGER_POSTFIX
:param requires_execution_steps: whether the execution steps should be caused from the parent manager or not.
If not, the step method must be called manually
:param always_update_activation: if set to True the entire activation calculation of the sub manager is updated on each behaviour computation update
:param kwargs: args for the manager, except the prefix arg
"""
super(NetworkBehavior,
self).__init__(name=name,
requires_execution_steps=requires_execution_steps,
**kwargs)
self.requires_execution_steps = requires_execution_steps
self.always_update_activation = always_update_activation
manager_args = {}
manager_args.update(kwargs)
manager_args['prefix'] = self.get_manager_prefix()
self.__manager = Manager(activated=False,
use_only_running_behaviors_for_interRuptible=
only_running_for_deciding_interruptible,
**manager_args)
self.__goal_name_prefix = name + "/Goals/"
self.__goal_counter = 0
if correlations is not None:
self.add_effects(correlations)
def __init__(self):
###DEBUG MODE###
log_level = rospy.DEBUG if global_variables.DEBUG_MODE else rospy.INFO
################
rospy.logdebug("RhbpAgent::init")
rospy.init_node('agent_node', anonymous=True, log_level=log_level)
self._agent_name = rospy.get_param('~agent_name', 'agentA2') # default for debugging 'agentA1'
self._agent_topic_prefix = get_bridge_topic_prefix(agent_name=self._agent_name)
# ensure also max_parallel_behaviours during debugging
self._manager = Manager(prefix=self._agent_name, max_parallel_behaviours=1)
self.behaviours = []
self.goals = []
self.perception_provider = PerceptionProvider()
# start communication class
self._communication = Communication(self._agent_name)
# Task update topic
self._pub_subtask_update = self._communication.start_subtask_update(self._callback_subtask_update)
# auction structure
self.auction = Auction(self)
self.number_of_agents = 2 # number_of_agents needs to match amount of launched agents
self.map_communication = MapCommunication(self)
self._sim_started = False
# agent attributes
self.local_map = GridMap(agent_name=self._agent_name, agent_vision=5)
# instantiate the sensor manager passing a reference to this agent
self.sensor_manager = SensorManager(self)
# representation of tasks
self.tasks = {}
self.assigned_subtasks = [] # personal for the agent. the task at index 0 is the task the agent is currently executing
# subscribe to MAPC bridge core simulation topics
rospy.Subscriber(self._agent_topic_prefix + "request_action", RequestAction, self._action_request_callback)
rospy.Subscriber(self._agent_topic_prefix + "start", SimStart, self._sim_start_callback)
rospy.Subscriber(self._agent_topic_prefix + "end", SimEnd, self._sim_end_callback)
rospy.Subscriber(self._agent_topic_prefix + "bye", Bye, self._bye_callback)
rospy.Subscriber(self._agent_topic_prefix + "generic_action", GenericAction, self._callback_generic_action)
self._received_action_response = False
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_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_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 = SimpleTopicSensor(topic=topic_name,
message_type=Bool,
initial_value=False)
condition = Condition(sensor, BooleanActivator())
pddl_function_name = condition.getFunctionNames()[0]
SetTrueBehavior(effect_name=pddl_function_name,
topic_name=topic_name,
name=method_prefix + "SetTrue",
plannerPrefix=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 _create_manager(self, prefix):
"""
factory method for creating the actual manager instance, this simplifies custom implementations of the planner
node
:param prefix: manager prefix
:return: Manager
"""
return Manager(prefix=prefix)
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 = SimpleTopicSensor(topic=topic_name_1, message_type=Int32, initial_value=False)
non_interruptable_condition = Condition(non_interruptable_sensor, GreedyActivator())
condition_function_name = non_interruptable_condition.getFunctionNames()[0]
non_interruptable_behaviour = IncreaserBehavior(effect_name=condition_function_name, topic_name=topic_name_1,
name=method_prefix + "TopicIncreaser", plannerPrefix=planner_prefix, interruptable=False)
topic_name_2 = method_prefix + '/sensor_2'
interruptable_sensor = SimpleTopicSensor(topic=topic_name_2, message_type=Int32, initial_value=False)
interruptable_condition = Condition(interruptable_sensor, GreedyActivator())
condition_function_name2 = interruptable_condition.getFunctionNames()[0]
interruptable_behaviour = IncreaserBehavior(effect_name=condition_function_name2, topic_name=topic_name_2,
name=method_prefix + "TopicIncreaser2", plannerPrefix=planner_prefix, interruptable=True)
enable_sensor = Sensor(name='enable_sensor', initial_value=True)
enable_cond = Condition(enable_sensor, BooleanActivator())
non_interruptable_behaviour.addPrecondition(enable_cond)
goal = GoalBase(method_prefix + 'CentralGoal', plannerPrefix=planner_prefix, permanent=True)
goal.addCondition(non_interruptable_condition)
goal.addCondition(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.addPrecondition(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_factory(self):
manager = Manager()
algo = ActivationAlgorithmFactory.create_algorithm("default", manager)
self.assertNotEqual(algo, None, 'Algorithm is none')
self.assertTrue(isinstance(algo, BaseActivationAlgorithm), "Not the correct algorithm")
def setUp(self):
self._rospy_patcher = patch('rospy.Time.now')
self._rospy_patcher.start()
self._rospy_patcher.return_value = Time(0, 0)
DynamicReconfigureServer = collections.namedtuple('DynamicReconfigureServer', 'ns')
Manager.dynamic_reconfigure_server = DynamicReconfigureServer(ns="UNUSED")
self.manager = Manager(activationThreshold=7, createLogFiles=False, max_parallel_behaviours=1)
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 __init__(self):
rospy.logdebug("RhbpAgent::init")
rospy.init_node('agent_node', anonymous=True, log_level=rospy.DEBUG)
self._agent_name = rospy.get_param('~agent_name', 'agentA1') # default for debugging 'agentA1'
self._agent_topic_prefix = get_bridge_topic_prefix(agent_name=self._agent_name)
# ensure also max_parallel_behaviours during debugging
self._manager = Manager(prefix=self._agent_name, max_parallel_behaviours=1)
# static things (terrain)
self.behaviours = []
self.goals = []
self.perception_provider = PerceptionProvider()
self.local_map = GridMap(agent_name=self._agent_name, live_plotting=True)
# auction structure
self.bids = {}
self.number_of_agents = 2 # TODO: check if there's a way to get it automatically
self._sim_started = False
# subscribe to MAPC bridge core simulation topics
rospy.Subscriber(self._agent_topic_prefix + "request_action", RequestAction, self._action_request_callback)
rospy.Subscriber(self._agent_topic_prefix + "start", SimStart, self._sim_start_callback)
rospy.Subscriber(self._agent_topic_prefix + "end", SimEnd, self._sim_end_callback)
rospy.Subscriber(self._agent_topic_prefix + "bye", Bye, self._bye_callback)
rospy.Subscriber(self._agent_topic_prefix + "generic_action", GenericAction, self._callback_generic_action)
# start communication class
self._communication = Communication(self._agent_name)
# Map topic
self._pub_map = self._communication.start_map(self._callback_map)
# Personal message topic
self._pub_agents = self._communication.start_agents(self._callback_agents)
# Auction topic
self._pub_auction = self._communication.start_auction(self._callback_auction)
self.time_to_bid = True # only test debug puposes
self.task_subdivision = {"task1":
{"agents_needed": 3,
"agents_assigned": []
}
}
self._received_action_response = False
def setUpClass(self):
rospy.init_node('TestConfigurationCreation_node')
TestConfigurationCreation.m = Manager()
# Wait for manager becoming available
while True:
try:
rospy.wait_for_service('/AddBehaviour', timeout=0.1)
except rospy.ROSException:
rospy.sleep(1)
continue
break
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 __init__(self):
rospy.init_node('behaviourPlannerManager', log_level=rospy.WARN)
prefix = rospy.get_param("~prefix", "")
self._manager = Manager(prefix=prefix)
self.rate = rospy.Rate(rospy.get_param("~frequency", 1))
self.automatic_stepping = rospy.get_param("~automatic_stepping", True)
if not self.automatic_stepping:
rospy.logwarn("Started in manual stepping mode")
self._init_services(prefix)
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 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 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 __init__(self):
log_level = rospy.DEBUG if config.DEBUG_MODE else rospy.INFO
rospy.logdebug("RhbpAgent::init")
rospy.init_node('agent_node', anonymous=True, log_level=log_level)
self._agent_name = rospy.get_param(
'~agent_name', 'agentA1') # default for debugging 'agentA1'
self._agent_topic_prefix = get_bridge_topic_prefix(
agent_name=self._agent_name)
# ensure also max_parallel_behaviours during debugging
self._manager = Manager(prefix=self._agent_name,
max_parallel_behaviours=1)
self.behaviours = []
self.goals = []
self.perception_provider = PerceptionProvider()
self.sensor_manager = SensorManager(agent=self)
self.map = Map(agent=self, agent_name=self._agent_name)
self.communication = Communication(agent_name=self._agent_name)
self.map_pub = self.communication.start_map_pub_sub(
callback=self._map_callback)
self.received_map_messages = []
self.tasks = TaskCollection()
self._sim_started = False
# subscribe to MAPC bridge core simulation topics
rospy.Subscriber(self._agent_topic_prefix + "request_action",
RequestAction, self._action_request_callback)
rospy.Subscriber(self._agent_topic_prefix + "start", SimStart,
self._sim_start_callback)
rospy.Subscriber(self._agent_topic_prefix + "end", SimEnd,
self._sim_end_callback)
rospy.Subscriber(self._agent_topic_prefix + "bye", Bye,
self._bye_callback)
rospy.Subscriber(self._agent_topic_prefix + "generic_action",
GenericAction, self._callback_generic_action)
self._received_action_response = False
def test_interruptible_warning(self, mock_rhbp_logging):
"""
This test tests if logwarn is generated if a NetworkBehaviour is created with a interruptable parameter
:param mock_rhbp_logging:
"""
#with mock.patch('__main__.Foo.foo', new_callable=mock.PropertyMock) as mock_foo:
method_prefix = self.__message_prefix + "/test_interruptible_warning"
planner_prefix = method_prefix + "/Manager"
m = Manager(activationThreshold=7, prefix=planner_prefix)
first_level_network = NetworkBehaviour(name=method_prefix + '/FirstLevel', planner_prefix=planner_prefix,
interruptable=True)
mock_rhbp_logging.assert_has_calls(mock_rhbp_logging.logwarn, "logwarn not triggered")
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 __init__(self,
name,
requires_execution_steps=True,
only_running_for_deciding_interruptible=Manager.
USE_ONLY_RUNNING_BEHAVIOURS_FOR_INTERRUPTIBLE_DEFAULT_VALUE,
correlations=None,
always_update_activation=False,
guarantee_decision=False,
**kwargs):
"""
:param correlations: tuple <Effect>
:param name: name of the behaviour that is also used to create the sub manager name together with the NetworkBehaviour.MANAGER_POSTFIX
:param requires_execution_steps: whether the execution steps should be caused from the parent manager or not.
If not, the step method must be called manually
:param always_update_activation: if set to True the entire activation calculation of the sub manager is updated on each behaviour computation update
:param guarantee_decision: if there are executable behaviours in the local network, adjust the thresholds until
at least one behaviour is selected
:param kwargs: args for the manager, except the prefix arg
"""
super(NetworkBehaviour,
self).__init__(name=name,
requires_execution_steps=requires_execution_steps,
**kwargs)
if "interruptable" in kwargs:
rhbplog.logwarn(
"Interruptable parameter will be ignored in a NetworkBehaviour. Interruptable attribute is "
"evaluated based on the running or registered parameters, "
"see 'only_running_for_deciding_interruptible'")
self.requires_execution_steps = requires_execution_steps
self.always_update_activation = always_update_activation
self.guarantee_decision = guarantee_decision
manager_args = {}
manager_args.update(kwargs)
manager_args['prefix'] = self.get_manager_prefix()
self.__manager = Manager(enabled=False,
use_only_running_behaviors_for_interRuptible=
only_running_for_deciding_interruptible,
**manager_args)
self.__goal_name_prefix = name + "/Goals/"
self.__goal_counter = 0
if correlations is not None:
self.add_effects(correlations)
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_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 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)
average_condition = AverageSensorSatisfactionCondition(
sensors=[sensor1, sensor2], activator=activator_decreasing)
average_condition.sync()
average_condition.updateComputation()
wish_average = average_condition.getWishes()
average_precon_pddl = average_condition.getPreconditionPDDL(
satisfaction_threshold=satisfaction_threshold)
average_state = average_condition.getStatePDDL()
behaviour1 = BehaviourBase("behaviour_1", plannerPrefix=planner_prefix)
behaviour1.add_effect(
Effect(sensor_name=sensor1.name, indicator=-0.1,
sensor_type=float))
behaviour1.addPrecondition(condition_increasing)
behaviour2 = BehaviourBase("behaviour_2", plannerPrefix=planner_prefix)
behaviour2.add_effect(
Effect(sensor_name=sensor1.name, indicator=0.1, sensor_type=float))
behaviour2.addPrecondition(condition_decreasing)
behaviour2.addPrecondition(average_condition)
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 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_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)