rover-gimbal-client.py

#!/usr/bin/env python3
# gimbal-client.py is the Rover client app to receive commands from Remote and respond with ACK/WAK
## attribution: https://realpython.com/python-sockets/

from __future__ import division
from Raspi_PWM_Servo_Driver import PWM
from datetime import datetime, timedelta
import sys, socket, selectors, traceback, time, atexit, logging
import configparser

sel = selectors.DefaultSelector()

# setup logging
log_file = '/home/rover_logs/rover-gimbal-log-' + datetime.now().strftime('%m-%d-%y') + '.txt'
logging.basicConfig(filename=log_file,level=logging.DEBUG)
logging.info('*********************************** Rover Gimbal Client App Logging Started ' + datetime.now().strftime('%m-%d-%y %H:%M:%S'))
import rover_libclient

def restart_logging():
    log_file = '/home/rover_logs/rover-gimbal-log-' + datetime.now().strftime('%m-%d-%y') + '.txt'
    logging.basicConfig(filename=log_file,level=logging.DEBUG)
    logging.info('*********************************** Rover Gimbal Client App Logging Restarting ' + datetime.now().strftime('%m-%d-%y %H:%M:%S'))

def shutdown():
    print('Exiting app, shutting down gracefully.')
    try:
        pwm.setPWM(0, 0, 0)
        pwm.setPWM(1, 0, 0)
        pwm.softwareReset
        sel.close()
    except: pass
atexit.register(shutdown)

# Initialise the Gimbal using the configuration values
config = configparser.ConfigParser()
config.read('/home/apps/rover.cfg')
ping_seconds = int(config.get('Gimbal Parameters', 'ping_seconds'))
pan_channel = int(config.get('Gimbal Parameters', 'pan_channel'))
tilt_channel = int(config.get('Gimbal Parameters', 'tilt_channel'))
pan_center = int(config.get('Gimbal Parameters', 'pan_center'))
tilt_center = int(config.get('Gimbal Parameters', 'tilt_center'))
step_amount = int(config.get('Gimbal Parameters', 'step_amount'))
servoMin = int(config.get('Gimbal Parameters', 'servoMin'))
servoMax = int(config.get('Gimbal Parameters', 'servoMax'))
port = int(config.get('Gimbal Parameters', 'port'))
host = config.get('Rover Parameters', 'host')
device = config.get('Rover Parameters', 'device')
BUFSIZ = int(config.get('Rover Parameters', 'BUFSIZ'))
NoWCK = config.get('Rover Parameters', 'NoWCK')
rover_commands = config.get('Rover Parameters', 'rover_commands')

# Setup Adafruit Motor init and values, needs to match stepper motor configuration

pwm = PWM(0x6F)
pwm.setPWMFreq(60)

pan_home = pan_center
tilt_home = tilt_center

pan_servo_min = servoMin
pan_servo_max = servoMax
tilt_servo_min = servoMin
tilt_servo_max = 470

pan_couch = 486
tilt_couch = 310

pan_current = pan_center
tilt_current = tilt_center

#	Connection startup - Socket and Message Object Creation

def start_connection(host, port, device, request):
    global sock, events, message
    addr = (host, port)
    logging.debug("***** gimbal-client starting connection to " + str(addr))
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.setblocking(False)
    sock.connect_ex(addr)
    events = selectors.EVENT_READ | selectors.EVENT_WRITE
    message = rover_libclient.Message(sel, sock, addr, device, request)
    sel.register(sock, events, data=message)
    logging.debug("***** gimbal-client completed connection to Rover Server socket at" + str(addr))

#	Open port to local rover comm server and receive Connected before entering receive mode from remote 

request = dict(encoding="utf-8", content=dict(device=device, action='connect'))
start_connection(host, port, device, request)	# Sends rover name and connect request
message.buffer_size = BUFSIZ

