def test_undispach_event(self): sm_test = StateMachine() sm_test.sm['load'] = {1: print("test")} sm_test.inicia_tread() sm_test.on_event(event=1, args=None) sm_test.finaliza_tread() assert 1 == sm_test.events_queue.getLenth()
states.on_event('n') return elif (number == 3): for x in range (random.randint(100000,500000)): i = 0 print("in m") states.on_event('m') return elif (number == 4): for x in range (random.randint(100000,500000)): i = 0 print("in o") states.on_event('o') return else: return count = 0 # loop it 50 times for x in range(50) : try: #set up timer for 0.006000 seconds, if time up run except part of the code func_timeout(0.006000, random_state) except FunctionTimedOut: print("Time Up") count = count + 1 states.on_event('t') print(count)
class Behaviour: def __init__(self): rospy.init_node('behaviour_node', anonymous=True) self.ui_feedback_publisher = rospy.Publisher('ui_feedback', UI_feedback, queue_size=10) self.simple_goal_publisher = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) self.timer = None self.ac_goal = actionlib.SimpleActionClient('move_base', MoveBaseAction) # Creation of service proxies print("Waiting for db_adapter and locator services...") rospy.wait_for_service('perform_database_request', timeout=10) rospy.wait_for_service('locate_book', timeout=10) print("Services found, setting up proxies") try: self.db_adapter_proxy = rospy.ServiceProxy('perform_database_request', db_request) self.locator_proxy = rospy.ServiceProxy('locate_book', book_locator) except rospy.ServiceException: print("Service proxy creation failed !") self.state_machine = StateMachine(self) rospy.Subscriber("ui_command", UI, self.state_machine.on_event) def feedback_books(self, books_string): feedback_msg = UI_feedback() feedback_msg.type = UI_feedback.SEARCH_RESPONSE feedback_msg.payload = books_string self.ui_feedback_publisher.publish(feedback_msg) def feedback_message(self, msg, speak=True, title='', author=''): feedback_msg = UI_feedback() feedback_msg.type = UI_feedback.COMMUNICATION feedback_msg.payload = json.dumps({'type': msg, 'speak': speak, 'title': title, 'author': author}) self.ui_feedback_publisher.publish(feedback_msg) def feedback_loading(self, state=True): feedback_msg = UI_feedback() feedback_msg.type = UI_feedback.LOADING feedback_msg.payload = json.dumps(state) self.ui_feedback_publisher.publish(feedback_msg) def wait_reached_goal(self): wait = self.ac_goal.wait_for_result() if wait and self.ac_goal.get_result(): print("Goal reached !") self.state_machine.on_event(GoalReachedEvent()) def new_goal(self, book_location): print("Waiting for moving base actionlib to be available") self.ac_goal.wait_for_server(rospy.Duration(5.0)) print("Action goal server found") goal_msg = MoveBaseGoal() goal_msg.target_pose.header.frame_id = "map" goal_msg.target_pose.header.stamp = rospy.Time.now() goal_msg.target_pose.pose = book_location.pose print(book_location.pose) self.ac_goal.send_goal(goal_msg) thread.start_new_thread(self.wait_reached_goal, ()) print("New goal set !") def timeout_callback(self, event): self.timer = None self.state_machine.on_event(TimeOutEvent()) def set_timer(self, duration): if isinstance(self.timer, rospy.Timer): self.timer.shutdown() self.timer = rospy.Timer(rospy.Duration(duration), self.timeout_callback, True) def spin_node(self): print("Behaviour ready !") rospy.spin()
from func_timeout import * import time states = StateMachine() timer = time.time() #used for substituting the time waiting part of the state machine def Timer(): events = input() return events while 1: event = input() states.on_event(event) #print(states.state) #print(name) name = str(states.state) #print(name == 'Wait5Sec' or name == "Wait5Sec_I") if (name == 'Wait5Sec'): try: events = func_timeout(5, Timer) states.on_event(events) except FunctionTimedOut: print("Timed Out") events = 't' states.on_event(events) name = str(states.state) if (name == 'Operation' or name == 'InverseOperation'): try:
while True: # used for gathering the data of the force sensor from the UART # to determine if the pressure changed or not prev_rv = int_rv value = 0 for x in range(50): received_data = port.read(4) value += int.from_bytes(received_data,sys.byteorder) int_rv = value # this determines whether the pressure changes when cat stepped in if((int_rv/1000000) - (prev_rv/1000000) >= 10000): print("cat entering_pressure") states.on_event('p') if(timer_started == True): timer_started = False timer.cancel() if(timer2_started == True): timer2_started = False timer_started = False timer2.cancel() if(timer3_started == True): timer3_started = False timer3.cancel() # this determines whether the pressure changes when cat stepped out if((prev_rv/1000000) - (int_rv/1000000) >= 10000): print("cat leaving_pressure") states.on_event('o')