def get_robot_device(robot: Robot, name: str, kind: Type[TDevice]) -> TDevice:
device: Optional[Device] = None
try:
# webots 2020b is buggy and always raises TypeError when passed a str,
# however we're aiming to be forwards compatible with 2021a, so try this first.
device = robot.getDevice(name)
except TypeError:
pass
if device is None: # webots 2020b always returns None when not raising TypeError
if kind is Emitter:
device = robot.getEmitter(name)
elif kind is Receiver:
device = robot.getReceiver(name)
elif kind is Motor:
device = robot.getMotor(name)
elif kind is LED:
device = robot.getLED(name)
elif kind is DistanceSensor:
device = robot.getDistanceSensor(name)
elif kind is TouchSensor:
device = robot.getTouchSensor(name)
elif kind is Compass:
device = robot.getCompass(name)
elif kind is Display:
device = robot.getDisplay(name)
if not isinstance(device, kind):
raise TypeError
return device
f.close()
robot = Robot()
timestep = int(robot.getBasicTimeStep() * 4)
panHeadMotor = robot.getMotor('PRM:/r1/c1/c2-Joint2:12')
tiltHeadMotor = robot.getMotor('PRM:/r1/c1/c2/c3-Joint2:13')
panHeadMotor.setPosition(float('+inf'))
tiltHeadMotor.setPosition(float('+inf'))
panHeadMotor.setVelocity(0.0)
tiltHeadMotor.setVelocity(0.0)
camera = robot.getCamera('PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1')
camera.enable(timestep)
width = camera.getWidth()
height = camera.getHeight()
display = robot.getDisplay('display')
display.attachCamera(camera)
display.setColor(0xFF0000)
targetPoint = []
targetRadius = 0
k = 0
flag_start_kalman = True
sigmaMeasure = 3
sigmaNoise = 1
err = 10
while robot.step(timestep) != -1:
if targetPoint:
display.setAlpha(0.0)
radius = targetRadius
if radius < 5:
radius = 5
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
#KEYBOARD INITIALIZATION
keyboard = Keyboard()
keyboard.enable(timestep)
print("Select the 3D window and control the robot using the W, A, S, D keys.")
#LIDAR INITIALIZATION
lidar = robot.getLidar("lidar")
lidar.enable(timestep)
#DISPLAY INITIALIZATION
i = 0
display = robot.getDisplay("extra_display")
grapher = Grapher(display)
while robot.step(timestep) != -1:
i += 1
#HANDLING LIDAR IMAGE ACQUISITION
imageArray = np.array(lidar.getRangeImageArray()).T
#HANDLING KEYBOARD BEHAVIOUR
ascii_val = keyboard.getKey()
if (ascii_val == -1):
left_motor.setVelocity(0)
right_motor.setVelocity(0)
else:
key = chr(ascii_val).lower()
if (key == 'w'):
left_motor.setVelocity(VELOCITY)
#cozmoSpeaker.setEngine("microsoft")
cozmoSpeaker.setLanguage("fr-FR") # "en-US" for American English (default value). "en-UK" for British English. "de-DE" for German. "es-ES" for Spanish. "fr-FR" for French. "it-IT" for Italian.
## Motors
leftMotor = robot.getMotor("LeftWheelMotor")
rightMotor = robot.getMotor("RightWheelMotor")
liftMotor = robot.getMotor("LiftMotor")
headMotor = robot.getMotor("HeadMotor")
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)
liftMotor.setPosition(0.5)
headMotor.setPosition(0.2)
leftMotor.setPosition(-1)
rightMotor.setPosition(-1)
display = robot.getDisplay("face_display")
awe = display.imageLoad("cozmo-awe.png")
surprised = display.imageLoad("cozmo-surprised.png");
happy = display.imageLoad("cozmo-happy.png");
display.imagePaste(awe, 0, 0, False);
# Controller welcome message in webots console
print("Autre TechLab Cozmo controller")
print("TIME STAMP = " +str(timestep))
# Main loop:
# - perform simulation steps until Webots is stopping the controller
left = 0.2
right = 0.2
flag = -2
from RobotControls import RobotControls
from socket_client import SocketClient
from VisionDisplay import VisionDisplay
import Constants
ENABLE_SOCKET = True
ENABLE_VISION_DISPLAY = False
VISION_DISPLAY_NAME = 'vision_display'
# create the Robot instance.
robot = Robot()
# initialize vision display
vision_display = False
if ENABLE_VISION_DISPLAY:
vision_display = VisionDisplay(robot.getDisplay(VISION_DISPLAY_NAME),
robot.getCamera('camera'), Display.RGB)
# get the time step of the current world.
Constants.TIMESTEP = int(robot.getBasicTimeStep())
# get the robot movement logic
rbc = RobotControls(robot)
# get the keyboard for user input
kb = robot.getKeyboard()
kb.enable(Constants.TIMESTEP)
socket = False
if ENABLE_SOCKET:
socket = SocketClient('localhost', 4444)
