79 lines
2.2 KiB
Python
Executable File
79 lines
2.2 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import rospy
|
|
|
|
from teleoperation.srv import InformController, InformControllerResponse
|
|
|
|
|
|
STATE = 'idle' # Also walk, imitate, fallen, recognizing
|
|
|
|
|
|
def inform_controller_factory(who):
|
|
def inform_controller(what):
|
|
try:
|
|
inform_controller = rospy.ServiceProxy('inform_controller',
|
|
InformController)
|
|
perm = inform_controller(who, what).permission
|
|
except rospy.service.ServiceException:
|
|
rospy.signal_shutdown('Controller is dead')
|
|
perm = False
|
|
return perm
|
|
return inform_controller
|
|
|
|
|
|
def handle_request(r):
|
|
global STATE
|
|
module = r.module
|
|
message = r.message
|
|
_state_old = STATE
|
|
permission = False
|
|
|
|
if module == 'walker':
|
|
if message == 'move':
|
|
if STATE in ('idle', 'walk'):
|
|
STATE = 'walk'
|
|
permission = True
|
|
elif message == 'stop':
|
|
if STATE == 'walk':
|
|
STATE = 'idle'
|
|
permission = True
|
|
|
|
elif module == 'fall_detector':
|
|
permission = True
|
|
if message in ('falling', 'fallen'):
|
|
STATE = 'fallen'
|
|
elif message == 'recovered':
|
|
STATE = 'idle'
|
|
|
|
elif module == 'imitator':
|
|
if message == 'imitate':
|
|
if STATE == 'imitate':
|
|
permission = True
|
|
|
|
elif module == 'speech':
|
|
if message == 'recognize':
|
|
if STATE in ('idle', 'imitate'):
|
|
permission = True
|
|
elif message == 'imitate':
|
|
if STATE == 'idle':
|
|
STATE = 'imitate'
|
|
permission = True
|
|
elif message == 'stop':
|
|
if STATE == 'imitate':
|
|
STATE = 'idle'
|
|
permission = True
|
|
|
|
rospy.logdebug(
|
|
'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % (
|
|
module, message, permission, STATE
|
|
)
|
|
)
|
|
if _state_old != STATE:
|
|
rospy.loginfo('{} -> {}'.format(_state_old, STATE))
|
|
return InformControllerResponse(permission)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('controller', log_level=rospy.INFO)
|
|
ic = rospy.Service('inform_controller', InformController, handle_request)
|
|
rospy.spin()
|