Close roslaunch in another state of Smach with userdata

edit

Hi there,

I am using smach (state machine) for a small project. I launched a file in one state (SearchFace), and would like to terminate the launched file in another state(MoveCloser).

Here is the peudo code:

import rospy
import roslaunch
import rospkg
import smach
import smach_ros


class SearchFace(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded'], input_keys=['uuid', 'launch_face_tracking_path'], output_keys=['uuid', 'launch_face_tracking_path'])

    def execute(self, userdata):
        rospy.loginfo('Executing state SEARCH_FACE')
        # create launch file handles
        launch_face_tracking = roslaunch.parent.ROSLaunchParent(userdata.uuid, [userdata.launch_face_tracking_path])
        launch_face_tracking.start()
        return 'succeeded'


class MoveCloser(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded'], input_keys=['uuid', 'launch_face_tracking_path'])

    def execute(self, userdata):
        rospy.loginfo('Executing state MOVE_CLOSER')
        # The  launch_face_tracking does not refer to the object created previously, even with uuid.
        launch_face_tracking = roslaunch.parent.ROSLaunchParent(userdata.uuid, [userdata.launch_face_tracking_path])
        launch_face_tracking.shutdown()
        return 'succeeded'

class DanceInvitation():
    def __init__(self):
        rospy.on_shutdown(self._shutdown)

        # create roslaunch handles: what is the usage of uuid if it cannot refer to the same launch process
        self._uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
        roslaunch.configure_logging(self._uuid)

        # construct state machine
        self._sm = self._construct_sm()
        rospy.loginfo('State machine Constructed')

        # Run state machine introspection server for smach viewer
        self._intro_spect = smach_ros.IntrospectionServer('dance_invitation', self._sm, '/DANCE_INVITATION')
        self._intro_spect.start()
        outcome = self._sm.execute()
        rospy.spin()
        self._intro_spect.stop()

    def _construct_sm(self):
        rospy.loginfo('Constructing state machine')
        sm = smach.StateMachine(outcomes = ['succeeded'])

        # create launch file handles
        # I tried that userdata cannot pass roslaunch object, such as launch_face_tracking above. That is why I try to pass uuid instead.
        sm.userdata.launch_face_tracking_path = rospkg.RosPack().get_path('movo_demos') + '/launch/face_tracking/face_tracking.launch'
        sm.userdata.uuid = self._uuid

        with sm:
            smach.StateMachine.add('SEARCH_FACE', SearchFace(), transitions={'succeeded': 'MOVE_CLOSER'}, remapping = {'uuid':'uuid', 'launch_face_tracking_path':'launch_face_tracking_path'})
            smach.StateMachine.add('MOVE_CLOSER', MoveCloser(), transitions={'succeeded': 'succeeded'})

        return sm


if __name__ == "__main__":
    rospy.loginfo('start dance invitation')
    rospy.init_node('dance_invitation', log_level=rospy.INFO)
    # rospy.init_node('dance_invitation', log_level=rospy.DEBUG)

    DanceInvitation()

What I tried but failed: 1. pass roslaunch object to state via userdata. Errors, I think userdata may not support object, but only primitive types as int, double, string, etc. 2. pass uuid of roslaunch, use uuid to start and shutdown the roslaunch in different states (which is shown above). No error, but not working.

I am aware that I can put one node in launch file and mark it required:=true to allow closing node == closing launch file, or pass launch object as another argument of State class, such as in __init__(self, DanceInvitation.launch_face_tracking). However, as learning smach, I wish how smach handle it gracefully with userdata.

Many thanks!