'low battery': 'Go to Charging Station',
'dead battery': 'Dead Robot'
})
# If the robot runs out of power, send for the graduate student.
smach.StateMachine.add('Dead Robot',
Recovery(),
transitions={'failure': 'failure'})
# For each of the rooms in the list, make a navigation state for it, and link it to the next room in the
# list on success. On failure, we'll just try again.
for i in range(len(rooms) - 1):
smach.StateMachine.add(rooms[i],
GoTo(rooms[i]),
transitions={
'success': rooms[i + 1],
'failure': rooms[i],
'low battery': 'Go to Charging Station',
'dead battery': 'Dead Robot'
})
# Start up an introspection server, so that we can look at the state machine in smach_viewer.
sis = smach_ros.IntrospectionServer('room_checker_machine', state_machine,
'/STATE_MACHINE_ROOT')
sis.start()
# Start the state machine.
final_outcome = state_machine.execute()
# We're going to put in a spin() just to keep the node alive, so we can look at the state machine structure.
rospy.spin()
def main():
print("first")
rospy.init_node('smach_state_machine')
global arm_publisher
arm_publisher = rospy.Publisher('vizzyArmRoutines/command',Int16,queue_size=100)
global index_pub
index_pub = rospy.Publisher('gaze_index',Int16,queue_size=100)
global list_of_points
list_of_points =[]
list_of_points.append(point(13,18,0.685,0.727))
list_of_points.append(point(14.63,-4.6,-0.99,0.018))
#list_of_points.append(point(14.63,-4.6,-0.99,0.018))
intent_sub = rospy.Subscriber('continuos_intent_detector_win',intent_msg_all,callback)
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['No_one_detected', 'person_detected','following','lost_person_in_follow','reach_person','fail_speak','speaked','fail_doing_gesture',
'succeed_doing_gesture','detecting_gesture','gesture_detected','fail_to_detect_gesture','fail_Reach_the_point','point_reached'])
#variable about the person being detected
sm.userdata.id_person_detected = -1
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('STOP', Stop(),
transitions={'continue_stop':'STOP',
'start_roaming':'ROAMING'},
remapping={'Stop_id_in':'id_person_detected',
'Stop_id_out':'id_person_detected'})
smach.StateMachine.add('ROAMING', Roaming(),
transitions={'No_one_detected':'ROAMING',
'person_detected':'FOLLOW'},
remapping={'Roaming_id_in':'id_person_detected',
'Roaming_id_out':'id_person_detected'})
smach.StateMachine.add('FOLLOW', Follow(),
transitions={'following':'FOLLOW',
'lost_person_in_follow':'ROAMING',
'reach_person':'SPEAK'},
remapping={'Follow_id_in':'id_person_detected',
'Follow_id_out':'id_person_detected'})
smach.StateMachine.add('SPEAK', Speak(),
transitions={'fail_speak':'ROAMING',
'speaked':'DO_GESTURE'},
remapping={'Speak_id_in':'id_person_detected',
'Speak_id_out':'id_person_detected'})
smach.StateMachine.add('DO_GESTURE', Do_gesture(),
transitions={'fail_doing_gesture':'ROAMING',
'succeed_doing_gesture':'DETECT_GESTURE'},
remapping={'Do_gesture_id_in':'id_person_detected',
'Do_gesture_id_out':'id_person_detected'})
smach.StateMachine.add('DETECT_GESTURE', Detect_gesture(),
transitions={'detecting_gesture':'DETECT_GESTURE',
'gesture_detected':'GO_TO_POINT',
'fail_to_detect_gesture':'ROAMING'},
remapping={'Detect_gesture_id_in':'id_person_detected',
'Detect_gesture_id_out':'id_person_detected'})
smach.StateMachine.add('GO_TO_POINT', Go_to_point(),
transitions={'fail_Reach_the_point':'ROAMING',
'point_reached':'ROAMING'},
remapping={'Go_point_id_in':'id_person_detected',
'Go_point_id_out':'id_person_detected'})
#to visualization
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('planning_coordinator')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['OVERALL_FAILURE', 'OVERALL_PREEMPT'])
# Open the container
with sm:
# upload intrinsic knowledge
smach.StateMachine.add(
'UPLOAD_INTRINSIC_KNOWLEDGE',
send_event('/upload_knowledge/event_in', 'e_trigger'),
transitions={'success': 'LOOK_FOR_UNFINISHED_GOALS'})
# wait for upload intrinsic knowledge succes response TODO!! at the moment this is open loop
#refbox communication
#smach.StateMachine.add('GET_TASK', refbox.get_task(),
# transitions={'task_received':'TRIGGER_REFBOX',
# 'wrong_task':'GET_TASK',
# 'wrong_task_format':'GET_TASK'})
#smach.StateMachine.add('TRIGGER_REFBOX', send_event('/refbox_parser/event_in', 'e_trigger'),
# transitions={'success':'LOOK_FOR_UNFINISHED_GOALS'})
# send even in to look for goals component to process and see if there are unfinished goals on the knowledge base
smach.StateMachine.add(
'LOOK_FOR_UNFINISHED_GOALS',
send_event('/knowledge_base_analyzer/pending_goals/event_in',
'e_start'),
transitions={'success': 'WAIT_FOR_LOOK_FOR_UNFINISHED_GOALS'
}) #WAIT_FOR_LOOK_FOR_UNFINISHED_GOALS
# wait for new goals query response
smach.StateMachine.add(
'WAIT_FOR_LOOK_FOR_UNFINISHED_GOALS',
wait_for_event(
'/knowledge_base_analyzer/pending_goals/event_out',
custom_event_msg=['e_goals_available', 'e_no_goals']),
transitions={
'success': 'GENERATE_PDDL_PROBLEM',
'failure': 'RE_ADD_GOALS',
'timeout': 'RE_ADD_GOALS',
'preempted': 'OVERALL_PREEMPT'
})
smach.StateMachine.add('RE_ADD_GOALS',
re_add_goals(),
transitions={
'success': 'LOOK_FOR_UNFINISHED_GOALS',
'failure': 'LOOK_FOR_UNFINISHED_GOALS'
})
# generate problem.pddl from knowledge base snapshot
smach.StateMachine.add(
'GENERATE_PDDL_PROBLEM',
send_event('/task_planning/pddl_problem_generator_node/event_in',
'e_trigger'),
transitions={'success': 'WAIT_FOR_GENERATE_PDDL_PROBLEM'})
# wait for pddl problem creator output
smach.StateMachine.add(
'WAIT_FOR_GENERATE_PDDL_PROBLEM',
wait_for_event(
'/task_planning/pddl_problem_generator_node/event_out'),
transitions={
'success': 'MAKE_PLAN', #MAKE_PLAN
'failure': 'OVERALL_FAILURE',
'timeout': 'GENERATE_PDDL_PROBLEM',
'preempted': 'OVERALL_PREEMPT'
})
# make plan
smach.StateMachine.add(
'MAKE_PLAN',
send_event('/mcr_task_planning/mercury_planner/event_in',
'e_trigger'),
transitions={'success': 'WAIT_FOR_MAKE_PLAN'})
# wait for planner to solve the planning problem
smach.StateMachine.add(
'WAIT_FOR_MAKE_PLAN',
wait_for_event('/mcr_task_planning/mercury_planner/event_out',
response_timeout=5.,
custom_event_msg=['e_success', 'e_failure']),
transitions={
'success': 'ANALYZE_PLAN_SUCCESS',
'failure': 'OVERALL_PREEMPT',
'timeout': 'WAIT_FOR_MAKE_PLAN',
'preempted': 'OVERALL_PREEMPT'
})
# analize plan success
smach.StateMachine.add(
'ANALYZE_PLAN_SUCCESS',
send_event('/task_planning/plan_success_analizer/event_in',
'e_start'),
transitions={'success': 'WAIT_FOR_ANALYZE_PLAN_SUCCESS'
}) #WAIT_FOR_ANALIZE_PLAN_SUCCESS
# wait for new goals query response
smach.StateMachine.add(
'WAIT_FOR_ANALYZE_PLAN_SUCCESS',
wait_for_event('/task_planning/plan_success_analizer/event_out'),
transitions={
'success': 'EXECUTE_PLAN',
'failure': 'LOOK_FOR_UNFINISHED_GOALS',
'timeout': 'ANALYZE_PLAN_SUCCESS',
'preempted': 'OVERALL_PREEMPT'
})
smach.StateMachine.add(
'EXECUTE_PLAN',
execute_plan(os.getenv("HOME") + '/.ros/mercury.plan'),
transitions={
'success': 'LOOK_FOR_UNFINISHED_GOALS',
'failure': 'LOOK_FOR_UNFINISHED_GOALS',
'preempted': 'OVERALL_PREEMPT'
})
# smach viewer
sis = smach_ros.IntrospectionServer('planning_coordinator_viewer', sm,
'/PLANNING_COORDINATOR_SM')
sis.start()
# Execute SMACH plan
#outcome = sm.execute()
# Create a thread to execute the smach container
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
# Wait for ctrl-c
rospy.spin()
rospy.logwarn("ctrl + c detected!!! preempting smach execution")
# Request the container to preempt
sm.request_preempt()
# Block until everything is preempted
# (you could do something more complicated to get the execution outcome if you want it)
smach_thread.join()
def main():
"""RSDK Inverse Kinematics Pick and Place Example
A Pick and Place example using the Rethink Inverse Kinematics
Service which returns the joint angles a requested Cartesian Pose.
This ROS Service client is used to request both pick and place
poses in the /base frame of the robot.
Note: This is a highly scripted and tuned demo. The object location
is "known" and movement is done completely open loop. It is expected
behavior that Baxter will eventually mis-pick or drop the block. You
can improve on this demo by adding perception and feedback to close
the loop.
"""
rospy.init_node("pick_n_place_client")
#rospy.on_shutdown(shutdown)
#rospy.wait_for_message("/robot/sim/started", Empty)
#ipdb.set_trace()
sm = smach.StateMachine(outcomes=['Done'])
sm.userdata.sm_pick_object_pose = Pose()
sm.userdata.sm_place_object_pose = Pose()
sm.userdata.sm_hover_distance = 0.15
with sm:
smach.StateMachine.add('Go_to_Start_Position',Go_to_Start_Position(),
transitions={'Succeed':'Setting_Start_and_End_Pose'})
smach.StateMachine.add('Setting_Start_and_End_Pose',Setting_Start_and_End_Pose(),
transitions={'Succeed':'Go_to_Pick_Hover_Position'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'place_object_pose':'sm_place_object_pose'})
smach.StateMachine.add('Go_to_Pick_Hover_Position',Go_to_Pick_Hover_Position(),
transitions={'Succeed':'Pick_Object',
'IK_Fail':'Go_to_Start_Position',
'Time_Out':'Go_to_Start_Position'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('Pick_Object',Pick_Object(),
transitions={'Succeed':'Go_to_Place_Hover_Position',
'IK_Fail':'Go_to_Start_Position',
'Time_Out':'Go_to_Start_Position'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('Go_to_Place_Hover_Position',Go_to_Place_Hover_Position(),
transitions={'Succeed':'Place_Object',
'IK_Fail':'Go_to_Start_Position',
'Time_Out':'Go_to_Start_Position'},
remapping={'place_object_pose':'sm_place_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('Place_Object',Place_Object(),
transitions={'Succeed':'Done',
'IK_Fail':'Go_to_Start_Position',
'Time_Out':'Go_to_Start_Position'},
remapping={'place_object_pose':'sm_place_object_pose',
'hover_distance':'sm_hover_distance'})
sis = smach_ros.IntrospectionServer('MY_SERVER', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
hmm_state_switch_client(0)
rospy.spin()
def main():
rospy.init_node("preemption_example")
# 配置第1个并行容器
foo_concurrence = smach.Concurrence(outcomes=['foo_reset', 'foo_done'],
default_outcome='foo_reset',
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with foo_concurrence:
smach.Concurrence.add('SPEEDUP_FH', foo())
smach.Concurrence.add(
'CHECK_FH', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb))
# 配置第2个并行容器
foo_concurrence2 = smach.Concurrence(outcomes=['foo_reset', 'foo_done'],
default_outcome='foo_reset',
child_termination_cb=child_term_cb2,
outcome_cb=out_cb2)
with foo_concurrence2:
smach.Concurrence.add('SPEEDUP_PF', foo())
smach.Concurrence.add(
'CHECK_PF', smach_ros.MonitorState("/sm_reset2", Empty,
monitor_cb))
# 配置状态机
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
motor_client = ControlReapAction()
motor_client.action_goal.goal.dishwasher_id = 40
motor_client.action_goal.goal.target_speed = 1500
smach.StateMachine.add('MOTOR40',
SimpleActionState(
'control_reap',
ControlReapAction,
goal=motor_client.action_goal.goal),
transitions={
'succeeded': 'MOTOR39',
'preempted': 'MOTOR40',
'aborted': 'MOTOR40'
})
motor_client = ControlReapAction()
motor_client.action_goal.goal.dishwasher_id = 39
motor_client.action_goal.goal.target_speed = 1500
smach.StateMachine.add('MOTOR39',
SimpleActionState(
'control_reap',
ControlReapAction,
goal=motor_client.action_goal.goal),
transitions={
'succeeded': 'MOTOR38',
'preempted': 'MOTOR39',
'aborted': 'MOTOR39'
})
motor_client = ControlReapAction()
motor_client.action_goal.goal.dishwasher_id = 38
motor_client.action_goal.goal.target_speed = 1500
smach.StateMachine.add('MOTOR38',
SimpleActionState(
'control_reap',
ControlReapAction,
goal=motor_client.action_goal.goal),
transitions={
'succeeded': 'MOTOR37',
'preempted': 'MOTOR38',
'aborted': 'MOTOR38'
})
motor_client = ControlReapAction()
motor_client.action_goal.goal.dishwasher_id = 37
motor_client.action_goal.goal.target_speed = 1500
smach.StateMachine.add('MOTOR37',
SimpleActionState(
'control_reap',
ControlReapAction,
goal=motor_client.action_goal.goal),
transitions={
'succeeded': 'TRY_SPEEDUP_FH',
'preempted': 'MOTOR37',
'aborted': 'MOTOR37'
})
# 第一个节点为普通节点，调用init节点函数，若节点函数返回值为init_done则转移到下一个节点FH
smach.StateMachine.add('TRY_SPEEDUP_FH',
init(),
transitions={'init_done': 'FH'})
# 第二个节点为并行节点，调用foo_concurrence节点函数，同样定义状态转移规则
smach.StateMachine.add('FH',
foo_concurrence,
transitions={
'foo_reset': 'TRY_AGAIN_FH',
'foo_done': 'TRY_SPEEDUP_PF'
})
smach.StateMachine.add('TRY_AGAIN_FH',
foo(),
transitions={
'foo_succeeded': 'FH',
'preempted': 'TRY_SPEEDUP_FH'
})
smach.StateMachine.add('TRY_SPEEDUP_PF',
init(),
transitions={'init_done': 'PF'})
smach.StateMachine.add('PF',
foo_concurrence2,
transitions={
'foo_reset': 'TRY_AGAIN_PF',
'foo_done': 'DONE'
})
smach.StateMachine.add('TRY_AGAIN_PF',
foo(),
transitions={
'foo_succeeded': 'PF',
'preempted': 'TRY_SPEEDUP_PF'
})
# 插入内部状态监控器
sis = smach_ros.IntrospectionServer('smach_server', sm, '/INPUT_COMMAND')
# 运行状态机
sis.start()
sm.execute()
rospy.spin()
sis.stop()
smach.StateMachine.add('ANNOUNCE',
AnnounceFoundObjects(),
transitions={
'announced': 'SELECT_GOAL',
'not_announced': 'SELECT_GOAL',
'failed': 'failed'
})
class SM(smach.StateMachine):
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['ended'])
with self:
smach.StateMachine.add('STATE',
Explore(),
transitions={
'finished': 'ended',
'failed': 'ended'
})
if __name__ == '__main__':
rospy.init_node('Explore')
sm = SM()
sis = smach_ros.IntrospectionServer('SM', sm, 'SM')
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('state_manager')
sub_cam = rospy.Subscriber('camera_info', camera_msg, clbk_cam)
# Control odometery
sub_odom = rospy.Subscriber('odom', Odometry, clbk_odom)
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['container_interface'])
with sm:
# Add states to the container
smach.StateMachine.add('SLEEP',
MIRO_Sleep(),
transitions={'normal_command': 'NORMAL'})
###
smach.StateMachine.add('NORMAL',
MIRO_Normal(),
transitions={
'sleep_command': 'SLEEP',
'play_command': 'PLAY',
'n_track_command': 'N_TRACK'
})
smach.StateMachine.add('N_TRACK',
N_Track(),
transitions={'normal_command': 'NORMAL'})
###
smach.StateMachine.add('PLAY',
MIRO_Play(),
transitions={
'normal_command': 'NORMAL',
'find_command': 'FIND'
})
smach.StateMachine.add('FIND',
MIRO_Find(),
transitions={
'play_command': 'PLAY',
'f_track_command': 'F_TRACK'
})
smach.StateMachine.add('F_TRACK',
F_Track(),
transitions={
'find_command': 'FIND',
'play_command': 'PLAY'
})
# Create and start the introspection server for visualization
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
cv2.destroyAllWindows()
sis.stop()
def execute(self, ud):
self._active = True
while not rospy.is_shutdown() and not self._tic_received:
self._ros_rate.sleep()
self._active = False
self._tic_received = False
return 'success'
def _listener_cb(self, msg):
if self._active:
self._tic_received = True
if __name__ == '__main__':
rospy.init_node('test_fsm', anonymous=True)
top = StateMachine(outcomes=['success'])
with top:
StateMachine.add('TIC_A', Tic(), transitions={'success':'TIC_B'})
StateMachine.add('TIC_B', Tic(), transitions={'success':'TIC_C'})
StateMachine.add('TIC_C', Tic(), transitions={'success':'TIC_A'})
sis = smach_ros.IntrospectionServer(str(rospy.get_name()), top, '/SM_ROOT' + str(rospy.get_name()))
sis.start()
top.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_actionlib')
sm = smach.StateMachine(outcomes=['success', 'aborted', 'preempted'])
with sm:
''' general '''
sm.userdata.true = True
sm.userdata.false = False
''' table poses '''
sm.userdata.pose_table_a = geometry_msgs.PoseStamped()
sm.userdata.pose_table_a.header.stamp = rospy.Time.now()
sm.userdata.pose_table_a.header.frame_id = "map"
sm.userdata.pose_table_a.pose.position.x = 2.0
sm.userdata.pose_table_a.pose.position.y = 5.0
sm.userdata.pose_table_a.pose.orientation.x = 0.0
sm.userdata.pose_table_a.pose.orientation.y = 0.0
sm.userdata.pose_table_a.pose.orientation.z = 0.851
sm.userdata.pose_table_a.pose.orientation.w = 0.526
sm.userdata.pose_table_b = geometry_msgs.PoseStamped()
sm.userdata.pose_table_b.header = sm.userdata.pose_table_a.header
sm.userdata.pose_table_b.pose.position.x = 2.0
sm.userdata.pose_table_b.pose.position.y = 1.0
sm.userdata.pose_table_b.pose.orientation.x = 0.0
sm.userdata.pose_table_b.pose.orientation.y = 0.0
sm.userdata.pose_table_b.pose.orientation.z = -0.509
sm.userdata.pose_table_b.pose.orientation.w = 0.861
''' Korus base pose '''
sm.userdata.base_position = geometry_msgs.PoseStamped()
smach.StateMachine.add('Idle',
smach_ros.MonitorState("/sm_start", Empty,
monitor_cb),
transitions={
'invalid': 'aborted',
'valid': 'MoveToTableA',
'preempted': 'preempted'
})
smach.StateMachine.add('MoveToTableA',
smach_ros.SimpleActionState(
'move_base',
move_base_msgs.MoveBaseAction,
goal_slots=['target_pose'],
result_slots=[]),
remapping={
'target_pose': 'pose_table_a',
'base_position': 'base_position'
},
transitions={
'succeeded': 'MoveToTableB',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('MoveToTableB',
smach_ros.SimpleActionState(
'move_base',
move_base_msgs.MoveBaseAction,
goal_slots=['target_pose'],
result_slots=[]),
remapping={
'target_pose': 'pose_table_b',
'base_position': 'base_position'
},
transitions={
'succeeded': 'MoveToTableA',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
rospy.signal_shutdown('All done.')
def configure(self):
sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys = ['action_feedback'], output_keys = ['action_feedback'])
sis = smach_ros.IntrospectionServer('logistic_scenario_app', sm0, '/logistic_scenario_app_sm')
sis.start()
with sm0:
#smach.StateMachine.add('MoveHomePTP', smach_ros.SimpleActionState('/MovePTP', MoveLinAction, self.MoveHomePTP_goal), {
# "succeeded":"MoveBaseHome",
#})
smach.StateMachine.add('MoveBaseHome', smach_ros.SimpleActionState('/move_base', MoveBaseAction, self.MoveBaseHome_goal), {
"succeeded":"MoveBaseShelf",
})
smach.StateMachine.add('MoveBaseShelf', smach_ros.SimpleActionState('/move_base', MoveBaseAction, self.MoveBaseShelf_goal), {
"succeeded":"MoveToNeutralFrontGrasp",
})
smach.StateMachine.add('DetectObjectsShelf', smach_ros.SimpleActionState('/cob_marker/object_detection', DetectObjectsAction, self.DetectObjectsShelf_goal), {
"succeeded":"MoveObjectPTP",
})
smach.StateMachine.add('MoveObjectPTP', smach_ros.SimpleActionState('/MovePTP', MoveLinAction, self.MoveObjectPTP_goal), {
"succeeded":"MoveLinGrasp",
})
smach.StateMachine.add('MoveLinGrasp', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinGrasp_goal), {
"succeeded":"MoveLinGraspUp",
})
smach.StateMachine.add('MoveLinGraspUp', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinGraspUp_goal), {
"succeeded":"MoveLinGraspBack",
})
smach.StateMachine.add('MoveLinGraspBack', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinGraspBack_goal), {
"succeeded":"MoveToRobotDeck",
})
smach.StateMachine.add('MoveToRobotDeck', smach_ros.SimpleActionState('/arm_controller/follow_joint_trajectory/', FollowJointTrajectoryAction, self.MoveToRobotDeck_goal), {
"succeeded":"MoveBaseDeliver",
})
smach.StateMachine.add('MoveBaseDeliver', smach_ros.SimpleActionState('/move_base', MoveBaseAction, self.MoveBaseDeliver_goal), {
"succeeded":"GetFromRobotDeck",
})
smach.StateMachine.add('GetFromRobotDeck', smach_ros.SimpleActionState('/arm_controller/follow_joint_trajectory/', FollowJointTrajectoryAction, self.GetFromRobotDeck_goal), {
"succeeded":"MoveToNeutralFrontDeliver",
})
smach.StateMachine.add('DetectObjectsDeliver', smach_ros.SimpleActionState('/cob_marker/object_detection', DetectObjectsAction, self.DetectObjectsDeliver_goal), {
"succeeded":"MovePTPDeliver",
})
smach.StateMachine.add('MovePTPDeliver', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MovePTPDeliver_goal), {
"succeeded":"MoveLinDeliver",
})
smach.StateMachine.add('MoveLinDeliverBack', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinDeliverBack_goal), {
"succeeded":"MoveToHome",
})
smach.StateMachine.add('MoveLinDeliverDown', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinDeliverDown_goal), {
"succeeded":"MoveLinDeliverBack",
})
smach.StateMachine.add('MoveLinDeliver', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinDeliver_goal), {
"succeeded":"MoveLinDeliverDown",
})
smach.StateMachine.add('MoveBaseHomeEnd', smach_ros.SimpleActionState('/move_base', MoveBaseAction, self.MoveBaseHomeEnd_goal), {
"succeeded":"succeeded",
})
smach.StateMachine.add('MoveToNeutralFrontGrasp', smach_ros.SimpleActionState('/arm_controller/follow_joint_trajectory/', FollowJointTrajectoryAction, self.MoveToNeutralFrontGrasp_goal), {
"succeeded":"DetectObjectsShelf",
})
smach.StateMachine.add('MoveToNeutralFrontDeliver', smach_ros.SimpleActionState('/arm_controller/follow_joint_trajectory/', FollowJointTrajectoryAction, self.MoveToNeutralFrontDeliver_goal), {
"succeeded":"DetectObjectsDeliver",
})
smach.StateMachine.add('MoveLinPreNeutral', smach_ros.SimpleActionState('/MoveLin', MoveLinAction, self.MoveLinPreNeutral_goal), {
"succeeded":"MoveToRobotDeck",
})
smach.StateMachine.add('MoveToHome', smach_ros.SimpleActionState('/arm_controller/follow_joint_trajectory/', FollowJointTrajectoryAction, self.MoveToHome_goal), {
"succeeded":"MoveBaseHomeEnd",
})
# Execute
outcome = sm0.execute()
# protected region configureCode on begin #
# protected region configureCode end #
pass
transitions={'succeeded':'PLACE MOTION','aborted':'aborted'})
StateMachine.add('PLACE MOTION', MANIPULATE(0.6, 0.0, z - 0.4), \
transitions={'succeeded':'PLACE','aborted':'aborted'})
StateMachine.add('PLACE', PLACE('box3'),\
transitions={'succeeded':'succeeded','aborted':'aborted'})
scenario_play = StateMachine(outcomes=['succeeded', 'aborted'])
with scenario_play:
StateMachine.add('GO TO SHELF', go_to_shelf,\
transitions={'succeeded':'ADD OBJECTS','aborted':'aborted'})
StateMachine.add('ADD OBJECTS', UPDATE_OBJECTS('add'),\
transitions={'succeeded':'PICK and PLACE 1','aborted':'aborted'})
StateMachine.add('PICK and PLACE 1', pick_place_1,\
transitions={'succeeded':'PICK and PLACE 2','aborted':'aborted'})
StateMachine.add('PICK and PLACE 2', pick_place_2,\
transitions={'succeeded':'PICK and PLACE 3','aborted':'aborted'})
StateMachine.add('PICK and PLACE 3', pick_place_3,\
transitions={'succeeded':'INITIALIZE','aborted':'aborted'})
StateMachine.add('INITIALIZE', INIT_POSE(),\
transitions={'succeeded':'REMOVE OBJECTS','aborted':'aborted'})
StateMachine.add('REMOVE OBJECTS', UPDATE_OBJECTS('remove'),\
transitions={'succeeded':'GO TO START POINT','aborted':'aborted'})
StateMachine.add('GO TO START POINT', go_to_start_point,\
transitions={'succeeded':'GO TO SHELF','aborted':'aborted'})
sis = smach_ros.IntrospectionServer('server_name', scenario_play,
'/SEED-Noid-Mover Scenario Play')
sis.start()
scenario_play.execute()
sis.stop()
def main():
rospy.init_node('perceive_location_server')
sleep_time = rospy.get_param('~sleep_time', 1.0)
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['perceive_location_goal'],
output_keys=['perceive_location_feedback', 'perceive_location_result'])
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'GET_GOAL',
getGoal(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'})
#transitions={'succeeded':'START_OBJECT_LIST_MERGER'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'SUBSCRIBE_TO_POINT_CLOUD'})
smach.StateMachine.add(
'SUBSCRIBE_TO_POINT_CLOUD',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/arm_cam3d/depth_registered/points')]),
transitions={'success': 'START_OBJECT_LIST_MERGER'})
smach.StateMachine.add(
'START_OBJECT_LIST_MERGER',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/object_list_merger/event_in', 'e_start'),
('/mcr_perception/object_selector/event_in', 'e_start')
],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'LOOK_AT_WORKSPACE',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# send arm to a position in which the depth camera can see the platformsmach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_LEFT'),
smach.StateMachine.add('LOOK_AT_WORKSPACE',
gms.move_arm('look_at_workspace_from_far'),
transitions={
'succeeded': 'START_SEGMENTATION',
'failed': 'LOOK_AT_WORKSPACE'
})
# this is new scene segmentation pipeline combined with object detection
smach.StateMachine.add(
'START_SEGMENTATION',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/scene_segmentation/event_in',
'e_start')],
event_out_list=[
('/mcr_perception/scene_segmentation/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'WAIT_LEFT',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('WAIT_LEFT',
wait_for(sleep_time=sleep_time),
transitions={'success': 'ADD_POINT_CLOUD'})
smach.StateMachine.add(
'ADD_POINT_CLOUD',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/scene_segmentation/event_in',
'e_add_cloud_start')],
event_out_list=[
('/mcr_perception/scene_segmentation/event_out',
'e_add_cloud_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'RECOGNIZE_OBJECTS',
'timeout': 'ADD_POINT_CLOUD',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# in the new scene_segmentation pipeline, object detector can be called by string msg e_segment
smach.StateMachine.add(
'RECOGNIZE_OBJECTS',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_perception/multimodal_object_identification/input/event_in',
'e_trigger'),
('/mcr_perception/scene_segmentation/event_in', 'e_segment')],
event_out_list=
[('/mcr_perception/scene_segmentation/event_out', 'e_done',
True),
('/mcr_perception/multimodal_object_identification/output/event_out',
'e_done', True)],
timeout_duration=7),
transitions={
'success': 'STOP_PUBLISH_ACCUMULATED_PC',
'timeout': 'STOP_PUBLISH_ACCUMULATED_PC',
'failure': 'STOP_PUBLISH_ACCUMULATED_PC'
})
smach.StateMachine.add(
'STOP_PUBLISH_ACCUMULATED_PC',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/scene_segmentation/event_in',
'e_stop')],
event_out_list=[
('/mcr_perception/scene_segmentation/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'STOP_COMPONENTS',
'timeout': 'STOP_COMPONENTS',
'failure': 'STOP_COMPONENTS'
})
smach.StateMachine.add(
'STOP_COMPONENTS',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/object_list_merger/event_in', 'e_stop'),
('/mcr_perception/scene_segmentation/event_in', 'e_stop')
],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_stopped', True),
('/mcr_perception/scene_segmentation/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'UNSUBSCRIBE_FROM_POINT_CLOUD',
'timeout': 'UNSUBSCRIBE_FROM_POINT_CLOUD',
'failure': 'UNSUBSCRIBE_FROM_POINT_CLOUD'
})
smach.StateMachine.add(
'UNSUBSCRIBE_FROM_POINT_CLOUD',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/empty_topic')]),
transitions={'success': 'PUBLISH_MERGED_OBJECT_LIST'})
smach.StateMachine.add(
'PUBLISH_MERGED_OBJECT_LIST',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_trigger')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out', 'e_done',
True)
],
timeout_duration=5),
transitions={
'success': 'SET_ACTION_LIB_SUCCESS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_SUCCESS'
})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('perceive_location_smach_viewer', sm,
'/PERCEIVE_LOCATION_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_location_server',
action_spec=PerceiveLocationAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='perceive_location_goal',
feedback_key='perceive_location_feedback',
result_key='perceive_location_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
with sm:
smach.StateMachine.add('SPIN_RIGHT', SpinRight(genome['center_drive_thresh']), transitions={ 'spin_right':'SPIN_RIGHT',
'drive_forward':'DRIVE_FORWARD','failed':'failed'
})
smach.StateMachine.add('SPIN_LEFT', SpinLeft(genome['center_drive_thresh']), transitions={ 'spin_left':'SPIN_LEFT',
'drive_forward':'DRIVE_FORWARD','failed':'failed'
})
smach.StateMachine.add('DRIVE_FORWARD', DriveForward(genome['center_stop_thresh'],genome['center_spin_thresh']), transitions={ 'spin_right':'SPIN_RIGHT',
'drive_forward':'DRIVE_FORWARD',
'spin_left':'SPIN_LEFT',
'within_threshold':'STOP',
'failed':'failed'
})
smach.StateMachine.add('STOP', Stop(genome['stopping_thresh']), transitions={ 'succeeded':'succeeded', 'spin_right':'SPIN_RIGHT', 'failed':'failed'})
sis = smach_ros.IntrospectionServer('test_sm',sm,'/SM_ROOT')
sis.start()
outcome = sm.execute()
current_time = getWorldProp().sim_time
print("Current Time: "+str(current_time))
if outcome == 'succeeded':
print(scan.getLeftCenterRightScanState())
else:
print("Robot failed to find the cylinder in time.")
resetWorld()
resetSimulation()
def main():
global cur_pos, twist_pub, client
rospy.init_node('moving')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['finished'])
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
sm.userdata.sm_counter = 0
rospy.Subscriber("/joy", Joy, joy_callback)
rospy.Subscriber("/ar_pose_marker", AlvarMarkers, ar_callback)
rospy.Subscriber("/odom", Odometry, odom_callback)
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('move_in',
move_in(),
transitions={
'moving': 'move_in',
'arrived': 'rotate_check'
})
smach.StateMachine.add('rotate_check',
rotate_check(),
transitions={
'found': 'rough_close',
'not_found': 'rotate_check',
'finished': 'finished'
})
smach.StateMachine.add('rough_close',
rough_close(),
transitions={
'moving': 'rough_close',
'arrived': 'moving_towards'
})
smach.StateMachine.add('moving_towards',
moving_towards(),
transitions={
'moving': 'moving_towards',
'arrived': 'docking',
'rotating': 'rotate_check'
})
smach.StateMachine.add('docking',
docking(),
transitions={'finished': 'go_back'})
smach.StateMachine.add('go_back',
go_back(),
transitions={'moving': 'rotate_check'})
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
'not centered': 'CENTER',
'lost': 'SEARCH_FOR_POSTS'
})
smach.StateMachine.add('BLIND_FORWARD',
blind_movement.move_forward(
self.time, self.speed),
transitions={'success': 'success'})
if __name__ == '__main__':
# To see debug messages add log_level=rospy.DEBUG argument to init_node
rospy.init_node('ai')
while rospy.get_time() == 0:
continue
sm = smach.StateMachine(outcomes=['success'])
with sm:
smach.StateMachine.add('START_SWITCH',
start_switch(),
transitions={'success': 'NAV_TASK'})
smach.StateMachine.add('NAV_TASK',
nav_channel(),
transitions={'success': 'success'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
outcomes = sm.execute()
sis.stop()
def main():
rospy.init_node('agv_task_smach')
coordinate = dict(rospy.get_param("~Waypoints"))
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['done', 'failed'])
sm_top.userdata.task = list()
with sm_top:
smach.StateMachine.add('General_Selection', general_selection_state(),
transitions={'Navigation':'Navigation','Docking':'Docking', 'Parking':'Parking', 'Idle':'Idle'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
# Create Navigation Container
sm_navigation = smach.StateMachine(outcomes=['Nav_State_1', 'Nav_State_2'],
input_keys=['task'],
output_keys=['task', 'current_task'])
with sm_navigation:
smach.StateMachine.add('Navigation_Waypoint', wy_states(coordinate),
transitions={'succeeded':'Nav_State_1','aborted':'Nav_State_2'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
smach.StateMachine.add('Navigation', sm_navigation,
transitions={'Nav_State_1':'General_Selection','Nav_State_2':'Crash'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
# Create Docking Container
sm_docking = smach.StateMachine(outcomes=['Docking_State_1', 'Docking_State_2'],
input_keys=['task'],
output_keys=['task', 'current_task'])
with sm_docking:
smach.StateMachine.add('Docking_Waypoint', docking_states(coordinate),
transitions={'succeeded':'Docking_State_1','aborted':'Docking_State_2'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
smach.StateMachine.add('Docking', sm_docking,
transitions={'Docking_State_1':'General_Selection','Docking_State_2':'Crash'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
# Create Parking Container
sm_parking = smach.StateMachine(outcomes=['Park_State_1', 'Park_State_2'],
input_keys=['task'],
output_keys=['task', 'current_task'])
with sm_parking:
smach.StateMachine.add('Parking_Waypoint', parking_states(coordinate),
transitions={'succeeded':'Park_State_1','aborted':'Park_State_2'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
smach.StateMachine.add('Parking', sm_parking,
transitions={'Park_State_1':'General_Selection','Park_State_2':'Crash'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
# Create Idle Container
sm_idle = smach.StateMachine(outcomes=['Idle_State_1', 'Idle_State_2'],
input_keys=['task'],
output_keys=['task', 'current_task'])
with sm_idle:
smach.StateMachine.add('Idle_Standby', idle_standby_state(),
transitions={'succeeded':'Idle_State_1', 'aborted':'Idle_State_2'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
smach.StateMachine.add('Idle', sm_idle,
transitions={'Idle_State_1':'General_Selection','Idle_State_2':'Crash'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
# Create Crash Container
sm_crash = smach.StateMachine(outcomes=['Crash_State_1', 'Crash_State_2'],
input_keys=['task'],
output_keys=['task', 'current_task'])
with sm_crash:
smach.StateMachine.add('Crash_Repair', crash_repair_state(),
transitions={'succeeded':'Crash_State_1', 'aborted':'Crash_State_2'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
smach.StateMachine.add('Crash', sm_crash,
transitions={'Crash_State_1':'General_Selection', 'Crash_State_2':'failed'},
remapping={ 'task_input':'task',
'task_output':'task',
'current_task_output':'current_task'})
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_TASK_SMACH')
sis.start()
sm_top.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('buoy_task_state_machine')
sm = smach.StateMachine(outcomes=['buoy_task_complete'])
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
buoy_flat_topic = {
'x': '/buoy_flat_x',
'y': '/buoy_flat_y',
'area': '/buoy_flat_area'
}
buoy_triangle_topic = {
'x': '/buoy_triangle_x',
'y': '/buoy_triangle_y',
'area': '/buoy_triangle_area'
}
TOUCH_FLAT_TIMER = 1000 # time required (in ticks) to touch the flat buoy
MOVE_BACK_1_TIMER = 700 # time required (in ticks) to move back, away from flat buoy
MOVE_FORWARD_TIMER = 2000 # time required (in ticks) to move past the flat buoy
TOUCH_TRIANGLE_TIMER = 1000 # time required (in ticks) to touch the triangle buoy
MOVE_BACK_2_TIMER = 1400 # time required (in ticks) to move back, away from triangle buoy
BUOY_ABOVE_DEPTH = -36 # Adjusts sub to move 3 ft up
BUOY_BELOW_DEPTH = 36 # Adjusts sub to move 3 ft down
TORPEDO_BOARD_DEPTH = 36 # Adjusts sub to move 3 ft down
DEPTH_VARIANCE = 2 # 2 inch
YAW_BUOY_BACK = 3.14 # the yaw (in radians) to make sub turn 180 degrees to face back buoys
YAW_TORPEDO_TASK = 0.5 # the yaw (in radians) to face the torpedo task
YAW_VARIANCE = 0.1 # in radians
with sm:
smach.StateMachine.add('IDLE',
StartState(),
transitions={
'ready': 'TRACK_FLAT',
'notready': 'IDLE'
})
smach.StateMachine.add('TRACK_FLAT',
TrackObjectState(buoy_flat_topic, 0),
transitions={
'done': 'TOUCH_FLAT',
'notdone': 'TRACK_FLAT',
'reset': 'RESET'
})
smach.StateMachine.add('TOUCH_FLAT',
MoveForwardState(TOUCH_FLAT_TIMER, True),
transitions={
'done': 'MOVE_BACK_1',
'notdone': 'TOUCH_FLAT',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_BACK_1',
MoveForwardState(MOVE_BACK_1_TIMER, False),
transitions={
'done': 'MOVE_UP',
'notdone': 'MOVE_BACK_1',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_UP',
ChangeDepthState(BUOY_ABOVE_DEPTH,
DEPTH_VARIANCE),
transitions={
'done': 'MOVE_FORWARD',
'notdone': 'MOVE_UP',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_FORWARD',
MoveForwardState(MOVE_FORWARD_TIMER, True),
transitions={
'done': 'MOVE_DOWN',
'notdone': 'MOVE_FORWARD',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_DOWN',
ChangeDepthState(BUOY_BELOW_DEPTH,
DEPTH_VARIANCE),
transitions={
'done': 'TURN_AROUND',
'notdone': 'MOVE_DOWN',
'reset': 'RESET'
})
smach.StateMachine.add('TURN_AROUND',
RotateYawState(YAW_BUOY_BACK, YAW_VARIANCE),
transitions={
'done': 'TRACK_TRIANGLE',
'notdone': 'TURN_AROUND',
'reset': 'RESET'
})
smach.StateMachine.add('TRACK_TRIANGLE',
TrackObjectState(buoy_triangle_topic, 0),
transitions={
'done': 'TOUCH_TRIANGLE',
'notdone': 'TRACK_TRIANGLE',
'reset': 'RESET'
})
smach.StateMachine.add('TOUCH_TRIANGLE',
MoveForwardState(TOUCH_TRIANGLE_TIMER, True),
transitions={
'done': 'MOVE_BACK_2',
'notdone': 'TOUCH_TRIANGLE',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_BACK_2',
MoveForwardState(MOVE_BACK_2_TIMER, False),
transitions={
'done': 'FACE_TORPEDO_TASK',
'notdone': 'MOVE_BACK_2',
'reset': 'RESET'
})
smach.StateMachine.add('FACE_TORPEDO_TASK',
RotateYawState(YAW_TORPEDO_TASK, YAW_VARIANCE),
transitions={
'done': 'MOVE_TORPEDO_DEPTH',
'notdone': 'FACE_TORPEDO_TASK',
'reset': 'RESET'
})
smach.StateMachine.add('MOVE_TORPEDO_DEPTH',
ChangeDepthState(TORPEDO_BOARD_DEPTH,
DEPTH_VARIANCE),
transitions={
'done': 'COMPLETED',
'notdone': 'MOVE_TORPEDO_DEPTH',
'reset': 'RESET'
})
smach.StateMachine.add('COMPLETED',
CompletedState('/buoy_task_complete'),
transitions={'done': 'IDLE'})
smach.StateMachine.add('RESET',
ResetState(),
transitions={
'done': 'IDLE',
'notdone': 'RESET'
})
outcome = sm.execute()
rospy.spin()
sis.stop()
def CreateStateMachine():
#Create the state machine
sm_rover = smach.StateMachine(outcomes=['DEAD'])
#Open the container
with sm_rover:
smach.StateMachine.add('INITIALISE',
INITIALISE(),
transitions={
'SUCCESS': 'READY',
'REPEAT': 'INITIALISE',
'FAIL': 'ERROR'
})
smach.StateMachine.add('READY',
READY(),
transitions={
'TO_GPS': 'REACH_GPS',
'FAIL': 'ERROR',
'REPEAT': 'READY'
})
smach.StateMachine.add('REACH_GPS',
REACH_GPS(),
transitions={
'SUCCESS': 'FIND_ARTAG',
'SUCCES12': 'DEINITIALISE',
'IMAGE_INTERRUPT': 'REACH_ARTAG',
'FAIL': 'ERROR',
'REPEAT': 'REACH_GPS'
})
smach.StateMachine.add('FIND_ARTAG',
FIND_ARTAG(),
transitions={
'SUCCESS': 'REACH_ARTAG',
'GO_APPROACH': 'APPROACH',
'FAIL': 'ERROR',
'REPEAT': 'FIND_ARTAG'
})
smach.StateMachine.add('APPROACH',
APPROACH(),
transitions={
'SUCCESS': 'PASS_GATE',
'FAIL': 'ERROR',
'REPEAT': 'APPROACH'
})
smach.StateMachine.add('PASS_GATE',
PASS_GATE(),
transitions={
'SUCCESS': 'DEINITIALISE',
'FAIL': 'ERROR',
'REPEAT_APPROACH': 'APPROACH'
})
smach.StateMachine.add('REACH_ARTAG',
REACH_ARTAG(),
transitions={
'SUCCESS': 'DEINITIALISE',
'FAIL': 'ERROR',
'REPEAT': 'REACH_ARTAG',
'BACK': 'FIND_ARTAG'
})
smach.StateMachine.add('DEINITIALISE',
DEINITIALISE(),
transitions={
'SUCCESS': 'INITIALISE',
'FAIL': 'ERROR',
'REPEAT': 'DEINITIALISE'
})
smach.StateMachine.add('ERROR',
ERROR(),
transitions={
'KILL': 'DEAD',
'REPEAT': 'ERROR',
'NOT_BAD': 'DEINITIALISE'
})
#Codes for smach viewer
sis = smach_ros.IntrospectionServer('rover20_state_machine', sm_rover,
'/ROVER_SM_ROOT')
sis.start()
outcome = sm_rover.execute()
sis.stop()
def main():
rospy.init_node('attention_bot')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['stop'])
sm.userdata.tool_id = -1
sm.userdata.joint_nums = [0, 5]
sm.userdata.PID_Final = [0, 0, 0]
sm.userdata.PID_Initial = [640, 5, 50]
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
rospy.sleep(5)
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('IDLE',
Idle(),
transitions={'gotToolInput': 'FINDTOOL'})
smach.StateMachine.add('FINDTOOL',
FindTool(),
transitions={
'foundTool': 'IK1',
'notfoundTool': 'IDLE'
})
smach.StateMachine.add('IK1',
IK1(),
transitions={
'noIK': 'stop',
'foundIK': 'GRAB'
})
# smach.StateMachine.add('MOVE', Move(),
# transitions={'reached':'GRAB'})
smach.StateMachine.add('GRAB',
Grab(),
transitions={
'grabSuccess': 'MOVEHOME2',
'grabFailure': 'FINDTOOL'
})
smach.StateMachine.add('MOVEHOME2',
MoveHome2(),
transitions={'reached': 'ATTENTIONSEEKER'})
# smach.StateMachine.add('ORIENTCAMERA', OrientCamera(),
# transitions={'reached':'ATTENTIONSEEKER'})
smach.StateMachine.add('ATTENTIONSEEKER',
FindAttention(),
transitions={'giveTool': 'IK2'})
smach.StateMachine.add('IK2',
IK2(),
transitions={
'foundIK': 'CHANGEPID',
'noIK': 'stop'
})
# smach.StateMachine.add('MOVEGIVE', MoveGive(),
# transitions={'reached':'CHANGEPID'})
smach.StateMachine.add('CHANGEPID',
ChangePID(),
transitions={
'changed': 'OPENGRIPPER',
'notchanged': 'CHANGEPID'
},
remapping={'PID': 'PID_Final'})
smach.StateMachine.add('OPENGRIPPER',
OpenGripper(),
transitions={'opened': 'CHANGEBACKPID'})
smach.StateMachine.add('CHANGEBACKPID',
ChangePID(),
transitions={
'changed': 'IDLE',
'notchanged': 'CHANGEPID'
},
remapping={'PID': 'PID_Initial'})
# Execute SMACH plan
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('adl')
rospy.loginfo('starting ADL...')
## ROS params
global uav_id, autopilot, acept_radio, rgb_images_on, thermal_images_on, stop_distance, record_telemetry
uav_id = rospy.get_param('~uav_id', 'uav_1')
autopilot = rospy.get_param('~autopilot', 'dji')
acept_radio = rospy.get_param('acept_radio', 1.2)
rgb_images_on = rospy.get_param('~rgb_images_on', False)
thermal_images_on = rospy.get_param('~thermal_images_on', False)
record_telemetry = rospy.get_param('~record_telemetry', True)
stop_distance = rospy.get_param('~stop_distance', 10)
# uav_id = rospy.get_param('uav_id', 1)
# acept_radio = rospy.get_param(rospy.search_param('acept_radio'), 1.2)
# rgb_images_on = rospy.get_param(rospy.search_param('rgb_images_on'), False)
# thermal_images_on = rospy.get_param(rospy.search_param('thermal_images_on'), False)
# rgb_images_interval = rospy.get_param('rgb_images_interval', 10.0)
# thermal_images_interval = rospy.get_param('thermal_images_interval', 10.0)
print ('uav_id', uav_id)
print ('acept_radio', acept_radio)
print ('thermal_images_on', thermal_images_on)
print ('rgb_images_on', rgb_images_on)
# Define ADL State publisher
adl_state_pub = rospy.Publisher('adl_state', String, queue_size=10)
# Define global position publisher
global_pos_publisher = rospy.Publisher('global_position', NavSatFix, queue_size=10)
battery_state_publisher = rospy.Publisher('battery_state', BatteryState, queue_size=1)
# Subscribe to gps position
def global_pos_cb(pose):
global current_gps_pos
current_gps_pos = pose
def battery_cb(msg):
global battery_state
battery_state = msg
if autopilot == 'dji':
global_pos_subscriber = rospy.Subscriber("dji_sdk/gps_position", NavSatFix, global_pos_cb, queue_size=1)
battery_subscriber = rospy.Subscriber("dji_sdk/battery_state", BatteryState, battery_cb, queue_size=1)
elif autopilot == 'mavros':
global_pos_subscriber = rospy.Subscriber("mavros/global_position/global", NavSatFix, global_pos_cb, queue_size=1)
# Subscribe to ual/pose topic
def local_pos_cb(pose):
global current_pos
current_pos.point = pose.pose.position
rospy.sleep(0.05)
global current_yaw
q = pose.pose.orientation
euler = euler_from_quaternion([q.x, q.y, q.z, q.w])
current_yaw = euler[2]
local_pos_subscriber = rospy.Subscriber("ual/pose", PoseStamped, local_pos_cb, queue_size=1)
# Subscribe to ual/velocity topic
def velocity_cb(velocity):
global current_vel
current_vel = velocity
rospy.sleep(0.05)
velocity_subscriber = rospy.Subscriber("ual/velocity", TwistStamped, velocity_cb, queue_size=1)
# Subscribe to ual/state topic
def ual_state_cb(state):
global ual_state
global last_ual_state
ual_state = state.state
if ual_state != last_ual_state:
rospy.loginfo('ADL: ual state: %s' %(ual_state))
last_ual_state = ual_state
ual_state_subscriber = rospy.Subscriber("ual/state", State, ual_state_cb, queue_size=1)
# Subscribe to flight status topic
def flight_status_cb(status):
global flight_status
flight_status = status.data
rospy.sleep(0.1)
flight_status_subscriber = rospy.Subscriber("dji_sdk/flight_status", UInt8, flight_status_cb, queue_size=1)
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=[])#'outcome4', 'outcome5'])
# Thread
def main_thread():
while not rospy.is_shutdown():
adl_state_pub.publish(adl_state)
global_pos_publisher.publish(current_gps_pos)
battery_state_publisher.publish(battery_state)
rospy.sleep(0.5)
t = threading.Thread(target=main_thread)
t.start()
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('STANDBY',Standby_State(),
transitions={'start_new_mission':'MISSION','to_paused_state':'PAUSE_STATE','download_files':'FILES_DOWNLOADING'})
## Create sub state machine for Mission
sm_mission = smach.StateMachine(outcomes=['end_without_downloading','download_files','mission_paused'])
# Open stop_mission server
stop_mission_server()
# Open the container
with sm_mission:
# Add states
smach.StateMachine.add('DELAY', Delay_State(), transitions={'end_delay':'TAKE_OFF','stop_mission':'LANDED'})
smach.StateMachine.add('TAKE_OFF', TakeOff_State(), transitions={'takeOff_finished':'GO_TO_WP_01','stop_mission':'LAND'}, remapping = {'wp_00':'wp_00'})
smach.StateMachine.add('GO_TO_WP_01', GoToWp01_State(), transitions={'at_WayPoint_01':'GO_TO_WP_1','stop_mission':'GO_TO_WP_00'})
smach.StateMachine.add('GO_TO_WP_1', GoToWp1_State(), transitions={'at_WayPoint_1':'SWEEP','stop_mission':'GO_TO_H_d'})
smach.StateMachine.add('SWEEP', Sweep_State(), transitions={'sweep_finished':'GO_TO_H_d','stop_mission':'SAVE_CURRENT_POSITION'}, remapping={'wayPoints_left':'wayPoints_left'})
smach.StateMachine.add('SAVE_CURRENT_POSITION',SaveCurrentPosition_State(),transitions={'current_position_saved':'GO_TO_H_d'}, remapping={'wayPoints_left':'wayPoints_left'})
smach.StateMachine.add('GO_TO_H_d', GoToHd_State(), transitions={'at_h_d':'GO_TO_WP_00'})
smach.StateMachine.add('GO_TO_WP_00', GoToWp00_State(), transitions={'at_WayPoint_00':'LAND'}, remapping = {'wp_00':'wp_00'})
smach.StateMachine.add('LAND', Land_State(), transitions={'landed':'LANDED'})
smach.StateMachine.add('LANDED', Landed_State(), transitions={'auto_download_on':'download_files','auto_download_off':'end_without_downloading','mission_paused':'mission_paused'})
# smach.StateMachine.add('', (), transitions={'':''})
smach.StateMachine.add('MISSION',sm_mission,
transitions={'download_files':'FILES_DOWNLOADING','end_without_downloading':'STANDBY','mission_paused':'PAUSE_STATE'},
remapping = {'H_d' : 'H_d', 'wayPoints_left' : 'wayPoints_left'})
# ## Create sub state machine for Pause Mission
smach.StateMachine.add('PAUSE_STATE', Pause_State(), transitions={'resume':'MISSION','cancel_mission':'STANDBY'})
# Create sub state machine for Files Downloading
sm_files_downloading = smach.StateMachine(outcomes=['download_finished'])
with sm_files_downloading:
# Add states
smach.StateMachine.add('WAITING_FOR_CONNECTION', WaitingForConnection_State(), transitions={'connection_successful':'DOWNLOAD'})
smach.StateMachine.add('DOWNLOAD', FilesDownload_State(), transitions={'download_finished':'download_finished'})
smach.StateMachine.add('FILES_DOWNLOADING', sm_files_downloading, transitions={'download_finished':'STANDBY'})
## loginfo
rospy.loginfo('ADL running')
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# while not rospy.is_shutdown():
# Wait for ctrl-c to stop the application
# rospy.spin()
sis.stop()
'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_msg.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()
rospy.loginfo("Stopping '%s' node...", rospy.get_name())
sis.stop()
rospy.signal_shutdown('All done.')
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
sis = smach_ros.IntrospectionServer('server1', sm, '/IEEE_OPEN')
sis.start()
sm.userdata.containersList = container_list
sm.userdata.containerColor = int(Colors.Unknown)
sm.userdata.containerPose = int(Positions.Unkown)
sm.userdata.robotPose = int(Positions.StartPosition)
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('goToFirstPose',
goToFirstPose(),
transitions={
'succeeded': 'strategyStep',
'aborted': 'failed'
},
remapping={'finalPose': 'robotPose'})
smach.StateMachine.add('recognizeContainers',
recognizeContainers(),
transitions={
'failedRecognize': 'failed',
'success': 'strategyStep'
},
remapping={'finalPose': 'robotPose'})
smach.StateMachine.add('strategyStep',
strategyStep(),
transitions={
'containersUnknown': 'recognizeContainers',
'pick': 'pickAndDrop',
'changePosition': 'changeIntersection',
'done': 'succeeded',
'failed': 'failed'
},
remapping={
'pose': 'robotPose',
'containersList': 'containersList',
'containerColor': 'containertColor',
'containerPose': 'containerPose',
})
smach.StateMachine.add('changeIntersection',
changeIntersection(),
transitions={
'succeeded': 'strategyStep',
'aborted': 'failed'
},
remapping={'robotPose': 'robotPose'})
pick_and_drop_sm = smach.StateMachine(outcomes=['succeeded', 'failed'],
input_keys=[
'robotPose',
'containersList',
'containerColor',
'containerPose'
],
output_keys=[
'robotPose',
'containersList',
'containerColor',
'containerPose'
])
sis2 = smach_ros.IntrospectionServer('server2', pick_and_drop_sm,
'/IEEE_OPEN/pickAndDrop')
sis2.start()
with pick_and_drop_sm:
smach.StateMachine.add(
'goToContainer',
goToContainer(),
transitions={
'succeeded': 'pickContainer',
'aborted': 'failed'
},
)
smach.StateMachine.add('pickContainer',
pickContainer(),
transitions={
'succeeded':
'goFromContainerToIntersection',
'preempted': 'failed',
'aborted': 'failed'
},
remapping={
'containersList': 'containersList',
'containerPose': 'containerPose',
})
smach.StateMachine.add(
'goFromContainerToIntersection',
goFromContainerToIntersection(),
transitions={
'succeeded': 'whereToGo',
'aborted': 'failed'
},
)
smach.StateMachine.add('whereToGo',
whereToGo(),
transitions={
'dock': 'goToDock',
'intersection': 'changeIntersection'
},
remapping={
'robotPose': 'robotPose',
'containerColor': 'containerColor'
})
smach.StateMachine.add(
'goToDock',
goToDock(),
transitions={
'succeeded': 'goFromDockToIntersection',
'aborted': 'failed'
},
remapping={'containerColor': 'containerColor'})
smach.StateMachine.add('changeIntersection',
changeIntersection(),
transitions={
'succeeded': 'goToDock',
'aborted': 'failed'
},
remapping={'robotPose': 'robotPose'})
smach.StateMachine.add(
'goFromDockToIntersection',
goFromDockToIntersection(),
transitions={
'succeeded': 'succeeded',
'aborted': 'failed'
},
remapping={'containerColor': 'containerColor'})
smach.StateMachine.add("pickAndDrop",
pick_and_drop_sm,
transitions={
'succeeded': 'strategyStep',
'failed': 'failed'
},
remapping={
'robotPose': 'robotPose',
'containersList': 'containersList',
'containerColor': 'containertColor',
'containerPose': 'containerPose',
})
outcome = sm.execute()
rospy.spin()
sis2.stop()
sis.stop()
def main():
rospy.init_node('tms_ts_smach_executive0')
sm_root = smach.StateMachine(['succeeded', 'aborted', 'preempted'])
with sm_root:
smach.StateMachine.add('move0',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(
9001, True, 2003, [7001])),
transitions={'succeeded': 'control0'})
smach.StateMachine.add('control0',
ServiceState('ts_state_control',
ts_state_control,
request=ts_state_controlRequest(
0, 0, 0, 0, "")),
transitions={
'succeeded': 'grasp1',
'aborted': 'aborted'
})
smach.StateMachine.add('grasp1',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(
9002, True, 2003, [7001])),
transitions={'succeeded': 'control1'})
smach.StateMachine.add('control1',
ServiceState('ts_state_control',
ts_state_control,
request=ts_state_controlRequest(
0, 0, 0, 0, "")),
transitions={
'succeeded': 'give2',
'aborted': 'aborted'
})
smach.StateMachine.add('give2',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(
9003, True, 2003, [1001])),
transitions={'succeeded': 'control2'})
smach.StateMachine.add('control2',
ServiceState('ts_state_control',
ts_state_control,
request=ts_state_controlRequest(
0, 0, 0, 0, "")),
transitions={
'succeeded': 'move3',
'aborted': 'aborted'
})
smach.StateMachine.add('move3',
ServiceState('rp_cmd',
rp_cmd,
request=rp_cmdRequest(
9001, True, 2003, [0])),
transitions={'succeeded': 'control3'})
smach.StateMachine.add('control3',
ServiceState('ts_state_control',
ts_state_control,
request=ts_state_controlRequest(
0, 0, 0, 0, "")),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
sis = smach_ros.IntrospectionServer('tms_ts_smach_test', sm_root,
'/ROS_TMS')
sis.start()
outcome = sm_root.execute()
rospy.spin()
sis.stop()
'out23': 'Z3_GRIPPING'})
# smach.StateMachine.add('Z3_GRIPPING', Gripping(), transitions={'out34': 'Z4_PLACING',
# 'out3f': 'finished'})
sm_Z3 = smach.StateMachine(outcomes=['out34', 'out35'])
with sm_Z3:
smach.StateMachine.add('Z31_XY_POSITIONING', xy_Positioning(), transitions={'3_out12': 'Z32_ORIENTATING'})
smach.StateMachine.add('Z32_ORIENTATING', Orientating(), transitions={'3_out23': 'Z33_Z_POSITIONING'})
smach.StateMachine.add('Z33_Z_POSITIONING', z_Positioning(), transitions={'3_out34': 'out34',
'3_out35': 'out35'})
smach.StateMachine.add('Z3_GRIPPING', sm_Z3, transitions={'out34': 'Z4_PLACING',
'out35': 'Z5_LIFTINGANDGO'})
# smach.StateMachine.add('Z4_PLACING', Placing(), transitions={'out42': 'Z2_MOVING'})
sm_Z4 = smach.StateMachine(outcomes=['out42'])
with sm_Z4:
smach.StateMachine.add('Z41_PLACING', Placing(), transitions={'4_out12': 'Z42_WITHDARWING'})
smach.StateMachine.add('Z42_WITHDARWING', Withdrawing(), transitions={'4_out2f': 'out42'})
smach.StateMachine.add('Z4_PLACING', sm_Z4, transitions={'out42': 'Z2_MOVING'})
smach.StateMachine.add('Z5_LIFTINGANDGO', LiftingandGo(), transitions={'out4f': 'finished'})
sis = smach_ros.IntrospectionServer('introspection_server', sm, '/Imagebased_Grasping_SM')
sis.start()
outcome = sm.execute()
try:
rospy.spin()
except KeyboardInterrupt:
print("Stop state machine!")
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome5'])
# Open the container
with sm_top:
smach.StateMachine.add('IDLE',
Idle(),
transitions={
'go_down': 'DOWN_SM',
'invalid_input': 'IDLE'
})
# Create the sub SMACH state machine
sm_down = smach.StateMachine(outcomes=['closed', 'aborted'])
# Open the container
with sm_down:
smach.StateMachine.add('CLEAR_WORKSPACE',
ClearWs(),
transitions={'ws_cleared': 'DOWN'})
smach.StateMachine.add('DOWN',
Down(),
transitions={'success': 'CLOSING'})
smach.StateMachine.add('CLOSING',
Closing(),
transitions={
'success': 'closed',
'failure': 'aborted'
})
smach.StateMachine.add('DOWN_SM',
sm_down,
transitions={
'closed': 'UP',
'aborted': 'UP'
})
smach.StateMachine.add('UP',
Up(),
transitions={
'success': 'PASS_BOX',
'success_no_box': 'BACK_HOME'
})
smach.StateMachine.add('PASS_BOX',
PassBox(),
transitions={'success': 'BACK_HOME'})
smach.StateMachine.add('BACK_HOME',
BackHome(),
transitions={'success': 'IDLE'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm_top, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main(testing_mode):
rospy.init_node('kuri_mbzirc_challenge3_state_machine')
start = float(str(time.time())) #rospy.get_time() # stamp should update
rospy.loginfo(">>>>>>>>>>>>>>>>>>>>>>>>>>>start time : %f", (start))
### MAIN
main_sm = smach.StateMachine(outcomes=[
'mission_accomplished', 'mission_incomplete', 'test_succeeded',
'test_failed'
])
with main_sm:
"""
#TODO testing modes are deleted (only choose normal run) --> later we will return them back
********************************************************************
* choose one of these modes: *
* 1- normalRun : runs the normal scenario *
* 2- testExplorer : test explorer states *
* 3- tastTaskAllocator : test task allocator *
* 4- testUAVWorkers : test the uav workers concurrent *
********************************************************************
"""
main_sm.userdata.testing_type = testing_mode
### exploration
exploration_sm = smach.StateMachine(
outcomes=['exp_failed', 'exp_preempted'],
input_keys=['exploration_sm_in'])
### detection and tracking
detection_sm = smach.StateMachine(
outcomes=['tracker_failed', 'tracker_preempted'])
### mapping
mapping_sm = smach.StateMachine(
outcomes=['mapping_failed', 'mapping_preempted'])
### task allocator
task_allocator_sm = smach.StateMachine(
outcomes=['tasksReady', 'allocationPreempted', 'allocationFailed'],
output_keys=['task_allocator_out1', 'task_allocator_out2'])
### Worker1 UAV
uav_worker1_sm = smach.StateMachine(
outcomes=[
'workerDone', 'objectFellFailure', 'uav1NavigationPreempted',
'uav1NavigationFailed'
],
input_keys=['uav_worker1_sm_in'],
)
### Worker2 UAV
uav_worker2_sm = smach.StateMachine(
outcomes=[
'workerDone', 'objectFellFailure', 'uav2NavigationPreempted',
'uav2NavigationFailed'
],
input_keys=['uav_worker2_sm_in'],
)
###state 1 in the main sm --> initialization
#TODO return back the testing modes ( it was removed since everything is running parallel)
smach.StateMachine.add(
'INITIALIZATION',
InitTestingMode(),
transitions={
'normalRun': 'STARTING'
#'testExplorer' : 'TEST_GENERATING_WAYPOINTS',
#'tastTaskAllocator' : 'TEST_TASK_ALLOCATOR',
#'testUAVWorkers' : 'TEST_UAV_WORKERS'
},
remapping={
'testing_type_in': 'testing_type',
'waypoint_generator_test_in': 'generate_waypoints_in',
'task_allocator_test_in': 'task_allocator_in',
'path_generator_test_in': 'tasks'
#'uav_worker1_test_in':'generating_navpaths_uav1_out',
#'uav_worker2_test_in':'generating_navpaths_uav2_out'
})
###state 2 in the main sm --> starting
smach.StateMachine.add('STARTING',
Starting(1),
transitions={
'waitingforGPS': 'STARTING',
'GPSFixed': 'GENERATING_WAYPOINTS'
},
remapping={'starting_out': 'uav_gps_loc'})
###state 3 in the main sm --> generating waypoints
#smach.StateMachine.add('GENERATING_WAYPOINTS', GenerateWaypoints(),
#transitions={
#'succeeded':'All_CC', #UAV_EXPLORER
#'aborted':'mission_incomplete',
#'preempted':'mission_incomplete'
#},
#remapping={
#'generate_waypoints_in':'uav_gps_loc',
#'generate_waypoints_out':'waypoints'
#}
#)
smach.StateMachine.add(
'GENERATING_WAYPOINTS',
ReadWaypoints(),
transitions={
'succeeded': 'All_CC', #UAV_EXPLORER
'aborted': 'mission_incomplete'
},
remapping={
'generate_waypoints_in': 'uav_gps_loc',
'generate_waypoints_out': 'waypoints'
})
###state 4 in the main sm --> concurruncy
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
explorer_allocator_workers_cc = smach.Concurrence(
outcomes=['comp_done', 'working'],
default_outcome='working',
child_termination_cb=main_cc_term_cb,
outcome_cb=main_cc_cb,
input_keys=['eaw_cc_in'])
with explorer_allocator_workers_cc:
smach.Concurrence.add('STATUS_CHECKING',
StatusChecking(rospy.get_time(), start))
smach.Concurrence.add('EXPLORE',
exploration_sm,
remapping={
'exploration_sm_in': 'eaw_cc_in',
})
smach.Concurrence.add('DETECT', detection_sm)
smach.Concurrence.add('MAP', mapping_sm)
#if (taskAllocFlag[0] == 'True'):
smach.Concurrence.add('TASKALLOCATION', task_allocator_sm)
#THE BIG CONCURRUNCY THAT YOU WILL EVER SEE IN YOUR LIFE >_<
smach.StateMachine.add('All_CC',
explorer_allocator_workers_cc,
transitions={
'comp_done': 'mission_accomplished',
'working': 'All_CC',
},
remapping={'eaw_cc_in': 'waypoints'})
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#TODO TO BE checked and replaced
# >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>remove me when you check concurrence (TO BE checked and replaced)
#for now, UAVFailed corresponds to object fail failure until we add other failures types
#uav_workers_cc = smach.Concurrence(outcomes=['workersFinished','uav1Failed','uav2Failed','uav1Preempted','uav2Preempted','uavsFailed','uavsPreempted'],
#default_outcome='workersFinished',
#outcome_cb = workers_cc_cb,
#input_keys=['uav_worker1_cc_in','uav_worker2_cc_in']
#)
##state 5 in the main sm--> workers concurruncy
#smach.StateMachine.add('UAV_WORKERS_CC', uav_workers_cc,
#transitions={
#'workersFinished':'mission_accomplished',
#'uav1Failed':'TASK_ALLOCATION',
#'uav2Failed':'TASK_ALLOCATION',
#'uavsFailed':'TASK_ALLOCATION',
#'uav1Preempted':'mission_incomplete',
#'uav2Preempted':'mission_incomplete',
#'uavsPreempted':'mission_incomplete'
#},
#remapping={
#'uav_worker1_cc_in':'uav1_task',
#'uav_worker2_cc_in':'uav2_task'
#}
#)
##*************************************
## Define the uav_workers concurruncy
#with uav_workers_cc:
#smach.Concurrence.add('UAV_Worker1', uav_worker1_sm,
#remapping={
#'uav_worker1_sm_in':'uav_worker1_cc_in'
#}
#)
#smach.Concurrence.add('UAV_Worker2', uav_worker2_sm,
#remapping={
#'uav_worker2_sm_in':'uav_worker2_cc_in'
#}
#)
## End uav_workers concurruncy
##************************************
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Define the exploreration machine
#TODO: assign namespaces for each physical drone
#TODO: confirm that after gps homming the local position is set to 0,0,0 (in real experiments)
with exploration_sm:
smach.StateMachine.add(
'EXPLORING',
Exploring('/uav_1/navigation_action_server1'),
transitions={
'succeeded': 'EXPLORING',
'aborted': 'exp_failed',
'preempted': 'exp_preempted'
},
remapping={
'navigation_task': 'exploration_sm_in',
})
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Define the detection machine
#TODO: add latch after the publish
#TODO: objects detector should return a list of objects and return an objects list even if it was one
with detection_sm:
smach.StateMachine.add('TRACKING',
DetectingObjects(),
transitions={
'succeeded': 'TRACKING',
'aborted': 'tracker_failed',
'preempted': 'tracker_preempted'
})
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Define the mapping machine
with mapping_sm:
smach.StateMachine.add('MAPPING',
Mapping(),
transitions={
'succeeded': 'MAPPING',
'aborted': 'mapping_failed',
'preempted': 'mapping_preempted'
})
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Define the task_allocator state machine
#TODO: it should genarate new tasks only if the workers are not performing a task
#TODO: it should be deployed in one system (centralized system, ground station) --> task allocator, path generator, and statemachine
with task_allocator_sm:
smach.StateMachine.add(
'ALLOCATING_TASKS',
AllocatingTasks(),
transitions={
'succeeded': 'GENERATING_NAVPATHS',
'aborted': 'allocationFailed',
'preempted': 'allocationPreempted'
},
remapping={'allocating_tasks_out': 'uavs_tasks'})
smach.StateMachine.add('GENERATING_NAVPATHS',
GeneratePaths(),
transitions={
'succeeded': 'tasksReady',
'preempted': 'allocationFailed',
'aborted': 'allocationPreempted'
},
remapping={
'tasks':
'uavs_tasks',
'generating_navpaths_uav1_out':
'task_allocator_out1',
'generating_navpaths_uav2_out':
'task_allocator_out2'
})
# End task_allocator state machine
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Define the uav_worker1 state machine
#TODO: landing : develop object picking controller (probably use butti object tracking)
#TODO: service call to check if the object is attached or not
#TODO: dropping : developing object dropping controller (butti drop box tracker)
with uav_worker1_sm:
#smach.StateMachine.add('LOOPING_NAVTASKS', NavTasksLoop(),
#transitions={
#'loopFinished':'workerDone',
#'looping':'NAVIGATING_2_OBJECT',
#},
#remapping={
#'looping_in':'uav_worker1_sm_in',
#'looping_out':'nav_task'
#}
#)
smach.StateMachine.add(
'NAVIGATING_2_OBJECT',
Navigating2Object('/uav_2/navigation_action_server2'),
transitions={
'succeeded': 'PICKING_OBJECT',
'preempted': 'uav1NavigationPreempted',
'aborted': 'uav1NavigationFailed'
},
remapping={'navigation_task': 'uav_worker1_sm_in'})
#TODO link the picking object to aerial manipulation action server [DONE]
smach.StateMachine.add('PICKING_OBJECT',
PickingObject(),
transitions={
'succeeded': 'NAVIGATING_2_DROPZONE',
'preempted': 'OBJECT_FELL',
'aborted': 'OBJECT_FELL'
})
smach.StateMachine.add('NAVIGATING_2_DROPZONE',
Navigating2DropZone(0.5),
transitions={
'navigating': 'NAVIGATING_2_DROPZONE',
'hovering': 'DROPPING_OBJECT'
})
smach.StateMachine.add('DROPPING_OBJECT',
DroppingObject(0.5),
transitions={
'dropping': 'DROPPING_OBJECT',
'dropped': 'workerDone',
'droppingFail': 'OBJECT_FELL'
})
smach.StateMachine.add('OBJECT_FELL',
ObjectFell(0.5),
transitions={
'canSee': 'PICKING_OBJECT',
'cannotSee': 'objectFellFailure'
})
# End uav_worker1 state machine
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Define the uav_worker2 state machine
#TODO: landing : develop object picking controller (probably use butti object tracking)
#TODO: service call to check if the object is attached or not
#TODO: dropping : developing object dropping controller (butti drop box tracker)
with uav_worker2_sm:
#smach.StateMachine.add('LOOPING_NAVTASKS', NavTasksLoop(),
#transitions={
#'loopFinished':'workerDone',
#'looping':'NAVIGATING_2_OBJECT',
#},
#remapping={
#'looping_in':'uav_worker2_sm_in',
#'looping_out':'nav_task'
#}
#)
smach.StateMachine.add(
'NAVIGATING_2_OBJECT',
Navigating2Object('/uav_3/navigation_action_server3'),
transitions={
'succeeded': 'PICKING_OBJECT',
'preempted': 'uav2NavigationPreempted',
'aborted': 'uav2NavigationFailed'
},
remapping={'navigation_task': 'uav_worker2_sm_in'})
#TODO link the picking object to aerial manipulation action server [DONE]
smach.StateMachine.add('PICKING_OBJECT',
PickingObject(),
transitions={
'succeeded': 'NAVIGATING_2_DROPZONE',
'preempted': 'OBJECT_FELL',
'aborted': 'OBJECT_FELL'
})
smach.StateMachine.add('NAVIGATING_2_DROPZONE',
Navigating2DropZone(0.5),
transitions={
'navigating': 'NAVIGATING_2_DROPZONE',
'hovering': 'DROPPING_OBJECT'
})
smach.StateMachine.add('DROPPING_OBJECT',
DroppingObject(0.5),
transitions={
'dropping': 'DROPPING_OBJECT',
'dropped': 'workerDone',
'droppingFail': 'OBJECT_FELL'
})
smach.StateMachine.add('OBJECT_FELL',
ObjectFell(0.5),
transitions={
'canSee': 'PICKING_OBJECT',
'cannotSee': 'objectFellFailure'
})
# End uav_worker2 state machine
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# Create the introspection server
sis = smach_ros.IntrospectionServer(
'kuri_mbzirc_challenge3_state_machine_viewer', main_sm,
'/kuri_mbzirc_challenge3_state_machine_root')
sis.start()
# Execute the main state machine
outcome = main_sm.execute()
rospy.spin()
sis.stop()
})
sm.add('sucker_secondary_check',
DoubleCheckObjectSucker(),
transitions={
'succeeded': 'drop_object_sucker',
'classification_failed': 'init_robot',
'failed': 'init_robot'
})
# Do Nothing
sm.add('stop_abort', StopAbort(), transitions={'loop': 'stop_abort'})
# Create and start the introspection server
# (smach_viewer is broken in indigo + 14.04, so need for that now)
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# run the state machine
# We start it in a separate thread so we can cleanly shut it down via CTRL-C
# by requesting preemption.
# The state machine normally runs until a terminal state is reached.
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
# sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
# request the state machine to preempt and wait for the SM thread to finish
sm.request_preempt()
def main():
rospy.init_node("cyborg_controller")
# Create emotions
emotion_system = EmotionSystem()
emotion_system.add_emotion(name="angry",
pleasure=-0.51,
arousal=0.59,
dominance=0.25)
emotion_system.add_emotion(name="bored",
pleasure=-0.65,
arousal=-0.62,
dominance=-0.33)
emotion_system.add_emotion(name="curious",
pleasure=0.22,
arousal=0.62,
dominance=-0.10)
emotion_system.add_emotion(name="dignified",
pleasure=0.55,
arousal=0.22,
dominance=0.61)
emotion_system.add_emotion(name="elated",
pleasure=0.50,
arousal=0.42,
dominance=0.23) # Happy
emotion_system.add_emotion(name="inhibited",
pleasure=-0.54,
arousal=-0.04,
dominance=-0.41) # Sadness
emotion_system.add_emotion(name="puzzled",
pleasure=-0.41,
arousal=0.48,
dominance=-0.33) # Suprized
emotion_system.add_emotion(name="loved",
pleasure=0.89,
arousal=0.54,
dominance=-0.18)
emotion_system.add_emotion(name="unconcerned",
pleasure=-0.13,
arousal=-0.41,
dominance=0.08)
homedir = os.path.expanduser("~")
path = homedir + "/controller.db"
# Fill database with default values
if (os.path.exists(path) == False):
event_cost = 0.45
database_handler = DatabaseHandler(filename=path)
database_handler.create()
database_handler.add_event(state="idle",
event="music_play",
reward_pleasure=0.08,
reward_arousal=0.01,
reward_dominance=-0.02,
event_cost=event_cost)
database_handler.add_event(state="idle",
event="navigation_emotional",
reward_pleasure=0.00,
reward_arousal=0.05,
reward_dominance=-0.01,
event_cost=event_cost)
database_handler.add_event(state="conversation",
event="weather_tell",
reward_pleasure=0.05,
reward_arousal=0.00,
reward_dominance=0.00,
event_cost=event_cost)
database_handler.add_event(state="conversation",
event="joke_tell",
reward_pleasure=0.05,
reward_arousal=0.02,
reward_dominance=0.00,
event_cost=event_cost)
database_handler.add_event(state="conversation",
event="selfie_take",
reward_pleasure=0.05,
reward_arousal=-0.02,
reward_dominance=0.01,
event_cost=event_cost * 1.5)
database_handler.add_event(state="conversation",
event="simon_says_play",
reward_pleasure=0.00,
reward_arousal=0.00,
reward_dominance=0.10,
event_cost=event_cost)
database_handler.add_event(state="conversation",
event="follower_follow",
reward_pleasure=0.00,
reward_arousal=0.05,
reward_dominance=-0.05,
event_cost=event_cost)
# Create motivator
motivator = Motivator(database_file=path)
motivator.start()
# Create a SMACH state machine
state_machine = smach.StateMachine(outcomes=["error"])
state_machine.userdata.state_machine_events = []
state_machine.userdata.last_state = "initializing"
state_machine.userdata.last_event = "start_up"
# Open the container
with state_machine:
# Remapp outputs, so userdata can be moved between states
state_machine_remapping = {
"input_events": "state_machine_events",
"output_events": "state_machine_events",
"previous_state": "last_state",
"current_state": "last_state",
"previous_event": "last_event",
"current_event": "last_event"
}
# Add states to the container
idle_transitions = {
"conversation_interest": "conversation",
"navigation_schedualer": "navigation_planing",
"navigation_emotional": "navigation_planing",
"aborted": "idle",
"navigation_command": "navigation_planing",
"music_play": "music"
}
idle_resources = {} # Idle does not require any resources
smach.StateMachine.add(label="idle",
state=Module(state_name="idle",
actionlib_name="cyborg_idle/idle",
transitions=idle_transitions,
resources=idle_resources),
transitions=idle_transitions,
remapping=state_machine_remapping)
conversation_transitions = {
"aborted": "idle",
"succeded": "idle",
"navigation_feedback": "navigation_talking",
"navigation_command": "navigation_talking",
"navigation_information": "navigation_talking",
"simon_says_play": "simon_says",
"selfie_take": "selfie",
"follower_follow": "follower",
"weather_tell": "weather",
"joke_tell": "joke"
}
conversation_resources = {"trollface": "cyborg_conversation"}
smach.StateMachine.add(
label="conversation",
state=Module(state_name="conversation",
actionlib_name="cyborg_conversation/conversation",
transitions=conversation_transitions,
resources=conversation_resources),
transitions=conversation_transitions,
remapping=state_machine_remapping)
navigation_transitions = {
"aborted": "navigation_talking",
"navigation_start_wandering": "navigation_moving",
"navigation_start_moving": "navigation_moving",
"succeded": "navigation_talking"
}
navigation_resources = {"base": "cyborg_navigation"}
smach.StateMachine.add(label="navigation_planing",
state=Module(
state_name="navigation_planing",
actionlib_name="cyborg_navigation/planing",
transitions=navigation_transitions,
resources=navigation_resources),
transitions=navigation_transitions,
remapping=state_machine_remapping)
navigation_transitions = {
"aborted": "navigation_talking",
"succeded": "navigation_talking",
"navigation_wandering_completed": "idle"
}
navigation_resources = {"base": "cyborg_navigation"}
smach.StateMachine.add(label="navigation_moving",
state=Module(
state_name="navigation_moving",
actionlib_name="cyborg_navigation/moving",
transitions=navigation_transitions,
resources=navigation_resources),
transitions=navigation_transitions,
remapping=state_machine_remapping)
navigation_transitions = {
"aborted": "idle",
"succeded": "idle",
"navigation_feedback_completed": "conversation",
"navigation_command": "navigation_planing"
}
navigation_resources = {
"base": "cyborg_navigation",
"trollface": "cyborg_navigation"
}
smach.StateMachine.add(label="navigation_talking",
state=Module(
state_name="navigation_talking",
actionlib_name="cyborg_navigation/talking",
transitions=navigation_transitions,
resources=navigation_resources),
transitions=navigation_transitions,
remapping=state_machine_remapping)
music_transitions = {
"aborted": "idle",
"succeded": "idle",
"conversation_interest": "conversation"
}
music_resources = {}
smach.StateMachine.add(label="music",
state=Module(
state_name="music",
actionlib_name="cyborg_music/music",
transitions=music_transitions,
resources=music_resources),
transitions=music_transitions,
remapping=state_machine_remapping)
smach.StateMachine.add(
label="simon_says",
state=Module(state_name="simon_says",
actionlib_name="cyborg_conversation/simon_says",
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
resources={"trollface": "cyborg_simon_says"}),
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
remapping=state_machine_remapping)
smach.StateMachine.add(label="selfie",
state=Module(
state_name="selfie",
actionlib_name="cyborg_conversation/selfie",
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
resources={"trollface": "cyborg_selfie"}),
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
remapping=state_machine_remapping)
smach.StateMachine.add(
label="follower",
state=Module(state_name="follower",
actionlib_name="cyborg_conversation/follower",
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
resources={"trollface": "cyborg_follower"}),
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
remapping=state_machine_remapping)
smach.StateMachine.add(
label="weather",
state=Module(state_name="weather",
actionlib_name="cyborg_conversation/weather",
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
resources={"trollface": "cyborg_weather"}),
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
remapping=state_machine_remapping)
smach.StateMachine.add(
label="joke", # name on state
state=Module(
state_name="joke", # name on state
actionlib_name="cyborg_conversation/joke", # actionlib name
transitions={
"aborted": "conversation",
"succeded": "conversation"
}, #event name:state - events that leads away from state
resources={"trollface": "cyborg_joke"
}), # for gatekeepers: resource_name:node_name
transitions={
"aborted": "conversation",
"succeded": "conversation"
},
remapping=state_machine_remapping)
################################################### ADD MORE STATES BELOW ###################################################
################################################### STOP ADDING YOUR STATES ###################################################
# Create a state machine monitorer
smm = StateMachineMonitor(state_machine, display_all=True)
rospy.loginfo("Controller: State Machine Monitor Activated...")
import smach_ros
sis = smach_ros.IntrospectionServer('controler_viewer', state_machine,
'/controler_viewer')
sis.start()
# Create a thread to execute the smach
smach_thread = threading.Thread(target=state_machine.execute)
smach_thread.daemon = True
smach_thread.start()
rospy.loginfo("Controller: SMACH activated...")
# Start ROS main looping
rospy.loginfo("Controller: Activated...")
rospy.spin()
sis.stop()
rospy.loginfo("Controller: Terminated...")
def main():
rospy.init_node('smach_example_state_machine')
# Create a Top-level state machine
sm = smach.StateMachine(outcomes=['end'])
# Set default values of userdata in Top-level State Machine
sm.userdata.startCommand = False
sm.userdata.object = 'coffee bag'
sm.userdata.navGoal = None
sm.userdata.armGoal = None
sm.userdata.tra = 'handOff'
sm.userdata.gripperCommand = 'Open'
sm.userdata.handOffSpeech = True
withR = True # Flag set true if the mobile base is used
# Open the container and Add States in Top-Level State Machine
with sm:
smach.StateMachine.add('Idle',
IdleState(),
transitions={
'idling': 'Idle',
'move': 'GoToPoint',
'use_arm': 'ExecTra',
'use_arm_sub': 'manip_sub',
'release_obj': 'ReleaseObject',
'done': 'end'
},
remapping={
'idleStartCommandIn': 'startCommand',
'idleNavGoalOut': 'navGoal',
'idleArmGoalOut': 'armGoal',
'idleArmGoalOut': 'tra'
})
smach.StateMachine.add('GoToPoint',
GoToPointState(withRobot=True),
transitions={
'succeeded': 'Idle',
'aborted': 'end'
},
remapping={'navGoalIn': 'navGoal'})
smach.StateMachine.add('ExecTra',
ExecuteTrajectoryState(),
transitions={
'succeeded': 'Idle',
'failed': 'end',
'aborted': 'end'
},
remapping={'trajectoryIn': 'tra'})
smach.StateMachine.add('ReleaseObject',
UseGripperState(),
transitions={
'succeeded': 'Idle',
'failed': 'end',
'aborted': 'end'
},
remapping={
'gripperCommandIn': 'gripperCommand',
'waitForSpeech': 'handOffSpeech',
'resultOut': 'graspResult'
})
# Create Manipulation Sub State Machine
manip_sub = smach.StateMachine(
outcomes=['succeededMO', 'failedMO', 'abortedMO'],
input_keys=['armGoalMO', 'objectMO'],
output_keys=['statusMO'])
# Set default values of userdata in Manipulation Sub State Machine
manip_sub.userdata.armGoalMO = None
manip_sub.userdata.objectMO = sm.userdata.object # Object name is the one specified in top-level SM
manip_sub.userdata.tra = None
manip_sub.userdata.objectLocation = None
manip_sub.userdata.gripperCommand = None
manip_sub.userdata.statusMO = None
manip_sub.userdata.pickSuccess = None
manip_sub.userdata.pathFound = False
manip_sub.userdata.targetPose = None
manip_sub.userdata.graspResult = False
# Get the arm goal into the sub state
# mixing and matching sequential transitions and 1 main state type structures
with manip_sub:
smach.StateMachine.add('ManipulateObjectMain',
ManipulateObjectMainState(),
transitions={
'findObject': 'FindObject',
'planPath': 'PlanTraj',
'moveArm': 'ExecTra',
'gripper': 'UseGripper',
'pickRetract': 'PickAndRetract',
'failed': 'failedMO',
'succeeded': 'succeededMO',
'aborted': 'abortedMO'
},
remapping={
'armGoalIn': 'armGoalMO',
'objectLocationIn': 'objectLocation',
'gripperCommandOut': 'gripperCommand',
'traOut': 'tra',
'pathFoundIn': 'pathFound',
'graspResultIn': 'graspResult',
'pickResultIn': 'pickResult',
'targetPoseIn': 'targetPose',
'statusOut': 'statusMO'
})
#smach.StateMachine.add('FindObject', FindObjectState(withRobot=withR, tr_root = 'odom'),
smach.StateMachine.add(
'FindObject',
FindObjectState(withRobot=withR, tr_root='base_link'),
transitions={
'succeeded': 'ManipulateObjectMain',
#'succeeded':'PlanTraj',
'failed': 'ManipulateObjectMain',
'aborted': 'abortedMO'
},
remapping={
'objectIn': 'objectMO',
'objectLocationOut': 'objectLocation'
})
smach.StateMachine.add('PlanTraj',
PlanTrajectoryState(),
transitions={
'succeeded': 'ManipulateObjectMain',
'failed': 'ManipulateObjectMain',
'aborted': 'abortedMO'
},
remapping={
'objectLocationIn': 'objectLocation',
'traOut': 'tra',
'pathFoundOut': 'pathFound',
'targetPoseOut': 'targetPose'
})
smach.StateMachine.add('ExecTra',
ExecuteTrajectoryState(),
transitions={
'succeeded': 'ManipulateObjectMain',
'failed': 'ManipulateObjectMain',
'aborted': 'abortedMO'
},
remapping={'trajectoryIn': 'tra'})
#smach.StateMachine.add('PickAndRetract', CompositePickAndRetractState(ik_root = 'odom'),
smach.StateMachine.add(
'PickAndRetract',
CompositePickAndRetractState(ik_root='base_link'),
transitions={
'succeeded': 'ManipulateObjectMain',
'failed': 'ManipulateObjectMain',
'aborted': 'abortedMO'
},
remapping={
'initArmPoseIn':
'targetPose', #should this be taken from forward kinematics?
'pickResultOut': 'pickResult'
})
smach.StateMachine.add('UseGripper',
UseGripperState(),
transitions={
'succeeded': 'ManipulateObjectMain',
'failed': 'ManipulateObjectMain',
'aborted': 'abortedMO'
},
remapping={
'gripperCommandIn': 'gripperCommand',
'resultOut': 'graspResult'
})
# Add the sub state machine to the top-level state machine
smach.StateMachine.add('manip_sub',
manip_sub,
transitions={
'succeededMO': 'Idle',
'failedMO': 'Idle',
'abortedMO': 'end'
},
remapping={
'armGoalMO': 'armGoal',
'objectMO': 'object'
})
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
print outcome
sis.stop()
def start_sm(path, common_string, monitor_state, setup_state, plot_state, time_limit = float("inf"), max_bag_file = 110, max_window_size = 75, start_window = 2, step=5):
sm = smach.StateMachine(outcomes=['END_SM'])
sm.userdata.window_size = start_window
#sm.userdata.bag_family = "cob3-attempt-2001-" #TODO
sm.userdata.window_size_array = list()
sm.userdata.results_ = dict()
sm.userdata.acc_results = init_dict()
print (sm.userdata.acc_results)
sm.userdata.cam_results = init_dict()
sm.userdata.odom_results = init_dict()
sm.userdata.imu_results = init_dict()
sm.userdata.lidar_results = init_dict()
sm.userdata.mic_results = init_dict()
reading_sm = smach.StateMachine(outcomes=['END_READING_SM'])
reading_sm.userdata.sm_counter = 1
reading_sm.userdata.path = path
reading_sm.userdata.bag_family = common_string #TODO
monitoring_sm = smach.StateMachine(outcomes=['END_MONITORING_SM'])
monitoring_sm.userdata.results_ = sm.userdata.results_
monitoring_sm.userdata.acc_results = sm.userdata.acc_results
monitoring_sm.userdata.cam_results = sm.userdata.cam_results
monitoring_sm.userdata.odom_results = sm.userdata.odom_results
monitoring_sm.userdata.imu_results = sm.userdata.imu_results
monitoring_sm.userdata.lidar_results = sm.userdata.lidar_results
monitoring_sm.userdata.mic_results = sm.userdata.mic_results
with reading_sm:
smach.StateMachine.add('RESET_READING', RestartReader(),
transitions={'NEXT_BAG':'READING'},
remapping={'bar_counter_in':'sm_counter'})
smach.StateMachine.add('READING', MyBagReader(limit = time_limit, max_bag_file = max_bag_file),
transitions={'RESTART_READER':'RESET_READING',
'END_READER':'END_READING_SM'},
remapping={'foo_counter_in':'sm_counter',
'shared_string':'bag_family',
'foo_counter_out':'sm_counter'})
#montoring_sm.userdata.window_size_array = sm.window_size_array
with monitoring_sm:
smach.StateMachine.add('WAIT_FOR_READER', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb),
transitions={'invalid':'MONITOR', 'valid':'WAIT_FOR_READER', 'preempted':'WAIT_FOR_READER'})
while rospy.Subscriber("/sm_reset", Empty).get_num_connections() < 1:
print ("WAit subscriber to monitor")
smach.StateMachine.add('MONITOR', monitor_state(),
transitions={'NEXT_MONITOR':'WAIT_FOR_READER', 'END_MONITOR':'END_MONITORING_SM'},
remapping={'result_cum':'results_',
'acc_cum':'acc_results',
'cam_cum':'cam_results',
'odom_cum': 'odom_results',
'lidar_cum': 'lidar_results',
'imu_cum': 'imu_results',
'mic_cum': 'mic_results'})
# Open the container
with sm:
smach.StateMachine.add('SETUP', setup_state(max_window_size,step),
transitions={'SETUP_DONE':'CON', 'FINISH': 'PLOT_RESULTS'},
remapping={'counter_in':'window_size',
'counter_out':'window_size',
'result_cum':'results_',
'acc_cum':'acc_results',
'cam_cum':'cam_results',
'odom_cum':'odom_results',
'imu_cum': 'imu_results',
'lidar_cum': 'lidar_results',
'mic_cum': 'mic_results',
'x_array': 'window_size_array'})
#Concurrent
sm_con = smach.Concurrence(outcomes=['END_CON'],
default_outcome='END_CON',
outcome_map={#'RESTART':
#{ 'MONITORING_SM':'RESTART_MONITOR',
#'READ_SM' : 'RESTART_READER'},
'END_CON':
{'READ_SM': 'END_READING_SM',
'MONITORING_SM': 'END_MONITORING_SM'}})
# Open the container
with sm_con:
# Add states to the container
#smach.Concurrence.add('WAIT_MONITOR', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb))
smach.Concurrence.add('READ_SM', reading_sm)
smach.Concurrence.add('MONITORING_SM', monitoring_sm)
#sm.userdata.data_in = monitoring_sm.userdata.acc_results #TODO
sm.userdata.data_in = monitoring_sm.userdata.results_ #TODO
smach.StateMachine.add('CON', sm_con,
transitions={#'RESTART':'CON',
'END_CON':'SETUP'})
smach.StateMachine.add('PLOT_RESULTS', plot_state(),
transitions={'PLOT_DONE':'END_SM'},
remapping={'data_in': 'data_in',
'x_array': 'window_size_array'})
# Execute SMACH plan
#rospy.sleep(10)
#outcome = sm.execute()
#rospy.spin()
#Instrospection
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
plt.show()
sis.stop()
