def test_userdata(self):
"""Test serial manipulation of userdata."""
pinger_thread = threading.Thread(target=pinger)
pinger_thread.start()
init_ud = UserData()
init_ud.b = 'A'
sm = StateMachine(['valid', 'invalid', 'preempted'])
sm.set_initial_state(['MON'], userdata=init_ud)
assert 'b' in sm.userdata
assert sm.userdata.b == 'A'
with sm:
StateMachine.add(
'MON',
MonitorState('/ping',
Empty,
cond_cb,
input_keys=['b'],
output_keys=['a']))
outcome = sm.execute()
assert outcome == 'invalid'
assert 'b' in sm.userdata
assert sm.userdata.b == 'A'
assert 'a' in sm.userdata
assert sm.userdata.a == 'A'
def concurrence_outcome_cb(self, outcome_map):
# If the battery is below threshold, return the 'recharge' outcome
if outcome_map['MONITOR_BATTERY'] == 'invalid':
return 'recharge'
# Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop'
elif outcome_map['SM_NAV'] == 'succeeded':
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
# If we have not completed all our patrols, start again at the beginning
if self.n_patrols == -1 or self.patrol_count < self.n_patrols:
self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
return 'succeeded'
# Otherwise, we are finished patrolling so return 'stop'
else:
self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData())
return 'stop'
# Recharge if all else fails
else:
return 'recharge'
def concurrence_outcome_cb(self, outcome_map):
# 如果电池电量低于设定的阈值,返回'recharge' outcome
if outcome_map['MONITOR_BATTERY'] == 'invalid':
return 'recharge'
# 否则,如果最后一个导航目标点成功,返回'succeeded' 或者 'stop'
elif outcome_map['SM_NAV'] == 'succeeded':
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
# 如果没有完成所有的巡逻，重新开始导航
if self.n_patrols == -1 or self.patrol_count < self.n_patrols:
self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
return 'succeeded'
# 否则,完成所有导航并返回 'stop'
else:
self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData())
return 'stop'
# 如果其他操作失败了，重新充电
else:
return 'recharge'
def concurrence_outcome_cb(self, outcome_map):
if outcome_map['MONITOR_BATTERY'] == 'invalid':
return 'recharge'
elif outcome_map['SM_NAV'] == 'succeeded':
self.sm_nav.set_initial_state(['NAV_STATE_2'], UserData())
return 'stop'
else:
return 'recharge'
def concurrence_child_termination_cb(self, outcome_map):
if outcome_map['SM_NAV'] == 'succeeded':
return True
if outcome_map['MONITOR_BATTERY'] == 'invalid':
rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...")
if self.last_nav_state is not None:
self.sm_nav.set_initial_state(self.last_nav_state, UserData())
return True
else:
return False
def concurrence_child_termination_cb(self, outcome_map):
# If the current navigation task has succeeded, return True
if outcome_map['SM_NAV'] == 'succeeded':
return True
# If the MonitorState state returns False (invalid), store the current nav goal and recharge
if outcome_map['MONITOR_BATTERY'] == 'invalid':
rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...")
if self.last_nav_state is not None:
self.sm_nav.set_initial_state(self.last_nav_state, UserData())
return True
else:
return False
def test_introspection(self):
"""Test introspection system."""
# Construct state machine
sm = StateMachine(['done'])
sm2 = StateMachine(['done'])
sm3 = StateMachine(['done'])
with sm:
# Note: the following "Getter" state should fail
StateMachine.add('GETTER1', Getter(), {})
StateMachine.add('SM2', sm2, {'done': 'SM3'})
with sm2:
StateMachine.add("SETTER", Setter(), {})
StateMachine.add('SM3', sm3, {'done': 'done'})
with sm3:
StateMachine.add("SETTER", Setter(), {})
StateMachine.add('GETTER2', Getter(), {'done': 'SM2'})
sm.set_initial_state(['GETTER1'])
sm2.set_initial_state(['SETTER'])
sm3.set_initial_state(['SETTER'])
# Run introspector
intro_server = IntrospectionServer('intro_test', sm, '/intro_test')
server_thread = threading.Thread(target=intro_server.start)
server_thread.start()
intro_client = IntrospectionClient()
servers = intro_client.get_servers()
while '/intro_test' not in servers and not rospy.is_shutdown():
servers = intro_client.get_servers()
rospy.loginfo("Smach servers: " + str())
rospy.sleep(0.1)
assert '/intro_test' in servers
# Set initial state
injected_ud = UserData()
injected_ud.a = 'A'
init_set = intro_client.set_initial_state('intro_test',
'/intro_test', ['SM2'],
injected_ud,
timeout=rospy.Duration(10.0))
assert init_set
outcome = sm.execute()
assert outcome == 'done'
def execute_smach_container(smach_container,
enable_introspection=False,
name='/SM_ROOT',
userdata=UserData()):
if not rospy.core.is_initialized():
rospy.init_node('smach')
if enable_introspection:
# Create and start the introspection server
sis = IntrospectionServer('smach_executor', smach_container, name)
sis.start()
outcome = smach_container.execute(userdata)
_logger.info("smach outcome: %s", outcome)
if enable_introspection:
sis.stop()
return outcome
def run(self, next_worldstate):
userdata = UserData()
self.translate_worldstate_to_userdata(next_worldstate, userdata)
self.state.execute(userdata)
self.translate_userdata_to_worldstate(userdata, next_worldstate)