def _test_WaitForGoalState(): rospy.init_node('smach') wfg = WaitForGoalState() print 'execute #1' wfg.execute(smach.UserData()) print 'execute #2' wfg.execute(smach.UserData()) print 'execute #3' wfg.execute(smach.UserData())
def _convert_userdata(self, state, userdata): inner_userdata = smach.UserData() for key in list(self._input_keys): if key.startswith(state.name + "_"): inner_userdata[key.replace(state.name + "_", "")] = userdata[key] return inner_userdata
def execute(self, parent_ud=smach.UserData()): """Run the state machine on entry to this state. This will set the "closed" flag and spin up the execute thread. Once this flag has been set, it will prevent more states from being added to the state machine. """ ##Create a the log file if ssm_enable_log is True if (rospy.get_param("ssm_enable_log", False) == True): date_str = datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_") self.userdata.logfile = rospy.get_param( "ssm_log_path", default="/tmp/") + date_str + "log.txt" try: file = open(self.userdata.logfile, "a") file.write(StrTimeStamped() + "SMART STATE MACHINE Started \n") file.close() except: rospy.logerr("Log Fail !") return 'preempt' container_outcome = super(ssmMainStateMachine, self).execute(parent_ud) if (rospy.get_param("ssm_enable_log", False) == True): file = open(self.userdata.logfile, "a") file.write(StrTimeStamped() + "SMART STATE MACHINE Finished \n") file.close() return container_outcome
def search_for_walls(self, robot_id): robot_path = {} #TODO Increase to 1.1 altitude for waypoints in real drone !!! points_path = [ Point(x=-22, y=17, z=7), Point(x=-21, y=17, z=1.2), Point(x=-0.5, y=17, z=1.2), Point(x=-21, y=17, z=1.2), Point(x=-0.5, y=17, z=1.2) ] robot_path = [] for point in points_path: waypoint = PoseStamped() waypoint.header.frame_id = 'arena' waypoint.pose.position = point robot_path.append(waypoint) print('sending goal to search_objects server {}'.format(robot_id)) userdata = smach.UserData() userdata.path = robot_path self.task_manager.start_task(robot_id, FollowPath(), userdata) while not rospy.is_shutdown() and not self.task_manager.is_idle( robot_id) and not uav_walls_are_found(self.wall_segment_ids): self.cluster_wall_segments() rospy.logwarn('len(self.wall_segment_ids) = {}'.format( len(self.wall_segment_ids))) rospy.sleep(1.0) if not self.task_manager.is_idle(robot_id): rospy.logwarn('preempting {}'.format(robot_id)) self.task_manager.preempt_task(robot_id) self.task_manager.wait_for([robot_id])
def main(): from bender_skills import robot_factory rospy.init_node("crowd_information") robot = robot_factory.build( [ "facial_features", # "AI", "person_detector", # "knowledge", "tts" ], core=False) robot.check() sm = getInstance(robot, True) ud = smach.UserData() ud.features = ['gender', 'age'] # introspection server sis = IntrospectionServer('crowd_information', sm, '/CROWD_SM') sis.start() outcome = sm.execute(ud) sis.stop()
def look_for_objects(self): robot_paths = {} point_paths = generate_uav_paths(len(self.available_robots), self.field_width, self.field_height, self.column_count) for i, robot_id in enumerate(self.available_robots): robot_path = [] flight_level = self.flight_levels[robot_id] point_path = set_z(point_paths[i], flight_level) for point in point_path: waypoint = PoseStamped() waypoint.header.frame_id = 'arena' waypoint.pose.position = point robot_path.append(waypoint) robot_paths[robot_id] = robot_path for robot_id in self.available_robots: print('sending goal to search_objects server {}'.format(robot_id)) userdata = smach.UserData() userdata.path = robot_paths[robot_id] self.task_manager.start_task(robot_id, FollowPath(), userdata) #TODO: what if objects never found? while not all_piles_are_found( self.uav_piles, self.ugv_piles) and not all_walls_are_found( self.uav_walls, self.ugv_wall) and not rospy.is_shutdown(): rospy.logwarn('len(self.uav_piles) = {}'.format(len( self.uav_piles))) rospy.logwarn('len(self.ugv_piles) = {}'.format(len( self.ugv_piles))) rospy.sleep(1.0) for robot_id in self.available_robots: rospy.logwarn('preempting {}'.format(robot_id)) self.task_manager.preempt_task(robot_id)
def execute(self, parent_ud=smach.UserData()): # First time in this state machine if self._current_state is None: # Spew some info smach.loginfo( "State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) # Set initial state self._set_current_state(self._initial_state_label) # Initialize preempt state self._preempted_label = None self._preempted_state = None # Call start callbacks self.call_start_cbs() # Copy input keys self._copy_input_keys(parent_ud, self.userdata) container_outcome = self._loopback_name if self.id is not None: # only top-level statemachine should loop for outcome while container_outcome == self._loopback_name and not smach.is_shutdown( ): # Update the state machine container_outcome = self._async_execute(parent_ud) else: container_outcome = self._async_execute(parent_ud) if smach.is_shutdown(): container_outcome = 'preempted' return container_outcome
def extinguish_ground_fire(self): userdata = smach.UserData() userdata.path = self.path userdata.color = 'fire' self.task_manager.start_task(self.robot_id, ExtinguishGroundFire(), userdata) self.task_manager.wait_for([self.robot_id])
def _init_cmd_cb(self, msg): """Initialize a tree's state and userdata.""" initial_states = msg.initial_states local_data = msg.local_data # Check if this init message is directed at this path rospy.logdebug('Received init message for path: ' + msg.path + ' to ' + str(initial_states)) if msg.path == self._path: if all(s in self._container.get_children() for s in initial_states): ud = smach.UserData() ud._data = pickle.loads(msg.local_data) rospy.logdebug("Setting initial state in smach path: '" + self._path + "' to '" + str(initial_states) + "' with userdata: " + str(ud._data)) # Set the initial state self._container.set_initial_state(initial_states, ud) # Publish initial state self._publish_status("REMOTE_INIT") else: rospy.logerr("Attempting to set initial state in container '" + self._path + "' to '" + str(initial_states) + "', but this container only has states: " + str(self._container.get_children()))
def _test_SleepState(): rospy.init_node('smach') ud = smach.UserData() ud.duration = None def exec_state(state): sm = simple_state_wrapper(state) return execute_smach_container(sm, userdata=ud) rospy.loginfo("Testing ud:None/arg:1") state = SleepState(1) outcome = exec_state(state) rospy.loginfo("outcome: %s" % outcome) rospy.loginfo("Testing ud:2/arg:None") ud.duration = 2 state = SleepState() outcome = exec_state(state) rospy.loginfo("outcome: %s" % outcome) rospy.loginfo("Testing ud:3/arg:4") ud.duration = 3 state = SleepState(4) outcome = exec_state(state) rospy.loginfo("outcome: %s" % outcome)
def __init__(self, hand_type, configuration): '''Constructor''' super(FingerConfigurationState, self).__init__(outcomes=['done', 'failed'], input_keys=['hand_side']) self._joint_names = dict() self._joint_names['left'] = dict() self._joint_names['left']['robotiq'] = ['left_f0_j1', 'left_f1_j0', 'left_f1_j1', 'left_f2_j1'] self._joint_names['left']['vt_hand'] = ['l_f0_j0', 'l_f1_j0'] self._joint_names['right'] = dict() self._joint_names['right']['robotiq'] = ['right_f0_j1', 'right_f1_j0', 'right_f1_j1', 'right_f2_j1'] self._joint_names['right']['vt_hand'] = ['r_f0_j0', 'r_f1_j0'] self._joint_values = dict() self._joint_values['robotiq'] = map(lambda x: x * configuration, [1.22, 0.08, 1.13, 1.13]) # 2nd value is the scissor joint self._joint_values['vt_hand'] = map(lambda x: x * configuration, [2.685, 2.685]) self._execution_state = HandTrajectoryState(hand_type) self._internal_userdata = smach.UserData() if not hand_type in self._joint_names['left'].keys(): raise Exception("Unspecified hand type %s, only have %s" % (hand_type, str(self._joint_names['left'].keys()))) self._hand_type = hand_type self._configuration = self._joint_values[hand_type] self._print = True self._failed = False
def execute(self, parent_ud=smach.UserData()): with self._condition: if self._root is None: raise smach.InvalidTransitionError("BehaviorTreeContainer has no root task") self._result = None self._current_exec_root = self._initial_task # Copy input keys self._copy_input_keys(parent_ud, self.userdata) result = self._current_exec_root.try_immediate(self.userdata) if result == Task.DEFERRED: self._current_exec_root.execute_deferred(self._root_termination_cb, self.userdata) while self._current_exec_root is not None: # Releases condition mutex during wait self._condition.wait(0.1) result = self._result else: self._current_exec_root = None # Copy output keys self._copy_output_keys(self.userdata, parent_ud) return self._outcome_from_result(result)
def _merge_userdata_back(self, state, inner_userdata, userdata): output_userdata = smach.UserData() for key in list(self._output_keys): if key.startswith(state.name + "_"): output_userdata[key] = inner_userdata[key.replace( state.name + "_", "")] userdata.update(output_userdata)
def _async_execute(self, parent_ud=smach.UserData()): """Run the state machine on entry to this state. This will set the "closed" flag and spin up the execute thread. Once this flag has been set, it will prevent more states from being added to the state machine. """ # This will prevent preempts from getting propagated to non-existent children with self._state_transitioning_lock: # Check state consistency try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.") return None # Set running flag self._is_running = True # Initialize container outcome container_outcome = None # Step through state machine if self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = self._update_once() if self._do_rate_sleep and self._current_state is not None and not isinstance( self._current_state, OperatableStateMachine): try: # sleep with the rate of the state and update container rate accordingly self._rate = self._current_state._rate self._rate.sleep() except ROSInterruptException: rospy.logwarn('Interrupted rate sleep.') if container_outcome is not None and container_outcome != self._loopback_name: # Copy output keys self._copy_output_keys(self.userdata, parent_ud) else: container_outcome = self._loopback_name # provide explicit sync as back-up functionality # should be used only if there is no other choice # since it requires additional 8 byte + header update bandwith and time to restart mirror if self._inner_sync_request and self._get_deep_state() is not None: self._inner_sync_request = False if self.id is None: self._parent._inner_sync_request = True else: msg = BehaviorSync() msg.behavior_id = self.id msg.current_state_checksum = zlib.adler32( self._get_deep_state()._get_path()) self._pub.publish('flexbe/mirror/sync', msg) # We're no longer running self._is_running = False return container_outcome
def take_off(self, robot_id): userdata = smach.UserData() #userdata.height = self.flight_levels[robot_id] userdata.height = 7.0 #TODO: take off altitude fixed self.task_manager.start_task(robot_id, TakeOff(), userdata) self.task_manager.wait_for([robot_id ]) # Sequential takeoff for safety reasons
def execute(self, userdata=smach.UserData()): outcome = smach.StateMachine.execute(self, userdata) if outcome == 'succeeded': self.get_logger().log_waypoint_success(userdata.goal_pose) elif outcome == 'failure': self.get_logger().log_waypoint_fail(userdata.goal_pose) return outcome
def execute(self, parent_ud = smach.UserData()): """Run the state machine on entry to this state. This will set the "closed" flag and spin up the execute thread. Once this flag has been set, it will prevent more states from being added to the state machine. """ # This will prevent preempts from getting propagated to non-existent children with self._state_transitioning_lock: # Check state consistency try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.") return None # Set running flag self._is_running = True # Initialize preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) if self._preempt_requested: smach.logwarn("Preempt on State machine requested before even executing initial state. This could be a bug. Did last execution not service preemption?") # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = self._update_once() # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False if self._preempt_requested: self.service_preempt() return container_outcome
def execute(self, userdata): for waypoint in userdata.path: if self.preempt_requested(): self.service_preempt() return 'preempted' child_userdata = smach.UserData() child_userdata.waypoint = waypoint self.go_to_task.execute(child_userdata) return 'succeeded'
def take_off(self): for robot_id in self.available_robots: # TODO: clear all regions? userdata = smach.UserData() userdata.height = self.flight_levels[ robot_id] # TODO: Why not directly inside tasks? self.task_manager.start_task(robot_id, TakeOff(), userdata) self.task_manager.wait_for( [robot_id]) # Sequential takeoff for safety reasons
def ltm_execute(_self, ud=smach.UserData()): # Raise any caught exception, but always execute the callbacks. # The default UserData is required, as nodes won't always get a 'ud' arg. fn_start(self) try: outcome = fn_execute(ud) finally: fn_end(self) return outcome
def execute(self, parent_ud=smach.UserData()): try: l = parent_ud.transition_label except KeyError: l = 'init' s = self._initial_state_dict[l] self.set_initial_state([s]) out = smach.StateMachine.execute(self, parent_ud) return out
def go_home(self): # Go to start pose at 6 meters -> Now go to safe pose userdata_goto = smach.UserData() userdata_goto.waypoint = PoseStamped() #userdata_goto.waypoint = self.path[0] userdata_goto.waypoint.pose.position.x = -6.0 userdata_goto.waypoint.pose.position.y = 3.0 userdata_goto.waypoint.pose.position.z = 6.0 self.task_manager.start_task(self.robot_id, GoTo(), userdata_goto) self.task_manager.wait_for([self.robot_id])
def execute_deferred(self, parent_cb, userdata=None): if userdata is None: userdata = smach.UserData() # With lock with self._lock: #log("ParallelTask.execute_deferred()") assert (self._runstate == Task.READY) self._runstate = Task.EXECUTING self._parent_cb = parent_cb self._start_children(userdata)
def set_initial_state(self, initial_states, userdata=smach.UserData()): smach.logdebug("Setting initial state to "+str(initial_states)) if len(initial_states) > 1: smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.") # Set the initial state label if len(initial_states) > 0: self._initial_state_label = initial_states[0] # Set local userdata self.userdata.update(userdata)
def main(sm): try: # setup machine ltm.setup(sm) # execute machine ud = smach.UserData() sm.execute(ud) except rospy.ROSInterruptException: pass
def _test_smach_execute(): rospy.init_node('smach') ud = smach.UserData() # state = SleepState(12) state = CheckSmachEnabledState() rospy.loginfo("Executing test...") outcome = state.execute(ud) rospy.loginfo("outcome: %s" % outcome)
def execute_deferred(self, parent_cb, userdata=None): if userdata is None: userdata = smach.UserData() with self._lock: assert (self._runstate == Task.READY) self._runstate = Task.EXECUTING self._parent_cb = parent_cb self._execute_deferred_child(self._active_task_idx, self._make_cb(self._active_task_idx), userdata)
def execute(self, parent_ud=smach.UserData()): outcome = super(DoOnExit, self).execute(parent_ud) # run all finally states associated to the parent's outcome, and pass the outcome through for label, state in self._exit_states[outcome] + self._exit_states[ '__ANY__']: state.execute( smach.Remapper(self.userdata, state.get_registered_input_keys(), state.get_registered_output_keys(), self._remappings[label])) return outcome
def __init__(self, outcomes=[], input_keys=[], output_keys=[]): """Initializes callback lists as empty lists.""" smach.state.State.__init__(self, outcomes, input_keys, output_keys) self.userdata = smach.UserData() """Userdata to be passed to child states.""" # Callback lists self._start_cbs = [] self._transition_cbs = [] self._termination_cbs = []
def execute(self, parent_ud=smach.UserData()): """Run this function on entry of this state, check if a start gait is passed to the state.""" self._initial_state_label = self.default_start_state if 'transition' in self._outcomes: start_state = parent_ud.start_state if start_state is not None: self._initial_state_label = start_state parent_ud.start_state = None return super(WalkStateMachine, self).execute(parent_ud)