def construct_sm():
sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
tutorial_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
it=lambda: range(0, 10),
it_label='index',
input_keys=[],
output_keys=[],
exhausted_outcome='succeeded')
with tutorial_it:
container_sm = utils.TargetSelectorContainer('explore')
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['target_sent'])
#close the tutorial_it
StateMachine.add('TUTORIAL_IT', tutorial_it, {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
return sm
def __init__(self):
rospy.init_node('speech_recognition')
rospy.on_shutdown(self.shutdown)
self.find_people = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
self.test_bool = False
with self.find_people:
self.answer = Iterator(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['people_condition'],
output_keys=[],
it=lambda: range(0, 5),
exhausted_outcome='succeeded')
with self.answer:
Iterator.set_contained_state('ANSWER_STATE',
Anwser(),
loop_outcomes=['succeeded'])
StateMachine.add('ANSWER',
self.answer,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
out = self.find_people.execute()
if out == 'succeeded':
self.test_bool = True
rospy.logwarn('test finished, all thing is done well!')
else:
rospy.logerr('test failed ,something goes wrong')
def construct_sm():
sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
## %Tag(ITERATOR)%
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
## %EndTag(ITERATOR)%
## %Tag(CONTAINER)%
with tutorial_it:
container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd
StateMachine.add('EVEN_OR_ODD',
ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('EVEN', CBState(even_cb),
{'succeeded':'continue'})
#add odd state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('ODD', CBState(odd_cb),
{'succeeded':'continue'})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
## %EndTag(ADDITERATOR)%
return sm
def construct_sm():
sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
## %Tag(ITERATOR)%
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
## %EndTag(ITERATOR)%
## %Tag(CONTAINER)%
with tutorial_it:
container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd
StateMachine.add('EVEN_OR_ODD',
ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('EVEN', CBState(even_cb),
{'succeeded':'continue'})
#add odd state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('ODD', CBState(odd_cb),
{'succeeded':'continue'})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
## %EndTag(ADDITERATOR)%
return sm
def execute(self):
sm_top = smach.StateMachine(
outcomes=['succeeded', 'preemted', 'aborted', 'continue'])
with sm_top:
self.rooms_go = Iterator(
outcomes=['succeeded', 'preemted', 'aborted'],
input_keys=[],
output_keys=['index'],
it=lambda: range(0, 3),
it_label='index',
exhausted_outcome='succeeded')
with self.rooms_go:
self.rooms_go_proton = StateMachine(
outcomes=['succeeded', 'aborted', 'continue'],
input_keys=['index'])
with self.rooms_go_proton:
#self.current_pos = self.RoomsPoses[index]
self.rooms_go_proton.userdata.current_poses = self.RoomsPoses
self.rooms_go_proton.userdata.current_pos2 = 0
self.rooms_go_proton.add('INDEXTRANS',
IndexTrans(),
transitions={'succeeded': 'GO'},
remapping={
'poses': 'current_poses',
'current_pos': 'current_pos2',
'index': 'index'
})
self.rooms_go_proton.add(
'GO',
NavStack(),
transitions={
'succeeded': 'continue',
'aborted': 'GO',
'error': 'continue'
},
remapping={'pos_xm': 'current_pos2'})
Iterator.set_contained_state('CONTAINER_STATE',
self.rooms_go_proton,
loop_outcomes=['continue'])
sm_top.add('ROOMS_GO',
self.rooms_go,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
},
remapping={'index': 'index'})
out = sm_top.execute()
rospy.logerr(out)
def make_iterator(container, max_iter=1):
it = Iterator(outcomes = ['preempted','aborted','time_out'],
it = lambda: range(0, max_iter),
it_label = 'index',
input_keys=[],
output_keys=[],
exhausted_outcome = 'time_out')
with it:
Iterator.set_contained_state('TARGET_CONTROLLER', container, loop_outcomes=['target_sent'])
return it
def make_iterator(container, max_iter=1):
it = Iterator(outcomes=['preempted', 'aborted', 'time_out'],
it=lambda: range(0, max_iter),
it_label='index',
input_keys=[],
output_keys=[],
exhausted_outcome='time_out')
with it:
Iterator.set_contained_state('TARGET_CONTROLLER',
container,
loop_outcomes=['target_sent'])
return it
def __init__(self):
Iterator.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['log_mission'],
it=[],
output_keys=['log_mission'],
it_label='waypoint_id',
exhausted_outcome='succeeded')
self.register_start_cb(self.set_iterator_list)
with self:
self.sm_nav = StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=['waypoint_id', 'log_mission'],
output_keys=['log_mission'])
self.sm_nav.register_start_cb(self.start_cb_nav)
self.sm_nav.register_termination_cb(self.termination_cb_nav)
with self.sm_nav:
StateMachine.add('MOVE_BASE',
SimpleActionState(
'move_base',
MoveBaseAction,
input_keys=['waypoint_id', 'log_mission'],
goal_cb=self.move_base_goal_cb),
transitions={
'succeeded': 'SAVE_DATA',
'aborted': 'continue'
})
self.save_data_service = ServiceState(
'make_data_acquisition',
RequestSaveData,
response_cb=self.save_data_response_cb)
StateMachine.add('SAVE_DATA',
self.save_data_service,
transitions={'succeeded': 'continue'})
# Close the sm_nav machine and add it to the iterator
Iterator.set_contained_state('FOLLOW_PATH',
self.sm_nav,
loop_outcomes=['continue'])
def construct_sm():
sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
with sm:
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
it = lambda: range(0, 10),
it_label = 'index',
input_keys=[],
output_keys=[],
exhausted_outcome = 'succeeded')
with tutorial_it:
container_sm = utils.TargetSelectorContainer('explore')
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['target_sent'])
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
return sm
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
self.init()
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move_base action server")
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.n_patrols = 2
# Turn the waypoints into SMACH states
nav_states = list()
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0))
# Initialize the top level state machine
self.sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm:
# Initialize the iterator
self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = [],
it = lambda: range(0, self.n_patrols),
output_keys = [],
it_label = 'index',
exhausted_outcome = 'succeeded')
with self.sm_patrol_iterator:
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','continue'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'})
# Close the sm_patrol machine and add it to the iterator
Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue'])
# Close the top level state machine
StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('tinker_mission_manipulation')
trans = tf.TransformListener()
rospy.loginfo("Waiting for tf ...")
rospy.sleep(3)
assert (len(trans.getFrameStrings()) > 0)
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
def kinect_callback(userdata, result):
userdata.objects = []
objects = []
sum_x = 0
for obj in result.objects.objects:
position = obj.pose.pose.pose.position
if position.y > 0.5 or position.y < -0.5:
continue
_levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80]
for l in _levels:
if fabs(l - position.z) < 0.05:
position.z = l + 0.05
if position.z > 1.1:
position.z += 0.05
obj.header.stamp = rospy.Time(0)
kinect_point = PointStamped(header=obj.header, point=position)
odom_point = trans.transformPoint('odom', kinect_point)
sum_x += odom_point.point.x
rospy.loginfo(
colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'),
odom_point.point.x, odom_point.point.y, odom_point.point.z)
objects.append(odom_point)
avg_x = sum_x / len(objects)
for from_point in objects:
to_point = copy.deepcopy(from_point)
to_point.point.x = avg_x
to_point.point.z = find_div(from_point.point.z)
userdata.objects.append({'from': from_point, 'to': to_point})
rospy.loginfo(
colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'),
to_point.point.x, to_point.point.y, to_point.point.z)
rospy.loginfo(colored('Total Object: %d', 'green'), len(objects))
return 'succeeded'
StateMachine.add('Arm_Mode_Kinect',
ArmModeState(ArmModeState.Arm_Mode_Kinect),
transitions={'succeeded': 'Start_Button'})
StateMachine.add('Start_Button',
MonitorStartButtonState(),
transitions={
'valid': 'Start_Button',
'invalid': 'S_Kinect_Recognition'
})
StateMachine.add('S_Kinect_Recognition',
ServiceState(service_name='/kinect_find_objects',
service_spec=FindObjects,
input_keys=['objects'],
output_keys=['objects'],
response_cb=kinect_callback),
transitions={'succeeded': 'Generate_Report'})
StateMachine.add('Generate_Report',
GenerateReportState(image='result.png',
text='object_names.txt'),
transitions={'succeeded': 'IT_Objects_Iterator'})
objects_iterator = Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['objects'],
output_keys=[],
it=lambda: state.userdata.objects,
it_label='target',
exhausted_outcome='succeeded')
with objects_iterator:
fetch_object_sequence = Sequence(
outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
input_keys=['target'],
connector_outcome='succeeded')
with fetch_object_sequence:
Sequence.add('Speak', SpeakState('New Object recognized'))
Sequence.add('Gripper_Photo',
GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_For_Photo',
MoveArmState(Point(-0.7, 0, 0),
target_key='from'),
transitions={'aborted': 'continue'})
concurrence = Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=['target'])
with concurrence:
Concurrence.add(
'Move_Fetch',
MoveArmState(Point(0.06, 0, 0), target_key='from'))
Concurrence.add(
'Gripper_Laser_sensor',
MonitorState('/gripper_laser_sensor',
Bool,
cond_cb=lambda x, y: False))
Sequence.add('Move_Fetch_Concurrence', concurrence)
Sequence.add('Gripper_Fetch',
GripperState(GripperState.GRIPPER_CLOSE))
Sequence.add(
'Move_Fetch_Back',
MoveArmState(Point(-0.7, 0, 0), target_key='from'))
Sequence.add('Move_Down',
MoveArmState(Point(-0.6, 0, 0), target_key='to'))
Sequence.add('Move_Put',
MoveArmState(Point(0, 0, 0), target_key='to'))
Sequence.add('Gripper_Put',
GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_Put_Back',
MoveArmState(Point(-0.6, 0, 0), target_key='to'),
transitions={'succeeded': 'continue'})
Iterator.set_contained_state('Seq_Fetch_Object',
fetch_object_sequence,
loop_outcomes=['continue'])
# end of objects_iterator
StateMachine.add('IT_Objects_Iterator',
objects_iterator,
transitions={
'succeeded': 'A_Move_Reset',
'aborted': 'A_Move_Reset'
})
StateMachine.add('A_Move_Reset',
ArmModeState(ArmModeState.Arm_Mode_Init),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_manipulation', state,
'/tinker_mission_manipulation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def construct_sm():
sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
## %Tag(ITERATOR)%
tutorial_it = Iterator(
outcomes=["succeeded", "preempted", "aborted"],
input_keys=["nums", "even_nums", "odd_nums"],
it=lambda: range(0, len(sm.userdata.nums)),
output_keys=["even_nums", "odd_nums"],
it_label="index",
exhausted_outcome="succeeded",
)
## %EndTag(ITERATOR)%
## %Tag(CONTAINER)%
with tutorial_it:
container_sm = StateMachine(
outcomes=["succeeded", "preempted", "aborted", "continue"],
input_keys=["nums", "index", "even_nums", "odd_nums"],
output_keys=["even_nums", "odd_nums"],
)
with container_sm:
# test wether even or odd
StateMachine.add(
"EVEN_OR_ODD",
ConditionState(
cond_cb=lambda ud: ud.nums[ud.index] % 2,
input_keys=["nums", "index"],
),
{
"true": "ODD",
"false": "EVEN"
},
)
# add even state
@smach.cb_interface(
input_keys=["nums", "index", "even_nums"],
output_keys=["odd_nums"],
outcomes=["succeeded"],
)
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return "succeeded"
StateMachine.add("EVEN", CBState(even_cb),
{"succeeded": "continue"})
# add odd state
@smach.cb_interface(
input_keys=["nums", "index", "odd_nums"],
output_keys=["odd_nums"],
outcomes=["succeeded"],
)
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return "succeeded"
StateMachine.add("ODD", CBState(odd_cb),
{"succeeded": "continue"})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
# close container_sm
Iterator.set_contained_state("CONTAINER_STATE",
container_sm,
loop_outcomes=["continue"])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
# close the tutorial_it
StateMachine.add("TUTORIAL_IT", tutorial_it, {
"succeeded": "succeeded",
"aborted": "aborted"
})
## %EndTag(ADDITERATOR)%
return sm
def main():
rospy.init_node('tinker_mission_manipulation')
trans = tf.TransformListener()
rospy.loginfo("Waiting for tf ...")
rospy.sleep(3)
assert (len(trans.getFrameStrings()) > 0)
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
def kinect_callback(userdata, result):
userdata.objects = []
objects = []
sum_x = 0
for obj in result.objects.objects:
position = obj.pose.pose.pose.position
if position.y > 0.5 or position.y < -0.5:
continue
_levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80]
for l in _levels:
if fabs(l - position.z) < 0.05:
position.z = l + 0.05
if position.z > 1.1:
position.z += 0.05
obj.header.stamp = rospy.Time(0)
kinect_point = PointStamped(header=obj.header, point=position)
odom_point = trans.transformPoint('odom', kinect_point)
sum_x += odom_point.point.x
rospy.loginfo(colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x,
odom_point.point.y, odom_point.point.z)
objects.append(odom_point)
avg_x = sum_x / len(objects)
for from_point in objects:
to_point = copy.deepcopy(from_point)
to_point.point.x = avg_x
to_point.point.z = find_div(from_point.point.z)
userdata.objects.append({'from': from_point, 'to': to_point})
rospy.loginfo(colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x,
to_point.point.y, to_point.point.z)
rospy.loginfo(colored('Total Object: %d','green'), len(objects))
return 'succeeded'
StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect),
transitions={'succeeded': 'Start_Button'})
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition'})
StateMachine.add('S_Kinect_Recognition',
ServiceState(service_name='/kinect_find_objects',
service_spec=FindObjects,
input_keys=['objects'],
output_keys=['objects'],
response_cb=kinect_callback),
transitions={'succeeded': 'Generate_Report'})
StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'),
transitions={'succeeded': 'IT_Objects_Iterator'} )
objects_iterator = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['objects'], output_keys=[],
it=lambda: state.userdata.objects, it_label='target',
exhausted_outcome='succeeded')
with objects_iterator:
fetch_object_sequence = Sequence(outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
input_keys=['target'],
connector_outcome='succeeded')
with fetch_object_sequence:
Sequence.add('Speak', SpeakState('New Object recognized'))
Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'),
transitions={'aborted':'continue'})
concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
child_termination_cb=lambda x: True,
input_keys=['target'])
with concurrence:
Concurrence.add('Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from'))
Concurrence.add('Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x,y: False))
Sequence.add('Move_Fetch_Concurrence', concurrence)
Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE))
Sequence.add('Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from'))
Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to'))
Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to'))
Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN))
Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'})
Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue'])
# end of objects_iterator
StateMachine.add('IT_Objects_Iterator', objects_iterator,
transitions= {'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset'})
StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
uci_topic = 'chessboard_uci_node'
chesspiece_topic = 'chessboard_chesspieces_node'
rospy.init_node('chessboard_sm_node')
sm = smach.StateMachine(
outcomes=['finished', 'succeeded', 'preempted', 'aborted'])
sm.userdata.target = []
sis = smach_ros.IntrospectionServer('chessboard_sm_sis', sm, '/SM_ROOT')
sis.start()
with sm:
sq_init = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
#Stockfish init
with sq_init:
rospy.loginfo('Stockfish init')
Sequence.add(
'PIECE_RESET',
SimpleActionState('chessboard_chesspieces_reset', ResetAction))
Sequence.add(
'UCI_INIT',
SimpleActionState(
uci_topic,
ucicommandAction,
goal=ucicommandGoal(cmd=ucicommandGoal.CMD_INIT)))
Sequence.add(
'UCI_NEWGAME',
SimpleActionState(
uci_topic,
ucicommandAction,
goal=ucicommandGoal(cmd=ucicommandGoal.CMD_NEWGAME)))
smach.StateMachine.add('STOCKFISH_SETUP',
sq_init,
transitions={'succeeded': 'UCI_ITER'})
#Test code
tutorial_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
it=lambda: range(0, 2),
output_keys=[],
it_label='index',
exhausted_outcome='succeeded')
with tutorial_it:
container_sm = smach.StateMachine(outcomes=[
'finished', 'succeeded', 'preempted', 'aborted', 'continue'
])
with container_sm:
def calc_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
result_coords = list(result.result.data)
print(result.result.data)
new_target = []
# convert chess notation to indices
# e.g. b8c6 -> 6,7,5,5
new_target.append(
7 - int(string.lowercase.index(result_coords[0])))
new_target.append(int(result_coords[1]) - 1)
new_target.append(
7 - string.lowercase.index(result_coords[2]))
new_target.append(int(result_coords[3]) - 1)
#print(new_target)
userdata.target = new_target
return 'succeeded'
smach.StateMachine.add(
'UCI_CALCMOVE',
SimpleActionState(
uci_topic,
ucicommandAction,
goal=ucicommandGoal(cmd=ucicommandGoal.CMD_CALC_MOVE),
result_cb=calc_result_cb,
output_keys=['target']),
transitions={'succeeded': 'PIECE_MOVE'},
remapping={'target': 'user_data_target'})
smach.StateMachine.add(
'PIECE_MOVE',
SimpleActionState(chesspiece_topic,
MoveActionAction,
goal_slots=['target']),
transitions={'succeeded': 'UCI_MOVE'},
remapping={'target': 'user_data_target'})
smach.StateMachine.add(
'UCI_MOVE',
SimpleActionState(
uci_topic,
ucicommandAction,
goal=ucicommandGoal(cmd=ucicommandGoal.CMD_MOVE)),
transitions={'succeeded': 'finished'})
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['finished'])
smach.StateMachine.add('UCI_ITER', tutorial_it, {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# uci_node -> CMD_CALC_MOVE
# uci_node -> CMD_MOVE
#get new target position
#movement
#set position in uci
outcome = sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
self.init()
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move_base action server")
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.n_patrols = 2
# Turn the waypoints into SMACH states
nav_states = list()
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
move_base_state = SimpleActionState(
'move_base',
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0))
# Initialize the top level state machine
self.sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm:
# Initialize the iterator
self.sm_patrol_iterator = Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
it=lambda: range(0, self.n_patrols),
output_keys=[],
it_label='index',
exhausted_outcome='succeeded')
with self.sm_patrol_iterator:
# Initialize the patrol state machine
self.sm_patrol = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted', 'continue'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0',
nav_states[0],
transitions={
'succeeded': 'NAV_STATE_1',
'aborted': 'NAV_STATE_1',
'preempted': 'NAV_STATE_1'
})
StateMachine.add('NAV_STATE_1',
nav_states[1],
transitions={
'succeeded': 'NAV_STATE_2',
'aborted': 'NAV_STATE_2',
'preempted': 'NAV_STATE_2'
})
StateMachine.add('NAV_STATE_2',
nav_states[2],
transitions={
'succeeded': 'NAV_STATE_3',
'aborted': 'NAV_STATE_3',
'preempted': 'NAV_STATE_3'
})
StateMachine.add('NAV_STATE_3',
nav_states[3],
transitions={
'succeeded': 'NAV_STATE_4',
'aborted': 'NAV_STATE_4',
'preempted': 'NAV_STATE_4'
})
StateMachine.add('NAV_STATE_4',
nav_states[0],
transitions={
'succeeded': 'continue',
'aborted': 'continue',
'preempted': 'continue'
})
# Close the sm_patrol machine and add it to the iterator
Iterator.set_contained_state('PATROL_STATE',
self.sm_patrol,
loop_outcomes=['continue'])
# Close the top level state machine
StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('fcsc_smach')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
sm.userdata.products = []
sm.userdata.sandwiches = []
sm.userdata.scrap_sandwiches = []
sm.userdata.normal_sandwiches = []
sm.userdata.damy_output = []
sm.userdata.adjust_count = 0;
sm.userdata.faceup_count = 0;
# Open the container
with sm:
def move_initial_pose_response_cb(userdata, response):
if response.success == True:
return 'succeeded'
else:
return 'aborted'
smach.StateMachine.add('MOVE_INITIAL_POSE',
ServiceState('move_initial_pose',
Trigger,
response_cb=move_initial_pose_response_cb),
# transitions={'succeeded':'DETECT_AND_RECOVER_SANDWICH'})
transitions={'succeeded':'NAVIGATE_TO_SHELF_A'})
navigate_to_shelf_a_sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with navigate_to_shelf_a_sm:
navigate_to_shelf_a_sm.add('GO_TO_NEAR_SHELF',
ServiceState('goto_near_shelf', GoToNearShelf, request=GoToNearShelfRequest(0)),
transitions={'succeeded':'DETECT_SHELF_A'})
navigate_to_shelf_a_sm.add('DETECT_SHELF_A',
ServiceState('detect_shelf_action', DetectShelf, request=DetectShelfRequest(0)),
transitions={'succeeded':'GO_TO_STOCKING_POSITION', 'aborted':'aborted'})
navigate_to_shelf_a_sm.add('GO_TO_STOCKING_POSITION',
ServiceState('goto_stocking_base_position', Trigger),
transitions={'succeeded':'succeeded'})
smach.StateMachine.add('NAVIGATE_TO_SHELF_A',
navigate_to_shelf_a_sm,
# transitions={'succeeded':'NAVIGATE_TO_SHELF_B'})
transitions={'succeeded':'DETECT_PRODUCT'})
smach.StateMachine.add('DETECT_PRODUCT',
ServiceState('detect_product',
DetectProduct,
response_slots=['detected_object_array']),
transitions={'succeeded':'STOCK_PRODUCTS'},
remapping={'detected_object_array':'products'})
stock_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['products', 'damy_output'],
it=lambda: range(0, len(sm.userdata.products.objects)),
output_keys=['damy_output'],
it_label='index',
exhausted_outcome='succeeded')
with stock_it:
stock_sm = smach.StateMachine(outcomes=['continue', 'succeeded', 'preempted', 'aborted'],
input_keys=['index', 'products'])
with stock_sm:
@smach.cb_interface(input_keys=['products', 'index'])
def pickup_request_cb(userdata, request):
request.object = userdata.products.objects[userdata.index]
return request
@smach.cb_interface(outcomes=['succeeded', 'aborted'])
def pickup_response_cb(userdata, response):
if response.success == True:
return 'succeeded'
else:
return 'aborted'
smach.StateMachine.add('PICKUP_PRODUCT',
ServiceState('pickup_product',
Manipulate,
request_cb=pickup_request_cb,
response_cb=pickup_response_cb,
input_keys=['products', 'index']),
transitions={'succeeded':'PLACE_PRODUCT', 'aborted':'continue'})
@smach.cb_interface(input_keys=['products', 'index'])
def place_request_cb(userdata, request):
request.object = userdata.products.objects[userdata.index]
return request
@smach.cb_interface(outcomes=['succeeded', 'aborted'])
def place_response_cb(userdata, response):
if response.success:
return 'succeeded'
else:
return 'aborted'
smach.StateMachine.add('PLACE_PRODUCT',
ServiceState('place_product',
Manipulate,
request_cb=place_request_cb,
response_cb=place_response_cb,
input_keys=['products', 'index']),
transitions={'succeeded':'continue', 'aborted':'continue'})
@smach.cb_interface(input_keys=['products', 'index'])
def return_request_cb(userdata, request):
request.object = userdata.products.objects[userdata.index]
return request
@smach.cb_interface(outcomes=['succeeded', 'aborted'])
def return_response_cb(userdata, response):
if response.success:
return 'succeeded'
else:
return 'aborted'
smach.StateMachine.add('RETURN_PRODUCT',
ServiceState('return_product',
Manipulate,
request_cb=return_request_cb,
response_cb=return_response_cb,
input_keys=['products', 'index']),
transitions={'succeeded':'continue', 'aborted':'aborted'})
Iterator.set_contained_state('STOCK_PRODUCTS_STATE',
stock_sm,
loop_outcomes=['continue'])
smach.StateMachine.add('STOCK_PRODUCTS',
stock_it,
transitions={'succeeded':'NAVIGATE_TO_SHELF_B', 'aborted':'aborted'})
navigate_to_shelf_b_sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with navigate_to_shelf_b_sm:
navigate_to_shelf_b_sm.add('GO_TO_NEAR_SHELF_B',
ServiceState('goto_near_shelf', GoToNearShelf, request=GoToNearShelfRequest(2)),
transitions={'succeeded':'DETECT_SHELF_B'})
navigate_to_shelf_b_sm.add('DETECT_SHELF_B',
ServiceState('detect_shelf_action', DetectShelf, request=DetectShelfRequest(1)),
transitions={'succeeded':'GO_TO_FACEUP_POSITION', 'aborted':'aborted'})
navigate_to_shelf_a_sm.add('GO_TO_FACEUP_POSITION',
ServiceState('goto_faceup_base_position', Trigger),
transitions={'succeeded':'succeeded'})
smach.StateMachine.add('NAVIGATE_TO_SHELF_B',
navigate_to_shelf_b_sm,
transitions={'succeeded':'DETECT_AND_RECOVER_SANDWICH'})
smach.StateMachine.add('DETECT_AND_RECOVER_SANDWICH',
ServiceState('detect_and_recover_sandwich',
DetectAndRecoverSandwich,
response_slots=['normal_sandwiches']),
transitions={'succeeded':'FACEUP_SANDWICHES'})
@smach.cb_interface(input_keys=['normal_sandwiches'])
def faceup_request_cb(userdata, request):
request.sandwiches = userdata.normal_sandwiches
return request
@smach.cb_interface(outcomes=['succeeded', 'aborted'])
def faceup_response_cb(userdata, response):
if response.success:
return 'succeeded'
else:
return 'aborted'
smach.StateMachine.add('FACEUP_SANDWICHES',
ServiceState('faceup_standing_sandwiches',
FaceupSandwich,
request_cb=faceup_request_cb,
response_cb=faceup_response_cb,
input_keys=['normal_sandwiches']
),
transitions={'succeeded':'GO_TO_HOME_POSITION'})
smach.StateMachine.add('GO_TO_HOME_POSITION',
ServiceState('goto_home_position', Trigger),
transitions={'succeeded':'succeeded'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
rospy.sleep(3)
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
def set_iterator_list(self, userdata, initial_states):
with self:
waypoints = userdata.log_mission['waypoints']
start_index = userdata.log_mission['patrol']['index_waypoint']
Iterator.set_iteritems(range(start_index, len(waypoints)),
'waypoint_id')
