Initial webots lidar stuff

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)

webots/python/controllers/LidarStuff/runtime.ini

@@ -0,0 +1,2 @@
+[environment variables with paths]
+PYTHONPATH=$(WEBOTS_HOME)/projects/languages/python/controllers/driver/

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 + ').')

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()

webots/python/controllers/slave/runtime.ini

@@ -0,0 +1,2 @@
+[environment variables with paths]
+PYTHONPATH=$(WEBOTS_HOME)/projects/languages/python/controllers/driver/

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()

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

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"
+}