#	Connection Startup (receive json field message) ---> Connected
result = ''
try:
    while True:
        events = sel.select(timeout=1)
        for key, mask in events:
            message.request = {'device': 'rover', 'action': 'connect'}
            try:
                message.process_events(mask)
                content = message.json_content
                if len(content) != 0:
                    result = content["result"]
            except Exception:
                logging.warning("***** gimbal-client startup error: exception for", f"{message.addr}:\n")
                message.close()
        if result == 'Connected': 
            logging.debug("***** gimbal-client Connection result = " + result + '.')
            break
        # Check for a socket being monitored to continue.
        if not sel.get_map():
            logging.error("***** gimbal-client Socket failure, exiting app.")
            break
except:
    logging.error("***** gimbal-client Failed to receive Connected response at startup. Attempting to poll.")
    pass

#	Enter receive mode for remote to send ping to respond with ACK

content = {}
events = selectors.EVENT_READ
message._jsonheader_len = None		# clear json values for read
message.jsonheader = None
message.response = None
message.request = None

def send_response(response):
    str_datetime = '***** gimbal-client Responding to remote with ' + str(response) + ' at ' + str(datetime.now())[0:19]
    logging.debug(str_datetime)
    message.request = response
    message.queue_request()
    message.json_content = {}
    events = selectors.EVENT_WRITE
    message.process_events(2)

# set state machine initial values
ping_period = timedelta(seconds = (ping_seconds * 2))
ping_timeout = datetime.now() + ping_period
remote_active = False

try:
    gimbal_busy = False
    while True:
        events = sel.select(timeout=.01)
        for key, mask in events:
            message = key.data
            try:
                message.process_events(mask)
            except Exception:
                logging.error("***** gimbal-client  main loop error: exception for", f"{message.addr}:\n{traceback.format_exc()}")
                message.close()
            content = message.json_content
            if len(content) != 0:
                device = content['device']
                if device == 'rover':		# If this command is for this app, rover, send ACK for tests.
                    command = content['action']
                    ACK_response = {'device':'remote','action': str(command), 'result':'ACK'}
                    WAK_response = {'device':'remote','action': str(command), 'result':'WAK'}
                    if command == 'ping':	# Send ACK in response to ping
                        remote_active = True
                        ping_timeout = datetime.now() + ping_period
                        send_response(ACK_response)

                    elif command == '27': # ESCape key
                        logging.debug('***** gimbal-client ESCape key Shutdown at ' + str(datetime.now())[0:19])
                        shutdown()
                        break

                    elif 'pingging=' in command: # This command sets the ping timer based on remote cfg file
                        restart_logging()
                        ping_seconds = int(command.replace('pingging=',''))
                        logging.debug('***** gimbal-client Sync ping timer with Remote to <' + str(ping_seconds) + '> seconds at ' + str(datetime.now())[0:19])
                        ping_period = timedelta(seconds = (ping_seconds * 2))
                        ping_timeout = datetime.now() + ping_period
                        config.set('Gimbal Parameters', 'ping_seconds', str(ping_seconds))
                        with open('/home/apps/rover.cfg', 'w') as configfile:
                            config.write(configfile)
                        send_response(ACK_response)

                    elif command == '258':  					# DOWN arrow
                        logging.debug("***** gimbal-client Received command = " + command + ' - processing DOWN arrow.')
                        tilt_current = tilt_current + step_amount
                        if tilt_current > tilt_servo_max:
                            tilt_current = tilt_servo_max
                        if tilt_current < tilt_servo_max:
                            pwm.setPWM(tilt_channel, 0, tilt_current)
                        send_response(ACK_response)

                    elif command == '259':  					# UP arrow
                        logging.debug("***** gimbal-client Received command = " + command + ' - processing UP arrow.')
                        tilt_current = tilt_current - step_amount
                        if tilt_current < tilt_servo_min:
                            tilt_current = tilt_servo_min
                        if tilt_current > tilt_servo_min:
                            pwm.setPWM(tilt_channel, 0, tilt_current)
                        send_response(ACK_response)

                    elif command == '260':  					# LEFT arrow
                        logging.debug("***** gimbal-client Received command = " + command + ' - processing LEFT arrow.')
                        pan_current += step_amount
                        if pan_current > pan_servo_max:
                            pan_current = pan_servo_max
                        if pan_current < pan_servo_max:
                            pwm.setPWM(pan_channel, 0, pan_current)
                        send_response(ACK_response)
                
                    elif command == '261':  					# RIGHT arrow
                        logging.debug("***** gimbal-client Received command = " + command + ' - processing RIGHT arrow.')
                        pan_current -= step_amount
                        if pan_current < pan_servo_min:
                            pan_current = pan_servo_min
                        if pan_current > pan_servo_min:
                            pwm.setPWM(pan_channel, 0, pan_current)
                        send_response(ACK_response)

                    elif command == '99':  						# c for CENTER
                        logging.debug("***** gimbal-client Received command = " + command + ' - processing c for Center.')
                        pan_current = pan_center
                        tilt_current = tilt_center
                        send_response(WAK_response)
                        pwm.setPWM(pan_channel, 0, pan_current)
                        pwm.setPWM(tilt_channel, 0, tilt_current)
                        time.sleep(2)
