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)
Esempio n. 2
0
    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