def setup_lidar(self, threshold):
from lidar_lite import Lidar_Lite as lidar
lidar = lidar()
connect = lidar.connect(1)
if connect <-1:
raise Exception("No LiDAR found")
lidar.setThreshold(threshold)
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 takeoffLand(connect_str):
print("connect to: " + connect_str)
vehicle = connect('udp:172.29.113.100:14550', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
sleep(15)
takeSample(lidar, camera)
takeSample(lidar, camera)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
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
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 four_points(connect_str):
print("connect to: " + connect_str)
vehicle = connect(connect_str, wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
time.sleep(15)
goto(10, 0, vehicle.simple_goto)
point_handler(1, camera, lidar)
goto(0, 10, vehicle.simple_goto)
point_handler(2, camera, lidar)
goto(-10, 0, vehicle.simple_goto)
point_handler(3, camera, lidar)
goto(0, -10, vehicle.simple_goto)
point_handler(4, camera, lidar)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
raw_input("Press any key to turn off the drone")
vehicle.close()
with open('4pts_dump/logs.json', 'w') as fp:
json.dump(l_samples, fp)
def takeoffLand():
print("connect...")
vehicle = connect('/dev/ttyUSB0', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("turn on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
sleep(15)
takeSample(lidar, camera)
takeSample(lidar, camera)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
raw_input("Press any key to turn off the drone")
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
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")
logFile.close()
#Rotate/create data file
if os.path.isfile('/home/pi/data/proximity_current_trip.json'):
file_count = len([name for name in os.listdir('/home/pi/data/proximity_archive/') if os.path.isfile(name)])
os.rename("/home/pi/data/proximity_current_trip.json","/home/pi/data/proximity_"+str(file_count+1)+".json")
#Create a new log file for this trip
dataFile = open('/home/pi/data/proximity_current_trip.json','a')
dataFile.write('Start new trip file')
dataFile.close
status = True
try:
#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()
#!/usr/bin/env python
import rospy
from lidar_lite import Lidar_Lite
from std_msgs.msg import Float32
rospy.loginfo("Initialize front radar...")
lidar = Lidar_Lite(1)
rospy.loginfo("Done Initialization!")
def radar():
pub = rospy.Publisher('/front_radar', Float32, queue_size=1)
rospy.init_node('radar_node', anonymous=True)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
dist_cm = lidar.getDistance()
pub.publish(dist_cm)
rate.sleep()
if __name__ == '__main__':
try:
radar()
except rospy.ROSInterruptException:
pass
from lidar_lite import Lidar_Lite
from firebase import firebase
lidar = Lidar_Lite()
connected = lidar.connect(1)
tempRaw = lidar.getVelocity()
tempAbs = abs(tempRaw)
dist = lidar.getDistance()
if tempAbs > 99:
tempAbs %= 10
firebase = firebase.FirebaseApplication(
'https://lidarspeedometer.firebaseio.com/')
firebase.post('/data', {"Distance": str(dist), "velocity": str(tempAbs)})
print dist
from picamera import PiCamera
from time import sleep
from lidar_lite import Lidar_Lite
import numpy as np
import json
from datetime import datetime
camera = PiCamera()
camera.resolution = (1024, 768)
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)))
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()
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()
def take_pic():
camera = PiCamera()
camera.resolution = (1024, 768)
print("capturing an image...")
time = strftime("%H:%M:%S")
date = strftime("%d/%m/%Y")
camera.start_preview()
sleep(3)
camera.capture('test1_{0}_{1}.jpg'.format(time, date))
# camera.capture("test1_{}.jpg".format(time))
print("finish pic")
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()
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software Foundation,
Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
"""
import sys, time, os
sys.path.append('/home/hexy/JimBob/Lidar-Lite/python')
import serial
import RPi.GPIO as GPIO
#import SerialOut
import config
import PServo
from espeak import espeak
from lidar_lite import Lidar_Lite
from AnaSensorData import readsensor
lidar = Lidar_Lite()
anasensor = readsensor()
a = 0
s = 0
l = 0
r = 0
lc=2
start_time = time.time()
delay = 0.1 # set rest time between command sends
checksum = 0
GPIO.setmode(GPIO.BCM)
def startup():
PServo.ResetServo()
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")
#!/usr/bin/env python
import time
import pigpio
import sys
sys.path.append('/root/lidar/Lidar-Lite/python')
from lidar_lite import Lidar_Lite
servos = 18
cur_pulse = 0
cur_angle = 0
pi = pigpio.pi()
lidar = Lidar_Lite()
distance = -1
velocity = -1
l_connected = lidar.connect(1)
def debug_print():
print("Current angle: {}, current pulse: {}".format(cur_angle, cur_pulse))
print("Lidar distance: {}, Lidar velocity: {}".format(distance, velocity))
def set_servo(pulse):
pi.set_servo_pulsewidth(servos, pulse)
distance = lidar.getDistance()
# velocity = lidar.getVelocity()
# time.sleep(.02)
# debug_print()
degree_change = 21.16 #1 degree = 10.588
GPIO 13 = pin 33
GPIO 6 = pin 31
)
#--------------------------------------------#
'''
from lidar_lite import Lidar_Lite
from time import sleep
from gpiozero import DigitalOutputDevice
lidarController = [ DigitalOutputDevice(26),
DigitalOutputDevice(19),
DigitalOutputDevice(13),
DigitalOutputDevice(6) ]
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Not Connected")
if __name__ == "__main__":
print("Starting Test")
while True:
for i in range(0,4):
if i == 0:
lidarController[3].off()
else:
lidarController[i-1].off()
lidarController[i].on()