#                        ACK_response = {'device':'remote','action': str(command), 'result':'ACK'}	# Have to do this because wak somehow replaced ACK.
                        send_response(ACK_response)
                        logging.debug("***** gimbal-client Sent response = " + str(ACK_response))

                    elif command == '67':  						# C for COUCH
                        logging.debug("***** gimbal-client Received command = " + command + ' - processing C for Couch.')
                        pan_current = pan_couch
                        tilt_current = tilt_couch
                        send_response(WAK_response)
                        pwm.setPWM(pan_channel, 0, pan_current)
                        pwm.setPWM(tilt_channel, 0, tilt_current)
                        time.sleep(2)
                        ACK_response = {'device':'remote','action': str(command), 'result':'ACK'}
                        send_response(ACK_response)

## From rover-gimbal-client.py:
                    elif command == '393' and not gimbal_busy:                      # SHIFT LEFT arrow - Pan Camera Left
                        gimbal_busy = True
                        pan_current += step_amount
                        if pan_current > pan_servo_max:
                            pan_current = pan_servo_max
                        if pan_current <= pan_servo_max:
#                            pos.goto((360-pan_current), tilt_current)
                            pwm.setPWM(pan_channel, 0, pan_current)
                        send_response(ACK_response)
                        gimbal_busy = False
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed Shift LEFT arrow, pan_current = ' + str(pan_current) + '.')

                    elif command == '402' and not gimbal_busy:                      # SHIFT RIGHT arrow - Pan Camera Right
                        gimbal_busy = True
                        pan_current -= step_amount
                        if pan_current < pan_servo_min:
                            pan_current = pan_servo_min
                        if pan_current > pan_servo_min:
#                            pos.goto((360-pan_current), tilt_current)
                            pwm.setPWM(pan_channel, 0, pan_current)
                        send_response(ACK_response)
                        gimbal_busy = False
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed Shift RIGHT arrow, pan_current = ' + str(pan_current) + '.')
						
                    elif command == '336' and not gimbal_busy:                      # SHIFT DOWN arrow - Tilt Camera Down
                        gimbal_busy = True
                        tilt_current += step_amount				# When migrating to Rover, replace these lines with commented lines below these
                        if tilt_current > tilt_servo_max:
                            tilt_current = tilt_servo_max
                        if tilt_current < tilt_servo_max:
                            pwm.setPWM(tilt_channel, 0, tilt_current)
                        send_response(ACK_response)

