def concurrence_child_termination_cb(self, outcome_map):
# 如果当前导航任务完成, return True
if outcome_map['SM_NAV'] == 'succeeded':
return True
# 如果MonitorState状态变成False则储存当前的导航目标点并充电
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 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 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_WAYPOINT'], UserData())
return 'succeeded'
# Otherwise, we are finished patrolling so return 'stop'
#else:
# self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
# return 'succeeded'
# Recharge if all else fails
else:
return 'recharge'
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)