# Create the Robot instance.
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
camera.enable(int(mTimeStep))
camera.getSamplingPeriod()
# width=camera.getWidth()
# height=camera.getHeight()
firstimage=camera.getImage()
ori_width = int(4 * 160) # 原始图像640x480
ori_height = int(3 * 160)
r_width = int(4 * 20) # 处理图像时缩小为80x60,加快处理速度，谨慎修改！
r_height = int(3 * 20)
color_range = {'yellow_door': [(10, 43, 46), (34, 255, 255)],
'red_floor1': [(0, 43, 46), (10, 255, 255)],
'red_floor2': [(156, 43, 46), (180, 255, 255)],
'green_bridge': [(35, 43, 20), (100, 255, 255)],
'yellow_hole': [(10, 70, 46), (34, 255, 255)],
'black_hole': [(0, 0, 0), (180, 255, 80)],
'black_gap': [(0, 0, 0), (180, 255, 100)],
'black_dir': [(0, 0, 0), (180, 255, 46)],
]
# 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)
