def main(): from lidar_lite import Lidar_Lite lidar = Lidar_Lite() connected = lidar.connect(1) if connected < -1: print("Not Connected") return else: print("Connected") for i in range(100): distance = lidar.getDistance() print("Distance to target = %s" % (distance)) if int(distance) < 300: commandimage = 'raspistill -n --timeout 1 -h 183 -w 275 -o imgtmp.jpg' os.system(commandimage) device = open_ncs_device() graph = load_graph(device) img_draw = skimage.io.imread(ARGS.image) img = pre_process_image(img_draw) infer_image(graph, img, distance) close_ncs_device(device, graph)
def range_dist(): sonar = False lidar_sens = False # lidar sensing True, else EZ4 sonar or VL53L1X if sonar: # does not work well on grass from smbus import SMBus ## i2cbus = SMBus(1) while True: try: i2cbus = SMBus(1) i2cbus.write_byte(0x70, 0x51) time.sleep(0.12) val = i2cbus.read_word_data(0x70, 0xE1) distance_in_cm = val >> 8 | (val & 0x0F) << 8 ## print distance_in_cm, 'cm' print(distance_in_cm, 'cm', file=f) msg_sensor(distance_in_cm, 25, 400), #sonar facing down except IOError as err: print(err) time.sleep(0.1) if lidar_sens: from lidar_lite import Lidar_Lite lidar = Lidar_Lite() connected = lidar.connect(1) if connected < 0: print("\nlidar not connected") else: print("\nlidar connected") while True: dist = lidar.getDistance() print(dist, 'cm') print(dist, 'cm', file=f) msg_sensor(dist, 25, 700), #lidar facing down time.sleep(0.2) else: # does not work in sunlight import VL53L1X tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29) tof.open() # Initialise the i2c bus and configure the sensor tof.start_ranging( 2 ) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range # Short range max:1.3m, Medium range max:3m, Long range max:4m while True: distance_in_cm = tof.get_distance( ) / 10 # Grab the range in cm (mm) time.sleep(0.2) # timeout mavlink rangefinder = 500 ms print(distance_in_cm, 'cm', file=f) msg_sensor(distance_in_cm, 25, 250), #sensor facing down tof.stop_ranging() # Stop ranging
showNumber(x) else: for y in range(5, 0, -1): #countdown showNumber(y) time.sleep(0.9) showNumber(00) n = 0 m = [] while(n<15): vel = lidar.getVelocity() #get lidar data velPos = abs(vel) #clean up data print(velPos) m.append(velPos) n += 1 time.sleep(.1) dist = lidar.getDistance() velMax = max(m) if velMax > 99: # '' velMax %= 10 velMax *= 1.60934 #in MPH velMax = round(velMax) print(m) print("Max speed recoded: " + str(velMax)) firebase.post('/data', { "Distance":str(dist), "velocity":str(velMax), "User ID":x, "Date":today}) #post to database j = 0 while(midButtonPos == True): midButtonPos =GPIO.input(backButton) while(j<=velMax): showNumber(j) #print to display j += 1 time.sleep(.002)
from lidar_lite import Lidar_Lite lidar = Lidar_Lite() connected = lidar.connect(1) if connected < -1: print "Not Connected" print lidar.getDistance() print lidar.getVelocity()
# main body with picamera.PiCamera() as camera: if os.stat("/media/pi/WEIZHE/capstone101/16thRun.csv").st_size == 0: # creating a csv file with six columns file.write("Time,Distance(m),Speed(km/h),Relative_Speed(km/h),Rear_Speed(km/h),Safe_Distance(m)\n") while True: now = dt.datetime.now() # reading the speed data from sensoduino and distance data from LIDAR-Lite v3 read_serial = float(ser.readline()) distance = float(lidar.getDistance()) # Giving condition to data from LIDAR-Lite if distance < 0: distance = 0 # convert the distance into meter distance_in_m = distance/100 print("Distance to target = %s" % (distance)) # Using the continous distance data to calculate the relative speed of rear vehicle using 1 second rule i += 1 if i%2 == 0:
from picamera import PiCamera from time import sleep from lidar_lite import Lidar_Lite camera = PiCamera() camera.resolution = (1024, 768) lidar = Lidar_Lite() connected = lidar.connect(1) if connected < -1: print("Not Connected") camera.start_preview(fullscreen=False, window=(100, 20, 640, 480)) while True: print(lidar.getDistance()) print("finish test")
#Todo: remove once all sensors are on Arduino. ser=serial.Serial(port='/dev/ttyACM0',baudrate=4800) lidar=Lidar_Lite() connected=lidar.connect(1) if connected<-1: raise valueError('Lidar not connected.') while True: GPIO.output(ERROR_PIN,True) # If this is alive, then it means loop is running. switchState = GPIO.input(SWITCH_PIN) if switchState: # This means you have to record. GPIO.output(STATUS_PIN,status) status = not status ser.flushInput() ser.flush() serialLine=ser.readline() lidarData=lidar.getDistance() arduinoData=serialLine.split() timeNow=datetime.datetime.now() dataTime=timeNow.strftime('%m-%d-%Y-%H-%M-%S') if len(arduinoData) !=3: logFile = open(logFileName,'a') logFile.write("Could not write data at time " + str(dataTime) + " Arduino data: " + ''.join(arduinoData) + "\n") logFile.close() continue # Incomplete data dataList = [dataTime] + arduinoData + [lidarData] #print dataList #dataFile = open('/home/pi/data/current_trip.csv','a') #dataWriter = csv.writer(dataFile) #dataWriter.writerow(dataList) #dataFile.close()
lidar = Lidar_Lite() connected = lidar.connect(1) if connected < -1: print("Not Connected") l_samples = {} camera.start_preview(fullscreen=False, window=(100, 20, 640, 480)) for i in range(5): try: input("sample number {}".format(i + 1)) except SyntaxError: pass s = [] for j in range(5): s.append(lidar.getDistance()) t = datetime.now().time() l_samples[str(t)] = (np.median(s)) camera.capture('{}.jpg'.format(str(t))) #sleep(2) print(l_samples) with open('data.json', 'w') as fp: #feeds = json.load(fp) json.dump(l_samples, fp) #camera.capture('test1_{0}_{1}.jpg'.format(time, date)) #camera.capture("{}.jpg".format(time)) print("finish test")
if __name__ == '__main__': lidar = Lidar_Lite() connected = lidar.connect(1) if connected < -1: print("Not Connected") exit() vehicle = connect('udp:127.0.0.1:14551', wait_ready=True) vehicle.mode = VehicleMode('GUIDED') vehicle.armed = True time.sleep(5) if vehicle.armed: print("Taking off") vehicle.simple_takeoff(10) time.sleep(15) take_pic() height = lidar.getDistance() print("Height from LiDAR: ", height) time.sleep(4) print("Sea level alt: ", vehicle.location.global_frame.alt) print("Global location: ", vehicle.location.global_frame) print("Battery level: ", vehicle.battery.level) else: print("Vehicle not armed") print("Landing") vehicle.mode = VehicleMode('LAND') time.sleep(15) vehicle.close()
view = 0 ldata = [0,0,0,0,0] lpos = [1312,1282,1252,1222,1192] while 1: #'RF','LF','FF','PX','PY','LR','DF','PR' rawdata = readsensor() PServo.MoveServo(5,lpos[lc]) if lc < 4: lc += 1 else: lc =0 ldata[lc] = lidar.getDistance() print (ldata[lc],":", lc) sensordata = { "US" : int(rawdata[0]), "LF" : int(rawdata[1]), "RF" : int(rawdata[2])} print sensordata if c > 50: s = 0 l = 0 r = 0 c = 0 c += 1 espeak.synth("Clear the area") if (ldata[3] > 65 and sensordata["LF"] > 40 and sensordata["RF"] > 40):
class Drone: def __init__(self): self.alt = None self.cam = None self.lidar = None self.logs = {} self.h_logs = {} self.vehicle = self._connect_drone() self.vehicle.mode = VehicleMode('GUIDED') self.f = open("dump/logs.txt", "w") self.num_samples = 0 self.num_rows = 0 self.row_size = 0 def _connect_drone(self): ''' Connect to the drone's control unit (Pixhawk4) :return: dronekit lib object. ''' print("connecting...") print("Mode: ", MODE) if MODE == DEBUG: str = DEBUG_CONNECTION_STR print(str) else: # MODE == LIVE: str = LIVE_CONNECTION_STR vehicle = connect(str, wait_ready=True) print("connected successfully") return vehicle def _arming(self): self.vehicle.armed = True self.alt = 0 time.sleep(5) if not self.vehicle.armed: print("Vehicle not armed") return False if MODE == LIVE: print("Activating devices...") self.cam = PiCamera() self.cam.resolution = CAM_RES self.lidar = Lidar_Lite() connected = self.lidar.connect(1) if connected < -1: #TODO: check this cond print("Lidar not Connected") return False print("Armed successfully") return True def take_off(self, height): ''' Arming the drone and taking off to the given height. :param height: in meters ''' if not self._arming(): return print("Taking off...") self.vehicle.simple_takeoff(height) self.alt = height time.sleep(height * 1.4) self.goto(-14.14, 14.14) #TODO: Delete this line def land(self): ''' Landing. ''' print("Landing...") self.vehicle.mode = VehicleMode('LAND') time.sleep(20) def goto(self, dNorth, dEast): ''' Go x meters to North and y meters to East. ''' currentLocation = self.vehicle.location.global_relative_frame targetLocation = get_location_metres(currentLocation, dNorth, dEast, self.alt) targetDistance = get_distance_metres(currentLocation, targetLocation) self.vehicle.simple_goto(targetLocation) while self.vehicle.mode.name == "GUIDED": # Stop action if we are no longer in guided mode. remainingDistance = get_distance_metres(self.vehicle.location.global_frame, targetLocation) print("Distance to target: ", remainingDistance, ", Heading: ", self.vehicle.heading) # if remainingDistance <= targetDistance * 0.05: # Just below target, in case of undershoot. if remainingDistance <= 0.4: print("Reached target") break time.sleep(2) def take_sample(self): ''' Capture image and takes height sample with the Lidar sensor. ''' print("taking sample...") if MODE == DEBUG: sea_level_alt = self.vehicle.location.global_frame.alt print("sea level alt:" + str(sea_level_alt)) return # *** LiDAR **** self.num_samples += 1 s = [] for j in range(5): s.append(self.lidar.getDistance()) t = datetime.now().time() sea_level_alt = self.vehicle.location.global_frame.alt height_median = np.median(s) / 100 relative_alt = sea_level_alt - height_median # self.h_logs[str(t)] = (height_median, sea_level_alt, relative_alt) self.h_logs["s_" + str(self.num_samples)] = {"Time" : str(t), "Height" : str(relative_alt), "Hading" : str(self.vehicle.heading)} # ==== write to file - DEBUG ==== self.f.write(str(self.num_samples) + "- LiD height:" + str(height_median) + "| Sea level:" + str( sea_level_alt) + "| Relative: " + str(relative_alt) + "| Heading: " + str(self.vehicle.heading) +'\n') # *** Camera **** self.cam.capture('dump/{}.jpg'.format("im_"+str(self.num_samples))) time.sleep(2) def close_connection(self): ''' Close the connection of the dronekit object and save the flight logs. ''' if MODE == LIVE: self.logs["Logs"] = self.h_logs self.logs["num_imgs"] = self.num_samples self.logs["num_rows"] = self.num_rows self.logs["row_size"] = self.row_size with open('dump/logs.json', 'w') as fp: json.dump(self.logs, fp) self.vehicle.close() self.f.close() time.sleep(2) print("drone disconnected") def _scan_straight(self, height, length, width, adv_size): raw_input("Place the drone in the South-East corner of the scan " + "area, and press any key to start the scan") self.take_off(height) for i in range(int(length // (2 * adv_size))): self.take_sample() for j in range(int(width // adv_size)): self.goto(0, -adv_size) self.take_sample() self.goto(adv_size, 0) self.take_sample() for j in range(int(width // adv_size)): self.goto(0, adv_size) self.take_sample() self.goto(adv_size, 0) takeoff_return = int(length // adv_size) * adv_size + adv_size self.goto(int(-takeoff_return), 0) self.land() raw_input("Press any key to turn off the drone") self.close_connection() def _scan_diagonal(self, height, length, width, north, east, adv_size): fly_size = math.sqrt(adv_size ** 2 / 2) self.take_off(height) for i in range(int(length // (2 * adv_size))): self.take_sample() for j in range(int(width // adv_size)): self.goto(north * fly_size, -fly_size) self.take_sample() self.goto(fly_size, east * fly_size) self.take_sample() time.sleep(7) for j in range(int(width // adv_size)): self.goto(-north * fly_size, fly_size) self.take_sample() self.goto(fly_size, east * fly_size) takeoff_return = int(length // adv_size) * adv_size fly_return = math.sqrt(takeoff_return ** 2 / 2) self.goto(-fly_return, -east * fly_return) self.land() raw_input("Press any key to turn off the drone") self.close_connection() def scan(self, height, width, length): pic_size = get_pic_size(height) adv_size = 0.6 * pic_size self.num_rows = int(length // (2 * adv_size)) self.row_size = int(width // adv_size) found = False while not found: print("Select the orientation of the scan area: \n" + "1. North \n" + "2. North - East \n" + "3. North - West") direction = int(input()) if direction == 1: found = True self._scan_straight(height, length, width, adv_size) elif direction == 2: found = True raw_input("Place the drone in the Southern corner of the scan " + "area, and press any key to start the scan") self._scan_diagonal(height, length, width, 1, 1, adv_size) elif direction == 3: found = True raw_input("Place the drone in the Eastern corner of the scan " + "area, and press any key to start the scan") self._scan_diagonal(height, length, width, -1, -1, adv_size) else: print("Bad direction")
Location = "SJSU West" Row = "B" SpaceTopic = "car/space" Floor = 15 if connected < -1: print("Not Connected") else: print("Lidar scan bgins") while True: space = 0 for i in range(7): # 7 different positions for servo motor to stop pwm.setPWM(0, 0, position[i]) time.sleep(1) distance = lidar.getDistance() if (distance < 200): space = space + 1 print("Currnet degree = %s" % degree[i]) for j in range(3): # Lidar will collect data 3 times for one position distance = lidar.getDistance() print("Distance to target = %s" % (distance)) if (distance < 1000): print("Start Recognizing at position:", j + 1) camera.capture(rawCapture, format="bgr") image = cv2.cvtColor(rawCapture.array, cv2.COLOR_BGR2GRAY) results = alpr.recognize_ndarray(image) for plate in results['results']: for candidate in plate['candidates']: # The Prefix is not necessary for the program. if (candidate['confidence'] > 85):
for i in range(0, 32): try: strData += chr(bus.read_byte(comms900)) time.sleep(0.05) except IOError: print("Failed Read") subprocess.call(['i2cdetect', '-y', '1']) return time.sleep(0.1) return strData def convertReadStringToIntArray(myStr): readStr = readStr.split("_")[0] strArray = readStr.split(" ") intArray = [int(x) for x in strArray] return intArray while True: writeNumber(100) readStr = readString() intArray = convertReadStringToIntArray(readStr) print "Lidar info: ", lidar.getDistance(), lidar.getVelocity() print("intArray", intArray) time.sleep(.1)
# Gear display if reverse == True: gear = 0x1D else: gear = 0x1B # For Lead Vehicle Start Alert - track lowest distance achieved while throttle is not pressed. if throttle == True: lastDistance = 0 lvsaTriggered = False if esObstacleData == 0x04: esObstacleData = 0 # Get the distance. try: distance = lidar.getDistance() / 30.48 prevDst = distance except: distance = prevDst pass #print("LD: " + str(lastDistance) + ", D: " + str(distance)) # Set lowest distance to object if throttle is false and a forward gear is chosen if throttle == False and lvsaTriggered == False and reverse == False: if lastDistance == 0: lastDistance = distance if distance < lastDistance: lastDistance = distance if distance - lastDistance > 15:
lidarController[i-1].off() lidarController[i].on() if i == 0: self.getFR() elif i == 1: self.getFL() elif i == 2: self.getBL() elif i == 3: self.getBR() print( str(i) + " " + str(lidarController[i].value)) j = 0 while j != 50: try: print("Distance: "+ str(i) + " " + str(lidar.getDistance())) sleep(0.01) except: print("\tRemote I/O Error Occur, Check connection") sleep(0.01) j += 1 def getFR(): print("FRONT RIGHT") return lidar.getDistance() def getFL(): print("FRONT LEFT") return lidar.getDistance() def getBL():