/udom_control/udom_shape_control/src/udom_shape_control/arandas_controller_node.py

#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
This node computes the poses for a set of robots/points to move from a current
configuration to a target configuration.

**Input(s):**

  * `event_in`: The desired event for the node:

      `e_start`: starts the component.

      `e_stop`: stops the component.

    - *type:* `std_msgs/String`

  * `target_poses`: Target poses for the controller to reach.

    - *type:* `geometry_msgs/PoseArray`

  * `current_poses`: Current poses of the robots/points.

    - *type:* `geometry_msgs/PoseArray`

**Output(s):**

  * `poses_out`: The commanded poses for the robots/points.

    - *type:* `geometry_msgs/PoseArray`

  * `event_out`: The current event of the node.

      `e_running`: when the component is running.

      `e_stopped`: when the component is stopped.

    - *type:* `std_msgs/String`

**Parameter(s):**

  * `loop_rate`: Node cycle rate (in Hz).

  * `number_of_robots`: Number of robots to control.

  * `gain`: Controller gain.

  * `scale`:Desired scale.

  * `weights`: Controller weights for each term.

  * `saturation`: Saturation threshold for the controller.

"""

import numpy as np
import rospy
import std_msgs.msg
import geometry_msgs.msg
import udom_shape_control.arandas_controller_utils as utils
from dynamic_reconfigure.server import Server
from udom_shape_control.cfg import ArandasControllerConfig as ArandasController


class ArandasControllerNode(object):
    """
    Computes the poses for a set of robots/points to move from a current configuration
    to a target configuration.

    """
    def __init__(self):
        """
        Instantiates a Aranda's controller node.

        :return: A node to move a set of robots/points to a desired configuration.
        :rtype: ArandasControllerNode

        """
        # Params
        self.event = None
        self.target = None
        self.current = None
        self.centroid = None

        # Number of robots to control.
        self.number_of_robots = rospy.get_param('~number_of_robots', 5)

        # Controller gain.
        self.gain = rospy.get_param('~gain', 1e-2)

        # Desired scale.
        self.scale = rospy.get_param('~scale', 1.0)

        # Saturation threshold for the controller.
        self.saturation = rospy.get_param('~saturation', 0.03)

        # Controller weights for each term.
        self.weights = rospy.get_param('~weights', [0.1, 0.1, 0.1, 0.1, 0.1])
        assert len(self.weights) == 5, "weights must be of dimension 5."

        # Node cycle rate (in Hz).
        rate = rospy.get_param('~loop_rate', 100)
        self.loop_rate = rospy.Rate(rate)

        # Create controller
        self.controller = utils.ArandasController(
            self.number_of_robots, self.gain, self.weights, self.scale)

        # Publishers
        self.event_out = rospy.Publisher("~event_out", std_msgs.msg.String, queue_size=10)
        self.command = rospy.Publisher(
            "~command", geometry_msgs.msg.PoseArray, queue_size=1, tcp_nodelay=True)
        self.target_pub = rospy.Publisher(
            "~target_logger", geometry_msgs.msg.PoseArray, queue_size=1, tcp_nodelay=True)
        self.current_pub = rospy.Publisher(
            "~current_logger", geometry_msgs.msg.PoseArray, queue_size=1, tcp_nodelay=True)

        self.destination = rospy.Publisher(
            "~destination", geometry_msgs.msg.PoseArray, queue_size=1, tcp_nodelay=True)
        self.destination_g = rospy.Publisher(
            "~destination_g", geometry_msgs.msg.PoseArray, queue_size=1, tcp_nodelay=True)

        # Subscribers
        rospy.Subscriber("~event_in", std_msgs.msg.String, self.event_in_cb)
        rospy.Subscriber('~target', geometry_msgs.msg.PoseArray, self.target_cb)
        rospy.Subscriber('~current', geometry_msgs.msg.PoseArray, self.current_cb)
        rospy.Subscriber('~centroid', geometry_msgs.msg.PoseStamped, self.centroid_cb)

        # Dynamic Server Reconfiguration
        dynamic_reconfig_srv = Server(ArandasController, self.dynamic_reconfig_cb)

    def dynamic_reconfig_cb(self, config, level):
        """
        Reconfigures the parameters of the Aranda's controller.

        """
        self.gain = config.gain
        self.scale = config.scale
        self.saturation = config.saturation
        self.weights[0] = config.similarity_weight
        self.weights[1] = config.affine_weight
        self.weights[2] = config.centroid_weight
        self.weights[3] = config.scale_weight
        self.weights[4] = config.orientation_size_weight
        return config

    def event_in_cb(self, msg):
        """
        Obtains the event for the node (e.g. start, stop).

        :param msg: Event message for the node.
        :type msg: std_msgs.msg.String

        """
        self.event = msg.data

    def target_cb(self, msg):
        """
        Obtains the target configuration.

        :param msg: Target configuration.
        :type msg: geometry_msgs.msg.PoseArray

        """
        self.target = msg

    def current_cb(self, msg):
        """
        Obtains the current configuration.

        :param msg: Current configuration.
        :type msg: geometry_msgs.msg.PoseArray

        """
        self.current = msg

    def centroid_cb(self, msg):
        """
        Obtains the desired centroid.

        :param msg: Desired centroid.
        :type msg: geometry_msgs.msg.PoseStamped

        """
        self.centroid = msg

    def start(self):
        """
        Starts the node.

        """
        rospy.loginfo("Ready to start...")
        state = 'INIT'

        while not rospy.is_shutdown():

            if state == 'INIT':
                state = self.init_state()
            elif state == 'IDLE':
                state = self.idle_state()
            elif state == 'RUNNING':
                state = self.running_state()

            rospy.logdebug("State: {0}".format(state))
            self.loop_rate.sleep()

    def init_state(self):
        """
        Executes the INIT state of the state machine.

        :return: The updated state.
        :rtype: str

        """
        if self.event == 'e_start':
            return 'IDLE'
        else:
            return 'INIT'

    def idle_state(self):
        """
        Executes the IDLE state of the state machine.

        :return: The updated state.
        :rtype: str

        """
        if self.event == 'e_stop':
            self.event_out.publish('e_stopped')
            self.reset_component_data()
            return 'INIT'
        elif (self.target is not None) and (self.current is not None):
            return 'RUNNING'
        else:
            return 'IDLE'

    def running_state(self):
        """
        Executes the RUNNING state of the state machine.

        :return: The updated state.
        :rtype: str

        """
        if self.event == 'e_stop':
            self.event_out.publish('e_stopped')
            self.reset_component_data()
            return 'INIT'
        else:
            command = self.compute_command()

            self.event_out.publish('e_running')
            self.command.publish(command)
            self.target_pub.publish(self.target)
            self.current_pub.publish(self.current)
            self.reset_component_data()
            return 'IDLE'

    def compute_command(self):
        """
        Computes the required command to move towards a target configuration.

        :return: Command to move towards a target configuration.
        :rtype: geometry_msgs.msg.PoseArray

        """
        current = utils.pose_array_to_matrix(self.current)
        target = utils.pose_array_to_matrix(self.target)

        # Update controller parameters.
        self.controller.gain = self.gain
        self.controller.scale = self.scale
        self.controller.weights = self.weights
        self.controller.saturation = self.saturation

        if self.centroid is not None:
            centroid = np.array([
                self.centroid.pose.position.x, self.centroid.pose.position.y,
                self.centroid.pose.position.z])
            command_matrix = self.controller.compute_command(current, target, centroid)
        else:
            command_matrix = self.controller.compute_command(current, target)

        command = utils.matrix_to_pose_array(command_matrix)

        command.header.frame_id = self.current.header.frame_id
        command.header.stamp = rospy.Time.now()

        destination_matrix = self.controller.compute_destination(current, target)
        destination = utils.matrix_to_pose_array(destination_matrix)
        destination.header.frame_id = self.current.header.frame_id
        destination.header.stamp = rospy.Time.now()
        self.destination.publish(destination)

        destination_matrix_g = self.controller.compute_destination_g(current, target)
        destination_g = utils.matrix_to_pose_array(destination_matrix_g)
        destination_g.header.frame_id = self.current.header.frame_id
        destination_g.header.stamp = rospy.Time.now()
        self.destination_g.publish(destination_g)

        return command

    def reset_component_data(self):
        """
        Clears the data of the component.

        """
        self.event = None
        self.current = None


def main():
    rospy.init_node("arandas_controller_node", anonymous=True)
    arandas_controller_node = ArandasControllerNode()
    arandas_controller_node.start()