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()
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)