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)