def __init__(self):
# Get ROS PARAMS
self.node = rospy.init_node('nav_state_machine')
self.alclient = SimpleActionClient('tshirt_size', TShirtDispenseAction)
self.waypoints = waypoints
self.sm = None # smach.StateMachine()
self._define_state_mahcine()
self.asw = smach_ros.ActionServerWrapper(
'tshirt_size', TShirtDispenseAction,
wrapped_container=self.sm,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'],
goal_key='size',
result_key='result'
)
# Run the server in a background thread
self.asw.run_server()
self.alclient.wait_for_server()
rospy.spin()
def main():
rospy.init_node('robust_controller_server')
# Construct master state machine
sm = smach.StateMachine(outcomes=[
'arrived', 'arrived_with_trouble', 'aborted_home', 'aborted_stuck',
'preempted'
],
input_keys=['kuser_goal'],
output_keys=['kuser_result'])
#Pose parameters
HOME_POSE = [0, 0, 0] #this should be in the map coordinates
NEAR_POSE = [-1, 0, 0] #this is in the local coordinates
#initialize pose for testing
"""sm.userdata.kuser_goal = PoseStamped() #input pose
sm.userdata.kuser_goal.header.frame_id = "map"
sm.userdata.kuser_goal.header.stamp = rospy.get_rostime()
sm.userdata.kuser_goal.pose.position.x = 1
sm.userdata.kuser_goal.pose.position.y = -1
sm.userdata.kuser_goal.pose.orientation.w = 1"""
#initialize userdata
sm.userdata.kPrevPose = PoseStamped() #previous 'good' pose
sm.userdata.kRecCounter = 0 #counter for number of recovery attempts
sm.userdata.kHomePose = HOME_POSE
sm.userdata.kuser_result = RobustControllerResult()
""" Result values: 0 - default/not started, 1-arrived, 2-arrived with trouble,
3-aborted_home, 4-aborted_stuck, 5-preempted """
# Add states to the master SM
with sm:
#Main move state to try to reach the goal
smach.StateMachine.add('USER_GOAL',
UserGoalState(),
transitions={
'outcome_movebase_arrived': 'arrived',
'outcome_movebase_arrived_trouble':
'arrived_with_trouble',
'outcome_movebase_failed': 'RECOVERY_SM',
'outcome_movebase_failed_too_many':
'GO_HOME'
},
remapping={
'sPose': 'kuser_goal',
'sRecCounter_in': 'kRecCounter',
'sRecCounter_out': 'kRecCounter',
'sPrevPose': 'kPrevPose',
'sResult': 'kuser_result'
})
#state to try and return home if recovery has failed
smach.StateMachine.add(
'GO_HOME',
mb_states.MoveBaseStateWithListandResult(frame='map'),
transitions={
'succeeded': 'aborted_home',
'aborted': 'aborted_stuck'
},
remapping={
'xyt': 'kHomePose',
'sResult': 'kuser_result',
'sRecCounter_out': 'kRecCounter'
})
#Create a sub-state machine for recovery
sm_recovery = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['kPrevPose'])
#Initialize recovery userdata
sm_recovery.userdata.kNearPose = NEAR_POSE
with sm_recovery:
#add state to set goal behind robot
smach.StateMachine.add(
'GOAL_NEAR',
mb_states.MoveBaseStateWithList(frame='base_link'),
transitions={
'succeeded': 'succeeded',
'aborted': 'REVERT_POSE'
},
remapping={'xyt': 'kNearPose'})
#add state to revert to previous pose
smach.StateMachine.add('REVERT_POSE',
mb_states.MoveBaseStateWithPose(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
},
remapping={'pose': 'kPrevPose'})
#set up recovery state machine to be connected just like a state
smach.StateMachine.add('RECOVERY_SM',
sm_recovery,
transitions={
'succeeded': 'USER_GOAL',
'aborted': 'GO_HOME'
},
remapping={'kPrevPose': 'kPrevPose'})
# Construct action server wrapper
asw = smach_ros.ActionServerWrapper('robust_controller_server',
RobustControllerAction,
sm,
['arrived', 'arrived_with_trouble'],
['aborted_home', 'aborted_stuck'],
['preempted'],
goal_key='kuser_goal',
result_key='kuser_result')
# Run the server in a background thread
asw.run_server()
# Wait for control-c
rospy.spin()
smach.StateMachine.add('RelaxArmAndStop',
smach_ros.ServiceState('servos/relax_all',
arbotix_srvs.Relax),
transitions={
'succeeded': 'stop',
'preempted': 'stop',
'aborted': 'error'
})
# Construct action server wrapper for top-level sm to control it with keyboard commands
asw = smach_ros.ActionServerWrapper('user_commands_action_server',
thorp_msgs.UserCommandAction,
wrapped_container=sm,
succeeded_outcomes=['stop'],
aborted_outcomes=['aborted'],
preempted_outcomes=['error'],
goal_key='user_command',
feedback_key='ucmd_progress',
result_key='ucmd_outcome')
# Run the server in a background thread
asw.run_server()
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('object_manipulation', sm, '/SM_ROOT')
sis.start()
# Wait for control-c
rospy.spin()
smach.StateMachine.add('INDIVIDUAL_RUN',
individual_machine,
remapping={
'fis_array': 'fis_array',
'cost': 'lm_cost'
},
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
land_machine_action = smach_ros.ActionServerWrapper(
server_name='land_machine_action',
action_spec=DoLandMachineAction,
wrapped_container=land_machine,
succeeded_outcomes=['succeeded'],
preempted_outcomes=['preempted'],
aborted_outcomes=['aborted'],
goal_key='fis_array',
result_key='lm_cost')
smach_introspect = smach_ros.IntrospectionServer('individual_viewer',
land_machine, 'INDIVIDUAL')
smach_introspect.start()
land_machine_action.run_server()
rospy.loginfo('land_machine_action: online')
rospy.spin()
def main():
'''rospy.Subscriber("/recognizer_1/output", String, handleRecognizerMessage, queue_size=1)'''
rospy.init_node('interpreter')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['success', 'aborted', 'preempted'],
input_keys=['goal'],
output_keys=['result'])
with sm:
# Add states to the container
smach.StateMachine.add('Idle',
Idle(),
transitions={
'Sarah': 'WaitingCommand',
'Stop': 'Idle'
},
remapping={
'Idle_lastWord_in': 'lastWord',
'Idle_lastState_in': 'lastState',
'Idle_lastWord_out': 'lastWord',
'Idle_lastState_out': 'lastState'
})
smach.StateMachine.add('WaitingCommand',
WaitingCommand(),
transitions={
'Stop': 'Idle',
'DoIt': 'DoSomething',
'Sarah': 'WaitingCommand',
'Command': 'WaitingConfirmation',
'Timeout': 'Idle'
},
remapping={
'WComm_lastWord_in': 'lastWord',
'WComm_lastState_in': 'lastState',
'WComm_lastWord_out': 'lastWord',
'WComm_lastState_out': 'lastState',
'WComm_lastCommand_out': 'lastCommand'
})
smach.StateMachine.add('WaitingConfirmation',
WaitingConfirmation(),
transitions={
'Timeout': 'Idle',
'Yes': 'Idle',
'No': 'Idle',
'Stop': 'Idle',
'Sarah': 'WaitingCommand'
},
remapping={
'WConf_lastWord_in': 'lastWord',
'WConf_lastState_in': 'lastState',
'WConf_lastWord_out': 'lastWord',
'WConf_lastState_out': 'lastState'
})
smach.StateMachine.add('DoSomething',
DoSomething(),
transitions={
'Done': 'success',
'Fail': 'aborted'
},
remapping={
'DSome_lastWord_in': 'lastWord',
'DSome_lastState_in': 'lastState',
'DSome_lastCommand_in': 'lastCommand',
'DSome_lastWord_out': 'lastWord',
'DSome_lastState_out': 'lastState',
'DSome_result_out': 'result'
})
# Construct action server wrapper
asw = smach_ros.ActionServerWrapper('SaraComm',
CommAction,
wrapped_container=sm,
goal_key='goal',
result_key='result',
succeeded_outcomes=['success'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'])
'''sis = smach_ros.IntrospectionServer('server_name', asw.wrapped_container, '/ASW_ROOT')'''
# Create a thread to execute the smach container
'''asw_thread = threading.Thread(target=asw.run_server);
asw_thread.start()'''
'''asw_thread.join()'''
asw.run_server()
rospy.spin()
# Request the container to preempt
sm.request_preempt()
goal_slots=['target_type', 'named_target']),
remapping={
'target_type': 'named_pose_target_type',
'named_target': 'arm_folded_named_pose'
},
transitions={
'succeeded': 'quit',
'preempted': 'quit',
'aborted': 'error'
})
# Construct action server wrapper
asw = smach_ros.ActionServerWrapper('user_commands_action_server',
UserCommandAction,
wrapped_container=sm,
succeeded_outcomes=['quit'],
aborted_outcomes=['aborted'],
preempted_outcomes=['error'],
goal_key='user_command',
result_key='ucmd_outcome')
# Run the server in a background thread
asw.run_server()
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('object_manipulation', sm, '/SM_ROOT')
sis.start()
# Wait for control-c
rospy.spin()
# Actionlib interface to the task controller for Hubo valve-turning task. #
# #
#############################################################################
import roslib
roslib.load_manifest('task_controller')
import rospy
import smach
import smach_ros
from hubo_controller import *
from task_controller.msg import *
if __name__ == '__main__':
rospy.init_node("hubo_valve_task_action_controller")
controller = HuboController()
# Make an actionserver out of the controller
actionserver = smach_ros.ActionServerWrapper(
"hubo_valve_action",
HuboValveAction,
wrapped_container=controller.sm,
succeeded_outcomes=['DONE'],
aborted_outcomes=['FAILED'],
preempted_outcomes=['SAFE'],
goal_key='sm_input',
result_key='sm_output')
# Run server
actionserver.run_server()
# Spin
rospy.spin()