def handle_client(req):
ser = get_serial()
global reqFeedback
global reqInWaiting
pdsResponse = ArmRequestResponse()
timeout = 0.3 # 300ms timeout
reqInWaiting = True
sinceRequest = time.time()
rospy.loginfo('received ' + req.msg + ' request from GUI, sending to PDS MCU')
ser.write(str.encode(req.msg + '\n')) # ping the teensy
while pdsResponse.success is False and ((time.time() - sinceRequest) < timeout):
if reqFeedback is not '':
for request in requests:
for response in requests[request]:
if request == req.msg and response in reqFeedback:
pdsResponse.response = reqFeedback
pdsResponse.success = True
break
if pdsResponse.success:
break
else:
pdsResponse.response = reqFeedback
rospy.Rate(100).sleep()
rospy.loginfo('took ' + str(time.time() - sinceRequest) + ' seconds, sending this back to GUI: ')
rospy.loginfo(pdsResponse)
reqFeedback = ''
reqInWaiting = False
return pdsResponse
def handle_client(req):
ser = get_serial()
global reqFeedback
global reqInWaiting
armResponse = ArmRequestResponse()
timeout = 0.1 # 100ms timeout
reqInWaiting = True
sinceRequest = time.time()
rospy.loginfo('received ' + req.msg +
' request from GUI, sending to arm Teensy')
ser.write(str.encode(req.msg + '\n')) # ping the teensy
while armResponse.success is False and (time.time() - sinceRequest <
timeout):
if reqFeedback is not '':
print(reqFeedback)
for request in requests:
for response in requests[request]:
if request == req.msg and response in reqFeedback:
armResponse.response = reqFeedback
armResponse.success = True #a valid request and a valid response from the
break
if armResponse.success:
break
else:
armResponse.response += reqFeedback
rospy.Rate(100).sleep()
rospy.loginfo('took ' + str(time.time() - sinceRequest) +
' seconds, sending this back to GUI: ')
rospy.loginfo(armResponse)
reqFeedback = ''
reqInWaiting = False
return armResponse
def subscriber_callback(message):
ser = get_serial()
rospy.loginfo('received: ' + message.data +
' command from GUI, sending to ' + mcuName + ' Teensy')
command = str.encode(message.data + '\n')
ser.write(command) # send command to teensy
return
def handle_client(req):
# feedback to tell if script itself is responsive
if req.msg == 'ping':
feedbackPub.publish('listener received ping request')
ser = get_serial()
global reqFeedback
global reqInWaiting
roverResponse = ArmRequestResponse()
timeout = 0.2 # 200ms timeout
reqInWaiting=True
sinceRequest = time.time()
rospy.loginfo('received '+req.msg+' request from GUI, sending to rover Teensy')
ser.write(str.encode(req.msg+'\n')) # ping the teensy
while roverResponse.success is False and (time.time()-sinceRequest < timeout):
if reqFeedback is not '':
rospy.loginfo(reqFeedback)
for request in requests:
for response in requests[request]:
if request == req.msg and response in reqFeedback:
roverResponse.response = reqFeedback
roverResponse.success = True #a valid request and a valid response from the mcu
break
if roverResponse.success:
break
else:
roverResponse.response += reqFeedback
rospy.Rate(100).sleep()
rospy.loginfo('took '+str(time.time()-sinceRequest)+' seconds, sending this back to GUI: ')
rospy.loginfo(roverResponse)
reqFeedback=''
reqInWaiting=False
return roverResponse
def pds_command_subscriber_callback(message):
ser = get_serial()
rospy.loginfo('received: ' + message.data +
' command from GUI, sending to PDS')
command = str.encode(message.data + '\n')
ser.write(command) # send command to PDS
return
def rover_command_callback(message):
ser = get_serial()
rospy.loginfo('received: ' + message.data +
' command from GUI, sending to rover Teensy')
command = str.encode(message.data + '\n')
ser.write(command) # send command to teensy
ser.reset_input_buffer()
ser.reset_output_buffer()
def twist_callback(twist_msg):
""" Handles the twist message and sends it to the wheel MCU """
ser = get_serial()
linear, angular = accelerate_twist(twist_msg)
string_command = twist_to_rover_command(linear, angular)
command = str.encode(string_command)
ser.write(command) # send move command to wheel teensy
ser.reset_input_buffer()
ser.reset_output_buffer()
subscribe_topic = '/pds_command'
rospy.loginfo('Beginning to subscribe to "' + subscribe_topic + '" topic')
sub = rospy.Subscriber(subscribe_topic, String, pds_command_subscriber_callback)
service_name = '/pds_request'
rospy.loginfo('Waiting for "' + service_name + '" service request from client')
serv = rospy.Service(service_name, ArmRequest, handle_client)
search_success = init_serial(19200, mcuName)
if not search_success:
sys.exit(1)
# service requests are implicitly handled but only at the rate the node publishes at
ser = get_serial()
global reqFeedback
reqFeedback = ''
global reqInWaiting
reqInWaiting = False
try:
while not rospy.is_shutdown():
# if I try reading from the serial port inside callbacks, bad things happen
# instead I send the data elsewhere if required but only read from serial here.
# not sure if I need the same precautions when writing but so far it seems ok.
if ser.in_waiting:
data = ''
feedback = None
try:
data = ser.readline().decode()
feedback = stripFeedback(data)
