def __init__(self):
self.root = tk.Tk()
self.fd = tk.Button(self.root, text='Forwards', command=self.ha)
self.bk = tk.Button(self.root, text='Back', command=self.ha)
self.rt = tk.Button(self.root, text='Right', command=self.ha)
self.lt = tk.Button(self.root, text='Left', command=self.ha)
self.fd.pack()
self.bk.pack()
self.rt.pack()
self.lt.pack()
self.root.bind('<Left>', lambda x: self.leftKey(x))
self.root.bind('<Right>', lambda x: self.rightKey(x))
self.root.bind('<Up>', lambda x: self.upKey(x))
self.root.bind('<Down>', lambda x: self.downKey(x))
self.root.bind('<space>', lambda x: self.spaceKey(x))
self.speed = 140
self.forwards = 0
self.twist = 0
self.time = time.time()
# Publisher.broadcast_ip = "<broadcast>"
self.pub = Publisher(self.fields, self.typ, self.port)
self.root.after(100, self.publish)
self.root.mainloop()
def __init__(self):
self.master = tk.Tk()
self.PIX_per_M = 50
self.canvas = tk.Canvas(self.master, width=400, height=400)
self.canvas.pack()
self.robot = Robot(200, 200, self.canvas)
self.master.bind('<Left>', lambda e: self.robot.move(0, 0.1))
self.master.bind('<Right>', lambda e: self.robot.move(0, -0.1))
self.master.bind('<Up>', lambda e: self.robot.move(1, -0))
self.master.bind('<Down>', lambda e: self.robot.move(-1, -0))
self.canvas.bind('<Button-1>', self.mouse_draw_down)
self.canvas.bind('<B1-Motion>', self.mouse_draw_move)
self.canvas.bind('<ButtonRelease-1>', self.mouse_draw_up)
self.canvas.bind('<Button-2>', self.mouse_delete)
self.obstacles = []
self.mouse_mode = None
self.scan_points = []
self.lidar = Publisher(8110)
self.odom = Publisher(8810) # publishes twist not wheel rotations
self.master.after(1000, self.scan)
self.master.after(100, self.odom_pub)
tk.mainloop()
def __init__(self):
self.imu = Subscriber(8220, timeout=1)
self.gps = Subscriber(8280, timeout=2)
self.auto_control = Subscriber(8310, timeout=5)
self.cmd_vel = Publisher(8830)
# self.lights = Publisher(8590)
self.servo = Publisher(8120)
self.aruco = Subscriber(8390, timeout=2)
time.sleep(3)
self.start_point = {
"lat": self.gps.get()['lat'],
"lon": self.gps.get()['lon']
}
self.lookahead_radius = 6
self.final_radius = 1.5 # how close should we need to get to last endpoint
self.search_radius = 20 # how far should we look from the given endpoint
self.reached_destination = False # switch modes into tennis ball seach
self.robot = None
self.guess = None # where are we driving towards
self.guess_radius = 3 # if we are within this distance we move onto the next guess
self.guess_time = None
self.last_tennis_ball = 0
self.past_locations = []
self.stuck_time = 0
while True:
self.update()
time.sleep(0.1)
def main():
cam_pipes, depth_scale = c_man.get_pipes()
#print("{} pipes created!".format(len(cam_pipes)))
dec_filter = rs.decimation_filter()
dep_scan = im_scan.scan_portait()
traj_recv = Trajectory_Reciever()
stay_go_sender = Publisher(3500)
while True:
frameset = c_man.get_frames(cam_pipes, dec_filter)
scan_mat = dep_scan.get_portrait_from_frames(frameset, depth_scale)
#dep_scan.conv_reduce()
_, trans_position = dep_scan.get_pose()
xz_pos = np.delete(trans_position, 1, axis=0)
x_vel, y_vel = traj_recv.get_trajectory()
control_vec = dep_scan.vector_from_vel(x_vel, y_vel)
print("!dir_vec: ", control_vec)
soft_danger, rgb_mat = real_oord_test(dep_scan,
dir_vec=control_vec,
position=xz_pos)
stay_go_sender.send({"soft_danger": soft_danger})
print("-------------------------pass_fail: ", soft_danger)
cv2.imshow("circumstances", rgb_mat)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
def __init__(self):
GPIO.setmode(GPIO.BCM)
GPIO.setup(SHOULDER_HOME_INPUT, GPIO.IN, pull_up_down = GPIO.PUD_UP)
GPIO.setup(ELBOW_HOME_INPUT, GPIO.IN, pull_up_down = GPIO.PUD_UP)
self.target_vel = Subscriber(8410)
self.xyz_names = ["x", "y","yaw"]
self.output_pub = Publisher(8420)
self.cartesian_motors = ["shoulder","elbow","yaw"]
self.motor_names = ["shoulder","elbow","yaw","roll","grip"]
self.pwm_names = ["pitch"]
self.ordering = ["shoulder","elbow","pitch","yaw","grip","roll"]
self.native_positions = { motor:0 for motor in self.motor_names}
self.currents = { motor:0 for motor in self.motor_names}
self.xyz_positions = { axis:0 for axis in self.xyz_names}
self.elbow_left = True
self.CPR = {'shoulder': -12.08 * 4776.38,
'elbow' : -12.08 * 2442.96,
'yaw' : -float(48)/27 * 34607,
'roll' : 455.185*float(12*53/20),
'grip' : 103.814*float(12*36/27)}
# 'pitch' : -2 * 34607}
self.SPEED_SCALE = 20
self.rc = RoboClaw(find_serial_port(), names = self.ordering,\
addresses = [130,128,129])
self.zeroed = False
self.storageLoc = [0,0]
self.limits = {'shoulder':[-2.18,2.85],
'elbow' : [-4,2.24], #-2.77
'yaw' : [-3.7,3.7] }
self.dock_pos = {'shoulder': 2.76,
'elbow' : -2.51,
'yaw' : -3.01 }
self.dock_speeds = [.01,.006]
self.forcing = False
try:
while 1:
start_time = time.time()
self.update()
while (time.time() - start_time) < 0.1:
pass
except KeyboardInterrupt:
self.send_speeds( {motor: 0 for motor in self.motor_names}, {motor: 0 for motor in self.pwm_names} )
raise
except:
self.send_speeds( {motor: 0 for motor in self.motor_names}, {motor: 0 for motor in self.pwm_names} )
raise
def main():
cv_publisher = Publisher(105)
MODELS_DIR = '/home/cerbaris/pupper_code/PupperPy/pupperpy/Vision/models/'
MODEL_PATH = MODELS_DIR + 'ssd_mobilenet_v2_pupper_quant_edgetpu.tflite'
LABEL_PATH = MODELS_DIR + 'pupper_labels.txt'
LOG_FILE = '/home/cerbaris/pupper_code/PupperPy/pupperpy/Vision/vision_log.txt'
labels = dataset_utils.read_label_file(LABEL_PATH)
engine = DetectionEngine(MODEL_PATH)
with picamera.PiCamera() as camera:
camera.resolution = (640, 480)
camera.framerate = 30
_, height, width, _ = engine.get_input_tensor_shape()
try:
stream = io.BytesIO()
#count = 0
for _ in camera.capture_continuous(stream, format='rgb', use_video_port=True, resize=(width, height)):
stream.truncate()
stream.seek(0)
input_tensor = np.frombuffer(stream.getvalue(), dtype=np.uint8)
#image = Image.frombuffer('RGB',(width,height), stream.getvalue())
image = Image.frombuffer('RGB',(320,304), stream.getvalue()) # to account for automatic upscaling by picamera when format='rgb'
#draw = ImageDraw.Draw(image)
start_ms = time.time()
results = engine.detect_with_image(image,threshold=0.2,keep_aspect_ratio=True,relative_coord=False,top_k=10)
elapsed_ms = time.time() - start_ms
detectedObjs = []
for obj in results:
if (obj.label_id in range(3)):
box = obj.bounding_box.flatten().tolist()
#draw.rectangle(box, outline='red')
#draw.text((box[0],box[1]), labels[obj.label_id] + " " + str(obj.score))
w = box[0] - box[2]
h = box[1] - box[3]
objInfo = {'bbox_x':float(box[0]),
'bbox_y':float(box[1]),
'bbox_h':float(h),
'bbox_w':float(w),
'bbox_label':labels[obj.label_id],
'bbox_confidence': float(obj.score)
}
detectedObjs.append(objInfo)
try:
cv_publisher.send(detectedObjs)
except BaseException as e:
print('Failed to send bounding boxes. CV UDP subscriber likely not initialized')
pass
#print(detectedObjs)
#with open('/home/cerbaris/pupper_code/PupperPy/pupperpy/Vision/test_images_120120/' + str(count) + '.png','wb') as f:
# image.save(f)
#count+=1
except BaseException as e:
with open(LOG_FILE,'w') as f:
f.write("Failed to run detection loop:\n {0}\n".format(traceback.format_exc()))
def main(argv):
shutter = int(argv[0])
iso = 400
cam_params = {"shutter": shutter, "iso": iso}
reference = Publisher(9010)
camera = picamera.PiCamera(sensor_mode=2,
resolution=capture_res,
framerate=10)
camera.shutter_speed = shutter
camera.iso = iso
while True:
image = np.empty((capture_res[1] * capture_res[0] * 3), dtype=np.uint8)
camera.capture(image, 'bgr')
image = image.reshape((capture_res[1], capture_res[0], 3))
img_h, img_w = image.shape[0:2]
truth = image[int(img_h * 0):int(img_h * 0.3),
int(img_w * 0):int(img_w * 0.3), :]
truth_hsv = cv2.cvtColor(truth, cv2.COLOR_BGR2HSV)
fname = "truth.jpg"
cv2.imwrite(fname, truth)
brightpoint = np.percentile(truth_hsv[:, :, 2], 95)
toobright = brightpoint > 180
toodark = brightpoint < 80
if (toodark or toobright):
if toobright:
iso -= iso_step
if toodark:
iso += iso_step
iso = max(ISO_MIN, min(iso, ISO_MAX))
camera.iso = iso
print("ISO changed: %d" % (iso))
continue
# exposure is ok, let's send over reference colors
cam_params["range_hue"] = np.percentile(truth_hsv[:, :, 0],
(5, 95)).tolist()
cam_params["range_sat"] = np.percentile(truth_hsv[:, :, 1],
(5, 95)).tolist()
cam_params["range_val"] = np.percentile(truth_hsv[:, :, 2],
(5, 95)).tolist()
cam_params["iso"] = iso
reference.send(cam_params)
print(cam_params)
class Logger:
def __init__(self, port):
self.pub = Publisher(port)
self.points = []
def next(self):
self.pub.send(self.points)
self.points = []
def add(self, pose, color):
self.points.append([(pose.x, pose.y), pose.a, color])
def __init__(self):
self.pub = Publisher(8120)
self.root = tk.Tk()
self.fd = tk.Button(self.root,
text='Up',
command=lambda: self.upKey(None))
self.bk = tk.Button(self.root,
text='Down',
command=lambda: self.downKey(None))
self.rt = tk.Button(self.root,
text='Right',
command=lambda: self.rightKey(None))
self.lt = tk.Button(self.root,
text='Left',
command=lambda: self.leftKey(None))
self.cam2 = tk.Button(self.root,
text='POV',
command=lambda: self.launchCam('camera2'))
self.cam3 = tk.Button(self.root,
text='Side',
command=lambda: self.launchCam('camera3'))
self.ptz = tk.Button(self.root,
text='PTZ',
command=lambda: self.launchCam('ptz'))
self.drive = tk.Button(self.root,
text='drive',
command=lambda: self.launchCam('drive'))
self.fd.pack()
self.bk.pack()
self.rt.pack()
self.lt.pack()
self.cam2.pack()
self.cam3.pack()
self.ptz.pack()
self.drive.pack()
self.root.bind('<Left>', lambda x: self.leftKey(x))
self.root.bind('<Right>', lambda x: self.rightKey(x))
self.root.bind('<Up>', lambda x: self.upKey(x))
self.root.bind('<Down>', lambda x: self.downKey(x))
self.root.bind('<space>', lambda x: self.spaceKey(x))
self.pan = 0
self.tilt = 0
self.speed = 2
self.time = time.time()
self.root.after(100, self.update)
self.root.mainloop()
def main():
cv_publisher = Publisher(105)
MODELS_DIR = '/home/cerbaris/pupper_code/PupperPy/pupperpy/Vision/models/'
MODEL_PATH = MODELS_DIR + 'ssd_mobilenet_v2_coco_quant_postprocess_edgetpu.tflite'
LABEL_PATH = MODELS_DIR + 'coco_labels.txt'
LOG_FILE = '/home/cerbaris/pupper_code/PupperPy/pupperpy/Vision/vision_log.txt'
labels = dataset_utils.read_label_file(LABEL_PATH)
engine = DetectionEngine(MODEL_PATH)
with picamera.PiCamera() as camera:
camera.resolution = (640, 480)
camera.framerate = 30
_, height, width, _ = engine.get_input_tensor_shape()
stream = io.BytesIO()
count = 0
for _ in camera.capture_continuous(stream, format='rgb', use_video_port=True, resize=(width, height)):
stream.truncate()
stream.seek(0)
input_tensor = np.frombuffer(stream.getvalue(), dtype=np.uint8)
#image = Image.frombuffer('RGB',(width,height), stream.getvalue())
image = Image.frombuffer('RGB',(320,304), stream.getvalue()) # to account for automatic upscaling by picamera when format='rgb'
draw = ImageDraw.Draw(image)
start_ms = time.time()
results = engine.detect_with_image(image,threshold=0.1,keep_aspect_ratio=True,relative_coord=False,top_k=51)
elapsed_ms = time.time() - start_ms
detectedObjs = []
for obj in results:
if (obj.label_id == 0 or obj.label_id == 36):
if (obj.label_id == 36):
print('Tennis ball detected')
box = obj.bounding_box.flatten().tolist()
draw.rectangle(box, outline='red')
draw.text((box[0],box[1]), labels[obj.label_id] + " " + str(obj.score))
w = box[0] - box[2]
h = box[1] - box[3]
objInfo = {'bbox_x':float(box[0]),
'bbox_y':float(box[1]),
'bbox_h':float(h),
'bbox_w':float(w),
'bbox_label':labels[obj.label_id],
'bbox_confidence': float(obj.score)
}
detectedObjs.append(objInfo)
cv_publisher.send(detectedObjs)
#print(detectedObjs)
with open('/home/cerbaris/pupper_code/PupperPy/pupperpy/Vision/test_images/' + str(count) + '.png','wb') as f:
image.save(f)
count+=1
class Sim:
def __init__(self):
self.master = tk.Tk()
self.canvas = tk.Canvas(self.master, width=800, height=200, bg="white")
self.canvas.pack()
self.robot = Robot(200)
self.master.bind('<Left>', lambda e: self.robot.change_speed(-1))
self.master.bind('<Right>', lambda e: self.robot.change_speed(+1))
self.master.bind('<space>', lambda e: self.robot.stop())
self.pub = Publisher(8810, "127.0.0.1")
self.canvas.create_rectangle(0, 100, 800, 200, fill="blue")
self.canvas.create_rectangle(0, 0, 10, 200, fill="blue")
self.master.after(0, self.update)
tk.mainloop()
def update(self):
self.robot.update()
self.publish(self.robot)
self.plot(self.robot)
self.master.after(RATE, self.update)
def publish(self, robot):
truth = robot.pos
pos = robot.pos + random.gauss(0, 40)
vel = robot.vel + random.gauss(0, 5)
msg = {"truth": truth, "dist": pos, "vel": vel}
try:
self.pub.send(msg)
except:
pass
def plot(self, robot):
if (robot.id != None):
self.canvas.delete(robot.id)
robot.id = self.canvas.create_rectangle(robot.pos,
50,
robot.pos + 100,
100,
fill="red")
def __init__(self):
self.imu = Subscriber(8220, timeout=2)
self.gps = Subscriber(8280, timeout=3)
self.auto_control = Subscriber(8310, timeout=5)
self.lights = Publisher(8311)
self.cmd_vel = Publisher(8830)
self.logger = Logger(8312)
self.aruco = Subscriber(8390, timeout=3)
time.sleep(2) # delay to give extra time for gps message
self.start_gps = self.gps.recv()
self.cmd_sent = False
def __init__(self, port=8830, rate=20):
self.keybinds = {
"space": self.toggle_trot,
"v": self.activate,
"w": self.move_forward,
"a": self.rotate_left,
"s": self.move_backward,
"d": self.rotate_right,
"q": self.move_left,
"e": self.move_right,
"x": self.move
}
self.velocity = 1 # Needs tuning
self.lx = 0
self.ly = 0
self.rx = 0
self.l_alpha = 0.15
self.r_alpha = 0.3
self.pub = Publisher(port)
self.rate = rate
self.blank_msg = {
"ly": 0,
"lx": 0,
"rx": 0,
"ry": 0,
"L2": 0,
"R2": 0,
"R1": 0,
"L1": 0,
"dpady": 0,
"dpadx": 0,
"x": 0,
"square": 0,
"circle": 0,
"triangle": 0,
"message_rate": self.rate,
}
keyboard.on_press(lambda c: self.keypress(c))
self.active = False
self.ready = False
print("Controller initialized")
def __init__(self, target='tennis_ball'):
self.timer = RepeatTimer(1 / MESSAGE_RATE, self._step)
self.control_state = ControllerState()
self.pos = PositionTracker(self.control_state)
self.obj_sensors = ObjectSensors()
self.active = False
self.walking = False
self.user_stop = False
self.cmd_pub = Publisher(CMD_PUB_PORT)
self.cmd_sub = Subscriber(CMD_SUB_PORT)
self.cv_sub = Subscriber(CV_SUB_PORT)
self.joystick = Joystick()
self.joystick.led_color(**PUPPER_COLOR)
self.state = 'off'
self.target = target
self.current_target = None
self.pusher_client = PusherClient()
self._last_sensor_data = (None, None, None)
def __init__(self):
self.master = tk.Tk()
self.canvas = tk.Canvas(self.master, width=800, height=200, bg="white")
self.canvas.pack()
self.robot = Robot(200)
self.master.bind('<Left>', lambda e: self.robot.change_speed(-1))
self.master.bind('<Right>', lambda e: self.robot.change_speed(+1))
self.master.bind('<space>', lambda e: self.robot.stop())
self.pub = Publisher(8810, "127.0.0.1")
self.canvas.create_rectangle(0, 100, 800, 200, fill="blue")
self.canvas.create_rectangle(0, 0, 10, 200, fill="blue")
self.master.after(0, self.update)
tk.mainloop()
def main(argv):
idx_camera = int(argv[0])
offset_degrees = float(argv[1])
reference = Subscriber(9010)
detection_results = Publisher(902 + idx_camera)
camera = picamera.PiCamera(sensor_mode=2,
resolution=capture_res,
framerate=10)
cam_params = {}
while True:
try:
cam_params = reference.get()
print(cam_params)
except UDPComms.timeout:
if "range_hue" not in cam_params:
continue
camera.shutter_speed = cam_params["shutter"]
camera.iso = cam_params["iso"]
image = np.empty((capture_res[1] * capture_res[0] * 3), dtype=np.uint8)
camera.capture(image, 'bgr')
image = image.reshape((capture_res[1], capture_res[0], 3))
hsv_ranges = (cam_params["range_hue"], cam_params["range_sat"],
cam_params["range_val"])
heading, distance = find_ball_direct(image, hsv_ranges, offset_degrees)
if (distance > 0):
result = {"heading": heading, "distance": distance}
detection_results.send(result)
print("Found ball!")
print(result)
from httplib import HTTPConnection
from base64 import b64encode
import pynmea2, time
from user_pass import user
from UDPComms import Publisher
publish_interval = 1
pub = Publisher(8290)
server = "rtgpsout.unavco.org:2101"
headers = {
'Ntrip-Version': 'Ntrip/2.0',
'User-Agent': 'NTRIP ntrip_ros',
'Connection': 'close',
'Authorization': 'Basic ' + b64encode(user)
}
connection = HTTPConnection(server)
connection.request('GET', '/SLAC_RTCM3', body=None, headers=headers)
response = connection.getresponse()
# Message format
# https://forum.u-blox.com/index.php/16898/decoding-rtcm3-message
def get_length(msg):
assert (msg[0] == chr(0xd3))
return (ord(msg[1]) * 8 + ord(msg[2])) & 0x3ff
# [ 0. , 671.14819063, 204.40567262],
# [ 0. , 0. , 1. ]])
#c930
dist = np.array([[-9.25076213e-01], [2.10481105e+01], [-6.69224079e-03],
[-1.78445274e-02], [3.48224610e+01], [-9.95776799e-01],
[2.07642164e+01], [3.42550795e+01], [0.00000000e+00],
[0.00000000e+00], [0.00000000e+00], [0.00000000e+00],
[0.00000000e+00], [0.00000000e+00]])
mtx = np.array([[526.38372224, 0., 344.76856495],
[0., 526.38372224, 246.66391938], [0., 0., 1.]])
from CameraStream import Server
s = Server(Server.INPUT.OPENCV)
pub = Publisher(8390)
def main():
cap = cv2.VideoCapture(0)
while (True):
# Capture frame-by-frame
ret, frame = cap.read()
# Display the resulting frame
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
frame, dictionary)
image = cv2.aruco.drawDetectedMarkers(frame, corners, ids)
if mtx is not None:
from UDPComms import Publisher, Subscriber, timeout
import time
import bluetooth
import json
from pupperpy.BluetoothInterface import BluetoothServer
## Configurable ##
hostMACAddress = 'B8:27:EB:5E:D6:8F' ## MAC address to bluetooth adapter on pi
BLE_PORT = 3
BLE_MSG_SIZE = 1024
MESSAGE_RATE = 20
## End Config ##
PUPPER_COLOR = {"red":0, "blue":0, "green":255}
pupper_pub = Publisher(8830)
pupper_sub = Subscriber(8840, timeout=0.01)
def send_command(values):
left_y = -values["left_analog_y"]
right_y = -values["right_analog_y"]
right_x = values["right_analog_x"]
left_x = values["left_analog_x"]
L2 = values["l2"]
R2 = values["r2"]
R1 = values["r1"]
L1 = values["l1"]
#!/usr/bin/env python3
import odrive
from odrive.enums import *
from UDPComms import Subscriber, Publisher, timeout
import time
import os
if os.geteuid() != 0:
exit(
"You need to have root privileges to run this script.\nPlease try again, this time using 'sudo'. Exiting."
)
cmd = Subscriber(8830, timeout=0.3)
telemetry = Publisher(8810)
print("finding an odrives...")
middle_odrive = odrive.find_any(serial_number="206230804648")
print("found middle odrive")
front_odrive = odrive.find_any(serial_number="206C35733948")
print("found middle odrive")
back_odrive = odrive.find_any(serial_number="207D35903948")
print("found back odrive")
print("found all odrives")
def clear_errors(odrive):
if odrive.axis0.error:
print("axis 0", odrive.axis0.error)
class PTZPannel:
def __init__(self):
self.pub = Publisher(8120)
self.root = tk.Tk()
self.fd = tk.Button(self.root,
text='Up',
command=lambda: self.upKey(None))
self.bk = tk.Button(self.root,
text='Down',
command=lambda: self.downKey(None))
self.rt = tk.Button(self.root,
text='Right',
command=lambda: self.rightKey(None))
self.lt = tk.Button(self.root,
text='Left',
command=lambda: self.leftKey(None))
self.cam2 = tk.Button(self.root,
text='POV',
command=lambda: self.launchCam('camera2'))
self.cam3 = tk.Button(self.root,
text='Side',
command=lambda: self.launchCam('camera3'))
self.ptz = tk.Button(self.root,
text='PTZ',
command=lambda: self.launchCam('ptz'))
self.drive = tk.Button(self.root,
text='drive',
command=lambda: self.launchCam('drive'))
self.fd.pack()
self.bk.pack()
self.rt.pack()
self.lt.pack()
self.cam2.pack()
self.cam3.pack()
self.ptz.pack()
self.drive.pack()
self.root.bind('<Left>', lambda x: self.leftKey(x))
self.root.bind('<Right>', lambda x: self.rightKey(x))
self.root.bind('<Up>', lambda x: self.upKey(x))
self.root.bind('<Down>', lambda x: self.downKey(x))
self.root.bind('<space>', lambda x: self.spaceKey(x))
self.pan = 0
self.tilt = 0
self.speed = 2
self.time = time.time()
self.root.after(100, self.update)
self.root.mainloop()
def launchCam(self, cam):
os.system("bash launch_camera.sh " + cam + ".local")
def spaceKey(self, event):
print "space"
self.pan = 0
self.tilt = 0
self.time = time.time()
def leftKey(self, event):
print "left"
self.pan = -self.speed
self.tilt = 0
self.time = time.time()
def rightKey(self, event):
print "right"
self.pan = self.speed
self.tilt = 0
self.time = time.time()
def upKey(self, event):
print "up"
self.pan = 0
self.tilt = -self.speed
self.time = time.time()
def downKey(self, event):
print "down"
self.pan = 0
self.tilt = self.speed
self.time = time.time()
def update(self):
print('update')
try:
if (time.time() - self.time) > 0.3:
self.pan = 0
self.tilt = 0
print('sending')
self.pub.send({'pan': self.pan, 'tilt': self.tilt})
except:
raise
finally:
self.root.after(100, self.update)
from UDPComms import Publisher
from PS4Joystick import Joystick
import time
## you need to git clone the PS4Joystick repo and run `sudo bash install.sh`
## Configurable ##
MESSAGE_RATE = 20
PUPPER_COLOR = {"red":255, "blue":255, "green":0}
drive_pub = Publisher(8830)
j = Joystick()
while True:
print("running")
values = j.get_input()
j.led_color(**PUPPER_COLOR)
forward_left = - values['left_analog_y']
forward_right = - values['right_analog_y']
twist_right = values['right_analog_x']
twist_left = values['left_analog_x']
L2 = values['l2_analog']
R2 = values['r2_analog']
on_right = values['button_r1']
on_left = values['button_l1']
square = values['button_square']
#!/usr/bin/env python3
import odrive
from odrive.enums import *
from UDPComms import Subscriber, Publisher, timeout
import time
import os
if os.geteuid() != 0:
exit(
"You need to have root privileges to run this script.\nPlease try again, this time using 'sudo'. Exiting."
)
cmd = Subscriber(8830, timeout=0.3)
telemetry = Publisher(8810)
print("finding an odrives...")
middle_odrive = odrive.find_any(serial_number="206230804648")
print("found front odrive")
front_odrive = odrive.find_any(serial_number="206C35733948")
print("found middle odrive")
back_odrive = odrive.find_any(serial_number="207D35903948")
print("found back odrive")
print("found all odrives")
def clear_errors(odrive):
if odrive.axis0.error:
print("axis 0", odrive.axis0.error)
from UDPComms import Publisher, Subscriber, timeout
from PS4Joystick import Joystick
import time
## you need to git clone the PS4Joystick repo and run `sudo bash install.sh`
## Configurable ##
MESSAGE_RATE = 20
PUPPER_COLOR = {"red": 0, "blue": 0, "green": 255}
joystick_pub = Publisher(8830, 65530)
joystick_subcriber = Subscriber(8840, timeout=0.01)
joystick = Joystick()
joystick.led_color(**PUPPER_COLOR)
while True:
print("running")
values = joystick.get_input()
left_y = -values["left_analog_y"]
right_y = -values["right_analog_y"]
right_x = values["right_analog_x"]
left_x = values["left_analog_x"]
L2 = values["l2_analog"]
R2 = values["r2_analog"]
R1 = values["button_r1"]
L1 = values["button_l1"]
class Control(object):
STATES = ['off', 'rest', 'meander', 'goto', 'avoid']
SCREEN_MID_X = 150
def __init__(self, target='tennis_ball'):
self.timer = RepeatTimer(1 / MESSAGE_RATE, self._step)
self.control_state = ControllerState()
self.pos = PositionTracker(self.control_state)
self.obj_sensors = ObjectSensors()
self.active = False
self.walking = False
self.user_stop = False
self.cmd_pub = Publisher(CMD_PUB_PORT)
self.cmd_sub = Subscriber(CMD_SUB_PORT)
self.cv_sub = Subscriber(CV_SUB_PORT)
self.joystick = Joystick()
self.joystick.led_color(**PUPPER_COLOR)
self.state = 'off'
self.target = target
self.current_target = None
self.pusher_client = PusherClient()
self._last_sensor_data = (None, None, None)
def move_forward(self, vel=ControllerState.LEFT_ANALOG_Y_MAX):
self.control_state.left_analog_y = vel
def move_backward(self, vel=-ControllerState.LEFT_ANALOG_Y_MAX):
self.control_state.left_analog_y = vel
def move_stop(self):
self.control_state.left_analog_y = 0
def turn_right(self, rate=ControllerState.RIGHT_ANALOG_X_MAX):
self.control_state.right_analog_x = rate
def turn_left(self, rate=-ControllerState.RIGHT_ANALOG_X_MAX):
self.control_state.right_analog_x = rate
def turn_stop(self):
self.control_state.right_analog_x = 0
def start_walk(self):
if self.walking or not self.active:
return
self.reset()
self.control_state.r1 = True
self.toggle_cmd()
self.walking = True
self.control_state.walking = True
self.reset()
def activate(self):
if self.active:
return
self.reset()
self.control_state.l1 = True
self.toggle_cmd()
self.reset()
self.active = True
self.state = 'rest'
self.walking = False
self.control_state.walking = False
if not self.pos.running:
self.pos.run()
def stop_walk(self):
if not self.walking:
return
self.reset()
self.control_state.r1 = True
self.toggle_cmd()
self.walking = False
self.control_state.walking = False
self.reset()
self.state = 'rest'
def reset(self):
self.control_state.reset()
def run_loop(self):
self.timer.start()
def stop_loop(self):
self.timer.cancel()
self.reset()
self.stop_walk()
self.pos.stop()
self.active = False
self.send_cmd()
self.user_stop = True
def toggle_cmd(self):
# For toggle commands, they should be sent several times to ensure they
# are put into effect
for _ in range(3):
self.send_cmd()
time.sleep(1 / MESSAGE_RATE)
def send_cmd(self):
cmd = self.control_state.get_state()
self.cmd_pub.send(cmd)
try:
msg = self.cmd_sub.get()
self.joystick.led_color(**msg["ps4_color"])
except timeout:
pass
def get_sensor_data(self):
try:
obj = self.obj_sensors.read()
pos = self.pos.data
try:
cv = self.cv_sub.get()
except timeout:
cv = []
except:
obj, pos, cv = self._last_sensor_data
self._last_sensor_data = (obj, pos, cv)
return obj, pos, cv
def _step(self):
js_msg = self.joystick.get_input()
# Force stop moving
if js_msg['button_l2']:
self.user_stop = True
self.stop_walk()
return
# User activate
if js_msg['button_l1']:
self.user_stop = False
self.activate()
return
if self.user_stop or not self.active:
self.reset()
self.send_cmd()
self.send_pusher_message()
return
if not self.walking:
self.start_walk()
return
self.update_behavior()
self.send_cmd()
self.send_pusher_message()
def update_behavior(self):
obj, pos, cv = self.get_sensor_data()
if not any(obj.values()):
self.turn_stop()
if any(obj.values()):
# If object, dodge
self.state = 'avoid'
self.dodge(obj)
elif any([x['bbox_label'] == self.target for x in cv]):
# if target, chase
self.state = 'goto'
self.goto(cv)
else:
# if nothing, wander
self.state = 'meander'
self.meander()
def dodge(self, obj):
'''Takes the object sensor data and adjusts the command to avoid any
objects
'''
if obj['left'] and obj['center']:
self.move_stop()
self.turn_right()
elif (obj['right'] and obj['center']) or obj['center']:
self.move_stop()
self.turn_left()
elif obj['left']:
self.move_forward(vel=ControllerState.LEFT_ANALOG_Y_MAX / 2)
self.turn_right()
elif obj['right']:
self.move_forward(vel=ControllerState.LEFT_ANALOG_Y_MAX / 2)
self.turn_left()
elif not any(obj.values()):
self.turn_stop()
def meander(self):
'''moves forward and makes slight turns left and right to search for a
target -- eventually, for now just move forward
'''
self.current_target = None
self.move_forward()
def goto(self, cv):
'''takes a list of bounding boxes, picks the most likely ball and moves
toward it
'''
tmp = [x for x in cv if x['bbox_label'] == self.target]
if len(tmp) == 0:
self.meander()
conf = np.array([x['bbox_confidence'] for x in tmp])
idx = np.argmax(conf)
best = tmp[idx]
center_x = best['bbox_x'] + best['bbox_w'] / 2
if center_x < self.SCREEN_MID_X:
self.turn_left()
elif center_x > self.SCREEN_MID_X:
self.turn_right()
self.move_forward()
self.current_target = best
def send_pusher_message(self):
obj, pos, cv = self.get_sensor_data()
bbox = self.current_target
timestamp = time.time()
if bbox is None:
bbox = {
'bbox_x': None,
'bbox_y': None,
'bbox_h': None,
'bbox_w': None,
'bbox_label': None,
'bbox_confidence': None
}
message = {
'time': timestamp,
'x_pos': pos['x'],
'y_pos': pos['y'],
'x_vel': pos['x_vel'],
'y_vel': pos['y_vel'],
'x_acc': pos['x_acc'],
'y_acc': pos['y_acc'],
'yaw': pos['yaw'],
'yaw_rate': pos['yaw_rate'],
'left_sensor': obj['left'],
'center_sensor': obj['center'],
'right_sensor': obj['right'],
'state': self.state,
**bbox
}
self.pusher_client.send(message)
from UDPComms import Publisher
import time
from mpu6050 import mpu6050
pub = Publisher(8004)
sensor = mpu6050(0x68)
sensor.set_gyro_range(mpu6050.GYRO_RANGE_2000DEG)
last_send = time.time()
bias = sensor.get_gyro_data()
while 1:
now = time.time()
if (now - last_send >= 0.01):
accel = sensor.get_accel_data()
accel_done = time.time()
gyro = sensor.get_gyro_data()
gyro_done = time.time()
gyro = {k: (v - bias[k]) / 180.0 * 3.14159 for k, v in gyro.items()}
accel = {k: -v for k, v in accel.items()}
pub.send((accel['x'], accel['y'], accel['z'], gyro['x'], gyro['y'],
gyro['z']))
print('{:<1.3f}\t{:<1.3f}\t{:<1.3f}'.format(gyro['x'], gyro['y'],
gyro['z']))
#print('{:1.3f}\t{:1.3f}\t{:1.3f}'.format(accel['x'],accel['y'],accel['z']))
#print(round(gyro['x'],3), '\t', round(gyro['y'], 3), '\t', round(gyro['z'],3))
#print(now - last_send, accel_done - now, gyro_done - accel_done)
last_send = now
def clean_sensor_data(gyro, accel, bias):
gyro = dictvec_to_array(gyro)
gyro = (gyro - bias) / 180.0 * np.pi
accel = -dictvec_to_array(accel)
return gyro, accel
if __name__ == "__main__":
# Attach a MPU6050 to the raspberry pi over I2C
filt_comp = ComplementaryFilter(alpha=0.01)
filt_mekf = MEKF(Q_gyro=1e-6, Q_bias=1e-12, R=1)
comp_pub = Publisher(8007)
mekf_pub = Publisher(8008)
raw_sensor_pub = Publisher(8009)
print(
"Publishing mekf data to port 8008, comp filter to 8007, and gyro/accel to 8009."
)
sensor = mpu6050(0x68)
sensor.set_gyro_range(mpu6050.GYRO_RANGE_2000DEG)
print("Attached gyro at I2C address 0x68 and gyro range of 2000deg/s")
DT = 0.005
last_update = time.time()
t = 0
return -1
if trigger == opt2:
return 1
return 0
def direction_helper(opt1, opt2):
if opt1:
return -1
if opt2:
return 1
return 0
if __name__ == "__main__":
pub = Publisher(8830)
pygame.init()
pygame.display.init()
MESSAGE_RATE = 20
win = pygame.display.set_mode((500, 250))
pygame.font.init()
font = pygame.font.SysFont("Arial", 20)
text_surface = font.render("Click to enable.", False, (220, 0, 0))
win.fill((255, 255, 255))
win.blit(text_surface, (40, 100))
while True:
pygame.event.pump()
omega_rad = math.radians(omega)
delta_t = time() - self.lastGyroTime
self.lastGyroTime = time()
rad = math.atan2(self.lastSin, self.lastCos) + omega_rad * delta_t
self.lastSin = math.sin(rad)
self.lastCos = math.cos(rad)
self.lastGyro = (self.lastGyro + omega * delta_t) % 360
def get_angle(self):
rad = math.atan2(self.lastSin, self.lastCos)
return math.degrees(rad) % 360
pub = Publisher(8220)
offset = 10 #random starting value mostly correct aroudn stanford
offset_sub = Subscriber(8210, timeout=5)
# I2C connection to IMU
i2c = busio.I2C(board.SCL, board.SDA)
imu = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
# I2C connection to IMU
compass = HMC6343()
# initialize filter
# TODO find good value
filt = ComplementaryFilter(0.99)
import os
import pygame
from UDPComms import Publisher
import signal
drive_pub = Publisher(8830)
arm_pub = Publisher(8410)
# prevents quiting on pi when run through systemd
def handler(signum, frame):
print("GOT singal", signum)
signal.signal(signal.SIGHUP, handler)
# those two lines allow for running headless (hopefully)
os.environ["SDL_VIDEODRIVER"] = "dummy"
os.putenv('DISPLAY', ':0.0')
pygame.display.init()
pygame.joystick.init()
# wait until joystick is connected
while 1:
try:
pygame.joystick.Joystick(0).init()
break
except pygame.error:
pygame.time.wait(500)
# Prints the joystick's name
JoyName = pygame.joystick.Joystick(0).get_name()
print("Name of the joystick:")
