timestep = int(robot.getBasicTimeStep())
stopped = False
burger = robot.getFromDef('Burger')
# Supervisor
translationField = robot.getFromDef('ROBOT_POSITION')
rotationField = burger.getField('rotation')
groundtruth = np.zeros((1, 3))
burgerData = np.zeros((1, 4))
lidarData = np.zeros((1, 128))
freq = 0
keyboard = robot.getKeyboard()
keyboard.enable(timestep)
receiver = robot.getReceiver("receiver")
receiver.enable(timestep)
lastDist = 0
np.set_printoptions(suppress=True)
while robot.step(timestep) != -1:
if freq == 10:
translation = translationField.getPosition()
orientation = translationField.getOrientation()
zOri = np.array([orientation[6], orientation[8]])
MAX_MOTOR_VELOCITY = 50 # [rpm]
WHEEL_RADIUS = 0.025 # [m]
WHEEL_BASE = 0.165 # [m]
MAX_LINEAR_SPEED = 1 # [m.s-1]
MAX_ANGULAR_SPEED = 1.5708 # [rad.s-1]
LINEAR_ACCEL = 0.5 # [m.s-2]
ANGULAR_ACCEL = LINEAR_ACCEL / WHEEL_BASE # [rad.s-2]
# time in [ms] of a simulation step
TIME_STEP = 32
# create the Robot instance.
supervisor = Supervisor()
# Init keyboard
keyboard = supervisor.getKeyboard()
keyboard.enable(TIME_STEP)
# Init motors
leftMotor = supervisor.getDevice('left wheel motor')
rightMotor = supervisor.getDevice('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)
linearVelocity = 0
angularVelocity = 0
def check_keyboard_inputs():
