transitioning to multiple states smach

asked 2021-06-30 11:56:26 -0500

dinesh gravatar image

updated 2021-06-30 11:58:30 -0500

How can i make a transitions to multiple states in executable_smach one being the same state from which the transitons takes place such that this initial state remains alive. I found concurrent state in smach but didn't found that this can transsits to the state from which the transits takes place. Here is the state machine i'm creating. Here the execute_mission state needs to be alive everytime even if it transits to execute_goal state such that other service request can cal this state for tasks like update_mission etc which i'm trying to add.

#!/usr/bin/env python3

import roslib; roslib.load_manifest('cognition')
import rospy
import smach
import smach_ros

from geometry_msgs.msg import PoseStamped, Pose, PoseArray
import time
import threading
from smach_ros import ServiceState
from cognition.srv import Mission, MissionResponse
from tts_ros.srv import *

import actionlib
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal


class ExecuteMission(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['execute_goal','failed','in_progress'], output_keys=['execute_mission_counter_out'])
        self.mutex = threading.Lock()
        self.mission = PoseArray()
        self.mission_id = 0

        rospy.Subscriber("mission", PoseArray, self.callback)

    def callback(self, data):
        self.mutex.acquire()
        rospy.loginfo('Got mission!')
        self.mission_id = 0
        self.mission.poses.clear()
        self.mission = data
        self.mutex.release()

    def execute(self, userdata):
        rospy.loginfo('Executing state ExecuteMission')
        if self.mission_id < len(self.mission.poses):
            self.mutex.acquire()
            userdata.execute_mission_counter_out = self.mission.poses[self.mission_id]
            self.mission_id += 1
            self.mutex.release()
            return 'execute_goal' 
        return 'in_progress'


class ExecuteGoal(smach.State):
    def __init__(self):      
        smach.State.__init__(self, outcomes=['succeeded','failed','in_progress'], input_keys=['execute_goal_counter_in'])
        self.client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)

    def execute(self, userdata):
        rospy.loginfo('Executing state ExecuteMission')
        print('Sending goal \n', userdata.execute_goal_counter_in)
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()
        self.client.wait_for_server()

        # Creates a goal to send to the action server.
        goal.target_pose.pose = userdata.execute_goal_counter_in
        time.sleep(5)
        # Sends the goal to the action server.
        self.client.send_goal(goal)

        # Waits for the server to finish performing the action.
        self.client.wait_for_result()
        print('result:', self.client.get_result() )
        # Prints out the result of executing the action
        return 'succeeded'


def main():
    rospy.init_node('smach_example_state_machine')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['Failed', 'execute_goal', 'preempted'])
    sm.userdata.pose = Pose()

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('ExecuteMission', ExecuteMission(), 
                               transitions={'execute_goal':'ExecuteGoal', 'failed':'Failed', 'in_progress':'ExecuteMission'},
                               remapping={'execute_mission_counter_out':'pose'})

        smach.StateMachine.add('ExecuteGoal', ExecuteGoal(), 
                               transitions={'succeeded':'SpeakText', 'failed':'Failed', 'in_progress':'ExecuteGoal'},
                               remapping={'execute_goal_counter_in':'pose'})

        smach.StateMachine.add('SpeakText',
                            ServiceState('/tts_service',
                                            TTS,
                                            request = TTSRequest('Hello sir, would you like to have a bottle of watter?')),
                            transitions={'succeeded':'ExecuteMission', 'aborted':'Failed'})
    # Execute SMACH plan
    outcome = sm.execute()



if __name__ == '__main__':
    main()
edit retag flag offensive close merge delete