diff --git a/webots/python/controllers/LidarStuff/LidarStuff.py b/webots/python/controllers/LidarStuff/LidarStuff.py new file mode 100644 index 0000000..f7bb2fa --- /dev/null +++ b/webots/python/controllers/LidarStuff/LidarStuff.py @@ -0,0 +1,43 @@ +# Copyright 1996-2021 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. + +""" +This controller gives to its robot the following behavior: +According to the messages it receives, the robot change its +behavior. +""" + +from controller import Lidar +from controller import DistanceSensor +from controller import Robot + +TIME_STEP = 32 + +robot = Robot() + +us0 = DistanceSensor("us0") +us1 = DistanceSensor("us1") +us0.enable(TIME_STEP) +us1.enable(TIME_STEP) + + +lidar = Lidar("lidar") +lidar.enable(TIME_STEP) +lidar.enablePointCloud() +print(lidar.getFrequency()) +while robot.step(32) != -1: + print(lidar) +# print(help(lidar)) +# lidar = robot.getDevice("lidar") +# lidar.enable(TIME_STEP) \ No newline at end of file diff --git a/webots/python/controllers/LidarStuff/runtime.ini b/webots/python/controllers/LidarStuff/runtime.ini new file mode 100644 index 0000000..c7e973e --- /dev/null +++ b/webots/python/controllers/LidarStuff/runtime.ini @@ -0,0 +1,2 @@ +[environment variables with paths] +PYTHONPATH=$(WEBOTS_HOME)/projects/languages/python/controllers/driver/ diff --git a/webots/python/controllers/driver/common.py b/webots/python/controllers/driver/common.py new file mode 100644 index 0000000..f51de63 --- /dev/null +++ b/webots/python/controllers/driver/common.py @@ -0,0 +1,17 @@ +# Copyright 1996-2021 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. + + +def common_print(caller): + print('This module is common to both driver and slave controllers (called from ' + caller + ').') diff --git a/webots/python/controllers/driver/driver.py b/webots/python/controllers/driver/driver.py new file mode 100644 index 0000000..44ed6b9 --- /dev/null +++ b/webots/python/controllers/driver/driver.py @@ -0,0 +1,91 @@ +# Copyright 1996-2021 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. + +""" +This controller gives to its node the following behavior: +Listen the keyboard. According to the pressed key, send a +message through an emitter or handle the position of Robot1. +""" + +from controller import Supervisor +from common import common_print + + +class Driver (Supervisor): + timeStep = 128 + x = 0.1 + z = 0.3 + translation = [x, 0.0, z] + + def __init__(self): + super(Driver, self).__init__() + self.emitter = self.getDevice('emitter') + robot = self.getFromDef('ROBOT1') + self.translationField = robot.getField('translation') + self.keyboard.enable(Driver.timeStep) + self.keyboard = self.getKeyboard() + + def run(self): + self.displayHelp() + previous_message = '' + + # Main loop. + while True: + # Deal with the pressed keyboard key. + k = self.keyboard.getKey() + message = '' + if k == ord('A'): + message = 'avoid obstacles' + elif k == ord('F'): + message = 'move forward' + elif k == ord('S'): + message = 'stop' + elif k == ord('T'): + message = 'turn' + elif k == ord('I'): + self.displayHelp() + elif k == ord('G'): + translationValues = self.translationField.getSFVec3f() + print('ROBOT1 is located at (' + str(translationValues[0]) + ',' + str(translationValues[2]) + ')') + elif k == ord('R'): + print('Teleport ROBOT1 at (' + str(self.x) + ',' + str(self.z) + ')') + self.translationField.setSFVec3f(self.translation) + + # Send a new message through the emitter device. + if message != '' and message != previous_message: + previous_message = message + print('Please, ' + message) + self.emitter.send(message.encode('utf-8')) + + # Perform a simulation step, quit the loop when + # Webots is about to quit. + if self.step(self.timeStep) == -1: + break + + def displayHelp(self): + print( + 'Commands:\n' + ' I for displaying the commands\n' + ' A for avoid obstacles\n' + ' F for move forward\n' + ' S for stop\n' + ' T for turn\n' + ' R for positioning ROBOT1 at (0.1,0.3)\n' + ' G for knowing the (x,z) position of ROBOT1' + ) + + +controller = Driver() +common_print('driver') +controller.run() diff --git a/webots/python/controllers/slave/runtime.ini b/webots/python/controllers/slave/runtime.ini new file mode 100644 index 0000000..c7e973e --- /dev/null +++ b/webots/python/controllers/slave/runtime.ini @@ -0,0 +1,2 @@ +[environment variables with paths] +PYTHONPATH=$(WEBOTS_HOME)/projects/languages/python/controllers/driver/ diff --git a/webots/python/controllers/slave/slave.py b/webots/python/controllers/slave/slave.py new file mode 100644 index 0000000..adfea94 --- /dev/null +++ b/webots/python/controllers/slave/slave.py @@ -0,0 +1,37 @@ +# Copyright 1996-2021 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. + +""" +This controller gives to its robot the following behavior: +According to the messages it receives, the robot change its +behavior. +""" + +from controller import Lidar +from controller import Robot +from common import common_print + + +class LidarStuff (Robot): + + timeStep = 32 + maxSpeed = 10.0 + motors = [] + distanceSensors = [] + + + +controller = Slave() +common_print('slave') +controller.run() diff --git a/webots/python/worlds/.example.wbproj b/webots/python/worlds/.example.wbproj new file mode 100644 index 0000000..c763000 --- /dev/null +++ b/webots/python/worlds/.example.wbproj @@ -0,0 +1,13 @@ +Webots Project File version R2021b +perspectives: 000000ff00000000fd000000040000000000000069000003d6fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000000000010000023a0000039bfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000039b0000008900ffffff000000020000078000000242fc0100000001fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000007800000000000000000000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000005440000039b00000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000002010000000101 +sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 +maximizedDockId: -1 +centralWidgetVisible: 1 +projectionMode: PERSPECTIVE +renderingMode: PLAIN +orthographicViewHeight: 1.99796 +textFiles: 0 "controllers/LidarStuff/LidarStuff.py" +globalOptionalRendering: LidarRaysPaths::LidarPointClouds::DistanceSensorRays +consoles: Console:All:All +renderingDevicePerspectives: robot1:camera;1;1.40625;0;0 diff --git a/webots/python/worlds/example.wbt b/webots/python/worlds/example.wbt new file mode 100644 index 0000000..bd9c218 --- /dev/null +++ b/webots/python/worlds/example.wbt @@ -0,0 +1,347 @@ +#VRML_SIM R2021b utf8 +WorldInfo { + info [ + "The user drives a Supervisor by the Keyboard which drives slaves robots by using an emitter device." + ] + title "Omniscience" + coordinateSystem "NUE" + lineScale 0.15 +} +Viewpoint { + orientation 0.011982571754036471 0.990420797366239 0.13756112139184917 3.2259807768666806 + position -1.4961382249248047 1.1886174880513347 -2.8567792616605474 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +CircleArena { +} +WoodenBox { + translation -0.06339 0.05 0.372612 + rotation 0 1 0 0.5 + size 0.1 0.1 0.1 +} +WoodenBox { + translation 0.449512 0.05 -0.488552 + rotation 0 1 0 4.96782 + name "wooden box(1)" + size 0.1 0.1 0.1 +} +WoodenBox { + translation 0.5 0.05 0.35471 + rotation 0 1 0 5.36782 + name "wooden box(2)" + size 0.1 0.1 0.1 +} +WoodenBox { + translation -0.3 0.05 -0.36 + rotation 0 1 0 5.36782 + name "wooden box(3)" + size 0.1 0.1 0.1 +} +WoodenBox { + translation -0.645944 0.05 0.192254 + rotation 0 1 0 5.36782 + name "wooden box(4)" + size 5 10 0.1 +} +DEF ROBOT1 Robot { + translation -0.36230599992105533 -0.00023914673412048226 0.14025099964199306 + rotation 0.0021163498445359464 -0.9999977321736229 0.00023814060314790065 0.22175448381340965 + children [ + DEF LIDAR Lidar { + translation 0 0.12 0 + rotation 0 1 0 0.523599 + children [ + Transform { + translation 0 -0.01 0 + rotation 1 0 0 -1.5708 + scale 6 5 3 + children [ + DEF STRUCTURE_AXES Shape { + appearance Appearance { + material Material { + diffuseColor 0.02 0.02 0.02 + } + } + geometry IndexedLineSet { + coord Coordinate { + point [ + 0 0 0.01 + 0.01 0 -0.08 + -0.01 0 -0.08 + 0 0.01 -0.08 + 0 -0.01 -0.08 + ] + } + coordIndex [ + 0, 1, 0, 2, 0, 3, 0, 4 + ] + } + } + ] + } + ] + tiltAngle -0.1 + horizontalResolution 256 + fieldOfView 1.57 + numberOfLayers 6 + near 0.05 + minRange 0.05 + maxRange 8 + type "rotating" + noise 0.1 + defaultFrequency 2 + rotatingHead Solid { + children [ + Transform { + translation 0 0 -0.03 + children [ + DEF LENS Shape { + appearance PBRAppearance { + baseColor 0 0 0 + roughness 1.1102230246251565e-16 + metalness 0 + } + geometry Sphere { + radius 0.015 + subdivision 2 + } + } + ] + } + DEF CAMERA_SHAPE Transform { + rotation 1 0 0 1.57 + children [ + DEF CAMERA_OUTLINE Shape { + appearance PBRAppearance { + baseColor 0.898039 0.898039 0.898039 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.06 + radius 0.024 + } + } + ] + } + ] + } + } + DEF MAIN_BODY Transform { + translation 0 0.0415 0 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.0820075 0.364731 0.8 + roughness 1 + metalness 0 + } + geometry DEF BODY Cylinder { + height 0.08 + radius 0.045 + } + } + ] + } + HingeJoint { + jointParameters HingeJointParameters { + position -1.7866352851730753e-07 + axis -1 0 0 + anchor 0 0.025 0 + } + device [ + RotationalMotor { + name "left wheel motor" + } + PositionSensor { + name "left wheel sensor" + } + ] + endPoint DEF LEFT_WHEEL Solid { + translation -0.045 0.025 0 + rotation 1 0 0 4.984670178663533 + children [ + DEF WHEEL Transform { + rotation 0 0 1 1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 1 0 0 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.025 + } + } + ] + } + ] + name "left wheel" + boundingObject DEF WHEEL Transform { + rotation 0 0 1 1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 1 0 0 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.025 + } + } + ] + } + physics DEF PHYSICS_WHEEL Physics { + density -1 + mass 0.05 + } + } + } + HingeJoint { + jointParameters HingeJointParameters { + position 1.7866606142223756e-07 + axis -1 0 0 + anchor 0 0.025 0 + } + device [ + RotationalMotor { + name "right wheel motor" + } + PositionSensor { + name "right wheel sensor" + } + ] + endPoint DEF RIGHT_WHEEL Solid { + translation 0.045 0.025 0 + rotation 1 0 0 4.587349821333938 + children [ + DEF WHEEL Transform { + rotation 0 0 1 1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 1 0 0 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.025 + } + } + ] + } + ] + name "right wheel" + boundingObject USE WHEEL + physics DEF PHYSICS_WHEEL Physics { + density -1 + mass 0.05 + } + } + } + DEF LEFT_EYE DistanceSensor { + translation -0.02 0.063 -0.042 + rotation 0 1 0 2.07 + children [ + DEF INFRARED Transform { + rotation 0 0 1 1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.975691 0.981481 0.0252992 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.004 + radius 0.008 + } + } + ] + } + ] + name "ds0" + lookupTable [ + 0 1024 0 + 0.05 1024 0 + 0.15 0 0 + ] + numberOfRays 2 + aperture 1 + } + DEF RIGHT_EYE DistanceSensor { + translation 0.02 0.063 -0.042 + rotation 0 1 0 1.07 + children [ + DEF INFRARED Transform { + rotation 0 0 1 1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.975691 0.981481 0.0252992 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.004 + radius 0.008 + } + } + ] + } + ] + name "ds1" + lookupTable [ + 0 1024 0 + 0.05 1024 0 + 0.15 0 0 + ] + numberOfRays 2 + aperture 1 + } + Camera { + translation 0 0.045 -0.045 + children [ + Transform { + rotation 1 0 0 1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.8 0.8 0.8 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.007 + } + } + ] + } + ] + } + ] + name "robot1" + boundingObject DEF MYBOT_BOUNDING Transform { + translation 0 0.0415 0 + children [ + DEF BODY Cylinder { + height 0.08 + radius 0.045 + } + ] + } + physics DEF MYBOT_PHYSICS Physics { + density -1 + mass 0.5 + } + controller "LidarStuff" +}