def annotate_eval_plots(site):
path_to_rgb = "../" + site + "/plots/"
path_to_laz = path_to_rgb
path_to_annotations = "../" + site + "/annotations/"
#For each .laz file in directory.
laz_files = glob.glob(path_to_laz + "*.laz")
for laz in laz_files:
print(laz)
#Load laz
point_cloud = Lidar.load_lidar(laz)
#Find annotations
basename = os.path.basename(laz)
basename = os.path.splitext(basename)[0]
xml_path = os.path.join(path_to_annotations, basename + ".xml")
if (os.path.exists(xml_path)):
#Load annotations and get utm bounds from tif image
annotations = Lidar.load_xml(xml_path, path_to_rgb, res=0.1)
else:
print("{} does not exist, skipping image".format(xml_path))
continue
#Create boxes
boxes = Lidar.create_boxes(annotations)
#Drape RGB bounding boxes over the point cloud
point_cloud = Lidar.drape_boxes(boxes, point_cloud)
#Write Laz
write_label(point_cloud, laz)
def __init__(self):
self.stepper = Stepper()
self.lidar = Lidar()
self.actual_steps = 0
self.angle = 0
self.direction = "CCW"
self.scan_data = []
def run(csv_path, laz_path):
"""predictions_csv: path to csv holding bbox predictions
"""
#Load data
df = pd.read_csv(csv_path, index_col=0)
point_cloud = Lidar.load_lidar(laz_path)
#Drape RGB bounding boxes over the point cloud
point_cloud = Lidar.drape_boxes(df.values, point_cloud)
#Write Laz with label info
write_label(point_cloud, laz_path)
def annotate_tile(laz_path, path_to_rgb, xml_path):
annotations = Lidar.load_xml(xml_path, path_to_rgb, res=0.1)
point_cloud = Lidar.load_lidar(laz_path)
#Create boxes
boxes = Lidar.create_boxes(annotations)
#Drape RGB bounding boxes over the point cloud
point_cloud = Lidar.drape_boxes(boxes, point_cloud)
#Write Laz with label info
write_label(point_cloud, laz_path)
def __init__(self):
self.heading_step_size = 1.8
self.pitch_step_size = 2.0
self.stepper_heading = Stepper("Heading",
angle_per_step=self.heading_step_size,
pin_dir=35,
pin_step=37,
actual=0)
self.servo_pitch = Servo()
self.lidar = Lidar()
self.actual_steps = 0
self.heading = 0
self.pitch = 0
self.line_direction = "CCW"
self.vertical_direction = "DOWN"
self.local_data = []
self.scan_data = []
self.init_done = False
class Scanner():
def __init__(self):
self.stepper = Stepper()
self.lidar = Lidar()
self.actual_steps = 0
self.angle = 0
self.direction = "CCW"
self.scan_data = []
def do_scan(self, step=3, min_angle=-80, max_angle=80):
while self.actual_steps < step:
self.actual_steps += 1
dist = self.lidar.get_distance() # zero if LIDAR error
if dist > 10:
dx, dy = self.polar_to_kartesian(dist, self.angle)
self.scan_data.append([dx, dy])
if self.direction == "CCW":
self.angle = round(self.angle + 1.8, 2)
self.stepper.do_step(1)
else:
self.angle = round(self.angle - 1.8, 2)
self.stepper.do_step(-1)
if (self.angle >= max_angle):
self.direction = "CW"
if (self.angle <= min_angle):
self.direction = "CCW"
self.actual_steps = 0
def polar_to_kartesian(self, dist, winkel):
"""returns aus Dist und Winkel Dx,Dy"""
dx = int((dist * cos(radians(winkel))))
dy = int((dist * sin(radians(winkel))))
return (dx, dy)
def get_scan_data(self):
data = self.scan_data
self.scan_data = []
return (data)
def scanner_reset(self):
print("Set Scanner to Zero position")
self.stepper.do_step(-200)
self.stepper.do_step(76)
def __init__(self,
environment,
localino_tag,
xPos=100,
yPos=100,
orientation=0,
height=32,
width=32,
mass=300,
wheels=((32, 0), (32, 16))):
"""initialize
environment is the environment that the wheelchair is in
localino tag is the localino tag which is attached to the wheelchair
orientation is a clockwise from the verticle (rad)
xPos, yPos are coordinates of the center of the chair
width = length in x dir
height = length in y dir
"""
# save the wheelchair physical properties
self.image = tkinter.PhotoImage(file="Wheelchair.png")
self.height = height
self.width = width
self.mass = mass
self.wheels = wheels
# save the initial velocity and orientation
self.orientation = orientation
self.xPos = yPos
self.yPos = xPos
self.xVel = 0
self.yVel = 0
self.xAcc = 0
self.yAcc = 0
self.update_boundary_positions()
# initialize sensors on the wheelchair
self.localino_tag = localino_tag
self.lidar = Lidar(environment, self.xPos, self.yPos,
orientation) # lidar is in the center of the chair
self.encoder = Encoder(wheelchair=self)
self.berryimu = BerryIMU(wheelchair=self)
# update positions for all the sensors
self.update_sensor_positions()
def testLocilization():
lidar = Lidar.Lidar()
lidar.start()
while len(lidar.measures) == 0:
time.sleep(.1)
print("going into therad")
t1 = threading.Thread(target=move, args=(motors, ))
t1.start()
print("exit")
measures = []
while True:
measures = lidar.measures
measures = np.append([float(dx), float(dy), float(dth)], measures)
lo_c.sendData(measures)
time.sleep(.2)
if __name__ == "__main__":
encoder = example_encoder.Encoder()
encoder.start()
motors = example_motor.Motor()
PIDleft = PID.PID()
PIDleft.Kd = 0
PIDleft.Ki = .25
PIDleft.Kp = .25
PIDRight = PID.PID()
PIDRight.Kd = 0
PIDRight.Ki = .25
PIDRight.Kp = .25
lidar = Lidar.Lidar()
def turn(angle):
encoder.rightDistance = 0
encoder.leftDistance = 0
wheelBase = 21
wheelRadius = 4
print("rec dist : " + str((math.pi*(angle*(math.pi/180))* wheelBase)/(2* wheelRadius)))
while (np.average([abs(encoder.leftDistance), abs(encoder.rightDistance)]) <
((math.pi*(angle*(math.pi/180))* wheelBase)/(2* wheelRadius))):
time.sleep(.05)
measuredPhiDotLeft = -1*encoder.getPhiDotLeft()
measuredPhiDotRight = encoder.getPhiDotRight()
PIDleft.control(-.8, measuredPhiDotLeft, motors.setPhiDotDesiredLeft)
import glob
import os
import Lidar
path_to_rgb = "../TEAK/plots/"
path_to_laz = "../TEAK/plots/"
path_to_annotations = "../TEAK/annotations/"
#For each .laz file in directory.
laz_files = glob.glob(path_to_laz + "*.laz")
for laz in laz_files:
#Load laz
point_cloud = Lidar.load_lidar(laz)
#Find annotations
basename = os.path.basename(laz)
basename = os.path.splitext(basename)[0]
xml_path = os.path.join(path_to_annotations, basename + ".xml")
#Load annotations and get utm bounds from tif image
annotations = Lidar.load_xml(xml_path, path_to_rgb, res=0.1)
#Create boxes
boxes = Lidar.create_boxes(annotations)
#Drape RGB bounding boxes over the point cloud
point_cloud = Lidar.drape_boxes(boxes, point_cloud)
#Write Laz
class Scanner():
def __init__(self):
self.heading_step_size = 1.8
self.pitch_step_size = 2.0
self.stepper_heading = Stepper("Heading",
angle_per_step=self.heading_step_size,
pin_dir=35,
pin_step=37,
actual=0)
self.servo_pitch = Servo()
self.lidar = Lidar()
self.actual_steps = 0
self.heading = 0
self.pitch = 0
self.line_direction = "CCW"
self.vertical_direction = "DOWN"
self.local_data = []
self.scan_data = []
self.init_done = False
def init_3D_scan(
self,
min_pitch,
max_pitch,
min_heading,
max_heading,
):
"""Set the Stepper init values (no mechanical movement)"""
self.min_pitch = min_pitch
self.max_pitch = max_pitch
self.min_heading = min_heading
self.max_heading = max_heading
self.servo_pitch.set_servo_angle(self.min_pitch)
self.pitch = self.min_pitch
self.stepper_heading.set_actual_angle(self.min_heading)
self.heading = self.min_heading
self.min_dist = 999
#Stabilise
sleep(0.2)
self.init_done = True
def do_3D_scan(self, step=1):
"""Do a 3D Scan for maximal Stepper-Steps. INPUT: INT step, angle-limits"""
if not self.init_done:
print("Scanner3d Init nessesary!")
self.min_dist = 999
while True:
if self.actual_steps >= step:
break
self.actual_steps += 1
self.line_scan(self.min_heading, self.max_heading)
#sleep(1)
if self.vertical_direction == "UP":
self.pitch -= self.pitch_step_size
self.servo_pitch.set_servo_angle(servo_angle1=self.pitch)
else:
self.pitch += self.pitch_step_size
self.servo_pitch.set_servo_angle(servo_angle1=self.pitch)
if (self.pitch <= self.min_pitch):
self.vertical_direction = "DOWN"
if (self.pitch >= self.max_pitch):
self.vertical_direction = "UP"
self.actual_steps = 0
def line_scan(self, min_heading, max_heading):
"""Do a LIDAR - Scan one line, Maximal for total step"""
while True:
dist = self.lidar.get_distance() # zero if LIDAR error
if dist < self.min_dist and dist > 1:
self.min_dist = dist
self.local_data.append([self.pitch, self.heading, dist])
if self.line_direction == "CCW":
if self.heading < self.max_heading:
self.heading = round(self.heading + self.heading_step_size,
4)
self.stepper_heading.goto_angle(self.heading)
else:
self.line_direction = "CW"
self.scan_data.append(self.local_data)
self.local_data = []
return
else:
if self.heading > self.min_heading:
self.heading = round(self.heading - self.heading_step_size,
4)
self.stepper_heading.goto_angle(self.heading)
else:
self.line_direction = "CCW"
self.scan_data.append(list(reversed(self.local_data)))
self.local_data = []
return
def polar_to_kartesian(self, dist, winkel):
"""returns aus Dist und Winkel Dx,Dy"""
dx = int((dist * cos(radians(winkel))))
dy = int((dist * sin(radians(winkel))))
return (dx, dy)
def get_min_dist(self):
return (self.min_dist)
def get_scan_data(self):
"""RETURNS: LIST scan_data[pitch, heading, distance], clears data after"""
data = self.scan_data
self.scan_data = []
return (data)
def scanner_reset(self):
"""Move Steppers to init Pos. resets heading, pitch"""
self.stepper_heading.do_step(int((-1) * self.heading / 1.8))
self.servo_pitch.do_step(int((-1) * self.pitch / 1.8))
self.heading = 0
self.pitch = 0
self.line_direction = "CCW"
self.vertical_direction = "DOWN"
import smbus
from time import sleep
import Lidar
bus = smbus.SMBus(1)
lid = Lidar.Lidar_Lite()
if __name__ == '__main__':
while True:
try:
dist = lid.getDistance()
except IOError:
dist = 0
print("%.2f cm" % dist)
sleep(0.1)
#import RobotAPI as rapi
import cv2
import Lidar
#bot = PromobotLidar("/dev/ttyUSB0")
#robot = rapi.RobotAPI()
bot = Lidar.PromobotLidar("COM3")
ang = 0
pos = 0
#cv2.startWindowThread()
while 1:
'''while 1:
dist = bot.looksectors()
if dist == None:
bot.restart()
else:
break'''
# print(dist)
# if min(dist) == dist[0]:
# robot.serv(-60)
# elif min(dist) == dist[2]:
# robot.serv(60)
# else:
# robot.serv(0)
frame = bot.startdrawmap(640, 480)
cv2.imshow("frame", frame)
cv2.waitKey(0)
#robot.set_frame(fame, 30)
def initializeSensors(self):
try:
for x in range(0, self.connectAttempt):
self.openBus(self.tcaNOSE) #OPEN CHANNEL ON TCA
if (self.currentBus == self.tcaNOSE):
break
sleep(0.5)
except IOError:
print("Connection error with TCA Multiplexer")
self.TCA_status = False
try:
self.Lidar = Lidar.Lidar_Lite() #CREATE OBJECT FOR LIDAR LITE V3
self.bus.write_quick(
self.LID_ADDR) #QUICK WRITE TO TEST CONNECTION TO I2C BUS
print("Lidar Ready")
self.LID_status = True
except IOError:
print("Connection error with Lidar"
) #PRINT ERROR IF UNABLE TO CONNECT
try:
for x in range(0, self.connectAttempt):
self.BMP = BMP280(
address=self.BMP_ADDR) #CREATE OBJECT FOR BMP280
if (self.BMP.read_raw_temp()): #ATTEMPT READING FROM BMP
print("BMP Ready")
self.BMP_status = True
break
sleep(0.5)
except IOError:
print("Connection Error with BMP")
#return 0
try:
for x in range(0, self.connectAttempt):
self.bus.read_byte(self.MICRO_ADDR)
print("Arduino Micro Ready")
self.MICRO_status = True
break
except IOError:
print("Connection Error with Arduino Micro")
try:
for x in range(0, self.connectAttempt):
self.openBus(self.tcaPVL) #OPEN CHANNEL ON TCA
if (self.currentBus == self.tcaPVL):
break
sleep(0.5)
except IOError:
print("Connection error wit TCA Multiplexer")
self.TCA_status = 0
return 0
try:
for x in range(0, self.connectAttempt):
self.BMEL = BME280(address=self.BME_ADDR2
) #CREATE OBJECT FOR BME280 AT ADDRESS 2
if (self.BMEL.read_raw_temp()):
print("BME LPV Ready")
self.BME2_status = True
break
sleep(0.5)
except IOError:
print("Connection Error with BME2 LPV")
#return 0
try:
for x in range(0, self.connectAttempt):
self.openBus(self.tcaPVR) #OPEN TCA CHANNEL
if (self.currentBus == self.tcaPVR):
break
sleep(0.5)
except IOError:
print("Connection error with TCA Multiplexer")
self.TCA_status = False
return 0
try:
for x in range(0, self.connectAttempt):
self.Therm = MLX90614(
address=self.IR_ADDR
) #CREATE OBJECT FOR MLX90614 IR THERMOMETER
if (self.Therm.check_connect): #CHECK CONNECTION
print("MLX90614 Ready")
self.MLX_status = True
break
sleep(0.5)
except IOError:
print("Connection Error with Therm")
#return 0
try:
self.ADC = Adafruit_ADS1x15.ADS1115(
address=self.ADC_ADDR,
busnum=1) #CREATE OBJECT FOR ADS1115 ADC
print("ADC Ready")
self.ADC_status = True
except IOError:
print("Connection error with ADS ADC")
#return 0
try:
for x in range(0, self.connectAttempt):
self.openBus(self.tcaPVR2) #OPEN CHANNEL ON TCA
if (self.currentBus == self.tcaPVR2):
break
sleep(0.5)
except IOError:
print("Connection error with TCA Multiplexer")
self.TCA_status = False
return 0
try:
for x in range(0, self.connectAttempt):
self.BMER = BME280(address=self.BME_ADDR1
) #CREATE OBJECT FOR BME280 AT ADDRESS 2
if (self.BMER.read_raw_temp()):
print("BME RPV Ready")
self.BME1_status = True
break
sleep(0.5)
except IOError:
print("Connection Error with BME RPV")
#return 0
try:
for x in range(0, self.connectAttempt):
self.IMU1 = BNO055(address=self.IMU_ADDR1
) #CREATE OBJECT FOR BNO055 AT ADDRESS 1
if (
self.IMU1.begin(mode=0x08)
): #CHECK FOR CONNECTION TO I2C BUS AND SET IMU OPR MODE (MAG DISABLED)
while (self.IMU1.get_calibration_status()[1] != 3):
pass #WAIT FOR GYRO CALIBRATION COMPLETE
print("IMU1 Ready")
self.BNO1_status = True
break
sleep(0.5)
except IOError:
print("Connection error with IMU1")
#return 0
try:
self.X1OFFSET = self.getOrientation(1)[1]
print("IMU1 Y offset angle set")
except IOError:
print("IMU1 Y offset angle not set")
try:
self.Z1OFFSET = self.getOrientation(1)[2]
print("IMU1 Z offset angle set")
except IOError:
print("IMU1 Z offset angle not set")
try:
for x in range(0, self.connectAttempt):
self.IMU2 = BNO055(address=self.IMU_ADDR2
) #CREATE OBJECT FOR BNO055 AT ADDRESS 2
if (
self.IMU2.begin(mode=0x08)
): #CHECK FOR CONNECTION TO I2C BUS AND SET IMU OPR MODE (MAG DISABLED)
while (self.IMU2.get_calibration_status()[1] != 3):
pass #WAIT FOR GYRO CALIBRATION COMPLETE
print("IMU2 Ready")
self.BNO2_status = True
break
sleep(0.5)
except IOError:
print("Connection Error with IMU2")
#return 0
try:
self.X2OFFSET = self.getOrientation(2)[1]
print("IMU2 Y offset angle set")
except IOError:
print("IMU2 Y offset angle not set")
try:
self.Z2OFFSET = self.getOrientation(2)[2]
print("IMU2 Z offset angle set")
except IOError:
print("IMU2 Z offset angle not set")
return 1