/udom_demo/udom_deformation_sensing/src/udom_deformation_sensing/coordinator_kamal_approach.py

#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
This node uses a pipeline of components to demonstrate deformation sensing based
on force and position sensing.
The component serves as a configurator/coordinator, i.e. it sets the required
parameters for all the components and starts/stops them accordingly.

It uses the following nodes:
  * `udom_geometric_transformation/mesh_shifter`
  * `udom_geometric_transformation/position_based_force_calculator_node`
  * `udom_topic_tools/force_adder`
  * `udom_deformation_modeling/deformation_model`

**Input(s):**

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

      `e_start`: starts the component.

      `e_stop`: stops the component.

**Output(s):**

  * `mesh`: A mesh representation of the object with the updated nodes' position based
        on  the deformation.

    - *type:* `udom_modeling_msgs/Mesh`

  * `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).

"""

import rospy
import std_msgs.msg


class Coordinator(object):
    """
    Coordinates a set of components to estimate the deformation of an object caused by
    an external force.

    """
    def __init__(self):
        """
        Instantiates a node to coordinate the components of the deformation sensing
        pipeline.

        """
        # Params
        self.started_components = False
        self.event = None
        self.mesh_shifter_status = None
        self.position_based_force_calculator_status = None
        self.force_adder_status = None
        self.deformation_model_status = None

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

        # Publishers
        self.event_out = rospy.Publisher("~event_out", std_msgs.msg.String, queue_size=10)
        self.start_mesh_shifter = rospy.Publisher(
            "~start_mesh_shifter", std_msgs.msg.String, queue_size=10, latch=True)
        self.start_position_based_force_calculator = rospy.Publisher(
            "~start_position_based_force_calculator", std_msgs.msg.String, queue_size=10,
            latch=True)
        self.start_force_adder = rospy.Publisher(
            "~start_force_adder", std_msgs.msg.String, queue_size=10, latch=True)
        self.start_deformation_model = rospy.Publisher(
            "~start_deformation_model", std_msgs.msg.String, queue_size=10, latch=True)

        # Subscribers
        rospy.Subscriber("~event_in", std_msgs.msg.String, self.event_in_cb)
        rospy.Subscriber(
            "~mesh_shifter_status", std_msgs.msg.String, self.mesh_shifter_status_cb)
        rospy.Subscriber(
            "~force_adder_status", std_msgs.msg.String, self.force_adder_status_cb)
        rospy.Subscriber(
            "~position_based_force_calculator_status", std_msgs.msg.String,
            self.position_based_force_calculator_status_cb)
        rospy.Subscriber(
            "~force_adder_status", std_msgs.msg.String,self.force_adder_status_cb)
        rospy.Subscriber(
            "~deformation_model_status", std_msgs.msg.String,
            self.deformation_model_status_cb)

    def event_in_cb(self, msg):
        """
        Obtains an event for the component.

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

        """
        self.event = msg.data

    def mesh_shifter_status_cb(self, msg):
        """
        Obtains the status of the mesh shifter (as an event).

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

        """
        self.mesh_shifter_status = msg.data

    def force_adder_status_cb(self, msg):
        """
        Obtains the status of the force adder(as an event).

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

        """
        self.force_adder_status = msg.data

    def position_based_force_calculator_status_cb(self, msg):
        """
        Obtains the status of the position based force calculator (as an event).

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

        """
        self.position_based_force_calculator_status = msg.data

    def deformation_model_status_cb(self, msg):
        """
        Obtains the status of the deformation model (as an event).

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

        """
        self.deformation_model_status = msg.data

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

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

        while not rospy.is_shutdown():

            if state == 'INIT':
                state = self.init_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 in ['e_start', 'e_reset']:
            return 'RUNNING'
        else:
            return 'INIT'

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

        :return: The updated state.
        :rtype: str

        """
        self.toggle_components(self.event)

        if self.event == 'e_stop':
            status = 'e_stopped'
            self.event_out.publish(status)
            self.reset_component_data(status)
            return 'INIT'
        else:
            return 'RUNNING'

    def toggle_components(self, event):
        """
        Starts or stops the necessary components based on the event.

        :param event: The event that determines either to start or stop the components.
        :type event: str

        """
        if event == 'e_stop':
            self.start_mesh_shifter.publish('e_stop')
            self.start_position_based_force_calculator.publish('e_stop')
            self.start_force_adder.publish('e_stop')
            self.start_deformation_model.publish('e_stop')
            self.start_deformation_model.publish('e_stop')
            self.started_components = False

        if event == 'e_reset':
            self.start_deformation_model.publish('e_reset')
            self.event = 'e_start'
            self.started_components = False

        if event == 'e_start' and not self.started_components:
            self.start_mesh_shifter.publish('e_start')
            self.start_position_based_force_calculator.publish('e_start')
            self.start_force_adder.publish('e_start')
            self.start_deformation_model.publish('e_start')
            self.started_components = True

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

        :param result: The result of the component, e.g. stopped, failure, success.
        :type result: str

        """
        self.toggle_components(result)
        self.event = None
        self.mesh_shifter_status = None
        self.position_based_force_calculator_status = None
        self.force_adder_status = None
        self.deformation_model_status = None
        self.started_components = False


def main():
    rospy.init_node("coordinator", anonymous=True)
    coordinator = Coordinator()
    coordinator.start()