camera = driver.getCamera("camera")
Camera.enable(camera, timestep)
lms291 = driver.getLidar("Sick LMS 291")
Lidar.enable(lms291, timestep)
lms291_yatay = Lidar.getHorizontalResolution(lms291)
fig = plt.figure(figsize=(3, 3))
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while driver.step() != -1:
Camera.getImage(camera)
Camera.saveImage(camera, "camera.png", 1)
frame = cv2.imread("camera.png")
#cv2.imshow("Camera",frame)
#cv2.waitKey(1)
lms291_deger = []
lms291_deger = Lidar.getRangeImage(lms291)
if plot == 10:
y = lms291_deger
x = np.linspace(math.pi, 0, np.size(y))
plt.polar(x, y)
plt.pause(0.00001)
plt.clf()
plot = 0
leftSpeed -= 0.5 * MAX_SPEED
rightSpeed += 0.5 * MAX_SPEED
print("front_obstacle")
elif left_obstacle:
leftSpeed -= 0.5 * MAX_SPEED
rightSpeed += 0.5 * MAX_SPEED
print("left_obstacle")
elif right_obstacle:
leftSpeed += 0.5 * MAX_SPEED
rightSpeed -= 0.5 * MAX_SPEED
print("right_obstacle")
# set up the motor speeds at x% of the MAX_SPEED.
leftMotorFront.setVelocity(leftSpeed)
rightMotorFront.setVelocity(rightSpeed)
leftMotorBack.setVelocity(leftSpeed)
rightMotorBack.setVelocity(rightSpeed)
Camera.getImage(kinectColor)
Camera.saveImage(kinectColor, 'color.png', 1)
RangeFinder.getRangeImage(kinectDepth)
RangeFinder.saveImage(kinectDepth, 'depth.png', 1)
frameColor = cv.imread('color.png')
frameDepth = cv.imread('depth.png')
cv.imshow("Color", frameColor)
cv.imshow("Depth", frameDepth)
cv.waitKey(10)
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
def move_forward():
motor_lst[1 + 0*3].setPosition(math.pi * -1 / 8)
motor_lst[1 + 0*3].setVelocity(1.0)
def rotate(angle):
for i in range(6):
motor_lst[0 + i*3].setPosition(angle)
motor_lst[0 + i*3].setVelocity(1.0)
camera = Camera("camera_d435i")
camera.enable(15)
print(camera.getSamplingPeriod())
camera.saveImage("~/test.png", 100)
gyro = Gyro("gyro")
gyro.enable(60)
inertial_unit = InertialUnit("inertial_unit")
inertial_unit.enable(60)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
def default_low_pos():
for i in range(6):
motor_lst[0 + i*3].setPosition(0)
motor_lst[0 + i*3].setVelocity(1.0)
motor_lst[1 + i*3].setPosition(math.pi * 1 / 8)
# camera= Camera(camera1)
camera = Camera('Camera')
# print(robot.getCamera('Camera'))
# camera.wb_camera_enable()
mTimeStep = basicTimeStep
print(camera.enable(int(mTimeStep)))
print(camera.getSamplingPeriod())
print(camera.getWidth())
print(camera.getHeight())
image = camera.getImage()
# print(image)
if image == None:
print("none")
# print(image.size())
# cameradata = cv2.VideoCapture('Camera')
camera.saveImage('/home/luyi/webots.png', 100)
# print(len(cap))
# cv2.imshow("cap",cap)
# print(image[2][3][0])
# for x in range(0,camera.getWidth()):
# for y in range(0,camera.getHeight()):
# print(camera.getSamplingPeriod())
# red = image[x][y][0]
# green = image[x][y][1]
# blue = image[x][y][2]
# gray = (red + green + blue) / 3
# print ('r='+str(red)+' g='+str(green)+' b='+str(blue))
# Initialize motion manager.
motionManager = RobotisOp2MotionManager(robot)
# Get the time step of the current world.
sag_hiz+=0.5*hizi
print("sol bir engel var")
elif sag_engeller:
sol_hiz+=0.5*hizi
sag_hiz-=0.5*hizi
print("sag bir engel var")
# motoların hızını belirler
# - koyarsak araç geri geri gelir
solMotorİleri.setVelocity(sol_hiz)
solMotorGeri.setVelocity(sol_hiz)
sağMotorİleri.setVelocity(sag_hiz)
sağMotorGeri.setVelocity(sag_hiz)
Camera.getImage(camera)
Camera.saveImage(camera,"color.png",1)
frame=cv2.imread("color.png")
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# define range of blue color in HSV
# lower_blue = np.array([110,50,50])
lower_blue = np.array([0,191,0])
# lower_blue = np.array([91,159,255])#Sualtında ki Çember
# upper_blue = np.array([130,255,255])
upper_blue = np.array([15,255,255])
# upper_blue = np.array([112,255,255])#Sualtında ki Çember
# Threshold the HSV image to get only blue colors
mask = cv2.inRange(hsv, lower_blue, upper_blue)
# Bitwise-AND mask and original image
res = cv2.bitwise_and(frame,frame, mask= mask)
elif cont > 1000 and cont < 1500:
driver.setCruisingSpeed(speedBrake)
driver.setBrakeIntensity(1.0) # intensidade (0.0 a 1.0)
elif cont > 1500 and cont < 2500:
driver.setCruisingSpeed(-speedFoward) # acelerador (velocidade)
driver.setSteeringAngle(0.0) # volante (giro)
elif cont > 2500:
cont = 0
# print('speed (km/h) %0.2f' % driver.getCurrentSpeed())
cont += 1
# ler a camera
Camera.getImage(cameraRGB)
Camera.saveImage(cameraRGB, 'color.png', 1)
frameColor = cv.imread('color.png')
cv.imshow('color', frameColor)
cv.waitKey(1)
# ler o Lidar
lms291_values = []
lms291_values = Lidar.getRangeImage(lms291)
# plotar o mapa
if plot == 10:
y = lms291_values
x = np.linspace(math.pi, 0, np.size(y))
plt.polar(x, y)
plt.pause(0.0001)
plt.clf()
#print("sol bir engel var")
elif sag_engeller:
sol_hiz += 0.5 * hizi
sag_hiz -= 0.5 * hizi
#print("sag bir engel var")
# motoların hızını belirler
# - koyarsak araç geri geri gelir
solMotor.setVelocity(sol_hiz)
sağMotor.setVelocity(sag_hiz)
# araç üzerinden resimi getirirz
Camera.getImage(kinectColor)
#araç üzerinden resimi kayıt ederiz
Camera.saveImage(kinectColor, "color.png", 1)
#araç üzerinden
RangeFinder.getRangeImage(kinectRange)
RangeFinder.saveImage(kinectRange, "range.png", 1)
#resimi okuruz
frameColor = cv2.imread("color.png")
#resim kopyalanır
cikti = frameColor.copy()
#resimin renigini belirleriz
gray = cv2.cvtColor(frameColor, cv2.COLOR_BGR2GRAY)
#resim de blur blur getirmeisini sağlarız
gray_blur = cv2.blur(gray, (3, 3))
