mirror of
https://github.com/caperren/school_archives.git
synced 2025-11-09 21:51:15 +00:00
Added applied robotics code to archive.
This commit is contained in:
@@ -0,0 +1,87 @@
|
||||
#NoEnv ; Recommended for performance and compatibility with future AutoHotkey releases.
|
||||
; #Warn ; Enable warnings to assist with detecting common errors.
|
||||
SendMode Input ; Recommended for new scripts due to its superior speed and reliability.
|
||||
SetWorkingDir %A_ScriptDir% ; Ensures a consistent starting directory.
|
||||
|
||||
connect_status_box_x := 18
|
||||
connect_status_box_y := 86
|
||||
connect_status_green := "0x00FF00"
|
||||
|
||||
run_status_box_x := 13
|
||||
run_status_box_y := 106
|
||||
run_status_green := "0x00FF00"
|
||||
|
||||
error_status_box_x := 45
|
||||
error_status_box_y := 431
|
||||
error_status_grey := "0xFFFFFF"
|
||||
|
||||
connect_button_x := 73
|
||||
connect_button_y := 88
|
||||
|
||||
run_button_x := 72
|
||||
run_button_y := 106
|
||||
|
||||
clear_button_x := 88
|
||||
clear_button_y := 446
|
||||
|
||||
should_exit = 0
|
||||
|
||||
Loop
|
||||
{
|
||||
If (should_exit)
|
||||
{
|
||||
ExitApp
|
||||
}
|
||||
|
||||
IfWinNotExist, RobMaster
|
||||
{
|
||||
Run, C:\ORiN2\CAO\ProviderLib\DENSO\NetwoRC\Bin\RobMaster.exe
|
||||
Sleep, 3000
|
||||
}
|
||||
|
||||
IfWinExist, RobMaster
|
||||
{
|
||||
WinActivate
|
||||
}
|
||||
|
||||
IfWinActive, RobMaster
|
||||
{
|
||||
PixelGetColor, connect_status_color, %connect_status_box_x%, %connect_status_box_y%
|
||||
;MsgBox, Color is %connect_status_color%, other color is %connect_status_green%
|
||||
|
||||
If( connect_status_color <> connect_status_green )
|
||||
{
|
||||
Click, %connect_button_x%, %connect_button_y%
|
||||
Sleep, 250
|
||||
}
|
||||
|
||||
PixelGetColor, error_status_color, %error_status_box_x%, %error_status_box_y%
|
||||
|
||||
If( error_status_color <> error_status_grey )
|
||||
{
|
||||
Click, %clear_button_x%, %clear_button_y%
|
||||
Sleep, 250
|
||||
}
|
||||
else
|
||||
{
|
||||
PixelGetColor, run_status_color, %run_status_box_x%, %run_status_box_y%
|
||||
|
||||
If( run_status_color <> run_status_green )
|
||||
{
|
||||
Click, %run_button_x%, %run_button_y%
|
||||
Sleep, 250
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Sleep, 100
|
||||
}
|
||||
|
||||
^q::
|
||||
should_exit = 1
|
||||
return
|
||||
|
||||
Return
|
||||
|
||||
@@ -0,0 +1,260 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets
|
||||
import pythoncom
|
||||
import win32com.client
|
||||
from time import time
|
||||
import socket
|
||||
import json
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
THREAD_HERTZ = 100
|
||||
|
||||
P0 = (216.1302490234375, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
|
||||
P1 = (251.22869873046875, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
|
||||
P2 = (216.1302490234375, 0.10808953642845154, 606.7885131835938, 63.89561462402344, 8.09478759765625, 83.43250274658203)
|
||||
|
||||
J0 = (-2.4949951171875, -68.55029296875, 161.4649658203125, 0.2345581203699112, -40.739683151245117, 60.7391586303711)
|
||||
|
||||
BAD_VAL = -1000000
|
||||
|
||||
TCP_PORT = 9877
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class RAWControlReceiver(QtCore.QThread):
|
||||
|
||||
new_message__signal = QtCore.pyqtSignal(dict)
|
||||
|
||||
def __init__(self):
|
||||
super(RAWControlReceiver, self).__init__()
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.wait_time = 1.0 / THREAD_HERTZ
|
||||
|
||||
self.control_tcp_server = None
|
||||
self.client_connection = None
|
||||
self.client_address = None
|
||||
|
||||
self.current_message = ""
|
||||
|
||||
self.num_messages = 0
|
||||
self.last_time = time()
|
||||
|
||||
def run(self):
|
||||
self.initialize_tcp_server()
|
||||
while self.run_thread_flag:
|
||||
self.check_for_new_command_message()
|
||||
# self.msleep(2)
|
||||
|
||||
def initialize_tcp_server(self):
|
||||
self.control_tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.control_tcp_server.bind(('', TCP_PORT))
|
||||
self.control_tcp_server.listen(5)
|
||||
|
||||
def check_for_new_command_message(self):
|
||||
try:
|
||||
self.current_message += self.client_connection.recv(8)
|
||||
|
||||
found_pound = self.current_message.find("#####")
|
||||
|
||||
if found_pound != -1:
|
||||
split_message = str(self.current_message[:found_pound])
|
||||
|
||||
self.current_message = self.current_message[found_pound + 5:]
|
||||
|
||||
try:
|
||||
json_message = json.loads(split_message)
|
||||
|
||||
self.num_messages += 1
|
||||
|
||||
if time() - self.last_time > 1:
|
||||
print "Num commands received:", self.num_messages
|
||||
self.num_messages = 0
|
||||
self.last_time = time()
|
||||
|
||||
self.new_message__signal.emit(json_message)
|
||||
except Exception, e:
|
||||
print e, "could not parse"
|
||||
except Exception, e:
|
||||
print e, "other"
|
||||
self.client_connection, self.client_address = self.control_tcp_server.accept()
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class ArmControlReceiver(QtCore.QThread):
|
||||
def __init__(self, shared_objects):
|
||||
super(ArmControlReceiver, self).__init__()
|
||||
|
||||
# ########## Reference to class init variables ##########
|
||||
self.shared_objects = shared_objects
|
||||
self.status_sender_class = self.shared_objects["threaded_classes"]["Arm Status Sender"]
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.message_handler = RAWControlReceiver()
|
||||
self.message_handler.start()
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.wait_time = 1.0 / THREAD_HERTZ
|
||||
|
||||
self.control_tcp_server = None
|
||||
self.client_connection = None
|
||||
self.client_address = None
|
||||
|
||||
self.cao_engine = None
|
||||
self.controller = None
|
||||
self.arm = None
|
||||
|
||||
self.CONTROL_COMMANDS = {
|
||||
"enable_motors": self.enable_motors,
|
||||
"change_robot_speed": self.change_robot_speed,
|
||||
"move_position_abs": self.move_arm_position_absolute,
|
||||
"move_position_rel": self.move_arm_position_relative,
|
||||
"move_joint_abs": self.move_joints_absolute,
|
||||
"move_joint_rel": self.move_joints_relative,
|
||||
|
||||
"charge_tank_psi": 0,
|
||||
"fire_tank": 0
|
||||
}
|
||||
|
||||
self.current_message = ""
|
||||
|
||||
self.command_queue = []
|
||||
|
||||
self.num_commands = 0
|
||||
|
||||
self.last_commands_time = time()
|
||||
|
||||
def run(self):
|
||||
self.initialize_cao_engine()
|
||||
|
||||
while self.run_thread_flag:
|
||||
start_time = time()
|
||||
|
||||
# self.add_item_to_command_queue({"move_joint_rel": (10, 0, 0, 0, 0, 0)})
|
||||
# self.add_item_to_command_queue({"move_joint_rel": (-10, 0, 0, 0, 0, 0)})
|
||||
|
||||
self.process_command_queue_item()
|
||||
|
||||
if time() - self.last_commands_time > 1:
|
||||
print "Num commands processed:", self.num_commands
|
||||
self.num_commands = 0
|
||||
self.last_commands_time = time()
|
||||
|
||||
time_diff = time() - start_time
|
||||
self.msleep(max(int(self.wait_time - time_diff), 0))
|
||||
|
||||
def initialize_cao_engine(self):
|
||||
pythoncom.CoInitialize()
|
||||
self.cao_engine = win32com.client.Dispatch("CAO.CaoEngine")
|
||||
self.controller = self.cao_engine.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
|
||||
self.arm = self.controller.AddRobot("Arm1", "")
|
||||
|
||||
def on_new_message__signal(self, message):
|
||||
self.command_queue.append(message)
|
||||
|
||||
def process_command_queue_item(self):
|
||||
if self.command_queue:
|
||||
key = list(self.command_queue[0].keys())[0]
|
||||
data = self.command_queue[0][key]
|
||||
|
||||
del self.command_queue[0]
|
||||
|
||||
command_to_run = self.CONTROL_COMMANDS.get(key)
|
||||
command_to_run(data)
|
||||
|
||||
self.num_commands += 1
|
||||
|
||||
def add_item_to_command_queue(self, item):
|
||||
self.command_queue.append(item)
|
||||
|
||||
def enable_motors(self, should_enable):
|
||||
try:
|
||||
if should_enable:
|
||||
self.arm.Execute("Motor", 1)
|
||||
self.arm.Execute("TakeArm", 0)
|
||||
else:
|
||||
self.arm.Execute("Motor", 0)
|
||||
self.arm.Execute("GiveArm", 0)
|
||||
except:
|
||||
print("Arm not able to change to state", "on." if should_enable else "off.")
|
||||
|
||||
def change_robot_speed(self, speed):
|
||||
self.arm.Execute("ExtSpeed", (speed, speed, speed))
|
||||
|
||||
def move_arm_position_absolute(self, position):
|
||||
try:
|
||||
if self.status_sender_class.statuses["motor_enabled"]:
|
||||
self.arm.Move(1, "@P " + str(tuple(position)), "NEXT")
|
||||
except:
|
||||
pass
|
||||
|
||||
def move_arm_position_relative(self, position_offsets):
|
||||
current_position = self.status_sender_class.position
|
||||
|
||||
if current_position["rz"] == BAD_VAL or len(position_offsets) == position_offsets.count(0):
|
||||
return
|
||||
|
||||
new_position = (
|
||||
current_position["x"] + position_offsets[0],
|
||||
current_position["y"] + position_offsets[1],
|
||||
current_position["z"] + position_offsets[2],
|
||||
current_position["rx"] + position_offsets[3],
|
||||
current_position["ry"] + position_offsets[4],
|
||||
current_position["rz"] + position_offsets[5],
|
||||
)
|
||||
# print "here"
|
||||
self.move_arm_position_absolute(new_position)
|
||||
|
||||
def move_joints_absolute(self, joint_positions):
|
||||
try:
|
||||
if self.status_sender_class.statuses["motor_enabled"]:
|
||||
self.arm.Move(1, "J" + str(tuple(joint_positions)), "NEXT")
|
||||
except:
|
||||
pass
|
||||
|
||||
def move_joints_relative(self, joint_position_offsets):
|
||||
current_position = self.status_sender_class.joints
|
||||
|
||||
if current_position[6] == BAD_VAL or len(joint_position_offsets) == joint_position_offsets.count(0):
|
||||
return
|
||||
|
||||
new_joint_positions = (
|
||||
current_position[1] + joint_position_offsets[0],
|
||||
current_position[2] + joint_position_offsets[1],
|
||||
current_position[3] + joint_position_offsets[2],
|
||||
current_position[4] + joint_position_offsets[3],
|
||||
current_position[5] + joint_position_offsets[4],
|
||||
current_position[6] + joint_position_offsets[5],
|
||||
)
|
||||
|
||||
self.move_joints_absolute(new_joint_positions)
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
self.message_handler.new_message__signal.connect(self.on_new_message__signal)
|
||||
|
||||
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
|
||||
start_signal.connect(self.start)
|
||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||
kill_signal.connect(self.on_kill_threads_requested__slot)
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.message_handler.run_thread_flag = False
|
||||
self.message_handler.wait()
|
||||
self.run_thread_flag = False
|
||||
@@ -0,0 +1,208 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets
|
||||
import pythoncom
|
||||
import win32com.client
|
||||
import time
|
||||
from time import time
|
||||
import json
|
||||
import socket
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
THREAD_HERTZ = 60
|
||||
|
||||
MESSAGE_HERTZ = 5
|
||||
|
||||
TCP_PORT = 9876
|
||||
|
||||
BAD_VAL = -1000000
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class NetworkSender(QtCore.QThread):
|
||||
def __init__(self):
|
||||
pass
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class ArmStatusSender(QtCore.QThread):
|
||||
|
||||
position = {
|
||||
"x": BAD_VAL,
|
||||
"y": BAD_VAL,
|
||||
"z": BAD_VAL,
|
||||
"rx": BAD_VAL,
|
||||
"ry": BAD_VAL,
|
||||
"rz": BAD_VAL,
|
||||
"fig": BAD_VAL
|
||||
}
|
||||
|
||||
joints = {
|
||||
1: BAD_VAL,
|
||||
2: BAD_VAL,
|
||||
3: BAD_VAL,
|
||||
4: BAD_VAL,
|
||||
5: BAD_VAL,
|
||||
6: BAD_VAL
|
||||
}
|
||||
|
||||
statuses = {
|
||||
"motor_enabled": True,
|
||||
"arm_normal": False,
|
||||
"error": "",
|
||||
|
||||
"speed": 0,
|
||||
"arm_busy": True,
|
||||
|
||||
"tank_psi": 0
|
||||
}
|
||||
|
||||
def __init__(self, shared_objects):
|
||||
super(ArmStatusSender, self).__init__()
|
||||
|
||||
# ########## Reference to class init variables ##########
|
||||
self.shared_objects = shared_objects
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.wait_time = 1.0 / THREAD_HERTZ
|
||||
|
||||
self.status_tcp_server = None
|
||||
self.client_connection = None
|
||||
self.client_address = None
|
||||
|
||||
self.cao_engine = None
|
||||
self.controller = None
|
||||
self.arm = None
|
||||
|
||||
self.robot_enabled = None
|
||||
self.robot_normal = None
|
||||
self.robot_error = None
|
||||
self.robot_busy = None
|
||||
self.robot_speed = None
|
||||
|
||||
self.robot_current_position = None
|
||||
self.robot_current_joint_angles = None
|
||||
|
||||
self.last_packet_sent_time = time()
|
||||
|
||||
def run(self):
|
||||
self.initialize_cao_engine_and_watchers()
|
||||
self.initialize_tcp_server()
|
||||
|
||||
while self.run_thread_flag:
|
||||
start_time = time()
|
||||
|
||||
self.get_statuses()
|
||||
self.get_position()
|
||||
self.get_joint_angles()
|
||||
|
||||
message_diff = time() - self.last_packet_sent_time
|
||||
|
||||
# TODO: SPLIT THIS OUT IF IT CAUSES PROBLEMS!!!!!!
|
||||
if message_diff > (1.0 / MESSAGE_HERTZ):
|
||||
self.send_status_package()
|
||||
self.last_packet_sent_time = time()
|
||||
|
||||
time_diff = time() - start_time
|
||||
self.msleep(max(int(self.wait_time - time_diff) * 1000, 0))
|
||||
|
||||
def initialize_cao_engine_and_watchers(self):
|
||||
pythoncom.CoInitialize()
|
||||
self.cao_engine = win32com.client.Dispatch("CAO.CaoEngine")
|
||||
self.controller = self.cao_engine.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
|
||||
self.arm = self.controller.AddRobot("Arm1", "")
|
||||
|
||||
self.robot_enabled = self.arm.AddVariable("@SERVO_ON", "")
|
||||
self.robot_normal = self.controller.AddVariable("@NORMAL_STATUS", "")
|
||||
self.robot_error = self.controller.AddVariable("@ERROR_DESCRIPTION", "")
|
||||
self.robot_busy = self.arm.AddVariable("@BUSY_STATUS", "")
|
||||
|
||||
self.robot_speed = self.arm.AddVariable("@EXTSPEED", "")
|
||||
self.robot_current_position = self.arm.AddVariable("@CURRENT_POSITION", "")
|
||||
self.robot_current_joint_angles = self.arm.AddVariable("@CURRENT_ANGLE", "")
|
||||
|
||||
def initialize_tcp_server(self):
|
||||
self.status_tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.status_tcp_server.bind(('', TCP_PORT))
|
||||
self.status_tcp_server.listen(5)
|
||||
|
||||
def get_statuses(self):
|
||||
motor_on = self.robot_enabled.Value
|
||||
normal = self.robot_normal.Value
|
||||
error = self.robot_error.Value
|
||||
busy = self.robot_busy.Value
|
||||
robot_speed = self.robot_speed.Value
|
||||
|
||||
self.statuses = {
|
||||
"motor_enabled": motor_on,
|
||||
"arm_normal": normal,
|
||||
"error": error,
|
||||
|
||||
"speed": robot_speed,
|
||||
"arm_busy": busy,
|
||||
|
||||
"tank_psi": 0
|
||||
}
|
||||
|
||||
def get_position(self):
|
||||
x, y, z, rx, ry, rz, fig = self.robot_current_position.Value
|
||||
self.position = {
|
||||
"x": x,
|
||||
"y": y,
|
||||
"z": z,
|
||||
"rx": rx,
|
||||
"ry": ry,
|
||||
"rz": rz,
|
||||
"fig": fig
|
||||
}
|
||||
|
||||
def send_status_package(self):
|
||||
package = {
|
||||
"statuses": self.statuses,
|
||||
"position": self.position,
|
||||
"joints": self.joints
|
||||
}
|
||||
|
||||
try:
|
||||
self.client_connection.sendall(json.dumps(package))
|
||||
self.client_connection.sendall("#####")
|
||||
except:
|
||||
self.client_connection, self.client_address = self.status_tcp_server.accept()
|
||||
|
||||
def get_joint_angles(self):
|
||||
j1, j2, j3, j4, j5, j6, _, _ = self.robot_current_joint_angles.Value
|
||||
|
||||
self.joints = {
|
||||
1: j1,
|
||||
2: j2,
|
||||
3: j3,
|
||||
4: j4,
|
||||
5: j5,
|
||||
6: j6
|
||||
}
|
||||
# print(self.robot_current_joint_angles.Value)
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
|
||||
start_signal.connect(self.start)
|
||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||
kill_signal.connect(self.on_kill_threads_requested__slot)
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
@@ -0,0 +1,134 @@
|
||||
import json, socket
|
||||
|
||||
class Server(object):
|
||||
"""
|
||||
A JSON socket server used to communicate with a JSON socket client. All the
|
||||
data is serialized in JSON. How to use it:
|
||||
|
||||
server = Server(host, port)
|
||||
while True:
|
||||
server.accept()
|
||||
data = server.recv()
|
||||
# shortcut: data = server.accept().recv()
|
||||
server.send({'status': 'ok'})
|
||||
"""
|
||||
|
||||
backlog = 5
|
||||
client = None
|
||||
|
||||
def __init__(self, host, port):
|
||||
self.socket = socket.socket()
|
||||
self.socket.bind((host, port))
|
||||
self.socket.listen(self.backlog)
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def accept(self):
|
||||
# if a client is already connected, disconnect it
|
||||
if self.client:
|
||||
self.client.close()
|
||||
self.client, self.client_addr = self.socket.accept()
|
||||
return self
|
||||
|
||||
def send(self, data):
|
||||
if not self.client:
|
||||
raise Exception('Cannot send data, no client is connected')
|
||||
_send(self.client, data)
|
||||
return self
|
||||
|
||||
def recv(self):
|
||||
if not self.client:
|
||||
raise Exception('Cannot receive data, no client is connected')
|
||||
return _recv(self.client)
|
||||
|
||||
def close(self):
|
||||
if self.client:
|
||||
self.client.close()
|
||||
self.client = None
|
||||
if self.socket:
|
||||
self.socket.close()
|
||||
self.socket = None
|
||||
|
||||
|
||||
class Client(object):
|
||||
"""
|
||||
A JSON socket client used to communicate with a JSON socket server. All the
|
||||
data is serialized in JSON. How to use it:
|
||||
|
||||
data = {
|
||||
'name': 'Patrick Jane',
|
||||
'age': 45,
|
||||
'children': ['Susie', 'Mike', 'Philip']
|
||||
}
|
||||
client = Client()
|
||||
client.connect(host, port)
|
||||
client.send(data)
|
||||
response = client.recv()
|
||||
# or in one line:
|
||||
response = Client().connect(host, port).send(data).recv()
|
||||
"""
|
||||
|
||||
socket = None
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def connect(self, host, port):
|
||||
self.socket = socket.socket()
|
||||
self.socket.connect((host, port))
|
||||
return self
|
||||
|
||||
def send(self, data):
|
||||
if not self.socket:
|
||||
raise Exception('You have to connect first before sending data')
|
||||
_send(self.socket, data)
|
||||
return self
|
||||
|
||||
def recv(self):
|
||||
if not self.socket:
|
||||
raise Exception('You have to connect first before receiving data')
|
||||
return _recv(self.socket)
|
||||
|
||||
def recv_and_close(self):
|
||||
data = self.recv()
|
||||
self.close()
|
||||
return data
|
||||
|
||||
def close(self):
|
||||
if self.socket:
|
||||
self.socket.close()
|
||||
self.socket = None
|
||||
|
||||
|
||||
## helper functions ##
|
||||
|
||||
def _send(socket, data):
|
||||
try:
|
||||
serialized = json.dumps(data)
|
||||
except (TypeError, ValueError), e:
|
||||
raise Exception('You can only send JSON-serializable data')
|
||||
# send the length of the serialized data first
|
||||
socket.send('%d\n' % len(serialized))
|
||||
# send the serialized data
|
||||
socket.sendall(serialized)
|
||||
|
||||
def _recv(socket):
|
||||
# read the length of the data, letter by letter until we reach EOL
|
||||
length_str = ''
|
||||
char = socket.recv(1)
|
||||
while char != '\n':
|
||||
length_str += char
|
||||
char = socket.recv(1)
|
||||
total = int(length_str)
|
||||
# use a memoryview to receive the data chunk by chunk efficiently
|
||||
view = memoryview(bytearray(total))
|
||||
next_offset = 0
|
||||
while total - next_offset > 0:
|
||||
recv_size = socket.recv_into(view[next_offset:], total - next_offset)
|
||||
next_offset += recv_size
|
||||
try:
|
||||
deserialized = json.loads(view.tobytes())
|
||||
except (TypeError, ValueError), e:
|
||||
raise Exception('Data received was not in JSON format')
|
||||
return deserialized
|
||||
@@ -0,0 +1,88 @@
|
||||
# import time
|
||||
# from socket import socket, gethostbyname, AF_INET, SOCK_DGRAM
|
||||
#
|
||||
# SERVER_IP = '192.168.1.14'
|
||||
# PORT_NUMBER = 5000
|
||||
#
|
||||
# mySocket = socket( AF_INET, SOCK_DGRAM )
|
||||
# mySocket.connect((SERVER_IP,PORT_NUMBER))
|
||||
#
|
||||
# while True:
|
||||
# mySocket.send('cool')
|
||||
# time.sleep(.5)
|
||||
|
||||
# from socket import socket, gethostbyname, AF_INET, SOCK_DGRAM
|
||||
# import sys
|
||||
# PORT_NUMBER = 5000
|
||||
# SIZE = 1024
|
||||
#
|
||||
# hostName = gethostbyname( '0.0.0.0' )
|
||||
#
|
||||
# mySocket = socket( AF_INET, SOCK_DGRAM )
|
||||
# mySocket.bind( (hostName, PORT_NUMBER) )
|
||||
#
|
||||
# print ("Test server listening on port {0}\n".format(PORT_NUMBER))
|
||||
#
|
||||
# while True:
|
||||
# (data,addr) = mySocket.recvfrom(SIZE)
|
||||
# print data
|
||||
|
||||
# import jsocket
|
||||
# import logging
|
||||
# import time
|
||||
#
|
||||
# logger = logging.getLogger("jsocket.example_servers")
|
||||
#
|
||||
# class MyFactoryThread(jsocket.ServerFactoryThread):
|
||||
# def __init__(self):
|
||||
# super(MyFactoryThread, self).__init__()
|
||||
# self.address = "0.0.0.0"
|
||||
# self.port = 21151
|
||||
#
|
||||
# ##
|
||||
# # virtual method - Implementer must define protocol
|
||||
# def _process_message(self, obj):
|
||||
# if obj != '':
|
||||
# if obj['message'] == "new connection":
|
||||
# logger.info("new connection.")
|
||||
# else:
|
||||
# logger.info(obj)
|
||||
#
|
||||
#
|
||||
# if __name__ == "__main__":
|
||||
# server = jsocket.ServerFactory(MyFactoryThread)
|
||||
# server.timeout = 2.0
|
||||
# server.start()
|
||||
#
|
||||
# while True:
|
||||
# time.sleep(0.1)
|
||||
|
||||
import socket
|
||||
|
||||
HOST = ''
|
||||
PORT = 9876
|
||||
ADDR = (HOST,PORT)
|
||||
BUFSIZE = 4096
|
||||
|
||||
serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
|
||||
serv.bind(ADDR)
|
||||
serv.listen(5)
|
||||
|
||||
print 'listening ...'
|
||||
|
||||
while True:
|
||||
conn, addr = serv.accept()
|
||||
print 'client connected ... ', addr
|
||||
myfile = open('testfile.mov', 'w')
|
||||
|
||||
while True:
|
||||
data = conn.recv(BUFSIZE)
|
||||
if not data: break
|
||||
print data
|
||||
print 'writing file ....'
|
||||
|
||||
myfile.close()
|
||||
print 'finished writing file'
|
||||
conn.close()
|
||||
print 'client disconnected'
|
||||
@@ -0,0 +1,73 @@
|
||||
import sys
|
||||
from PyQt5 import QtWidgets, QtCore, QtGui, uic
|
||||
import signal
|
||||
|
||||
from Framework.arm_status_sender import ArmStatusSender as ArmStatusSender
|
||||
from Framework.arm_control_receiver import ArmControlReceiver as ArmControlReceiver
|
||||
|
||||
|
||||
#####################################
|
||||
# GroundStation Class Definition
|
||||
#####################################
|
||||
class NetworkSlave(QtCore.QObject):
|
||||
start_threads_signal = QtCore.pyqtSignal()
|
||||
connect_signals_and_slots_signal = QtCore.pyqtSignal()
|
||||
kill_threads_signal = QtCore.pyqtSignal()
|
||||
|
||||
def __init__(self, parent=None,):
|
||||
# noinspection PyArgumentList
|
||||
super(NetworkSlave, self).__init__(parent)
|
||||
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
# self.logger = logging.getLogger("groundstation")
|
||||
|
||||
self.shared_objects = {
|
||||
"regular_classes": {},
|
||||
"threaded_classes": {}
|
||||
}
|
||||
|
||||
# ##### Instantiate Regular Classes ######
|
||||
|
||||
# ##### Instantiate Threaded Classes ######
|
||||
self.__add_thread("Arm Status Sender", ArmStatusSender(self.shared_objects))
|
||||
self.__add_thread("Arm Control Receiver", ArmControlReceiver(self.shared_objects))
|
||||
|
||||
self.connect_signals_and_slots_signal.emit()
|
||||
self.__connect_signals_to_slots()
|
||||
self.start_threads_signal.emit()
|
||||
|
||||
def __add_thread(self, thread_name, instance):
|
||||
self.shared_objects["threaded_classes"][thread_name] = instance
|
||||
instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
|
||||
self.kill_threads_signal)
|
||||
|
||||
def __connect_signals_to_slots(self):
|
||||
pass
|
||||
|
||||
def on_exit_requested__slot(self):
|
||||
self.kill_threads_signal.emit()
|
||||
|
||||
# Wait for Threads
|
||||
for thread in self.shared_objects["threaded_classes"]:
|
||||
self.shared_objects["threaded_classes"][thread].wait()
|
||||
|
||||
QtCore.QCoreApplication.exit()
|
||||
|
||||
|
||||
#####################################
|
||||
# Main Definition
|
||||
#####################################
|
||||
if __name__ == "__main__":
|
||||
signal.signal(signal.SIGINT, signal.SIG_DFL) # This allows the keyboard interrupt kill to work properly
|
||||
|
||||
# ########## Start the QApplication Framework ##########
|
||||
application = QtCore.QCoreApplication(sys.argv) # Create the ase qt gui application
|
||||
|
||||
# ########## Set Organization Details for QSettings ##########
|
||||
QtCore.QCoreApplication.setOrganizationName("OSURC")
|
||||
QtCore.QCoreApplication.setOrganizationDomain("http://osurobotics.club/")
|
||||
QtCore.QCoreApplication.setApplicationName("denso_slave")
|
||||
|
||||
# ########## Start Ground Station If Ready ##########
|
||||
network_slave = NetworkSlave()
|
||||
application.exec_() # Execute launching of the application
|
||||
Reference in New Issue
Block a user