def __init__(self, GPIO):
self.HOST = '192.168.10.10'
self.PORT = 50007
self.socketClient = SocketClient(self.HOST, self.PORT)
self.GPIO_PIR = GPIO
# Threading init
threading.Thread.__init__(self)
self.setName("PIR" + str(self.GPIO_PIR))
self.daemon = True
def test_sending(self):
with patch('socket.socket') as socket_mock:
mock = Mock()
socket_mock.return_value = mock
sC = SocketClient()
sC.Send("ABBA")
del sC
mock.send.assert_called_with(b"ABBA")
def test_receive_nothing_to_receive(self):
with patch('socket.socket') as socket_socket_mock:
with patch('select.select') as select_select_mock:
socket_obj_mock = Mock()
socket_socket_mock.return_value = socket_obj_mock
select_select_mock.return_value = ([],[],[])
sC = SocketClient()
data = sC.Receive(1.5)
del sC
select_select_mock.assert_called_with([socket_obj_mock],[],[],1.5)
assert not socket_obj_mock.recv.called
assert data == ""
def main():
# Init motion detection and socketserver
socketClient = SocketClient()
motion = Motion(socketClient)
#capture = Capture()
# Start program
motion.start()
socketClient.start()
#capture.start()
# Wait until threads die
motion.join()
socketClient.join()
def test_receive(self):
with patch('socket.socket') as socket_socket_mock:
with patch('select.select') as select_select_mock:
socket_obj_mock = Mock()
socket_obj_mock.recv.return_value = b"ABCD"
socket_socket_mock.return_value = socket_obj_mock
select_select_mock.return_value = ([socket_obj_mock],[],[])
sC = SocketClient()
data = sC.Receive(1.5)
del sC
select_select_mock.assert_called_with([socket_obj_mock],[],[],1.5)
socket_obj_mock.recv.assert_called_with(1024)
assert data == b"ABCD"
def test_server_params(self):
with patch('socket.socket') as socket_mock:
mock = Mock()
socket_mock.return_value = mock
sC = SocketClient("1.2.3.4", 1234)
del sC
socket_mock.assert_called_with(socket.AF_INET, socket.SOCK_STREAM)
mock.connect.assert_called_with(("1.2.3.4", 1234))
def test_create_connect_destroy(self):
with patch('socket.socket') as socket_mock:
mock = Mock()
socket_mock.return_value = mock
sC = SocketClient()
del sC
socket_mock.assert_called_with(socket.AF_INET, socket.SOCK_STREAM)
mock.connect.assert_called_with(("127.0.0.1", 5001))
mock.close.assert_called()
def __init__(self, server_address):
'''
Creates the socket and defaults for CLI
'''
cmd.Cmd.__init__(self)
# create/setup client socket to pico
self.client = SocketClient(server_address)
self.client.set_listener_callback(self.pico_cmd_callback)
# setup cmd defaults
self.prompt = 'pico> '
self.intro = 'Connected to pico at %s. Press ctrl-D to exit' % str(server_address)
def main():
client = SocketClient() #用變數去接收class，是一個object
select_result = "initial"
while select_result != "exit":
select_result = print_menu()
try:
action_list[select_result](client).execute()
except:
pass
client.client_socket.close()
def __init__(self, socket_connection=False):
'''
Constructor
'''
self.socket_connection = socket_connection
self.implemented_functions = {
"MOVE": self.RC8Commands.MOVE,
"DRAW": self.RC8Commands.DRAW,
# "DRIVE" : self.RC8Commands.DRIVE
# "DRIVEA" : self.RC8Commands.DRIVEA
}
# Ensure there are no descrepancies between the implemented functions and the functions held within the enum
assert len(self.implemented_functions) == len(
NGS_RC8_API.RC8Commands
), "There is a descrepancy in the number of implemented RC8Commands and the dictionary used to map these functions. Dictionary's current state:\n\t" + str(
self.implemented_functions)
# Create a SocketClient connection if the API requests it. If not, enters echo mode.
if socket_connection == True:
self.socket_client = SocketClient()
self.socket_client.empty()
def __init__(self, std_vel, std_steer, dt):
self.std_vel = std_vel
self.std_steer = std_steer
self.max_angular = std_vel - std_steer
self.wheelbase = 0.135
self.odom = odom(self.wheelbase)
self.PID = PID(45, 0.0, 0.0) #P, I, D
self.client = SocketClient()
self.dt = dt
self.landmarks = []
self.R = np.diag([0.00025, 0.00025])
self.Q = np.diag([0.01, 0.01])
self.I = np.identity(
3) #inital robot_x , robot_y, robot_theta state vector?
self.current_path = 0
self.debug = False
self.update = 0
self.state = 0
self.stored_theta = 0
self.stream = PiVideoStream(
) #start the video stream on a seperate thread
self.measure = pixelCalibrate(1200,
90) #calibrate the camera for distances
def main():
client = SocketClient() #用變數去接收class，是一個object
select_result = "initial"
while select_result != "exit":
select_result = print_menu()
try:
#注意參數丟的位置
#下面參數是丟到"__init__"裡面
#和"SocketServer.py"去做比對
action_list[select_result](client).execute()
except:
pass
client.client_socket.close()
def main():
# Init socket comm
socketServer = SocketServer()
socketClient = SocketClient()
# Start threads
socketServer.start() # Always start the server before the client
socketClient.start()
time.sleep(1) # Allow the sockets to be setup before using them
try:
receivedMessages = Queue.Queue() # Init a queue to store received messages in
# Send/Recv example
while(True):
socketClient.send("Hasse gillar kaffe \n") # VERY IMPORTANT!!!! ALWAYS PROVIDE A NEWLINE AT THE END OF THE MESSAGE OR THE RECV FUNCTION WILL NEVER STOP!
time.sleep(1) # Make sure that our message has been received (Remove in real code)
receivedMessages = socketServer.getMessages()
handleMessages(receivedMessages)
except (KeyboardInterrupt, SystemExit):
raise
finally:
# Let threads die gracefully
socketClient.kill()
socketServer.kill()
# Wait until threads die
socketClient.join()
socketServer.join()
def main():
# Init motion detection and socketserver
socketClient = SocketClient()
motion = Motion(socketClient)
#capture = Capture()
# Start program
motion.start()
socketClient.start()
#capture.start()
# Wait until threads die
motion.join()
socketClient.join()
class PiButton():
def __init__(self):
self.HOST = '192.168.10.10'
self.PORT = 50007
self.socketClient = SocketClient(self.HOST, self.PORT)
def start(self):
# Use BCM GPIO references
# instead of physical pin numbers
GPIO.setmode(GPIO.BCM)
# Define GPIO to use on Pi
GPIO_BUTTON = 8
#print "PIR Module Holding Time Test (CTRL-C to exit)"
# Set pin as input
GPIO.setup(GPIO_BUTTON, GPIO.IN) # Echo
# Input states
input = 0
prevInput = 1
try:
# Loop until users quits with CTRL-C
while True:
input = GPIO.input(GPIO_BUTTON)
if ((not prevInput) and input):
#print "Button pressed"
time.sleep(0.3)
self.socketClient.connectUDP()
self.socketClient.send("Button pressed")
self.socketClient.close()
prevInput = input
time.sleep(0.01)
except KeyboardInterrupt:
print " Quit"
# Reset GPIO settings
self.clean()
def clean(self):
GPIO.cleanup()
def initialize_connection(self):
self.sock_client = SocketClient()
self.sock_client.initialize_connection(server_address)
class MainFrame(wx.Frame):
def __init__(self, *args, **kwargs):
super(MainFrame, self).__init__(*args, **kwargs)
self.initialize_connection()
self.initialize_frame_UI()
self.initialize_action_text_UI()
pub.subscribe(self.initialize_board_data, "initial")
pub.subscribe(self.on_waiting_opponent, "wait_connect")
# pub.subscribe(self.on_opponent_turn, "wait_turn")
# pub.subscribe(self.on_own_turn, "go_turn")
ReceiverThread(self.sock_client)
# request board + turn
# enable fire or #waitForOp
def initialize_connection(self):
self.sock_client = SocketClient()
self.sock_client.initialize_connection(server_address)
def initialize_frame_UI(self):
self.mainPanel = wx.Panel(self)
self.mainPanel.SetFont(wx.SystemSettings.GetFont(0))
self.sizeX, self.sizeY = wx.GetDisplaySize()
self.Show()
self.Maximize()
def initialize_action_text_UI(self):
action_text = "Waiting for opponent to connect ..."
self.action_text_field = wx.StaticText(self.mainPanel,
pos=(775 - SizeHelpers.getTextSizeX(action_text) / 2, 150),
label=action_text)
def initialize_board_data(self, arr):
self.ownBoardArray = arr
wx.CallAfter(self.initialize_boards_UI)
def initialize_boards_UI(self):
print "called init BOARDS"
quarterPointX = wx.GetDisplaySize()[0] / 4
wx.StaticText(self.mainPanel, label="Your board",
pos=(quarterPointX - SizeHelpers.getTextSizeX("Your board") / 2, 30))
wx.StaticText(self.mainPanel, label="Opponent board",
pos=(self.sizeX - quarterPointX - SizeHelpers.getTextSizeX("Opponent board") / 2, 30))
self.opponentBoardButtons = []
ownStartX, oppStartX, startY, offsetX, offsetY = 200, 950, 100, 0, 0
# init own buttons(from recv array)
# 2d 10x10 array
self.ownBoardButtons = []
for row in self.ownBoardArray:
for cell in row:
# own board buttons
ownBoardButton = wx.Button(self.mainPanel, label=cell, size=(40, 40),
pos=(ownStartX + offsetX, startY + offsetY))
ownBoardButton.Disable()
self.ownBoardButtons.append(ownBoardButton)
# opponent board buttons
oppBoardButton = wx.Button(self.mainPanel, label="A", size=(40, 40),
pos=(oppStartX + + offsetX, startY + offsetY))
oppBoardButton.Bind(wx.EVT_BUTTON, self.OnFire)
self.opponentBoardButtons.append(oppBoardButton)
offsetX += 40
if offsetX > 360:
offsetY += 40
offsetX = 0
ReceiverThread(self.sock_client)
def set_action_text(self, text):
self.action_text_field.SetLabelText(text)
def on_waiting_opponent(self):
ReceiverThread(self.sock_client)
def OnFire(self, e):
btnObj = e.GetEventObject()
# send guess to server
btnIndx = self.opponentBoardButtons.index(btnObj)
self.sock_client.send_data(btnIndx)
# result - True/False
isHit = True
import sys
from SocketClient import SocketClient
from CheckInput import CheckInput
from GaduGaduClient import GaduGaduClient
server_ip_address = "127.0.0.1"
if len(sys.argv) >= 2:
server_ip_address = sys.argv[1]
client = SocketClient(server_ip_address)
ggClient = GaduGaduClient(client)
print("Enter 'exit' to close client")
nick = input("Enter your nick: ")
ggClient.Login(nick)
print("You can chat with your friends")
while True:
msg = CheckInput()
if msg == 'exit':
break
if "" != msg:
ggClient.SendMsg(msg)
data = client.Receive()
if "" != data:
print (data.decode())
class picoClient(cmd.Cmd):
'''
Simple class to encapulate socket communications with
a picoceptor. Uses a SocketClient, which implements a
callback function for data coming into the socket. Also,
this client is a subclass of the Cmd class, which implements
a simple CLI.
History:
THR: Oct, 2012: Initial development
'''
def __init__(self, server_address):
'''
Creates the socket and defaults for CLI
'''
cmd.Cmd.__init__(self)
# create/setup client socket to pico
self.client = SocketClient(server_address)
self.client.set_listener_callback(self.pico_cmd_callback)
# setup cmd defaults
self.prompt = 'pico> '
self.intro = 'Connected to pico at %s. Press ctrl-D to exit' % str(server_address)
def pico_cmd_callback(self, pico_resp):
'''
Callback method used for socket input
'''
if pico_resp.find('*IDN') >= 0:
IDN_list = pico_resp.split(',')
print '----- Picoceptor Info ------'
print ' Model : %s' % IDN_list[1].split()[0]
print ' Rev : %s' % IDN_list[1].split()[1]
print ' S/N : %s' % IDN_list[2]
print ' CA version : %s' % IDN_list[3]
else:
print ">> " + pico_resp,
self._hist += [ "[resp]: %s" % pico_resp.strip() ]
def send_msg(self, msg_str, verbose=False):
'''
Sends string out socket
'''
self.client.send_msg(msg_str)
if verbose: print "Sent msg: " + msg_str;
time.sleep(0.050) # sleep a bit to allow reply
def send_msgs(self, msgs_str, verbose=False):
'''
Parse msgs str (separated by ';') into individual msgs and send
'''
for msg_str in msgs_str.split(';'):
self.send_msg(msg_str, verbose)
def default(self, line):
'''
This method is called whenever data is entered into the CLI
'''
if line != 'nocmd':
self.send_msg(line)
def preloop(self):
'''
Called (automatically) right before we enter CLI loop.
this starts the socket listener thread that
reads input and passes to callback
'''
self._hist = [] # create a list to hold history...
self.client.start()
time.sleep(0.050) # sleep a bit to allow listener thread to startup
def precmd(self, line):
""" This method is called after the line has been input but before
it has been interpreted. If you want to modifdy the input line
before execution (for example, variable substitution) do it here.
"""
if len(line) > 0:
if line[0] == '@':
pieces = line[1:].split(' ')
#print "running cmd: " + line[1:]
if pieces[0] == 'starttune':
self.starttune(pieces[1:])
elif pieces[0] == 'stoptune':
self.stoptune(pieces[1:])
elif pieces[0] == 'startpsd':
self.startpsd(pieces[1:])
elif pieces[0] == 'stoppsd':
self.stoppsd(pieces[1:])
elif pieces[0] == 'startscan':
self.startscan(pieces[1:])
elif pieces[0] == 'stopscan':
self.stopscan(pieces[1:])
elif pieces[0] == 'ref':
self.ref()
elif pieces[0] == 'reset':
self.reset()
elif pieces[0] == 'stat':
self.stat()
return ""
elif line[0] == '?':
self.printHelp()
return ""
else:
self._hist += [ line.strip() ]
return line
else:
return "nocmd"
def do_EOF(self, line):
'''
Called when EOF (ctrl-D) is entered on CLI
'''
self.stop()
return True
def stop(self):
'''
Call this to do any cleanup and stop the listener thread
'''
self.client.stop()
hist_file_path = "%s/.pico_history" % expanduser("~")
hist_file = open(hist_file_path, "a")
for hist in self._hist:
print>>hist_file, hist
hist_file.close()
def starttune(self, args):
''' args: chan(1 | 2) bws_setting(5->12) freq digitalAudio_flag(y | n, default = n)
'''
if len(args) > 0:
chan = int(args[0])
bws = int(args[1])
freq = float(args[2])
if len(args) > 3 and args[3] == 'y':
audio_config_str = "IQS 0;DAS 1;RSP 1"
else:
audio_config_str = "IQS 1;DAS 0"
startTuneStr = "RCH %d;DFM 1;^CLT 1;BWS %d;FRQ %f;FRD -00600000;%s;OPM 1" % (chan, bws, freq, audio_config_str)
else:
startTuneStr = "RCH 1;DFM 1;^CLT 1;BWS 8;FRQ 00162.800000;FRD ?;FRD -00600000;IQS 1;DAS 0;OPM 1"
self.send_msgs(startTuneStr, True)
def stoptune(self, args):
''' args: chan(1 | 2)
'''
if len(args) > 0:
chan = int(args[0])
self.send_msgs("RCH %d;OPM 0;BWS 7;IQS 0;^CLT 0" % (chan), True)
else:
self.send_msgs("RCH 1;OPM 0;BWS 7;IQS 0;^CLT 0", True)
def startpsd(self, args):
''' args: chan(1 | 2) start_freq end_freq bins
'''
if len(args) > 0:
chan = int(args[0])
start_freq = float(args[1])
end_freq = float(args[2])
bins = int(args[3])
fra = start_freq + (5.0 / 2.0)
if ((end_freq - start_freq) % 5.0) > 0:
frb = end_freq + (5.0 - ((end_freq - start_freq) % 5.0)) - (5.0 / 2.0)
else:
frb = end_freq - (5.0 / 2.0)
fbs = int(math.log(bins, 2))
startTuneStr = "RCH %d;SSO -01;FRA %f;FRB %f;INC 5.000;FBS %d;BWS 7;AGC 0;AGP 0;IQS 0;DAS 0;FTC 250;OPM 1;OPR 4" % (chan, fra, frb, fbs)
else:
startTuneStr = "RCH 1;SSO -01;FRA 090.500;FRB 105.500;INC 5.000;FBS 13;BWS 7;AGC 0;AGP 0;IQS 0;DAS 0;FTC 250;OPM 1;OPR 4"
self.send_msgs(startTuneStr, True)
def stoppsd(self, args):
''' args: chan(1 | 2)
'''
if len(args) > 0:
chan = int(args[0])
self.send_msgs("RCH %d;OPR 1;AGC 1;AGP 1" % (chan), True)
else:
self.send_msgs("RCH 1;OPR 1;AGC 1;AGP 1", True)
def startscan(self, args):
''' args: chan(1 | 2) dwell_time_s, freq_1, freq_2, ..., freq_n
Use the Pico SMD command to setup memory locs for each freq in scan list.
Then use STL command to set the channels, then change to operation mode 3 for
Scan.
'''
if len(args) > 0:
chan = int(args[0])
mem_chan = 0 # Memory Channel Number 1 to 200
IDM = "0" # Idle Mode Status (IDM) 0 or 1 When idle mode is enabled (IDM 1), AGC, threshold, and scan operations are suspended.
FRQ = "" # Tuned Frequency (FRQ) See FRQ
BWS = "9" # Bandwidth Slot (BWS) 5 to 12
COR = "10" # Carrier Output Relay (COR) -01 to 99 Threshold relative to NF that indicates a signal is present
DET = "0" # Detection Mode (DET) 0 to 9 "0" == I&Q
AGC = "1" # Automatic Gain Control (AGC) 0 or 1
ATN = "0" # Attenuation Setting (ATN) 0 to 84.0
AFC = "0" # Automatic Freq Control (AFC) 0 to 3
PDW = "50" # Pre-signal dwell (PDW) -1 to 996 ms
SDW = args[1] # Signal Dwell (SDW) -1 to 600 sec
LDW = "0" # Lost Signal Dwell (LDW) -1 to 60 sec
FRA = "88.0" # Sweep Start Frequency (FRA) See FRA
FRB = "108.0" # Sweep Stop Frequency (FRB) See FRB
INC = "0.05" # Sweep Increment Freq (INC) See INC
SWD = "0" # Sweep Direction (SWD) 0 or 1
BFO = "0" # Beat Freq Oscillator (BFO) -8 kHz to +8 kHz
for mem_chan, FRQ in enumerate(args[2:]):
addSMD = "SMD %d, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, 0" % ((mem_chan+1), IDM, FRQ, BWS, COR, DET, AGC, ATN, AFC, PDW, SDW, LDW, FRA, FRB, INC, SWD, BFO)
self.send_msgs(addSMD, True)
self.send_msgs("RCH %d;STL (1:%d)" % (chan, ( mem_chan+1 )), True)
self.send_msgs("RCH %d;IQS 1;OPM 1;OPR 3" % (chan), True)
def stopscan(self, args):
''' args: chan(1 | 2)
'''
if len(args) > 0:
chan = int(args[0])
self.send_msgs("RCH %d;OPR 1;IQS 0" % (chan), True)
else:
self.send_msgs("RCH 1;OPR 1;IQS 0", True)
def ref(self):
''' args:
'''
self.send_msgs("RCH 1;REF 0;PPS 1", True)
def reset(self):
''' args:
'''
print '\n*********************************\nResetting Pico...please stand by.\n*********************************\n'
self.send_msg("*RST", True)
time.sleep(6)
self.send_msg("BWL?", True)
self.send_msg("FRG?", True)
time.sleep(1)
self.send_msgs("RCH 1;OPR 2;OPR 1;REF 1;PPS 0;DFM 0;AGC 1;AGP 1;DET 0;SQL -150;SAO 0;IQS 0;DAS 0;BWS 7;BFO ?")
time.sleep(0.5)
self.send_msgs("RCH 2;OPR 2;OPR 1;AGC 1;AGP 1;DET 0;SQL -1;SAO 0;IQS 0;DAS 0;BWS 7;BFO ?;^CFC CC -2;^CFC OFC -2;FID ?;^FID ?;CDE?")
def stat(self):
''' args:
'''
print '\n*********************************\nStats for channel 1:'
self.send_msgs("RCH 1;OPR?;IQS?;DAS?;FTS?;OPM?")
time.sleep(0.5)
print '*********************************\nStats for channel 2:'
self.send_msgs("RCH 2;OPR?;IQS?;DAS?;FTS?;OPM?;RCH 1")
def printHelp(self):
helpStr = '''Special commands:
@starttune chan(1 | 2) bws_setting(5->12) freq [digitalAudio_flag]
@stoptune chan(1 | 2)
@startpsd chan(1 | 2) start_freq end_freq bins
@stoppsd chan(1 | 2)
@startscan chan(1 | 2) dwell_time_s, freq_1, freq_2, ..., freq_n
@stopscan chan(1 | 2)
@ref
@setup
Precede these with '@'"
'''
print helpStr
# -*- coding: utf-8 -*-
"""
Created on Mon Jul 16 11:10:21 2018
@author: Sander Oosterveld
"""
from MainScreen import MainScreen
from Widget import TextWidget, Position
from SocketClient import SocketClient
from queue import Queue
from ConnectionWidget import ConnectionWidget
q = Queue()
data = {'BackgroundColour': 'white'}
screen = MainScreen(q)
socket = SocketClient('130.89.226.101', 51234, q, data)
connectionWidget = ConnectionWidget(screen, socket.getConnectionData())
connectionWidget.make()
connectionWidget.updateConnection(socket)
socket.start()
screen.start()
socket.join()
from WorkWidgets.MainWidget import MainWidget
from PyQt5.QtWidgets import QApplication
from PyQt5 import sip
import sys
#要呼叫"Client"資料夾的function
import os
Client_path = os.path.abspath(os.path.join(os.getcwd(), "..")) #查看上級路徑
print("Client : {}".format(Client_path))
Client_path = os.path.join(Client_path, "Client") #合併路徑
import sys
sys.path.append(Client_path) #增加路徑
#print("Client_path : {}".format(Client_path))
#print("sys.path : {}".format(sys.path))
#要呼叫"Client"資料夾的function
#server 和 client之間傳送
from SocketClient import SocketClient
#server 和 client之間傳送
client = SocketClient()
app = QApplication([])
main_window = MainWidget(client)
main_window.setFixedSize(700, 400) #設定視窗大小
main_window.show()
# main_window.showFullScreen()
sys.exit(app.exec_())
class Pir(threading.Thread):
def __init__(self, GPIO):
self.HOST = '192.168.10.10'
self.PORT = 50007
self.socketClient = SocketClient(self.HOST, self.PORT)
self.GPIO_PIR = GPIO
# Threading init
threading.Thread.__init__(self)
self.setName("PIR" + str(self.GPIO_PIR))
self.daemon = True
def run(self):
self.detect()
def detect(self):
# Use BCM GPIO references
# instead of physical pin numbers
GPIO.setmode(GPIO.BCM)
#print "PIR Module Holding Time Test (CTRL-C to exit)"
# Set pin as input
GPIO.setup(self.GPIO_PIR, GPIO.IN) # Echo
Current_State = 0
Previous_State = 0
try:
#print "Waiting for PIR to settle ..."
# Loop until PIR output is 0
while GPIO.input(self.GPIO_PIR) == 1:
Current_State = 0
#print " Ready"
data = 0
# Loop until users quits with CTRL-C
while True:
time.sleep(0.01)
# Read PIR state
Current_State = GPIO.input(self.GPIO_PIR)
if Current_State == 1 and Previous_State == 0:
# PIR is triggered
print " Motion detected!"
self.socketClient.connectUDP()
self.socketClient.send(self.getName())
#print "motion sent"
#data = self.socketClient.recieve()
#time.sleep(1)
#print "Recieved ", data
self.socketClient.close()
# Record previous state
time.sleep(1)
Previous_State = 1
elif Current_State == 0 and Previous_State == 1:
time.sleep(1)
Previous_State = 0
except KeyboardInterrupt:
print " Quit"
# Reset GPIO settings
self.clean()
def clean(self):
GPIO.cleanup()
class robot(object):
def __init__(self, std_vel, std_steer, dt):
self.std_vel = std_vel
self.std_steer = std_steer
self.max_angular = std_vel - std_steer
self.wheelbase = 0.135
self.odom = odom(self.wheelbase)
self.PID = PID(45, 0.0, 0.0) #P, I, D
self.client = SocketClient()
self.dt = dt
self.landmarks = []
self.R = np.diag([0.00025, 0.00025])
self.Q = np.diag([0.01, 0.01])
self.I = np.identity(
3) #inital robot_x , robot_y, robot_theta state vector?
self.current_path = 0
self.debug = False
self.update = 0
self.state = 0
self.stored_theta = 0
self.stream = PiVideoStream(
) #start the video stream on a seperate thread
self.measure = pixelCalibrate(1200,
90) #calibrate the camera for distances
def update_pose(self, current_pose):
self.x = self.x + current_pose
self.x[2] = self.contain_pi(
self.x[2]) #contain robot theta between pi and - pi
def ekf_predict(self):
#PREDICT ROBOT POSE
self.u[0] = self.x[0]
self.u[1] = self.x[1]
self.u[2] = self.x[2]
#COVARIANCE
self.sigma = dot(self.xjac, self.sigma).dot(self.xjac.T) + dot(
self.ujac, self.R).dot(self.ujac.T)
def ekf_update(self, sensor):
#UPDATE STEP
landmark_id = sensor[2]
H = self.H_of(landmark_id, sensor)
S = (dot(H, self.sigma).dot(H.T)) + self.Q
K = dot(self.sigma, H.T).dot(np.linalg.inv(S))
hx = self.Hx(landmark_id)
y = self.residual(sensor, hx)
self.u = self.u + dot(K, y)
self.I = np.identity(
(3 + len(self.landmarks)
)) #the identity expands with N amount of landmarks init
I_KH = self.I - dot(K, H)
self.sigma = dot(I_KH, self.sigma)
def send(self):
try:
msg = np.empty(2, dtype=object)
if len(self.sigma) >= 3:
robot_sigma = self.sigma[0:2, 0:2]
reduced_sigma = robot_sigma
if len(self.sigma) >= 5:
L0_sigma = self.sigma[3:5, 3:5]
reduced_sigma = np.concatenate((robot_sigma, L0_sigma), axis=0)
if len(self.sigma) >= 7:
L1_sigma = self.sigma[5:7, 5:7]
reduced_sigma = np.concatenate(
(robot_sigma, L0_sigma, L1_sigma), axis=0)
if len(self.sigma) >= 9:
L2_sigma = self.sigma[7:9, 7:9]
reduced_sigma = np.concatenate(
(robot_sigma, L0_sigma, L1_sigma, L2_sigma), axis=0)
if len(self.sigma) >= 11:
L3_sigma = self.sigma[9:11, 9:11]
reduced_sigma = np.concatenate(
(robot_sigma, L0_sigma, L1_sigma, L2_sigma, L3_sigma),
axis=0)
if len(self.sigma) >= 13:
L4_sigma = self.sigma[11:13, 11:13]
reduced_sigma = np.concatenate(
(robot_sigma, L0_sigma, L1_sigma, L2_sigma, L3_sigma,
L4_sigma),
axis=0)
msg[:] = [self.u, reduced_sigma]
self.client.send(msg)
#another method is number vtsack
except Exception as e:
print(e)
def H_of(self, landmark_id, sensor):
px = float(self.u[int(3 + (landmark_id * 2))])
py = float(self.u[int(4 + (landmark_id * 2))])
hyp = float(sensor[0]**2)
dist = float(sensor[0])
#Expand with landmarks
n = len(self.landmarks)
robot_H = np.array([[
-(px - float(self.u[0])) / dist, -(py - float(self.u[1])) / dist, 0
], [(py - float(self.u[1])) / hyp, -(px - float(self.u[0])) / hyp,
-1]])
landmark_H = np.array([[
-(px - float(self.u[0])) / dist, -(py - float(self.u[1])) / dist
], [(py - float(self.u[1])) / hyp, -(px - float(self.u[0])) / hyp]])
zeros_H_before = np.zeros((2, int(2 * landmark_id)))
zeros_H_after = np.zeros((2, int(2 * ((n - 1) - landmark_id))))
H = np.concatenate(
(robot_H, zeros_H_before, landmark_H, zeros_H_after), axis=1)
return H
def Hx(self, landmark_id):
px = float(self.u[int(3 + (landmark_id * 2))])
py = float(self.u[int(4 + (landmark_id * 2))])
robot_x = float(self.u[0])
robot_y = float(self.u[1])
robot_theta = float(self.u[2])
Hx = np.array([[(np.sqrt((robot_x - px)**2 + (robot_y - py)**2))],
[(math.atan2(py - robot_y, px - robot_x)) - robot_theta]
])
#WRAP BETWEEN -pi AND pi
Hx[1] = Hx[1] % (2 * np.pi) # force in range [0, 2 pi)
if Hx[1] > np.pi: # move to [-pi, pi)
Hx[1] -= 2 * np.pi
return Hx
def residual(self, a, b):
""" compute residual (a-b) between measurements containing
[range, bearing]. Bearing is normalized to [-pi, pi)"""
y = a - b
#WRAP BETWEEN -pi AND pi
y[1] = y[1] % (2 * np.pi) # force in range [0, 2 pi)
if y[1] > np.pi: # move to [-pi, pi)
y[1] -= 2 * np.pi
return y
def contain_pi(self, theta):
'''
Little function to contain an angle between -pi and pi
'''
#WRAP BETWEEN -pi AND pi
theta = theta % (2 * np.pi) # force in range [0, 2 pi)
if theta > np.pi: # move to [-pi, pi)
theta -= 2 * np.pi
return theta
def stop(self):
print(self.name + ":stop")
ok = SocketClient.stop(socket = self.socket, address=self.address)
self.running = False
class NGS_RC8_API():
'''
' Class Description: API for communication with Denso RC8
' Author: Jacob Taylor Cassady - Undergraduate Research Assistant
' University of Louisville J.B. Speed School of Engineering
' Next Generation Systems Robotics Lab
'''
def __init__(self, socket_connection=False):
'''
Constructor
'''
self.socket_connection = socket_connection
self.implemented_functions = {
"MOVE": self.RC8Commands.MOVE,
"DRAW": self.RC8Commands.DRAW,
# "DRIVE" : self.RC8Commands.DRIVE
# "DRIVEA" : self.RC8Commands.DRIVEA
}
# Ensure there are no descrepancies between the implemented functions and the functions held within the enum
assert len(self.implemented_functions) == len(
NGS_RC8_API.RC8Commands
), "There is a descrepancy in the number of implemented RC8Commands and the dictionary used to map these functions. Dictionary's current state:\n\t" + str(
self.implemented_functions)
# Create a SocketClient connection if the API requests it. If not, enters echo mode.
if socket_connection == True:
self.socket_client = SocketClient()
self.socket_client.empty()
def __del__(self):
'''
' Destructor
'''
del self.implemented_functions
if self.socket_connection == True:
del self.socket_client
del self.socket_connection
del self
def reconnect(self):
'''
' Class Description: Ends connection with socket and prompts for a new one.
' Author: Jacob Taylor Cassady
'''
if self.socket_connection == True:
self.socket_client.end_connection()
self.socket_client.prompt_for_connection()
else:
print(
"Reconnect called but NGS_RC8_API was initialized without a socket connection. Reinitialize the object with argument socket_connect = True"
)
def call_function(self, input_string):
'''
' Class Description: 'Calls' a function by sending a message through the socket client to the RC8 PacScript Server.
' Author: Jacob Taylor Cassady
'''
print("Function input string: " + input_string)
encoded_input = NGS_RC8_API.encode_input(input_string)
print("Encoded function input: " + encoded_input + "\n")
if self.socket_connection == True:
self.socket_client.send_message(encoded_input + '\r')
class RC8Commands(enum.Enum):
'''
' Class Description: Enum class for keeping a list of implemented commands on RC8 PacScript Server.
' Author: Jacob Taylor Cassady
'''
MOVE = 1
'''
' Move Command: To move the robot to the designated coordinates.
'''
DRAW = 2
'''
' Draw Command: To move from the current position to a relative position.
'''
# DRIVE = 3
'''
' Drive Command: To move each axis to a relative position.
'''
# DRIVEA = 4
'''
' Draw Command: To move each axis to an absolute position.
'''
class InterpolationMethods(enum.Enum):
'''
' Class Description: Enum class for keeping a list of valid interpolation methods used by move implemented_functions
' on the RC8 PacScript Server.
' Author: Jacob Taylor Cassady
'''
PTP = 1 # Point to Point
P = PTP # Point to Point
'''
' PTP interpolation motion. This motion moves the fastest. TCP point track is not considered.
'''
L = 2 # Linear
'''
' CP motion. TCP point moves from the current position to the target position linearly at constant speed
' in other than acceleration/deceleration areas.
'''
# C = 3 # Circular
'''
' Circular interpolation motion. The robot moves along the circular arc consisting of the current position,
' target position and relay position in order of the current position -> relay position -> target position.
' The robot moves at constant speed in other than acceleration/deceleration areas.
' Refer to https://densorobotics.com/content/user_manuals/19/000441.html
'''
# S = 4 # Free Curve Motion
'''
' Free curve motion. The robot moves at constant speed along a smooth curve crossing the registered point.
' Refer to https://densorobotics.com/content/user_manuals/19/000442.html
'''
class MotionOptions(enum.Enum):
'''
' Class Description: Enum class for keeping a list of valid motion options used when moving the robotic arm.
' Refer to https://densorobotics.com/content/user_manuals/19/000521.html for more information.
' Author: Jacob Taylor Cassady
'''
S = 1 # Internal speed specification
SPEED = S
ACCEL = 2 # Internal acceleration specification
DECEL = 3 # Internal deceleration specification
TIME = 4 # Travel time specification
@staticmethod
def encode_input(input_string):
'''
' Class Description: Formats an input input string into a message to be sent to the RC8 PacScript Server.
' Author: Jacob Taylor Cassady
'''
input_string = input_string.rstrip().lstrip()
func = input_string.split(' ')[0].upper()
if NGS_RC8_API.RC8Commands[func] == NGS_RC8_API.RC8Commands.MOVE:
return NGS_RC8_API.format_for_move(input_string)
elif NGS_RC8_API.RC8Commands[func] == NGS_RC8_API.RC8Commands.DRAW:
return NGS_RC8_API.format_for_draw(input_string)
elif NGS_RC8_API.RC8Commands[func] == NGS_RC8_API.RC8Commands.DRIVE:
return NGS_RC8_API.format_for_drive(input_string)
elif NGS_RC8_API.RC8Commands[func] == NGS_RC8_API.RC8Commands.DRIVEA:
return NGS_RC8_API.format_for_driveA(input_string)
else:
print("function: ", func, " is not currently supported.")
@staticmethod
def process_argument(phrase):
'''
' Method Description: Formats an argument of the message to be sent to the RC8 PacScript Server
' by striping the phrase of all spaces and reformatting unique phrases such as 'S for Speed'.
' Author: Jacob Taylor Cassady
'''
phrase = phrase.rstrip().lstrip()
if (phrase[0].upper() == 'S'):
phrase = "S" + str(int("".join(list(filter(str.isdigit, phrase)))))
return phrase
@staticmethod
def retrieve_arguments(argument_string):
'''
' Method Description: Iterates over each argument and starts to build a message to be sent to the
' RC8 PacScript Server.
' Author: Jacob Taylor Cassady
'''
phrase = ""
message_string = ""
open_bracket = False
for character in argument_string:
if character == '(':
open_bracket = True
phrase += character
elif character == ')':
open_bracket = False
phrase += character
elif character == ' ':
pass
elif character == ',' and not open_bracket:
message_string += NGS_RC8_API.process_argument(phrase) + "|"
phrase = ""
else:
phrase += character
message_string += NGS_RC8_API.process_argument(phrase)
return message_string
@staticmethod
def verify_interpolation_method(interpolation_key):
denso_interpolation_methods = {
"P": NGS_RC8_API.InterpolationMethods.PTP,
"PTP": NGS_RC8_API.InterpolationMethods.PTP,
"L": NGS_RC8_API.InterpolationMethods.L,
# "C" : NGS_RC8_API.InterpolationMethods.C,
# "S" : NGS_RC8_API.InterpolationMethods.S,
}
assert len(denso_interpolation_methods) - 1 == len(
NGS_RC8_API.InterpolationMethods
), "There is a descrepancy in the number of interpolation methods and the dictionary used to map these methods. Dictionary's current state:\n\t" + str(
denso_interpolation_methods)
result = denso_interpolation_methods.get(interpolation_key,
"Not found")
assert result != "Not found", "Motion interpolation key: " + interpolation_key + " is not included in list of denso interpolation methods:\n\t" + str(
denso_interpolation_methods)
return result.name
@staticmethod
def verify_motion_option(motion_option_key):
motion_options = {
"S": NGS_RC8_API.MotionOptions.S,
"SPEED": NGS_RC8_API.MotionOptions.S,
"ACCEL": NGS_RC8_API.MotionOptions.ACCEL,
"DECEL": NGS_RC8_API.MotionOptions.DECEL,
"TIME": NGS_RC8_API.MotionOptions.TIME,
}
assert len(motion_options) - 1 == len(
NGS_RC8_API.MotionOptions
), "There is a descrepancy in the number of motion options and the dictionary used to map these options. Dictionary's current state:\n\t" + str(
motion_options)
result = motion_options.get(motion_option_key, "Not found")
assert result != "Not found", "The motion_option_key given: " + motion_option_key + " does not match any of the motion options supported by denso. Please see the below dictionary for a list of the motion options and refer to https://densorobotics.com/content/user_manuals/19/000521.html for more information:\n\t" + str(
motion_options)
return result.name
@staticmethod
def list_implemented_pacscript_functions():
print(
"\n- List of implemented pacscript functions and their matching enum -"
)
index = 1
for implemented_function, enum_obj in NGS_RC8_API.RC8Commands.__members__.items(
):
print("Function enum: " + str(index) + ", Function Name: " +
implemented_function.capitalize())
index += 1
print("")
@staticmethod
def format_for_move(input_string):
'''
' Method Description: Formats an input string matching the move function.
' Author: Jacob Taylor Cassady
'''
# Cut string to get rid of function
input_string = input_string[5:].lstrip().rstrip()
# Determine interpolation
interpolation = input_string[0]
interpolation = NGS_RC8_API.verify_interpolation_method(interpolation)
# Determine target positions
target_positions = input_string[2:]
target_positions = NGS_RC8_API.retrieve_arguments(target_positions)
#retrieve segments of draw call
return str(NGS_RC8_API.RC8Commands.MOVE.value) + ";" + str(
interpolation) + ";" + str(target_positions)
@staticmethod
def format_for_draw(input_string):
'''
' Method Description: Formats an input string matching the draw function.
' Author: Jacob Taylor Cassady
'''
# Cut string to get rid of function
input_string = input_string[5:].lstrip().rstrip()
# Determine interpolation
interpolation = input_string[0]
interpolation = NGS_RC8_API.verify_interpolation_method(interpolation)
# Determine target positions
target_positions = input_string[2:]
target_positions = NGS_RC8_API.retrieve_arguments(target_positions)
#retrieve segments of draw call
return str(NGS_RC8_API.RC8Commands.DRAW.value) + ";" + str(
interpolation) + ";" + str(target_positions)
@staticmethod
def format_for_drive(input_string):
'''
' Method Description: Formats an input string matching the drive function.
' Author: Jacob Taylor Cassady
'''
# Cut string to get rid of function
input_string = input_string[6:].lstrip().rstrip()
# locations = input_string[1:].split("(")
# for index, location in enumerate(locations):
# print(index, location)
#retrieve segments of draw call
return str(NGS_RC8_API.RC8Commands.DRIVE.value
) + ";" + NGS_RC8_API.retrieve_arguments(input_string)
@staticmethod
def format_for_driveA(input_string):
'''
' Method Description: Formats an input string matching the driveA (Absolute) function.
' Author: Jacob Taylor Cassady
'''
# Cut string to get rid of function
input_string = input_string[7:]
#retrieve segments of draw call
return str(NGS_RC8_API.RC8Commands.DRIVEA.value
) + ";" + NGS_RC8_API.retrieve_arguments(input_string)
def __init__(self):
self.HOST = '192.168.10.10'
self.PORT = 50007
self.socketClient = SocketClient(self.HOST, self.PORT)