#                        tilt_current -= step_amount
#                        if tilt_current < tilt_servo_min:
#                            tilt_current = tilt_servo_min
#                            return 'down arrow invalid, tilt_current = ' + str(tilt_current)
#                        gimbal_busy = True
#                        if tilt_current > tilt_servo_min:
#                            pos.goto(pan_current, tilt_current)
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed Shift DOWN arrow, pan_current = ' + str(tilt_current) + '.')
                        gimbal_busy = False
						
                    elif command == '337' and not gimbal_busy:                      # SHIFT UP arrow - Tilt Camera Up
                        gimbal_busy = True
                        tilt_current = tilt_current - step_amount
                        if tilt_current < tilt_servo_min:
                            tilt_current = tilt_servo_min
                        if tilt_current > tilt_servo_min:
                            pwm.setPWM(tilt_channel, 0, tilt_current)
                        send_response(ACK_response)

#                        tilt_current += step_amount
#                        if tilt_current > tilt_servo_max:
#                            tilt_current = tilt_servo_max
#                            return 'up  arrow invalid, tilt_current max = ' + str(tilt_current)
#                        gimbal_busy = True
#                        if tilt_current < tilt_servo_max:
#                            pos.goto(pan_current, tilt_current)
                        gimbal_busy = False
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed Shift UP arrow, tilt_current = ' + str(tilt_current) + '.')

                    elif command == '72' and not gimbal_busy:                           # H for HOME
                        gimbal_busy = True
                        pan_current = pan_center
                        tilt_current = tilt_center
                        send_response(WAK_response)
                        pwm.setPWM(pan_channel, 0, pan_current)
                        pwm.setPWM(tilt_channel, 0, tilt_current)
                        time.sleep(1)
                        send_response(ACK_response)
#                        gimbal_busy = True
#                        if pan_current == pan_home and tilt_current == tilt_home:
#							time.sleep(.2)  # waste time when nothing is going to happen and comm needs to delay ACK to remote
#                        pan_current = pan_home
#                        tilt_current = tilt_home
#                        pos.goto(pan_current, tilt_current)
                        gimbal_busy = False
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed H for Home pan = ' + str(pan_current) + ', tilt = ' + str(tilt_current) + '.')

                    elif command == '76' and not gimbal_busy:                           # L for LEFT 90 degrees
                        gimbal_busy = True
                        pan_current = pan_servo_max
                        send_response(WAK_response)
                        pwm.setPWM(pan_channel, 0, pan_current)
                        time.sleep(1)
                        send_response(ACK_response)
#                        pan_current = 85
#                        tilt_current = 0
#                        pos.goto(pan_current, tilt_current)
                        gimbal_busy = False
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed 90 degrees LEFT pan = ' + str(pan_current) + ', tilt = ' + str(tilt_current) + '.')

                    elif command == '82' and not gimbal_busy:                           # R for RIGHT 90 degrees
                        gimbal_busy = True
                        pan_current = pan_servo_min
                        send_response(WAK_response)
                        pwm.setPWM(pan_channel, 0, pan_current)
                        time.sleep(1)
                        send_response(ACK_response)
#                        pan_current = 270
#                        tilt_current = 0
#                        pos.goto(pan_current, tilt_current)
                        gimbal_busy = False
                        logging.debug("***** gimbal-client Received command = " + command + ' - processed 90 degrees RIGHT pan = ' + str(pan_current) + ', tilt = ' + str(tilt_current) + '.')

#                    elif c == 114 or c == 82:  				# r or R for RECALIBRATE
#                stdscr.addstr('RECALIBRATE code removed until later tests      ')
                else:
                    command = content['result']			# This should be echo back from sending last response, ignore
#                    logging.debug("***** gimbal-client Received Unexpected command = " + command + '.')
#                    str_datetime = 'gimbal-client response to Gimbal last command: ' + str(command) + ' at ' + str(datetime.now())[0:19]
#                    logging.debug(str_datetime)
#                    print('gimbal-client response to Gimbal ping: ', command + ' at ' + str(datetime.now())[0:19])
#                if device == 'rover': 

                if ping_timeout <= datetime.now() and remote_active:		# Check to see if past time for Remote to ping
                    remote_active = False
                    logging.debug('"***** gimbal-client Remote Control ping timed out at' + str(datetime.now())[0:19])

        # Check for a socket being monitored to continue.
        if not sel.get_map():
            break
