webots+python多机器人仿真

# Copyright 1996-2022 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import ctypes import sys import typing from .wb import wb from .node import Node from .device import Device from .accelerometer import Accelerometer from .altimeter import Altimeter from .brake import Brake from .camera import Camera from .compass import Compass from .connector import Connector from .display import Display from .distance_sensor import DistanceSensor from .emitter import Emitter from .gps import GPS from .gyro import Gyro from .inertial_unit import InertialUnit from .led import LED from .lidar import Lidar from .light_sensor import LightSensor from .motor import Motor from .pen import Pen from .position_sensor import PositionSensor from .radar import Radar from .range_finder import RangeFinder from .receiver import Receiver from .skin import Skin from .speaker import Speaker from .touch_sensor import TouchSensor from .joystick import Joystick from .keyboard import Keyboard from .mouse import Mouse class Robot: EVENT_QUIT = -1 EVENT_NO_EVENT = 0 EVENT_MOUSE_CLICK = 1 EVENT_MOUSE_MOVE = 2 EVENT_KEYBOARD = 4 EVENT_JOYSTICK_BUTTON = 8 EVENT_JOYSTICK_AXIS = 16 EVENT_JOYSTICK_POV = 32 MODE_SIMULATION = 0 MODE_CROSS_COMPILATION = 1 MODE_REMOTE_CONTROL = 2 created = None wb.wb_robot_get_name.restype = ctypes.c_char_p wb.wb_robot_get_model.restype = ctypes.c_char_p wb.wb_robot_get_custom_data.restype = ctypes.c_char_p wb.wb_robot_get_basic_time_step.restype = ctypes.c_double wb.wb_robot_get_time.restype = ctypes.c_double wb.wb_robot_get_name.restype = ctypes.c_char_p wb.wb_robot_battery_sensor_get_value.restype = ctypes.c_double wb.wb_robot_wwi_receive_text.restype = ctypes.c_char_p wb.wb_robot_get_urdf.restype = ctypes.c_char_p wb.wb_robot_get_project_path.restype = ctypes.c_char_p wb.wb_robot_get_world_path.restype = ctypes.c_char_p def __init__(self): if Robot.created: print('Error: only one Robot instance can be created per controller process.', file=sys.stderr) return Robot.created = self wb.wb_robot_init() self.devices = {} n = wb.wb_robot_get_number_of_devices() for i in range(0, n): tag = wb.wb_robot_get_device_by_index(i) name = wb.wb_device_get_name(tag).decode() type = wb.wb_device_get_node_type(tag) if type == Node.ACCELEROMETER: self.devices[name] = Accelerometer(tag) elif type == Node.ALTIMETER: self.devices[name] = Altimeter(name) elif type == Node.BRAKE: self.devices[name] = Brake(tag) elif type == Node.CAMERA: self.devices[name] = Camera(tag) elif type == Node.COMPASS: self.devices[name] = Compass(tag) elif type == Node.CONNECTOR: self.devices[name] = Connector(tag) elif type == Node.DISPLAY: self.devices[name] = Display(tag) elif type == Node.DISTANCE_SENSOR: self.devices[name] = DistanceSensor(tag) elif type == Node.EMITTER: self.devices[name] = Emitter(tag) elif type == Node.GPS: self.devices[name] = GPS(tag) elif type == Node.GYRO: self.devices[name] = Gyro(tag) elif type == Node.INERTIAL_UNIT: self.devices[name] = InertialUnit(tag) elif type == Node.LED: self.devices[name] = LED(tag) elif type == Node.LIDAR: self.devices[name] = Lidar(tag) elif type == Node.LIGHT_SENSOR: self.devices[name] = LightSensor(tag) elif type == Node.LINEAR_MOTOR or type == Node.ROTATIONAL_MOTOR: self.devices[name] = Motor(tag) elif type == Node.PEN: self.devices[name] = Pen(tag) elif type == Node.POSITION_SENSOR: self.devices[name] = PositionSensor(tag) elif type == Node.RADAR: self.devices[name] = Radar(tag) elif type == Node.RANGE_FINDER: self.devices[name] = RangeFinder(tag) elif type == Node.RECEIVER: self.devices[name] = Receiver(tag) elif type == Node.SKIN: self.devices[name] = Skin(tag) elif type == Node.SPEAKER: self.devices[name] = Speaker(tag) elif type == Node.TOUCH_SENSOR: self.devices[name] = TouchSensor(tag) else: print('Unsupported device type: ' + str(type) + ' for device named "' + name + '"', file=sys.stderr) self.keyboard = Keyboard(0) self.mouse = Mouse(0) self.joystick = Joystick(0) def __del__(self): wb.wb_robot_cleanup() def getAccelerometer(self, name: str) -> Accelerometer: print('DEPRECATION: Robot.getAccelerometer is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getAltimeter(self, name: str) -> Altimeter: print('DEPRECATION: Robot.getAltimeter is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getBrake(self, name: str) -> Brake: print('DEPRECATION: Robot.getBrake is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getCamera(self, name: str) -> Camera: print('DEPRECATION: Robot.getCamera is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getCompass(self, name: str) -> Compass: print('DEPRECATION: Robot.getCompass is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getConnector(self, name: str) -> Connector: print('DEPRECATION: Robot.getConnector is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getDisplay(self, name: str) -> Display: print('DEPRECATION: Robot.getDisplay is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getDistanceSensor(self, name: str) -> DistanceSensor: print('DEPRECATION: Robot.getDistanceSensor is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getEmitter(self, name: str) -> Emitter: print('DEPRECATION: Robot.getEmitter is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getGPS(self, name: str) -> GPS: print('DEPRECATION: Robot.getGPS is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getGyro(self, name: str) -> Gyro: print('DEPRECATION: Robot.getGyro is deprecated, please use Robot.getDevice instead.', file=sys.stderr) return self.getDevice(name) def getInertialUnit(self, name: str) -> InertialUnit: print('DEPRECATION: Robot.getInertialUnit is deprecated, please use Robo