37 lines
946 B
Python
Executable File
37 lines
946 B
Python
Executable File
#! /usr/bin/env python
|
|
import rospy
|
|
|
|
from teleoperation.srv import InformController, InformControllerResponse
|
|
|
|
|
|
STATE = 'idle' # Also walk, imitate and fallen
|
|
|
|
|
|
def handle_request(r):
|
|
module = r.module
|
|
message = r.message
|
|
global STATE
|
|
|
|
if module == 'walker':
|
|
if message == 'move':
|
|
if STATE in ('idle', 'walk'):
|
|
STATE = 'walk'
|
|
permission = True
|
|
else:
|
|
permission = False
|
|
elif message == 'stop':
|
|
if STATE == 'walk':
|
|
STATE = 'idle'
|
|
permission = True
|
|
print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
|
|
module, message, permission, STATE
|
|
)
|
|
return InformControllerResponse(permission)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('controller')
|
|
ic = rospy.Service('inform_controller', InformController, handle_request)
|
|
rospy.spin()
|
|
# initialize stuff
|