def initialization(self):
self.mode = self.Mode.AVOIDOBSTACLES
self.camera = self.getCamera('camera')
self.camera.enable(4 * self.timeStep)
width = Camera.getWidth(self.camera)
height = Camera.getHeight(self.camera)
imagecameraki = Camera.getImage(self.camera)
i = width / 3
j = height / 2
k = height / 4
for( l < 2 * i):
import csv
import numpy as np
from controller import Robot, Camera, PositionSensor
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# get and enable Camera
cameraTop = Camera("CameraTop")
cameraTop.enable(timestep)
print("Width: ", cameraTop.getWidth())
print("Height: ", cameraTop.getHeight())
#cameraTop.saveImage("cameraImg.png", 50)
cameraBottom = Camera("CameraBottom")
cameraBottom.enable(timestep)
#Move head in right position
#HeadYaw = robot.getMotor("HeadYaw")
#HeadPitch = robot.getMotor("HeadPitch")
#HeadYaw.setPosition(0.5)
#HeadYaw.setVelocity(1.0)
#HeadPitch.setPosition(0.2)
#HeadPitch.setVelocity(1.0)
# define all motors we going to use
robot = Robot()
robot.getSupervisor()
basicTimeStep = int(robot.getBasicTimeStep())
# print(robot.getDevice("camera"))
camera1 = robot.getCamera("Camera")
print(camera1)
# 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]
motors[i].setAcceleration(25)
camera = Camera('camera')
camera.enable(int(robot.getBasicTimeStep()))
SPEED = 2
while robot.step(TIME_STEP) != -1:
leftSpeed = 0.0
rightSpeed = 0.0
#get image and process it
image = camera.getImage()
leftSum = 0
rightSum = 0
cameraData = camera.getImage()
for x in range(0, camera.getWidth()):
for y in range(int(camera.getHeight() * 0.9), camera.getHeight()):
gray = Camera.imageGetGray(cameraData, camera.getWidth(), x, y)
if x < camera.getWidth() / 2:
leftSum += gray
else:
rightSum += gray
if leftSum > rightSum + 1000:
leftSpeed = SPEED * (1 - 0.8 * (leftSum - rightSum) / 460000)
rightSpeed = SPEED * (1 - 0.6 * (leftSum - rightSum) / 460000)
elif rightSum > leftSum + 1000:
leftSpeed = SPEED * (1 - 0.6 * (rightSum - leftSum) / 460000)
rightSpeed = SPEED * (1 - 0.8 * (rightSum - leftSum) / 460000)
else:
leftSpeed = SPEED
rightSpeed = SPEED
display.attachCamera(cameraTop)
keyboard = Keyboard()
# cameraFront.enable(32)
cameraTop.enable(32)
keyboard.enable(32)
while car.step() != -1:
display.setColor(0x000000)
display.setAlpha(0.0)
display.fillRectangle(0, 0, display.getWidth(), display.getHeight())
img = cameraTop.getImage()
image = np.frombuffer(img, np.uint8).reshape(
(cameraTop.getHeight(), cameraTop.getWidth(), 4))
# cv2.imwrite("img.png", image)
gray = cv2.cvtColor(np.float32(image), cv2.COLOR_RGB2GRAY)
#--- vira a imagem da camera em 90 graus
#gray270 = np.rot90(gray, 3)
#grayFlip = cv2.flip(gray270, 1)
#cv2.imwrite("grayflip.jpeg", grayFlip)
#--- gera o blur na imagem da camera
kernel_size = 5
blurGray = cv2.GaussianBlur(gray, (kernel_size, kernel_size), 0)
#--- converte a imagem
blurGrayConv = np.uint8(blurGray)
#cv2.imwrite("imagem.jpeg",blurGrayConv)
# this function same as above function. It focuses the bin.
def foundbin(k, l):
print("bin is detected")
robot.setSpeed(k, l)
# same as "foundtrash" method.
if ((k >= 190 and l >= 190) and (left7 >= 140 or right0 >= 140)
and (red[20] >= 100 and green[20] <= 10 and blue[20] <= 10)):
led[0].set(0)
print(" I am next to bin")
# enable camera.
camera = Camera("camera")
camera.enable(timestep * 2)
print("Camera width = ", camera.getWidth(), "Camera height =",
camera.getHeight())
# enable LEDs
led = [0] * 8
count = 0
for i in range(8):
name = "led" + str(i)
led[i] = LED(name)
robot.enableEncoders(timestep)
# enable distance and ground sensors
irLeft7 = DistanceSensor("ps7")
irLeft6 = DistanceSensor("ps6")
irLeft5 = DistanceSensor("ps5")
irRight0 = DistanceSensor("ps0")
irRight1 = DistanceSensor("ps1")
