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))