def __init__(self, buffer, frames=None, moving=False, static=True,
maxvel=1.0, minvel=0.5, frame='Pheromone', attraction=1,
ev_factor=0.9, ev_time=5):
"""
initialize behaviour
:param buffer: soBuffer
:param frames: frames to be included in list returned by buffer
:param moving: consider moving gradients in list returned by buffer
:param static: consider static gradients in list returned by buffer
:param maxvel: maximum velocity of agent
:param minvel: minimum velocity of agent
:param frame: pheromone frame
:param attraction: attraction value of pheromone
:param ev_factor: pheromone evaporation factor
:param ev_time: pheromone evaporation time
"""
super(DepositPheromonesMin, self).__init__(buffer, frames, moving,
static, maxvel, minvel)
self.frame = frame
self.attraction = attraction
self.ev_factor = ev_factor
self.ev_time = ev_time
self.diffusion = maxvel
self._broadcaster = SoBroadcaster()
def __init__(self, buffer, frame=None, goal_radius=0, ev_factor=1.0,
ev_time=0.0, diffusion=20, attraction=1, moving=False):
"""
initialization of decision patterns
:param buffer: SoBuffer
:param frame: gradient frame ID
:param goal_radius: goal radius of gradient to be spread
:param ev_factor: evaporation factor of gradient to be spread
:param ev_time: evaporation time of gradient to be spread
:param diffusion: diffusion radius of gradient to be spread
:param attraction: attraction value of gradient to be spread
:param moving: mark spread gradient as moving/static
"""
self._broadcaster = SoBroadcaster()
self._buffer = buffer
# message
self.frame = frame
self.goal_radius = goal_radius
self.ev_factor = ev_factor
self.ev_time = ev_time
self.attraction = attraction
self.diffusion = diffusion
self.moving = moving
def __init__(self,
buffer,
frame=None,
key=None,
value=None,
state=None,
moving=True,
static=False,
goal_radius=0,
ev_factor=1.0,
ev_time=0.0,
diffusion=20,
attraction=0,
requres_pos=True):
"""
initialization of decision patterns
:param buffer: SoBuffer
:param frame: frame that is used for the SoMessage describing the result of the decision
:param key: payload key with values required in decision pattern
:param value: initial value for decision pattern
:param state: initial state for decision pattern
:param moving: consider moving gradients
:param static: consider static gradients
:param goal_radius: goal radius of gradient to be spread (spread())
:param ev_factor: evaporation factor of gradient to be spread
:param ev_time: evaporation time of gradient to be spread
:param diffusion: diffusion radius of gradient to be spread
:param attraction: attraction value of gradient to be spread
"""
self._requires_pos = requres_pos
self._broadcaster = SoBroadcaster()
self._buffer = buffer
# frame to consider in decision
self.frame = frame
# payload keys to consider
self.key = key
self.moving = moving
self.static = static
# current used value
self.value = value
# current & last in behaviour used state
self.state = state
# message
self.goal_radius = goal_radius
self.ev_factor = ev_factor
self.ev_time = ev_time
self.attraction = attraction
self.diffusion = diffusion
def __init__(self, buffer, maxvel=1.0, minvel=0.5, frame='Pheromone',
attraction=1, ev_factor=0.9, ev_time=5):
"""
initialize behaviour
:param buffer: soBuffer
:param maxvel: maximum velocity of agent
:param minvel: minimum velocity of agent
:param frame: pheromone frame
:param attraction: attraction value of pheromone
:param ev_factor: pheromone evaporation factor
:param ev_time: pheromone evaporation time
"""
super(DepositPheromonesRandom, self).__init__(buffer, maxvel, minvel)
self.frame = frame
self.attraction = attraction
self.ev_factor = ev_factor
self.ev_time = ev_time
self.diffusion = maxvel
self._broadcaster = SoBroadcaster()
def test_de_serialization(self):
test_str = "FoooBArr\n"
test_float = np.pi
test_dict = {'bla': test_str, 'foo': test_float}
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 1.0, 1.0, 1.0, 0, None, True, [])
msg.payload = test_dict
original_msg = deepcopy(msg)
bffr = SoBuffer()
broadcaster = SoBroadcaster()
rospy.sleep(0.5)
broadcaster.send_data(msg)
rospy.sleep(0.5)
gradients = bffr.all_gradients()
self.assertEqual(gradients[0].payload, original_msg.payload)
# other simple payload
msg.payload = np.pi
msg.parent_frame = 'float_msg'
original_msg = deepcopy(msg)
broadcaster.send_data(msg)
rospy.sleep(0.5)
gradients = bffr.all_gradients()
self.assertEqual(gradients[0].payload, original_msg.payload)
s = rospy.Service('env_gradients', EnvGradient, self.store_gradients)
def store_gradients(self, req):
"""
stores list of environment gradients
:param req: EnvGradient.srv request - list of soMessages
:return: string
"""
self.gradients = req.gradients
resp = EnvGradientResponse()
resp.name = "Updated Gradient Set."
return resp
if __name__ == '__main__':
"""
Node to spread artificial gradients
"""
rospy.init_node('artificialGradient')
rate = rospy.Rate(rospy.get_param("~frequency", 1))
# artificial gradient - initialize spreading
env = EnvironmentGradients()
so_broadcaster = SoBroadcaster()
while not rospy.is_shutdown():
# send gradients
so_broadcaster.send_data(env.gradients)
rate.sleep()
def init_so_broadcaster(self):
return SoBroadcaster()
class DepositPheromonesRandom(Exploration):
"""
depositing pheromones while moving randomly
"""
def __init__(self, buffer, maxvel=1.0, minvel=0.5, frame='Pheromone',
attraction=1, ev_factor=0.9, ev_time=5):
"""
initialize behaviour
:param buffer: soBuffer
:param maxvel: maximum velocity of agent
:param minvel: minimum velocity of agent
:param frame: pheromone frame
:param attraction: attraction value of pheromone
:param ev_factor: pheromone evaporation factor
:param ev_time: pheromone evaporation time
"""
super(DepositPheromonesRandom, self).__init__(buffer, maxvel, minvel)
self.frame = frame
self.attraction = attraction
self.ev_factor = ev_factor
self.ev_time = ev_time
self.diffusion = maxvel
self._broadcaster = SoBroadcaster()
def move(self):
"""
deposit pheromone and return random movement vector
:return: movement vector
"""
# spread pheromone
self.spread()
return super(DepositPheromonesRandom, self).move()
def spread(self):
"""
method to spread pheromones
:return:
"""
# create gossip message
msg = SoMessage()
msg.header.frame_id = self.frame
msg.parent_frame = self._buffer.id
now = rospy.Time.now()
msg.header.stamp = now
msg.ev_stamp = now
# important to determine whether gradient is within view
current_pose = self._buffer.get_own_pose()
msg.p = current_pose.p
msg.q = current_pose.q
msg.attraction = self.attraction
msg.diffusion = self.diffusion
msg.goal_radius = 0 # no goal radius
msg.ev_factor = self.ev_factor
msg.ev_time = self.ev_time
msg.moving = False # static as pheromone is deposited in environment
# spread gradient
self._broadcaster.send_data(msg)
class SpreadGradient(object):
"""
mechanism to let agents spread gradients positioned at their current
position
"""
def __init__(self, buffer, frame=None, goal_radius=0, ev_factor=1.0,
ev_time=0.0, diffusion=20, attraction=1, moving=False):
"""
initialization of decision patterns
:param buffer: SoBuffer
:param frame: gradient frame ID
:param goal_radius: goal radius of gradient to be spread
:param ev_factor: evaporation factor of gradient to be spread
:param ev_time: evaporation time of gradient to be spread
:param diffusion: diffusion radius of gradient to be spread
:param attraction: attraction value of gradient to be spread
:param moving: mark spread gradient as moving/static
"""
self._broadcaster = SoBroadcaster()
self._buffer = buffer
# message
self.frame = frame
self.goal_radius = goal_radius
self.ev_factor = ev_factor
self.ev_time = ev_time
self.attraction = attraction
self.diffusion = diffusion
self.moving = moving
def spread(self):
"""
method to spread gradient
:return:
"""
# create gossip message
msg = SoMessage()
msg.header.frame_id = self.frame
msg.parent_frame = self._buffer.id
now = rospy.Time.now()
msg.header.stamp = now
msg.ev_stamp = now
# important to determine whether gradient is within view
current_pose = self._buffer.get_own_pose()
msg.p = current_pose.p
msg.q = current_pose.q
msg.attraction = self.attraction
msg.diffusion = self.diffusion
msg.goal_radius = self.goal_radius
msg.ev_factor = self.ev_factor
msg.ev_time = self.ev_time
msg.moving = self.moving
# spread gradient
self._broadcaster.send_data(msg)
def get_pos(self):
"""
:return: current position of robot
"""
return self._buffer.get_own_pose()
class DecisionPattern(object):
"""
abstract class for decision patterns / mechanisms
"""
__metaclass__ = ABCMeta
def __init__(self,
buffer,
frame=None,
key=None,
value=None,
state=None,
moving=True,
static=False,
goal_radius=0,
ev_factor=1.0,
ev_time=0.0,
diffusion=20,
attraction=0,
requres_pos=True):
"""
initialization of decision patterns
:param buffer: SoBuffer
:param frame: frame that is used for the SoMessage describing the result of the decision
:param key: payload key with values required in decision pattern
:param value: initial value for decision pattern
:param state: initial state for decision pattern
:param moving: consider moving gradients
:param static: consider static gradients
:param goal_radius: goal radius of gradient to be spread (spread())
:param ev_factor: evaporation factor of gradient to be spread
:param ev_time: evaporation time of gradient to be spread
:param diffusion: diffusion radius of gradient to be spread
:param attraction: attraction value of gradient to be spread
"""
self._requires_pos = requres_pos
self._broadcaster = SoBroadcaster()
self._buffer = buffer
# frame to consider in decision
self.frame = frame
# payload keys to consider
self.key = key
self.moving = moving
self.static = static
# current used value
self.value = value
# current & last in behaviour used state
self.state = state
# message
self.goal_radius = goal_radius
self.ev_factor = ev_factor
self.ev_time = ev_time
self.attraction = attraction
self.diffusion = diffusion
@abstractmethod
def calc_value(self):
"""
method to determine current key-value value and to set state
:return: [value, state]
"""
pass
def spread(self):
"""
method to spread calculated values (determined by calc_value)
:return:
"""
if not self._requires_pos or self.get_pos():
# set value and state
val = self.calc_value()
self.value = val[0]
self.state = val[1]
if val:
if self.frame is not None:
# create gossip message
msg = self.create_message(val)
# spread gradient (only if data is available)
self._broadcaster.send_data(msg)
else:
rospy.logerr(
"DecisionPattern(%s):: Spread not possible. Calculation failed ...",
str(type(self)))
else:
rospy.logerr(
"DecisionPattern(%s):: Spread not possible. Own position unknown ...",
str(type(self)))
def get_pos(self):
"""
:return: Header
"""
return self._buffer.get_own_pose()
def create_message(self, val):
"""
creating a SoMessage with the given value as payload. Payload is serialized in this function
:param val: arbitrary payload value, value is considered to be a tuple of (value,state)
:return: created SoMessage
"""
msg = SoMessage()
msg.header.frame_id = self.frame
msg.parent_frame = self._buffer.id
now = rospy.Time.now()
msg.header.stamp = now
msg.ev_stamp = now
# important to determine whether gradient is within view
current_pose = self.get_pos()
if current_pose == None:
rospy.logerr(
"Self organisation stopped. Last position not available")
return None
msg.p = current_pose.p
msg.q = current_pose.q
msg.attraction = self.attraction
msg.diffusion = self.diffusion
msg.goal_radius = self.goal_radius
msg.ev_factor = self.ev_factor
msg.ev_time = self.ev_time
msg.moving = True # set to moving as gradient is tied to agent
msg.payload = {self.key: val[0]}
return msg
class Patrol(MovementPattern):
"""
Patrol: Patrolling or exploring environment by avoiding deposited pheromones, hence the agent will move in the
space without pheromones
"""
def __init__(self,
buffer,
frames=None,
moving=True,
static=True,
maxvel=1.0,
minvel=0.7,
frame='Pheromone',
attraction=-1,
ev_factor=0.9,
ev_time=30,
ev_diffusion=1.0):
"""
initialize behaviour
:param buffer: soBuffer
:param frames: frames to be included in list returned by buffer
:param moving: consider moving gradients in list returned by buffer
:param static: consider static gradients in list returned by buffer
:param maxvel: maximum velocity of agent
:param minvel: minimum velocity of agent
:param frame: pheromone frame
:param attraction: attraction value of pheromone
:param ev_factor: pheromone evaporation factor
:param ev_time: pheromone evaporation time
"""
super(Patrol, self).__init__(buffer=buffer,
frames=frames,
moving=moving,
static=static,
maxvel=maxvel,
minvel=minvel)
self.frame = frame
self.attraction = attraction
self.ev_factor = ev_factor
self.ev_time = ev_time
self.diffusion = ev_diffusion
self._broadcaster = SoBroadcaster()
def move(self):
"""
calculates movement vector, handling all gradients as repulsive
:return: movement vector
"""
# spread pheromone
self.spread()
pose = self._buffer.get_own_pose()
result = None
# repulsive gradients
gradients = self._buffer.gradients(self.frames, self.static,
self.moving)
if pose:
if gradients:
result = Vector3()
if len(gradients) > 1:
for grdnt in gradients:
grad = gradient.calc_repulsive_gradient(grdnt, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if grad.x == np.inf or grad.x == -1 * np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grdnt.p)
dv = calc.adjust_length(
dv, grdnt.goal_radius + grdnt.diffusion)
result = calc.add_vectors(result, dv)
else:
result = calc.add_vectors(result, grad)
else:
grdnt = gradients[0]
result = calc.random_vector(grdnt.goal_radius +
grdnt.diffusion)
else:
rospy.logwarn("No gradients of frames '%s' available",
self.frames)
else:
rospy.logwarn("No own pose available!")
if not result:
rospy.logwarn("No gradient vector available!")
return None
# adjust length
d = calc.vector_length(result)
if d > self.maxvel:
result = calc.adjust_length(result, self.maxvel)
elif 0 < d < self.minvel:
result = calc.adjust_length(result, self.minvel)
else:
result = calc.random_vector(grdnt.goal_radius + grdnt.diffusion)
return result
def spread(self):
"""
method to spread pheromones
:return:
"""
# create gossip message
msg = SoMessage()
msg.header.frame_id = self.frame
msg.parent_frame = self._buffer.id
now = rospy.Time.now()
msg.header.stamp = now
msg.ev_stamp = now
current_pose = self._buffer.get_own_pose()
msg.p = current_pose.p
msg.q = current_pose.q
msg.attraction = self.attraction
msg.diffusion = self.diffusion
msg.goal_radius = 0 # no goal radius
msg.ev_factor = self.ev_factor
msg.ev_time = self.ev_time
msg.moving = False # static as pheromone is deposited in environment
# spread gradient
self._broadcaster.send_data(msg)
class DepositPheromones(ChemotaxisGe):
"""
Foraging: set state and move to nest while depositing pheromones
enhancement of ChemotaxisBalch behaviour
"""
def __init__(self, buffer, frames=None, moving=False, static=True,
maxvel=1.0, minvel=0.5, frame='Pheromone', attraction=1,
ev_factor=0.9, ev_time=5):
"""
initialize behaviour
:param buffer: soBuffer
:param frames: frames to be included in list returned by buffer
:param moving: consider moving gradients in list returned by buffer
:param static: consider static gradients in list returned by buffer
:param maxvel: maximum velocity of agent
:param minvel: minimum velocity of agent
:param frame: pheromone frame
:param attraction: attraction value of pheromone
:param ev_factor: pheromone evaporation factor
:param ev_time: pheromone evaporation time
"""
super(DepositPheromones, self).__init__(buffer, frames, moving, static,
maxvel, minvel)
self.frame = frame
self.attraction = attraction
self.ev_factor = ev_factor
self.ev_time = ev_time
self.diffusion = maxvel
self._broadcaster = SoBroadcaster()
def move(self):
"""
deposit pheromone and move towards nest
:return: movement vector
"""
# spread pheromone
self.spread()
return super(DepositPheromones, self).move()
def spread(self):
"""
method to spread pheromones
:return:
"""
# create gossip message
msg = SoMessage()
msg.header.frame_id = self.frame
msg.parent_frame = self._buffer.id
now = rospy.Time.now()
msg.header.stamp = now
msg.ev_stamp = now
current_pose = self._buffer.get_own_pose()
msg.p = current_pose.p
msg.q = current_pose.q
msg.attraction = self.attraction
msg.diffusion = self.diffusion
msg.goal_radius = 0 # no goal radius
msg.ev_factor = self.ev_factor
msg.ev_time = self.ev_time
msg.moving = False # static as pheromone is deposited in environment
# spread gradient
self._broadcaster.send_data(msg)