transitioning to multiple states smach
How can i make a transitions to multiple states in executablesmach 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 executemission state needs to be alive everytime even if it transits to executegoal state such that other service request can cal this state for tasks like updatemission 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()
Asked by dinesh on 2021-06-30 11:56:26 UTC
Comments