def __init__(self, webot: Robot, lock: threading.Lock) -> None:
self._webot = webot
self._timestep = int(webot.getBasicTimeStep())
self.camera = webot.getCamera("camera")
self.camera.enable(self._timestep)
self.camera.recognitionEnable(self._timestep)
self._lock = lock
def start_simulation(self):
self.total_energy = 0
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
camera = robot.getCamera('view')
camera.enable(TIME_STEP)
motor = robot.getMotor('motor')
sun_motor = robot.getMotor('sun_motor')
panel_sensor = robot.getPositionSensor('panel_sensor')
sun_sensor = robot.getPositionSensor('sun_sensor')
panel_sensor.enable(TIME_STEP)
sun_sensor.enable(TIME_STEP)
if self.moving_sun:
sun_motor.setVelocity(0.3)
sun_motor.setPosition(self.max_sun_position)
else:
sun_motor.setVelocity(10)
random_pos = random.random() * math.pi - math.pi / 2
sun_motor.setPosition(random_pos)
while (abs(sun_sensor.getValue() - random_pos) > 1e-2):
time.sleep(0.1)
start_time = time.time()
printed = False
while robot.step(TIME_STEP) != -1:
camera.getImage()
sun_position = sun_sensor.getValue()
panel_position = panel_sensor.getValue()
if (self.moving_sun and abs(sun_position - self.max_sun_position) < 1e-1)\
or (not self.moving_sun and time.time() - start_time > self.time_limit):
if not printed:
print("Energy harvested:", self.total_energy, "J")
printed = True
continue
motor.setTorque(self.motor_torque)
self.total_energy += max(
[0, 1 - abs(panel_position - sun_position)])
KeyB.enable(timeStep)
front_left_motor = robot.getMotor("front left thruster")
front_right_motor = robot.getMotor("front right thruster")
rear_left_motor = robot.getMotor("rear left thruster")
rear_right_motor = robot.getMotor("rear right thruster")
front_left_motor.setPosition(float('inf'))
front_right_motor.setPosition(float('inf'))
rear_left_motor.setPosition(float('inf'))
rear_right_motor.setPosition(float('inf'))
front_left_motor.setVelocity(0.0)
front_right_motor.setVelocity(0.0)
rear_left_motor.setVelocity(0.0)
rear_right_motor.setVelocity(0.0)
camera = robot.getCamera("camera")
camera.enable(timeStep)
rearcamera = robot.getCamera("rearcamera")
rearcamera.enable(timeStep)
FL_wheel = robot.getMotor("front left wheel")
FR_wheel = robot.getMotor("front right wheel")
RL_wheel = robot.getMotor("rear left wheel")
RR_wheel = robot.getMotor("rear right wheel")
FL_wheel.setPosition(float('inf'))
FR_wheel.setPosition(float('inf'))
RL_wheel.setPosition(float('inf'))
RR_wheel.setPosition(float('inf'))
FL_wheel.setVelocity(0.0)
FR_wheel.setVelocity(0.0)
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)
lidar.enablePointCloud()
#FRONT CAMERA INITIALIZATION
front_camera = robot.getCamera("front_camera")
front_camera.enable(timestep)
#REAR CAMERA INITIALIZATION
rear_camera = robot.getCamera("rear_camera")
rear_camera.enable(timestep)
while robot.step(timestep) != -1:
#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)
right_motor.setVelocity(VELOCITY)
# time in [ms] of a simulation step
TIME_STEP = 64
MAX_SPEED = 6.28
# maximal value returned by the sensors
MAX_SENSOR_VALUE = 1024
# minimal distance, in meters, for an obstacle to be considered
MIN_DISTANCE = 1.0
# create the robot instance
robot = Robot()
# inicializa kinect
kinectColor = robot.getCamera('kinect color')
kinectDepth = robot.getRangeFinder('kinect range')
print(kinectColor)
Camera.enable(kinectColor, TIME_STEP)
RangeFinder.enable(kinectDepth, TIME_STEP)
# get a handler to the motors and set target position to infinity (speed control)
leftMotorFront = robot.getMotor('front left wheel')
rightMotorFront = robot.getMotor('front right wheel')
leftMotorBack = robot.getMotor('back left wheel')
rightMotorBack = robot.getMotor('back right wheel')
leftMotorFront.setPosition(float('inf'))
rightMotorFront.setPosition(float('inf'))
leftMotorBack.setPosition(float('inf'))
rightMotorBack.setPosition(float('inf'))
timeStep = 32
max_velocity = 6.28
sensor_value = 0.07
messageSent = False
startTime = 0
duration = 0
robot = Robot()
wheel_left = robot.getMotor("left wheel motor")
wheel_right = robot.getMotor("right wheel motor")
camera = robot.getCamera("camera_left")
camera.enable(timeStep)
camerar = robot.getCamera("camera_right")
camerar.enable(timeStep)
cameral = robot.getCamera("camera_centre")
cameral.enable(timeStep)
colour_camera = robot.getCamera("colour_sensor")
colour_camera.enable(timeStep)
emitter = robot.getEmitter("emitter")
gps = robot.getGPS("gps")
gps.enable(timeStep)
left_heat_sensor = robot.getLightSensor("left_heat_sensor")
fileString = f.read()
fileString64 = base64.b64encode(fileString)
robot.wwiSendText("image[" + deviceName + "]:data:image/jpeg;base64," +
str(fileString64))
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:
import OpenCVClient as client
from controller import Robot
from OpenCVServer import OpenCVServer
import cv2
from cv2 import aruco
TIME_STEP = 64
print("Starting, getting camera...")
bot = Robot()
parameters = aruco.DetectorParameters_create()
# Use Aruco Dictionary for 4x4 markers (250)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
# Get the camera device, enable it, and store its width and height
camera = bot.getCamera("camera");
camera.enable(TIME_STEP);
width = camera.getWidth();
height = camera.getHeight();
print("Done.")
def start_cv_client():
time.sleep(10)
while True:
try:
client.main()
break
except:
print("Restarting OpenCVClient...")
time.sleep(3)
for i in range(6):
angles[i] = motor_sensors[i].getValue()
return angles
def respond(result, data = None):
cmd = {}
cmd["result"] = result
cmd["data"] = data
send_msg(pickle.dumps(cmd))
robot = Robot()
suction = Connector("suction")
gripper_connector = Connector("gripper_connector")
gripper_connector_for_box = Connector("gripper_connector_for_box")
cameraRGB = robot.getCamera("cameraRGB")
cameraDepth = robot.getRangeFinder("cameraDepth")
timestep = int(robot.getBasicTimeStep())
motors = [robot.getMotor("shoulder_pan_joint"), robot.getMotor("shoulder_lift_joint"), robot.getMotor("elbow_joint"),
robot.getMotor("wrist_1_joint"), robot.getMotor("wrist_2_joint"), robot.getMotor("wrist_3_joint"), robot.getMotor("rotational motor")]
motor_sensors = [robot.getPositionSensor("shoulder_pan_joint_sensor"), robot.getPositionSensor("shoulder_lift_joint_sensor"), robot.getPositionSensor("elbow_joint_sensor"),
robot.getPositionSensor("wrist_1_joint_sensor"), robot.getPositionSensor("wrist_2_joint_sensor"), robot.getPositionSensor("wrist_3_joint_sensor")]
finger_motors = [robot.getMotor("right_finger_motor"), robot.getMotor("left_finger_motor")]
finger_sensors = [robot.getPositionSensor("right_finger_sensor"), robot.getPositionSensor("left_finger_sensor")]
FINGER_CLOSED_POSITION = [0.005, 0.005] # right, left
FINGER_OPEN_POSITION = [-0.035, 0.045] # right, left
MAX_FINGER_DISTANCE = 80 # mm abs open pos
CLOSED_FINGER_DISTANCE = 10 # mm
left_heat_sensor = robot.getLightSensor("left_heat_sensor")
right_heat_sensor = robot.getLightSensor("right_heat_sensor")
gyro = robot.getGyro("gyro")
emitter = robot.getEmitter("emitter")
front_sensor_l = robot.getDistanceSensor("ps7")
front_sensor_r = robot.getDistanceSensor("ps0")
left_sensor = robot.getDistanceSensor("ps5")
right_sensor = robot.getDistanceSensor("ps2")
back_sensor_l = robot.getDistanceSensor("ps3")
back_sensor_r = robot.getDistanceSensor("ps4")
color = robot.getCamera("colour_sensor")
cam = robot.getCamera("camera_centre")
cam_right = robot.getCamera("camera_right")
cam_left = robot.getCamera("camera_left")
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
gps = robot.getGPS("gps")
gps.enable(timestep)
left_encoder.enable(timestep)
right_encoder.enable(timestep)
gyro.enable(timestep)
left_heat_sensor.enable(timestep)
right_heat_sensor.enable(timestep)
max_velocity = 6.28
#Rotacion global
globalRotation = 0
messageSent = False
# Instanciacion de robot
robot = Robot()
# Ruedas
wheel_left = robot.getMotor("left wheel motor")
wheel_right = robot.getMotor("right wheel motor")
# Camara
colour_camera = robot.getCamera("colour_sensor")
colour_camera.enable(timeStep)
# emitter
emitter = robot.getEmitter("emitter")
# gps
gps = robot.getGPS("gps")
gps.enable(timeStep)
#Sensores de calor
left_heat_sensor = robot.getLightSensor("left_heat_sensor")
right_heat_sensor = robot.getLightSensor("right_heat_sensor")
left_heat_sensor.enable(timeStep)
right_heat_sensor.enable(timeStep)
robot = Robot()
timestep = int(robot.getBasicTimeStep())
# sensors and motors
motorLeft = robot.getMotor('wheel_left')
motorRight = robot.getMotor('wheel_right')
motorLeft.setPosition(float('inf'))
motorRight.setPosition(float('inf'))
motorLeft.setVelocity(0.0)
motorRight.setVelocity(0.0)
ds = robot.getLidar('lidar')
ds.enable(timestep)
cam = robot.getCamera('camera')
cam.enable(timestep)
cam_back = robot.getCamera('camera_back')
cam_back.enable(timestep)
# program start time
start = True
start_time = robot.getTime()
# cosine values for wanderer
angles = np.cos(np.deg2rad(np.linspace(0, 179, 180)))
base_color = [0, 0, 0]
mode = 'free'
from controller import Robot
from controller import Camera
import math
#returns list in string form with number rounded to 2 decimals
def roundedList(myList):
string = "[ "
for i in myList:
string += str(round(i, 2)) + ", "
string += "]"
return string
recognitionColor = [0, 0, 0]
timeStep = 32
myRobot = Robot()
camera = myRobot.getCamera("camera")
#camera.enable(timeStep)
camera.recognitionEnable(timeStep)
while myRobot.step(timeStep) != -1:
objects = camera.getRecognitionObjects()
for obj in objects:
if obj.get_colors() == recognitionColor:
print("Recognized object!")
print("Position: " + roundedList(obj.get_position()))
break
import numpy as np
hizi = 6.28
maxMesafe = 1024
#sensörün mesafede nasıl algı m
min_uzaklık = 1.0
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
#Camera ve RangeFinder getirir
kinectColor = robot.getCamera("kinect color")
kinectRange = robot.getRangeFinder("kinect range")
#camera ve rangefinder başlatıldı
Camera.enable(kinectColor, timestep)
RangeFinder.enable(kinectRange, timestep)
# motorların tagını getirir
#motorları getirir
solMotor = robot.getMotor("left wheel")
sağMotor = robot.getMotor("right wheel")
#motorları hareket etirir
solMotor.setPosition(float("inf"))
sağMotor.setPosition(float("inf"))
left_heat_sensor = robot.getLightSensor("left_heat_sensor")
right_heat_sensor = robot.getLightSensor("right_heat_sensor")
gyro = robot.getGyro("gyro")
emitter = robot.getEmitter("emitter")
front_sensor_l = robot.getDistanceSensor("ps7")
front_sensor_r = robot.getDistanceSensor("ps0")
left_sensor = robot.getDistanceSensor("ps5")
right_sensor = robot.getDistanceSensor("ps2")
back_sensor_l = robot.getDistanceSensor("ps3")
back_sensor_r = robot.getDistanceSensor("ps4")
color = robot.getCamera("colour_sensor")
cam = robot.getCamera("camera_centre")
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
gps = robot.getGPS("gps")
gps.enable(timestep)
left_encoder.enable(timestep)
right_encoder.enable(timestep)
gyro.enable(timestep)
left_heat_sensor.enable(timestep)
class CustomRobotClass:
def __init__(self):
# Initialize Robot
self.robot = Robot()
self.time_step = int(self.robot.getBasicTimeStep())
print("Robot time step ", self.time_step)
# Initialize sensors
self.metacamera = self.robot.getCamera("MultiSense S21 meta camera")
self.leftcamera = self.robot.getCamera("MultiSense S21 left camera")
self.rightcamera = self.robot.getCamera("MultiSense S21 right camera")
self.rangeFinder = self.robot.getRangeFinder(
"MultiSense S21 meta range finder")
self.cameras = {
"left": self.leftcamera,
"right": self.rightcamera,
"meta": self.metacamera,
"depth": self.rangeFinder
}
self.leftpositionsensor = self.robot.getPositionSensor(
'back left wheel sensor')
self.rightpositionsensor = self.robot.getPositionSensor(
'back right wheel sensor')
self.keyboard = Keyboard()
self.pen = self.robot.getPen('pen')
# Enable sensors
self.metacamera.enable(self.time_step)
self.leftcamera.enable(self.time_step)
self.rightcamera.enable(self.time_step)
self.rangeFinder.enable(self.time_step)
self.leftpositionsensor.enable(self.time_step)
self.rightpositionsensor.enable(self.time_step)
self.keyboard.enable(self.time_step)
# Initialize wheels
backrightwheel = self.robot.getMotor('back right wheel')
backleftwheel = self.robot.getMotor('back left wheel')
frontrightwheel = self.robot.getMotor('front right wheel')
frontleftwheel = self.robot.getMotor('front left wheel')
self.wheels = [
backrightwheel, backleftwheel, frontrightwheel, frontleftwheel
]
self.set_velocity_control()
self.prevlspeed = 0
self.prevrspeed = 0
self.curlspeed = 0
self.currspeed = 0
def set_velocity_control(self):
for wheel in self.wheels:
wheel.setPosition((float('inf')))
wheel.setVelocity(0)
def step(self):
self.robot.step(self.time_step)
def set_speed(self, l, r):
print("Inside set speed", SPEED_UNIT * l, SPEED_UNIT * r)
if self.prevlspeed != l or self.prevrspeed != r:
self.wheels[0].setVelocity(SPEED_UNIT * r)
self.wheels[1].setVelocity(SPEED_UNIT * l)
self.wheels[2].setVelocity(SPEED_UNIT * r)
self.wheels[3].setVelocity(SPEED_UNIT * l)
self.prevlspeed = self.curlspeed
self.prevrspeed = self.currspeed
self.curlspeed = l
self.currspeed = r
def get_image(self, camera_name, depth_option=False):
if (depth_option == True):
return self.cameras[camera_name].getRangeImageArray()
else:
return self.cameras[camera_name].getImageArray()
(camera.getHeight(), camera.getWidth(), 4)))
img[:, :, 2] = np.zeros([img.shape[0], img.shape[1]])
#convert from BGR to HSV color space
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#apply threshold
thresh = cv2.threshold(gray, 140, 255, cv2.THRESH_BINARY)[1]
# draw all contours in green and accepted ones in red
contours, h = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
for c in contours:
if cv2.contourArea(c) > 1000:
coords = list(c[0][0])
coords_list.append(coords)
print("Victim at x=" + str(coords[0]) + " y=" + str(coords[1]))
return coords_list
robot = Robot()
timeStep = 32
camera = robot.getCamera("camera_centre")
camera.enable(timeStep)
while robot.step(timeStep) != -1:
img = camera.getImage()
process(img, camera)
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
TIME_STEP = 64
print("请用键盘和鼠标控制RoboMaster S1:")
print("- 'W': 前进。")
print("- 'S': 后退。")
print("- 'D': 向右平移。")
print("- 'A': 向左平移。")
print("- '左': YAW逆时针旋转。")
print("- '右': YAW顺时针旋转。")
print("- '上': pitch轴抬高。")
print("- '下': pitch轴下降。")
keyboard = robot.getKeyboard()
keyboard.enable(10)
maincamera = robot.getCamera('camera') #获取相机
maincamera.enable(10) #开启相机，并定义刷新间隔为10ms
yawmotor = robot.getMotor('yaw_motor') #获取yaw轴电机
yawmotor.setPosition(float('inf')) #定义旋转位置为无穷远
yawmotor.setVelocity(0.0) #定义初始速度为0
pitchmotor = robot.getMotor('pitch_motor')
pitchmotor.setPosition(float('inf'))
pitchmotor.setVelocity(0.0)
wheels = []
wheelsNames = ['wheel_fl_motor', 'wheel_fr_motor', 'wheel_br_motor', 'wheel_bl_motor']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
# 定义ID
# 1-------0
'To run this sample, please upgrade "pip" and install ikpy with this command: "pip install ikpy"'
)
import math
from controller import Supervisor
from controller import Robot
from controller import Camera
if ikpy.__version__[0] < '3':
sys.exit(
'The "ikpy" Python module version is too old. '
'Please upgrade "ikpy" Python module to version "3.0" or newer with this command: "pip install --upgrade ikpy"'
)
# Initialize the Webots Supervisor.
#supervisor = Supervisor()
#timeStep = int(4 * supervisor.getBasicTimeStep())
robot = Robot()
timestep = int(robot.getBasicTimeStep())
camera = robot.getCamera('upper_camera')
camera.enable(4 * timestep)
#max = camera.getMaxFov()
#min = camera.getMinFov()
#camera.setFov(min)
print(max, min)
w = camera.getWidth()
h = camera.getHeight()
print(w, h)
#camera.setFocalDistance(0.2)
dsNames = ['ds_right', 'ds_left', 'ds_front', 'ds_frontleft', 'ds_frontright']
for i in range(5):
ds.append(robot.getDistanceSensor(dsNames[i]))
ds[i].enable(TIME_STEP)
#Wheels Initialize
wheels = []
wheelsNames = ['tlwheel', 'trwheel', 'blwheel', 'brwheel']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
#Camera Initialize
camera0 = robot.getCamera('camera0')
camera0.enable(TIME_STEP)
camera1 = robot.getCamera('camera1')
#camera1.enable(TIME_STEP)
camera0.recognitionEnable(TIME_STEP)
camera1.recognitionEnable(TIME_STEP)
#gps= robot.getGPS('gps')
#gps.enable(TIME_STEP)
#def colorRecognise(image,w,x,y)
c = 0
# feedback loop: step simulation until receiving an exit event
while robot.step(TIME_STEP) != -1:
if c == 0:
arms.append(robot.getMotor("arm4"))
arms.append(robot.getMotor("arm5"))
# arms[0].setVelocity(0.2)
# arms[1].setVelocity(0.5)
# arms[2].setVelocity(0.5)
# arms[3].setVelocity(0.3)
# Set the maximum motor velocity.
finger1 = robot.getMotor("finger1")
finger2 = robot.getMotor("finger2")
fingerMin = finger1.getMinPosition()
fingerMax = finger1.getMaxPosition()
#-------------------- sensor --------------------------#
# Initialize arm position sensors.
# These sensors can be used to get the current joint position and monitor the joint movements.
camera1 = robot.getCamera('camera1')
camera1.enable(50)
camera1.recognitionEnable(50)
camera2 = robot.getCamera('camera2')
camera2.enable(50)
camera2.recognitionEnable(50)
compass = robot.getCompass("compass")
gps = robot.getGPS("gps")
compass.enable(timestep)
gps.enable(timestep)
sensors = []
wsensor = []
sensors.append(robot.getPositionSensor("arm1sensor"))
sensors.append(robot.getPositionSensor("arm2sensor"))
sensors.append(robot.getPositionSensor("arm3sensor"))
"""camera_controller controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot
TIME_STEP = 64
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = TIME_STEP
camera_name = ['camera1', 'camera2']
cameras = []
for i in range(2):
cameras.append(robot.getCamera(camera_name[i]))
cameras[i].enable(TIME_STEP)
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while robot.step(timestep) != -1:
# Read the sensors:
# Enter here functions to read sensor data, like:
# val = ds.getValue()
positionSensorNames = ('ShoulderR', 'ShoulderL', 'ArmUpperR', 'ArmUpperL',
'ArmLowerR', 'ArmLowerL', 'PelvYR', 'PelvYL',
'PelvR', 'PelvL', 'LegUpperR', 'LegUpperL',
'LegLowerR', 'LegLowerL', 'AnkleR', 'AnkleL',
'FootR', 'FootL', 'Neck', 'Head')
# List of position sensor devices.
positionSensors = []
# 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)
#returns list in string form with number rounded to 2 decimals
def roundedList(myList):
string = "[ "
for i in myList:
string += str(round(i, 2)) + ", "
string += "]"
return string
recognitionColor = [
0, 0, 0
] #Recognition Color of a human (the object we are looking for)
timeStep = 32
myRobot = Robot()
camera = myRobot.getCamera("camera") #Creates Camera object called "camera"
#Use this object to call camera related methods
camera.recognitionEnable(
timeStep) #recognitionEnable allows camera to detect Recognition Colors
while myRobot.step(timeStep) != -1:
objects = camera.getRecognitionObjects(
) #returns list of all objects in camera's view with a Recognition Color (walls, humans, etc.)
for obj in objects:
if obj.get_colors(
) == recognitionColor: #checks if Recognition Color of obj matches Recognition Color of a human
print("Recognized object!")
print(
"Position: " + roundedList(obj.get_position())
) #obj.get_position() returns position of detected object relative to robot
from controller import Robot
import cv2 as cv
import numpy as np
## tiempo sampleo de la simulacion
TIME_STEP = 10
robot = Robot()
## parte de los sensores de distancia
##acceso a los nodos de vision del robot
camera_id=[]
camara=['camera1']
for i in range(1):
camera_id.append(robot.getCamera(camara[i]))
camera_id[i].enable(TIME_STEP)
## datos para la deteccion de bolas azules
## componenetes en rojo a filtrar
redbajo1=np.array([0,100,20],np.uint8)
redAlto1=np.array([8,255,255],np.uint8)
redbajo2=np.array([175,100,20],np.uint8)
redAlto2=np.array([179,255,255],np.uint8)
#3 componentes en azul a filtrar
azulbajo=np.array([115,100,20],np.uint8)
azulalto=np.array([140,255,255],np.uint8)
## matri de trnaformacion para el sistema de cordenadas
T=np.matrix([[0,1,0,-400],[-1,0,0,150],[0,0,1,0],[0,0,0,1]])
## matriz de transformacion para velocidades generales
r=0.0205
L=0.0710
wheelsNames = ['wheelleft1', 'wheelleft2', 'wheelright1', 'wheelright2']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
lidar_front = robot.getLidar("lidar_front")
lidar_front.enable(TIME_STEP)
lidar_left = robot.getLidar("lidar_left")
lidar_left.enable(TIME_STEP)
lidar_right = robot.getLidar("lidar_right")
lidar_right.enable(TIME_STEP)
camera_front = robot.getCamera("camera_front")
camera_front.enable(TIME_STEP)
camera_front.recognitionEnable(TIME_STEP)
camera_left = robot.getCamera("camera_left")
camera_left.enable(TIME_STEP)
camera_left.recognitionEnable(TIME_STEP)
camera_right = robot.getCamera("camera_right")
camera_right.enable(TIME_STEP)
camera_right.recognitionEnable(TIME_STEP)
################################################
#functions for the maze navigation
################################################
yaw = np.tan(rise / run)
return [x, altitude, z, yaw]
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
camera = robot.getCamera("camera")
camera.enable(timestep)
camera.recognitionEnable(timestep)
front_left_led = robot.getLED("front left led")
front_right_led = robot.getLED("front right led")
imu = robot.getInertialUnit("inertial unit")
imu.enable(timestep)
gps = robot.getGPS("gps")
gps.enable(timestep)
compass = robot.getCompass("compass")
compass.enable(timestep)
gyro = robot.getGyro("gyro")
gyro.enable(timestep)
camera_roll_motor = robot.getMotor("camera roll")
camera_pitch_motor = robot.getMotor("camera pitch")
emitter = robot.getEmitter('emitter')
import math
import numpy as np
import struct
import time
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
keyboard = Keyboard()
keyboard.enable(timestep)
receiver = robot.getReceiver("receiver")
receiver.enable(timestep)
imu = robot.getInertialUnit("inertial unit")
imu.enable(timestep)
camera = robot.getCamera("camera")
camera.enable(timestep)
gps = robot.getGPS("gps")
gps.enable(timestep)
compass = robot.getCompass("compass")
compass.enable(timestep)
gyro = robot.getGyro("gyro")
gyro.enable(timestep)
camera_roll_motor = robot.getCamera("camera roll")
camera_pitch_motor = robot.getCamera("camera pitch")
front_left_motor = robot.getMotor("front left propeller")
front_right_motor = robot.getMotor("front right propeller")
rear_left_motor = robot.getMotor("rear left propeller")
rear_right_motor = robot.getMotor("rear right propeller")
motors = [
front_left_motor, front_right_motor, rear_left_motor, rear_right_motor
leftSensors = []
for i in range(3):
sensor = robot.getDistanceSensor('so' + str(i))
leftSensors.append(sensor)
leftSensors[i].enable(timestep)
rightSensors = []
for i in range(3):
sensor = robot.getDistanceSensor('so' + str(5 + i))
rightSensors.append(sensor)
rightSensors[i].enable(timestep)
camera = robot.getCamera('camera')
camera.enable(timestep)
camera.recognitionEnable(timestep)
width = camera.getWidth()
left_wheel.setPosition(999)
right_wheel.setPosition(999)
red_led = [
robot.getLED("red led 1"),
robot.getLED("red led 2"),
robot.getLED("red led 3")
]
delay = 0
from controller import Camera
from bson.objectid import ObjectId
TIME_STEP = 64
EMOTIONS_LIST = ['BALL', 'EMPTY', 'BOX']
model = tf.keras.models.load_model('keras_model.h5')
robot = Robot()
timestep = int(robot.getBasicTimeStep())
keyboard = Keyboard()
keyboard.enable(timestep)
lr = robot.getMotor('linear')
rm = robot.getMotor('RM')
cm = robot.getCamera('CAM')
cm.enable(TIME_STEP)
counter = 0
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
linear = 0.0
rotate = 0.0
c = 0
while robot.step(TIME_STEP) != -1:
if c == 0:
k = ord('A')
else:
