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')
