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 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 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 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)
middle_odrive.axis0.controller.config.vel_gain = .2
middle_odrive.axis1.controller.config.vel_gain = .2
back_odrive.axis0.controller.config.vel_gain = .2
back_odrive.axis1.controller.config.vel_gain = .2
#print front_odrive.axis0.controller.config
while True:
try:
msg = cmd.get()
print(msg)
try:
telemetry.send([
middle_odrive.vbus_voltage,
front_odrive.axis0.motor.current_control.Iq_measured,
front_odrive.axis1.motor.current_control.Iq_measured,
middle_odrive.axis0.motor.current_control.Iq_measured,
middle_odrive.axis1.motor.current_control.Iq_measured,
back_odrive.axis0.motor.current_control.Iq_measured,
back_odrive.axis1.motor.current_control.Iq_measured
])
except:
pass
clear_errors(front_odrive)
clear_errors(middle_odrive)
clear_errors(back_odrive)
if (msg['t'] == 0 and msg['f'] == 0):
send_state(front_odrive, AXIS_STATE_IDLE)
send_state(middle_odrive, AXIS_STATE_IDLE)
send_state(back_odrive, AXIS_STATE_IDLE)
on_right = values['button_r1']
on_left = values['button_l1']
square = values['button_square']
x = values['button_cross']
circle = values['button_circle']
triangle = values['button_triangle']
d_pad_x = values['dpad_right'] - values['dpad_left']
d_pad_y = values['dpad_up'] - values['dpad_down']
msg = { "ly" : forward_left,
"lx" : twist_left,
"rx" : twist_right,
"ry" : forward_right,
"L2" : L2,
"R2" : R2,
"R1" : on_right,
"L1" : on_left,
"dpady" : d_pad_y,
"dpadx" : d_pad_x,
"x": x,
"square": square,
"circle": circle,
"triangle": triangle,
"message_rate" : MESSAGE_RATE
}
drive_pub.send(msg)
time.sleep(1/MESSAGE_RATE)
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)
dpadx = values["dpad_right"] - values["dpad_left"]
dpady = values["dpad_up"] - values["dpad_down"]
msg = {
"ly": left_y,
"lx": left_x,
"rx": right_x,
"ry": right_y,
"L2": L2,
"R2": R2,
"R1": R1,
"L1": L1,
"dpady": dpady,
"dpadx": dpadx,
"x": x,
"square": square,
"circle": circle,
"triangle": triangle,
"message_rate": MESSAGE_RATE,
}
joystick_pub.send(msg)
try:
msg = joystick_subcriber.get()
joystick.led_color(**msg["ps4_color"])
except timeout:
pass
time.sleep(1 / MESSAGE_RATE)
# TODO
timestamp = 0
try:
x, y = project(msg.latitude, msg.longitude,
correction['lat'], correction['lon'])
local = [True, x, y]
except:
local = [False, 0, 0]
to_send = {
"time": timestamp,
"lat": msg.latitude,
"lon": msg.longitude,
"alt": msg.altitude,
"sats": msg.num_sats,
"qual": msg.gps_qual,
"age": msg.age_gps_data,
"local": local
}
pub_gps.send(to_send)
except KeyboardInterrupt:
print('closing')
ser.close()
raise
except:
ser.close()
raise
on_right = (pygame.joystick.Joystick(0).get_button(5))
on_left = (pygame.joystick.Joystick(0).get_button(4))
l_trigger = (pygame.joystick.Joystick(0).get_axis(3))
if on_left or on_right:
if on_right:
forward = forward_right
else:
forward = forward_left
slow = 150
fast = 500
max_speed = (fast+slow)/2 + l_trigger*(fast-slow)/2
print(max_speed)
drive_pub.send({'f':(max_speed*forward),'t':-150*twist})
else:
pass
#without button go into freewheel
drive_pub.send({'f':0,'t':0})
if mode.startswith('arm'):
r_forward = -(pygame.joystick.Joystick(0).get_axis(5))
r_side = (pygame.joystick.Joystick(0).get_axis(2))
l_forward = -(pygame.joystick.Joystick(0).get_axis(1))
l_side = (pygame.joystick.Joystick(0).get_axis(0))
r_shoulder = (pygame.joystick.Joystick(0).get_button(5))
l_shoulder = (pygame.joystick.Joystick(0).get_button(4))
class Controller:
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 keypress(self, c):
print("Key pressed: ", c.name)
if c.name in self.keybinds:
self.keybinds[c.name]()
def move(self, lx=0.0, ly=0.0, yaw=0.0):
msg = self.blank_msg.copy()
self.lx = self.l_alpha * lx + (1 - self.l_alpha) * self.lx
msg["lx"] = self.lx
self.ly = self.l_alpha * ly + (1 - self.l_alpha) * self.ly
msg["ly"] = self.ly
self.rx = self.r_alpha * yaw + (1 - self.r_alpha) * self.rx
msg["rx"] = self.rx
print(msg)
self.pub.send(msg)
def move_forward(self, yaw=0.0):
self.move(ly=self.velocity, yaw=yaw)
def move_backward(self, yaw=0.0):
self.move(ly=-self.velocity, yaw=yaw)
# Move/rotate left/right might be reversed
def move_left(self):
self.move(lx=-self.velocity)
def move_right(self):
self.move(lx=self.velocity)
def rotate_right(self):
self.move(yaw=self.velocity)
def rotate_left(self):
self.move(yaw=-self.velocity)
def activate(self):
msg = self.blank_msg.copy()
msg["L1"] = 1
self.pub.send(msg)
self.active = not self.active
def toggle_trot(self):
msg = self.blank_msg.copy()
msg["R1"] = 1
self.pub.send(msg)
self.ready = self.active and not self.ready
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
class Arm:
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
#deadbands & nonlinear controls on speed
def condition_input(self,target):
target['x'] = - target['x']
if(target['pitch'] > .1):
target['pitch'] = -1.23* (target['pitch']-.1)**2
elif(target['pitch'] < -.1):
target['pitch'] = 1.23* (target['pitch']+.1)**2
else:
target['pitch'] = 0
target['grip'] = .04*target['grip']
target['roll'] = .01*target['roll']
target['z'] = - target['z']
if(target['yaw'] > .1):
target['yaw'] = 0.008* (1.1 * (target['yaw']-.1))**3
elif(target['yaw'] < -.1):
target['yaw'] = 0.008* (1.1 * (target['yaw']+.1))**3
else:
target['yaw'] = 0
# rotates command frame to end effector orientation
angle = self.xyz_positions['yaw']
x = -target['x']*abs(target['x'])
y = target['y']*abs(target['y'])
if(target['trueXYZ'] == 0):
target['x'] = x*math.cos(angle) - y*math.sin(angle)
target['y'] = x*math.sin(angle) + y*math.cos(angle)
return target
#output all speeds to all motors
def send_speeds(self, speeds, target):
for motor in self.motor_names:
print('driving', motor, 'at', int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]))
if int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]) == 0:
self.rc.drive_duty(motor, 0)
else:
self.rc.drive_speed(motor, int(self.SPEED_SCALE * self.CPR[motor] * speeds[motor]))
for motor in self.pwm_names:
print('driving pwm', motor, 'at', int(20000*target[motor]))
self.rc.drive_duty(motor, int(20000*target[motor]))
#check if motors are homed, get position & currents
def get_status(self):
self.forcing = False
for i,motor in enumerate(self.cartesian_motors):
encoder = self.rc.read_encoder(motor)[1]
print(motor,encoder)
self.native_positions[motor] = 2 * math.pi * encoder/self.CPR[motor]
self.currents[motor] = self.rc.read_current(motor)
if(self.currents[motor] > 1.5):
self.forcing = True
self.xyz_positions = self.native_to_xyz(self.native_positions)
print("Current Native: ", self.native_positions)
print("Current XYZ: ", self.xyz_positions)
def xyz_to_native(self, xyz):
native = {}
distance = math.sqrt(xyz['x']**2 + xyz['y']**2)
angle = -math.atan2(xyz['x'], xyz['y'])
offset = math.acos( ( FIRST_LINK**2 + distance**2 - SECOND_LINK**2 ) / (2*distance * FIRST_LINK) )
inside = math.acos( ( FIRST_LINK**2 + SECOND_LINK**2 - distance**2 ) / (2*SECOND_LINK * FIRST_LINK) )
if self.elbow_left:
# is in first working configuration
native['shoulder'] = angle + offset - math.pi
native['elbow'] = - (math.pi - inside)
else:
native['shoulder'] = angle - offset - math.pi
native['elbow'] = (math.pi - inside)
native['yaw'] = xyz['yaw'] -native['shoulder']-native['elbow']
# native['pitch'] = xyz['pitch']
return native
def native_to_xyz(self, native):
xyz = {}
xyz['x'] = -FIRST_LINK * math.sin(native['shoulder']) - SECOND_LINK * math.sin(native['shoulder'] + native['elbow'])
xyz['y'] = FIRST_LINK * math.cos(native['shoulder']) + SECOND_LINK * math.cos(native['shoulder'] + native['elbow'])
xyz['yaw'] = native['yaw'] + (native['shoulder']+native['elbow'])
# xyz['pitch'] = native['pitch']
return xyz
# analytic jacobian of angles -> XYZ
def dnative(self, dxyz):
x = self.xyz_positions['x']
y = self.xyz_positions['y']
shoulder_diff_x = y/(x**2 + y**2) - (x/(FIRST_LINK*math.sqrt(x**2 + y**2)) - x*(FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)/(2*FIRST_LINK*(x**2 + y**2)**(3/2)))/math.sqrt(1 - (FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)**2/(4*FIRST_LINK**2*(x**2 + y**2)))
shoulder_diff_y = -x/(x**2 + y**2) - (y/(FIRST_LINK*math.sqrt(x**2 + y**2)) - y*(FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)/(2*FIRST_LINK*(x**2 + y**2)**(3/2)))/math.sqrt(1 - (FIRST_LINK**2 - SECOND_LINK**2 + x**2 + y**2)**2/(4*FIRST_LINK**2*(x**2 + y**2)))
elbow_diff_x = -x/(FIRST_LINK*SECOND_LINK*math.sqrt(1 - (FIRST_LINK**2 + SECOND_LINK**2 - x**2 - y**2)**2/(4*FIRST_LINK**2*SECOND_LINK**2)))
elbow_diff_y = -y/(FIRST_LINK*SECOND_LINK*math.sqrt(1 - (FIRST_LINK**2 + SECOND_LINK**2 - x**2 - y**2)**2/(4*FIRST_LINK**2*SECOND_LINK**2)))
dnative = {}
dnative['shoulder'] = shoulder_diff_x * dxyz['x'] + shoulder_diff_y * dxyz['y']
dnative['elbow'] = elbow_diff_x * dxyz['x'] + elbow_diff_y * dxyz['y']
print("Dxyz : ", dxyz)
print("Dnative: ", dnative)
print("new location: ", self.native_to_xyz ( {motor:dnative[motor] + self.native_positions[motor] for motor in self.motor_names}) )
return dnative
#numerical jacobian of angles -> XYZ
def dnative2(self, dxyz):
h = 0.00000001
# print dxyz, self.xyz_positions
x_plus_h = { axis:self.xyz_positions[axis] + h*dxyz[axis] for axis in self.xyz_names}
f_x_plus_h = self.xyz_to_native(x_plus_h)
f_x = self.xyz_to_native(self.xyz_positions)
dnative = {motor:(f_x_plus_h[motor] - f_x[motor])/h for motor in self.cartesian_motors}
print("Dxyz : ", dxyz)
print("Dnative: ", dnative)
print("new location: ", self.native_to_xyz ( {motor:dnative[motor] + f_x[motor] for motor in self.cartesian_motors}) )
return dnative
def sign(self,val):
return int(val > 0) - int(val < 0)
#prevent movement near singularity or if the motor is out of bounds or at home
def check_in_bounds(self, speeds):
inBounds = True
if(abs(self.native_positions['elbow']) < .4):
if(self.native_positions['elbow'] < 0 and self.sign(speeds['elbow']) == 1):
inBounds = False
print("SINGULARITY")
elif(self.native_positions['elbow'] > 0 and self.sign(speeds['elbow']) == -1):
inBounds = False
print("SINGULARITY")
for motor in self.cartesian_motors:
if(self.sign(speeds[motor]) == -1):
if(self.native_positions[motor] < self.limits[motor][0]):
inBounds = False
elif(self.native_positions[motor] > self.limits[motor][1]):
inBounds = False
if(not inBounds):
for motor in self.cartesian_motors:
speeds[motor] = 0
return speeds
def check_limits(self, speeds):
inBounds = True
if(GPIO.input(SHOULDER_HOME_INPUT) and self.sign(speeds['elbow']) == 1):
self.rc.set_encoder('shoulder',SHOULDER_HOME)
inBounds = False
if(GPIO.input(ELBOW_HOME_INPUT) and self.sign(speeds['elbow']) == -1):
self.rc.set_encoder('elbow',ELBOW_HOME*self.cpr['elbow'])
inBounds = False
if(not inBounds):
for motor in self.cartesian_motors:
speeds[motor] = 0
return speeds
#return to our docking position
def dock(self,speeds):
if(abs(self.native_positions['yaw']-self.dock_pos['yaw']-.03)>.01):
speeds['shoulder'] = 0
speeds['elbow'] = 0
speeds['yaw'] = self.dock_speed(self.native_positions['yaw'],self.dock_pos['yaw']+.03,self.dock_speeds[0],self.dock_speeds[1])
elif(abs(self.native_positions['elbow']-self.dock_pos['elbow']+.03)>.01):
speeds['shoulder'] = 0
speeds['elbow'] = self.dock_speed(self.native_positions['elbow'],self.dock_pos['elbow']-.03,self.dock_speeds[0],self.dock_speeds[1])
speeds['yaw'] = 0
elif(abs(self.native_positions['shoulder']-self.dock_pos['shoulder']-.03)>.01):
speeds['shoulder'] = self.dock_speed(self.native_positions['shoulder'],self.dock_pos['shoulder']+.03,self.dock_speeds[0],self.dock_speeds[1])
speeds['elbow'] = 0
speeds['yaw'] = 0
return speeds
#PID for docking speed
def dock_speed(self,curPos,desiredPos,P,maxV):
dir = self.sign(desiredPos-curPos)
output = abs(curPos-desiredPos)*P+.0005
if(output > maxV):
output = maxV
return output*dir
def update(self):
print()
print("new iteration")
self.get_status()
output = {}
print("read status of shoulder", GPIO.input(20))
for d in (self.native_positions,self.xyz_positions):
output.update(d)
#print("HOME STATUS ", self.home_status)
#output.update({"shoulder limit", self.home_status["shoulder"]})
output.update({"forcing":self.forcing})
self.output_pub.send(output)
try:
target = self.target_vel.get()
# TODO: This shouldn't be necessary, how to fix in UDPComms?
target = {bytes(key): value for key, value in target.iteritems()}
target_f = self.condition_input(target)
speeds = self.dnative2(target_f)
if(not self.zeroed):
speeds['elbow'] = 0
speeds['shoulder'] = 0
speeds = self.check_in_bounds(speeds)
speeds['elbow'] -= 0.002 * target_f['hat'][0]
speeds['shoulder'] -= 0.002 * target_f['hat'][1]
speeds['yaw'] += target_f['yaw']
speeds['roll'] = target_f['roll']
speeds['grip'] = target_f['grip'] + speeds['roll']
speeds = self.check_limits(speeds)
if speeds['elbow'] == 0 and speeds['shoulder'] == 0:
self.elbow_left = self.native_positions['elbow'] < 0
if(target_f['dock']):
speeds=self.dock(speeds)
if target_f["reset"]:
print ("RESETTING!!!")
self.zeroed = True
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
self.send_speeds(speeds, target_f)
self.rc.set_encoder("shoulder",0)
self.rc.set_encoder("elbow",0)
self.rc.set_encoder("yaw",0)
elif target_f["resetdock"]:
print ("RESETTING (in dock position)!!!")
self.zeroed = True
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
self.send_speeds(speeds, target_f)
self.rc.set_encoder("shoulder",int(self.CPR["shoulder"]*self.dock_pos['shoulder']/6.28))
self.rc.set_encoder("elbow",int(self.CPR["elbow"]*self.dock_pos['elbow']/6.28))
self.rc.set_encoder("yaw",int(self.CPR["yaw"]*self.dock_pos['yaw']/6.28))
except timeout:
print ("TIMEOUT No commands recived")
speeds = {motor: 0 for motor in self.motor_names}
target_f = {}
target_f = {motor: 0 for motor in self.pwm_names}
except ValueError:
print ("ValueError The math failed")
speeds = {motor: 0 for motor in self.motor_names}
speeds['elbow'] -= 0.002 * target_f['hat'][0]
speeds['shoulder'] -= 0.002 * target_f['hat'][1]
except:
speeds = {motor: 0 for motor in self.motor_names}
target_f = {motor: 0 for motor in self.pwm_names}
raise
finally:
print ("SPEEDS"), speeds, target_f
self.send_speeds(speeds, target_f)
try:
lidar = rplidar.RPLidar(port)
break
except rplidar.RPLidarException:
pass
else:
raise rplidar.RPLidarException(
"Can't find serial port to connect to sensor")
def signal_term_handler(signal, frame):
lidar.stop()
lidar.stop_motor()
exit()
signal.signal(signal.SIGTERM, signal_term_handler)
try:
for scan in lidar.iter_scans(
scan_type="express" if use_express else "normal"):
pub.send(scan)
print('sending')
print("end")
except:
raise
pass
finally:
signal_term_handler(None, None)
class Simualtor:
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 mouse_draw_down(self, event):
self.mouse_mode = self.canvas.create_line(event.x, event.y, event.x,
event.y)
def mouse_draw_move(self, event):
if self.mouse_mode != None:
coords = self.canvas.coords(self.mouse_mode)
self.canvas.coords(self.mouse_mode, coords[0], coords[1], event.x,
event.y)
def mouse_draw_up(self, event):
if self.mouse_mode != None:
coords = self.canvas.coords(self.mouse_mode)
self.canvas.coords(self.mouse_mode, coords[0], coords[1], event.x,
event.y)
self.obstacles.append(self.mouse_mode)
self.mouse_mode = None
def mouse_delete(self, event):
obj, = self.canvas.find_closest(event.x, event.y)
if obj in self.obstacles:
self.canvas.delete(obj)
def odom_pub(self):
out = self.robot.get_odom()
self.odom.send(
{"single": {
"odom": (out[1], 1000 * out[0] / self.PIX_per_M)
}})
self.master.after(100, self.odom_pub)
def scan(self):
coords = [self.canvas.coords(obj) for obj in self.obstacles]
output = []
for s in self.scan_points:
self.canvas.delete(s)
start = 20
end = 300
step = 50
for angle in np.linspace(0, 2 * np.pi, step):
s = np.sin(angle)
c = np.cos(angle)
x = self.robot.x - s * start
y = self.robot.y + c * start
for dist in range(start, end):
x -= s
y += c
items = self.canvas.find_overlapping(x, y, x + 2, y + 2)
if len(items) != 0:
output.append((0, np.degrees(angle + self.robot.a),
1000 * dist / self.PIX_per_M))
self.scan_points.append(
self.canvas.create_line(x, y, x + 1, y, fill='red'))
break
print(output)
self.lidar.send(output)
self.master.after(500, self.scan)
class CommandPannel:
fields = "forward twist"
typ = "ff"
port = 8830
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()
@staticmethod
def ha():
print "ha"
def spaceKey(self, event):
print "space"
self.forwards = 0
self.twist = 0
self.time = time.time()
def leftKey(self, event):
print "left"
self.forwards = 0
self.twist = self.speed
self.time = time.time()
def rightKey(self, event):
print "right"
self.forwards = 0
self.twist = -self.speed
self.time = time.time()
def upKey(self, event):
print "up"
self.forwards = self.speed
self.twist = 0
self.time = time.time()
def downKey(self, event):
print "down"
self.forwards = -self.speed
self.twist = 0
self.time = time.time()
def publish(self):
if (time.time() - self.time) > 1:
self.forwards = 0
self.twist = 0
self.pub.send(self.forwards, self.twist)
print "magic"
self.root.after(100, self.publish)
class Pursuit:
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 get_guess(self, endpoint):
x, y = endpoint
rand_x = (2 * random.random() - 1) * self.search_radius
rand_y = (2 * random.random() - 1) * self.search_radius
self.random_corrd = (rand_x, rand_y)
print(rand_x, rand_y)
return (x + rand_x, y + rand_y)
def find_ball(self, cmd):
if cmd['end_mode'] == 'none':
print("REACHED ENDPOINT")
# self.lights.send({'r':0, 'g':1, 'b':0})
self.servo.send({'pan': 0, 'tilt': 90})
self.send_stop()
elif cmd['end_mode'] == 'tennis':
print("Entering search program")
try:
marker = self.aruco.get()
except timeout:
marker = None
last_waypoint = cmd['waypoints'][-1]
endpoint = self.project(last_waypoint['lat'], last_waypoint['lon'])
if marker == None:
if (time.time() - self.last_tennis_ball) < 2:
# if we saw a ball in the last 2 sec
out = {"f": 70, "t": 0}
print('cont to prev seen marker', out)
self.cmd_vel.send(out)
else:
if self.guess == None:
self.guess = self.get_guess(endpoint)
self.guess_time = time.time()
print("NEW RANDOM GUESS - first")
elif self.distance(self.guess) < self.guess_radius:
self.guess = self.get_guess(endpoint)
print("NEW RANDOM GUESS - next")
self.guess_time = time.time()
elif (time.time() - self.guess_time) > 40:
self.guess = self.get_guess(endpoint)
print("NEW RANDOM GUESS - timeout")
self.guess_time = time.time()
self.guess = self.get_guess(endpoint)
print("random corrds", self.random_corrd)
diff_angle = self.get_angle(self.guess)
self.send_velocities(diff_angle)
else:
# self.guess_time = time.time() # will this matter?
self.last_tennis_ball = time.time()
if marker['dist'] < 1.5:
print("REACHED MARKER")
self.send_stop()
else:
self.send_velocities_slow(marker['angle'])
else:
print("incorrect mode")
def update(self):
try:
self.robot = self.gps.get()
cmd = self.auto_control.get()
except:
print('lost control')
return
if cmd['command'] == 'off':
print("off")
self.reached_destination = False
self.start_point['lat'] = self.gps.get()['lat']
self.start_point['lon'] = self.gps.get()['lon']
# self.lights.send({'r':1, 'g':0, 'b':0})
self.servo.send({'pan': 0, 'tilt': -90})
elif (cmd['command'] == 'auto'):
last_waypoint = cmd['waypoints'][-1]
if self.reached_destination:
self.find_ball(cmd)
elif( self.distance(self.project(last_waypoint['lat'], last_waypoint['lon'])) \
< self.final_radius ):
self.reached_destination = True
print("REACHED final End point")
elif self.analyze_stuck():
self.un_stick()
else:
lookahead = self.find_lookahead(cmd['waypoints'])
diff_angle = self.get_angle(lookahead)
self.send_velocities(diff_angle)
else:
raise ValueError
def analyze_stuck(self):
self.past_locations.append([
self.project(self.robot['lat'], self.robot['lon']),
self.imu.get()['angle'][0],
time.time()
])
if len(self.past_locations) < 100:
return False
while len(self.past_locations) > 100:
self.past_locations.pop(0)
if (time.time() - self.stuck_time < 14):
return False
abs_angle = lambda x, y: min(abs(x - y + i * 360) for i in [-1, 0, 1])
# location, t = self.past_locations[0]
# if self.distance(location)/(time.time() - t) < 0.1
locations = [self.past_locations[10 * i + 5][0] for i in range(9)]
angles = [self.past_locations[10 * i + 5][1] for i in range(9)]
max_loc = max([self.distance(loc) for loc in locations])
max_angle = max(
[abs_angle(ang,
self.imu.get()['angle'][0]) for ang in angles])
print("we are", max_loc, "from stcuk and angle", max_angle)
if max_loc < 1 and max_angle < 30:
print("WE ARE STUCK")
# self.stuck_location = location
self.stuck_time = time.time()
return True
return False
def un_stick(self):
start_time = time.time()
while (time.time() - start_time) < 4 and self.auto_control.get(
)['command'] == 'auto':
self.analyze_stuck()
out = {"f": -100, "t": -20}
print('unsticking', out)
self.cmd_vel.send(out)
time.sleep(0.1)
start_time = time.time()
start_angle = self.imu.get()['angle'][0]
while (time.time() - start_time) < 1 and self.auto_control.get(
)['command'] == 'auto':
self.analyze_stuck()
out = {"f": 0, "t": -70}
print('unsticking part 2', out)
self.cmd_vel.send(out)
time.sleep(0.1)
start_time = time.time()
while (time.time() - start_time) < 2 and self.auto_control.get(
)['command'] == 'auto':
self.analyze_stuck()
out = {"f": 70, "t": 0}
print('unsticking part 2', out)
self.cmd_vel.send(out)
time.sleep(0.1)
def find_lookahead(self, waypoints):
start_waypoints = [self.start_point] + waypoints
waypoint_pairs = zip(start_waypoints[::-1][1:],
start_waypoints[::-1][:-1])
i = 0
for p1, p2 in waypoint_pairs:
lookahead = self.lookahead_line(p1, p2)
if lookahead is not None:
print("point intersection", i)
return lookahead
i += 1
else:
print("NO intersection found!")
distances = []
for p1, p2 in waypoint_pairs:
lookahead = self.lookahead_line(p1, p2, project=True)
if lookahead is not None:
distances.append((self.distance(lookahead), lookahead))
for p1 in waypoints:
point = self.project(p1['lat'], p1['lon'])
distances.append((self.distance(point), point))
return min(distances)[1]
def distance(self, p1):
x_start, y_start = p1
# self.robot = self.gps.get()
x_robot, y_robot = self.project(self.robot['lat'], self.robot['lon'])
return math.sqrt((x_start - x_robot)**2 + (y_start - y_robot)**2)
def lookahead_line(self, p1, p2, project=False):
x_start, y_start = self.project(p1['lat'], p1['lon'])
x_end, y_end = self.project(p2['lat'], p2['lon'])
# self.robot = self.gps.get()
x_robot, y_robot = self.project(self.robot['lat'], self.robot['lon'])
if (x_robot - x_end)**2 + (y_robot -
y_end)**2 < self.lookahead_radius**2:
return (x_end, y_end)
a = (x_end - x_start)**2 + (y_end - y_start)**2
b = 2 * ((x_end - x_start) * (x_start - x_robot) + (y_end - y_start) *
(y_start - y_robot))
c = (x_start - x_robot)**2 + (y_start -
y_robot)**2 - self.lookahead_radius**2
if b**2 - 4 * a * c <= 0:
if project:
t = -b / (2 * a)
else:
return None
else:
t = (-b + math.sqrt(b**2 - 4 * a * c)) / (2 * a)
if not t <= 1:
return None
if not t >= 0:
return None
x_look = t * x_end + (1 - t) * x_start
y_look = t * y_end + (1 - t) * y_start
return (x_look, y_look)
def get_angle(self, lookahead):
heading_deg = self.imu.get()['angle'][0]
head_rad = math.radians(heading_deg)
x_robot, y_robot = self.project(self.robot['lat'], self.robot['lon'])
x_look, y_look = lookahead
target = math.atan2(x_look - x_robot, y_look - y_robot)
# anti clockwise positive
return ((target - head_rad + math.pi) % (2 * math.pi) - math.pi)
def send_velocities_slow(self, angle):
# turn_rate = -100*math.tanh(1*angle)
turn_rate = -200 * angle / math.pi
if math.fabs(angle) < math.radians(10):
forward_rate = 90
elif math.fabs(angle) < math.radians(60):
forward_rate = 50
elif math.fabs(angle) < math.radians(140):
forward_rate = 30
else:
forward_rate = 0
out = {"f": forward_rate, "t": turn_rate}
# out = {"f": 70, "t": -150*angle/math.pi }
print(out)
self.cmd_vel.send(out)
def send_velocities(self, angle):
# turn_rate = -100*math.tanh(1*angle)
turn_rate = -200 * angle / math.pi
if math.fabs(angle) < math.radians(10):
forward_rate = 130
elif math.fabs(angle) < math.radians(60):
forward_rate = 80
elif math.fabs(angle) < math.radians(140):
forward_rate = 30
else:
forward_rate = 0
out = {"f": forward_rate, "t": turn_rate}
# out = {"f": 70, "t": -150*angle/math.pi }
print(out)
self.cmd_vel.send(out)
def send_stop(self):
out = {"f": 0, "t": 0}
# out = {"f": 70, "t": -150*angle/math.pi }
print('stoping', out)
self.cmd_vel.send(out)
def project(self, lat, lon):
lat_orig = self.start_point['lat']
lon_orig = self.start_point['lon']
RADIUS = 6371 * 1000
lon_per_deg = RADIUS * 2 * math.pi / 360
lat_per_deg = lon_per_deg * math.cos(math.radians(lat_orig))
x = (lon - lon_orig) * lon_per_deg
y = (lat - lat_orig) * lat_per_deg
return (x, y)
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:")
print(JoyName)
# Gets the number of axes
JoyAx = pygame.joystick.Joystick(0).get_numaxes()
print("Number of axis:")
print(JoyAx)
while True:
pygame.event.pump()
forward = (pygame.joystick.Joystick(0).get_axis(3))
twist = (pygame.joystick.Joystick(0).get_axis(2))
on = (pygame.joystick.Joystick(0).get_button(5))
if on:
print({'f': -150 * forward, 't': -80 * twist})
drive_pub.send({'f': -150 * forward, 't': -80 * twist})
else:
drive_pub.send({'f': 0, 't': 0})
pygame.time.wait(100)
on_left = values['button_l1']
l_trigger = values['l2_analog']
if on_left or on_right:
if on_right:
forward = forward_right
else:
forward = forward_left
slow = 150
fast = 500
max_speed = (fast+slow)/2 + l_trigger*(fast-slow)/2
out = {'f':(max_speed*forward),'t':-150*twist}
drive_pub.send(out)
print(out)
else:
drive_pub.send({'f':0,'t':0})
elif mode == MODES.ARM:
r_forward = - values['right_analog_y']
r_side = values['right_analog_x']
l_forward = - values['left_analog_y']
l_side = values['left_analog_x']
r_shoulder = values['button_r1']
l_shoulder = values['button_l1']
r_trigger = values['r2_analog']
motor_vels[3] = middle_odrive.axis0.encoder.vel_estimate
motor_vels[4] = back_odrive.axis1.encoder.vel_estimate
motor_vels[5] = back_odrive.axis0.encoder.vel_estimate
return motor_currents, motor_vels
while True:
try:
msg = cmd.get()
# print(msg)
# print(control_state)
currents, vels = get_data(front_odrive, middle_odrive, back_odrive)
print(currents)
# print("measured:", front_odrive.axis0.motor.current_control.Iq_measured)
try:
telemetry.send([middle_odrive.vbus_voltage] + currents)
except:
pass
clear_errors(front_odrive)
clear_errors(middle_odrive)
clear_errors(back_odrive)
if (msg['cur'] == 1):
# print ("switching")
control_state = 2
stalled_motor = 6
if (msg['vel'] == 1):
control_state = 1
if (msg['t'] == 0 and msg['f'] == 0):
class Rover:
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 project(self, lat, lon):
lat_orig = self.start_gps['lat']
lon_orig = self.start_gps['lon']
RADIUS = 6371 * 1000
lon_per_deg = RADIUS * 2 * math.pi / 360
lat_per_deg = lon_per_deg * math.cos(math.radians(lat_orig))
x = (lon - lon_orig) * lon_per_deg
y = (lat - lat_orig) * lat_per_deg
return (x, y)
def get_pose(self):
gps = self.gps.get()
# heading = math.radians( 360 -self.imu.get()['angle'][0] )
heading = math.radians(self.imu.get()['angle'][0])
return Pose(*self.project(gps['lat'], gps['lon']), heading)
def send_vel(self, forward, twist):
msg = {"f": forward, "t": twist}
if self.cmd_sent:
print("ERROR Multiple commands being sent!")
self.cmd_vel.send(msg)
self.cmd_sent = True
print(msg)
def get_aruco(self):
markers = self.aruco.get()
out = {}
for marker in markers:
idx, dist, angle = marker['id'], marker['dist'], marker['angle']
out[idx] = Bearing(dist, math.radians(angle))
return out
def get_cmd(self):
return self.auto_control.get()
def get_waypoints(self) -> List["Pose"]:
wps = self.auto_control.get()['waypoints']
out = []
for waypoint in wps:
out.append(Pose(*self.project(waypoint['lat'], waypoint['lon'])))
return out
def next_iteration(self):
self.cmd_sent = False
self.logger.next()
print "responce closed"
break
buf += response.read(100)
length = get_length(buf)
next_header = buf[length:].find(chr(0xd3))
# print(length,next_header, len(buf), len(msgs))
# print buf[length: length + next_header].encode("hex")
if (next_header == -1):
print "no header found"
continue
msgs.append(buf[:length + next_header])
buf = buf[length + next_header:]
if (time.time() - last_message) > publish_interval:
rtcm = "".join(msgs)
print(len("".join(msgs)))
msgs = []
last_message = time.time()
# timestamp = (msg.timestamp - datetime(1970, 1, 1)).total_seconds()
to_send = {
"time": 0,
"lat": 37.41652,
"lon": -122.20427,
"alt": 64,
"sats": 0,
"rtcm": rtcm
}
pub.send(to_send)
)
else:
key = pygame.key.get_pressed()
msg = {}
msg["lx"] = 0.75 * direction_helper(key[pygame.K_a],
key[pygame.K_d])
msg["ly"] = 0.75 * direction_helper(key[pygame.K_s],
key[pygame.K_w])
msg["rx"] = 0.75 * direction_helper(key[pygame.K_LEFT],
key[pygame.K_RIGHT])
msg["ry"] = 0.75 * direction_helper(key[pygame.K_DOWN],
key[pygame.K_UP])
msg["x"] = 1 if key[pygame.K_x] else 0
msg["square"] = 1 if key[pygame.K_u] else 0
msg["circle"] = 1 if key[pygame.K_c] else 0
msg["triangle"] = 1 if key[pygame.K_t] else 0
msg["dpady"] = 1.0 * direction_helper(key[pygame.K_k],
key[pygame.K_i])
msg["dpadx"] = 1.0 * direction_helper(key[pygame.K_j],
key[pygame.K_l])
msg["L1"] = 1 if key[pygame.K_q] else 0
msg["R1"] = 1 if key[pygame.K_e] else 0
msg["L2"] = 0
msg["R2"] = 0
msg["message_rate"] = MESSAGE_RATE
print(msg)
pub.send(msg)
pygame.display.flip()
pygame.time.wait(int(1000 / MESSAGE_RATE))
# bias = dictvec_to_array(sensor.get_gyro_data())
bias = np.zeros(3)
np.set_printoptions(suppress=True, precision=3)
while 1:
now = time.time()
if now - last_update > DT:
gyro_data = sensor.get_gyro_data()
accel_data = sensor.get_accel_data()
gyro_data, accel_data = clean_sensor_data(gyro_data, accel_data,
bias)
filt_comp.update_gyro(gyro_data, DT)
filt_comp.update_acel(accel_data)
filt_mekf.update_gyro(gyro_data, DT)
filt_mekf.update_acel(accel_data)
comp_pub.send(list(filt_comp.quat().q))
mekf_pub.send(list(filt_mekf.quat().q))
raw_sensor_pub.send((list(gyro_data), list(accel_data)))
if t % 10 == 0:
print("dt: {:1.3f} gyro: {} q: {}".format(
now - last_update, gyro_data, filt_comp.quat()))
last_update = now
t += 1
square = (pygame.joystick.Joystick(0).get_button(0))
cross = (pygame.joystick.Joystick(0).get_button(1))
circle = (pygame.joystick.Joystick(0).get_button(2))
triangle = (pygame.joystick.Joystick(0).get_button(3))
forward = 0
turn = 0
if on_left:
forward = -forward_left
turn = twist
elif on_right:
forward = -forward_right
turn = twist
vel = 1 if r_trigger > .5 else 0
cur = 1 if l_trigger > .5 else 0
drive_pub.send({'f':forward,'t':turn, 'power_left':square, 'power_back':cross, 'power_right':circle, 'power_mid': triangle,'vel':vel,'cur':cur})
print({'f':forward,'t':turn, 'power_left':square, 'power_back':cross, 'power_right':circle, 'power_mid': triangle,'vel':vel,'cur':cur})
if mode.startswith('arm'):
r_forward = -(pygame.joystick.Joystick(0).get_axis(5))
r_side = (pygame.joystick.Joystick(0).get_axis(2))
l_forward = -(pygame.joystick.Joystick(0).get_axis(1))
l_side = (pygame.joystick.Joystick(0).get_axis(0))
r_shoulder = (pygame.joystick.Joystick(0).get_button(5))
l_shoulder = (pygame.joystick.Joystick(0).get_button(4))
r_trigger = (pygame.joystick.Joystick(0).get_axis(4))
l_trigger = (pygame.joystick.Joystick(0).get_axis(3))
compass = HMC6343()
# initialize filter
# TODO find good value
filt = ComplementaryFilter(0.99)
lastGyro = 0
lastMag = 0
lastPub = 0
while True:
if time() - lastMag > 0.10:
heading = compass.readHeading()
filt.update_mag(heading)
lastMag = time()
if time() - lastGyro > 0.01:
gyro_x, gyro_y, gyro_z = imu.gyro
filt.update_gyro(-gyro_z)
lastGyro = time()
if time() - lastPub > 0.05:
try:
offset = float(offset_sub.get())
except:
pass
angle = filt.get_angle() + offset
print(angle, heading, filt.lastGyro)
pub.send({'angle': [angle, None, None], 'mag': heading + offset})
lastPub = time()
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)