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)
def create_msg(self):
"""
creates soMessage with set parameters
:return: gradient message / SoMessage
"""
msg = SoMessage()
# current time
now = rospy.Time.now()
msg.header.frame_id = self._frame
msg.parent_frame = self._id
msg.header.stamp = now
msg.p = self.p
msg.q = self.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.ev_stamp = now
msg.direction = self.direction
msg.moving = self.moving
msg.payload = self.payload
return msg
def create_gradient(position,
attraction=0,
diffusion=3.0,
q=Quaternion(),
moving=False,
direction=Vector3(1, 0, 0),
goal_radius=1.0,
payload='',
ev_time=0,
ev_factor=1.0,
frameid='',
parentframe='',
time=None):
"""
creates a soMessage to specify a gradient
:param position: agent position
:param attraction: repulsion(-1), attraction(1)
:param diffusion: diffusion radius
:param q: Quaternion defining rotation
:param direction: initial direction to specify current gradient orientation
:param goal_radius: goal radius of gradient
:param payload: payload data
:param ev_time: evaporation time
:param ev_factor: evaporation factor
:param frameid: header frame id of gradient
:param: parentframe: parent frame id of gradient
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: soMessage
"""
if time is None:
time = rospy.Time.now()
msg = SoMessage()
msg.p = position
msg.q = q
msg.direction = direction
msg.attraction = attraction
msg.header.stamp = time
msg.header.frame_id = frameid
msg.parent_frame = parentframe
msg.diffusion = diffusion
msg.goal_radius = goal_radius
msg.ev_factor = ev_factor
msg.ev_time = ev_time
msg.ev_stamp = time
msg.moving = moving
msg.payload = payload
return msg
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