except KeyboardInterrupt:
    print("\ncaught keyboard interrupt, exiting\n\n")
finally:
	sel.close()

rover-gimbal-server.py

#!/usr/bin/env python3
## attribution: https://realpython.com/python-sockets/

# This gimbal-server.py app simply opens the Gimbal port and listens for requests.
# Once request received, libserver sends appropriate connection responses,
# then this module broadcasts all incoming requests to connected client devices.

import sys, socket, selectors, traceback, logging
from datetime import datetime
import configparser

sel = selectors.DefaultSelector()
clients = {}

# setup logging
log_file = '/home/rover_logs/rover-gimbal-log-' + datetime.now().strftime('%m-%d-%y') + '.txt'
logging.basicConfig(filename=log_file,level=logging.DEBUG)
logging.info('=================================== Rover Gimbal Server App Logging Started ' + datetime.now().strftime('%m-%d-%y %H:%M:%S'))
import rover_libserver			# import after logging starts.

# Initialise the Gimbal using the configuration values
config = configparser.ConfigParser()
config.read('/home/apps/rover.cfg')
port = int(config.get('Gimbal Parameters', 'port'))
host = ''

def restart_logging():
    log_file = '/home/rover_logs/rover-gimbal-log-' + datetime.now().strftime('%m-%d-%y') + '.txt'
    logging.basicConfig(filename=log_file,level=logging.DEBUG)
    logging.info('=================================== Rover Gimbal Server App Logging Restarting ' + datetime.now().strftime('%m-%d-%y %H:%M:%S'))

def accept_wrapper(sock):
    client, client_address = sock.accept()  # Should be ready to read
    str_datetime = '===== gimbal-server accepted connection from ' + str(client_address) + ', at ' + datetime.now().strftime('%m-%d-%y %H:%M:%S')
    logging.info(str_datetime)
    client.setblocking(False)
    message = rover_libserver.Message(sel, client, client_address)
    clients[client] = message
    sel.register(client, selectors.EVENT_READ, data=message)

lsock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# Avoid bind() exception: OSError: [Errno 48] Address already in use
lsock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
lsock.bind((host, port))
lsock.listen()
print("listening on", (host, port))
lsock.setblocking(False)
sel.register(lsock, selectors.EVENT_READ, data=None)

#	Now handle incoming devices (remote/rover) connection requests
#	This routine will handle all broadcast messages after startup

try:
    while True:
        events = sel.select(timeout=None)
        for key, mask in events:
            if key.data is None:
                accept_wrapper(key.fileobj)
            else:
                message = key.data
                try:
                    message.process_events(mask)
                    content = message.json_content
                    if len(content) != 0:
                        device=content["device"]
                        if message.name == None:	# If this is the first time, save name connected 			
                            message.name = device
                            message.request = {"result": "Connected"}
                            message.queue_request()
                            message.peer_closed = False
                            events = selectors.EVENT_WRITE
                            message.selector.modify(message.client, events, data=message)
                            if device == 'rover':
                                restart_logging()
                            logging.debug('===== gimbal-server Connected new client name = ' + str(device))
                        else:
                            logging.debug('===== gimbal-server broadcasting {' + str(content) + '} at ' + str(datetime.now())[0:19])
                            for client in clients:
                                message = clients[client]
                                message.request = content
                                if '[closed]' not in str(client): 
                                    message.queue_request()
                                    message.response_created = True
                                    message.broadcast()
                except Exception as e:
                    pass
                    logging.error("===== gimbal-server error: exception for" + str(message.client_address) + ':' + str(e))
                    try:
                        message.close()
                    except:
                        logging.error('===== gimbal-server Exception failed to close message.')
                        pass
                try:				# Garbage clean dropped clients
                    for client in clients:
                        if '[closed]' in str(client): 
                            logging.info("===== gimbal-server deleting record " + str(clients[client]))
                            del clients[client]
                except: pass
except KeyboardInterrupt:
    print("\ncaught keyboard interrupt, exiting")
finally:
    sel.close()