Skip to content

sensor

LidarSensor #

Bases: Sensor

Specialized LIDAR sensor.

Its impact entity is represented by a small blue sphere.

Source code in cogip/entities/sensor.py 
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
class LidarSensor(Sensor):
    """
    Specialized LIDAR sensor.

    Its impact entity is represented by a small blue sphere.
    """

    nb_lidar_sensors = 0

    def __init__(
        self,
        asset_entity: AssetEntity,
        name: str,
        origin_x: int,
        origin_y: int,
        direction_x: int,
        direction_y: int,
    ):
        """
        Class constructor.

        Arguments:
            asset_entity: Entity containing the sensor
            name: Name of the sensor
            origin_x: X origin of the ray caster
            origin_y: Y origin of the ray caster
            direction_x: X direction of the ray caster
            direction_y: Y direction of the ray caster
        """

        super().__init__(
            asset_entity=asset_entity,
            name=name,
            origin_x=origin_x,
            origin_y=origin_y,
            origin_z=360,
            direction_x=direction_x,
            direction_y=direction_y,
            direction_z=0,
            impact_radius=20,
            impact_color=QtCore.Qt.cyan,
        )

        self.lidar_id = LidarSensor.nb_lidar_sensors
        LidarSensor.nb_lidar_sensors += 1

__init__(asset_entity, name, origin_x, origin_y, direction_x, direction_y) #

Class constructor.

Parameters:

Name Type Description Default
asset_entity AssetEntity

Entity containing the sensor

 required
name str

Name of the sensor

 required
origin_x int

X origin of the ray caster

 required
origin_y int

Y origin of the ray caster

 required
direction_x int

X direction of the ray caster

 required
direction_y int

Y direction of the ray caster

 required
Source code in cogip/entities/sensor.py 
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
def __init__(
    self,
    asset_entity: AssetEntity,
    name: str,
    origin_x: int,
    origin_y: int,
    direction_x: int,
    direction_y: int,
):
    """
    Class constructor.

    Arguments:
        asset_entity: Entity containing the sensor
        name: Name of the sensor
        origin_x: X origin of the ray caster
        origin_y: Y origin of the ray caster
        direction_x: X direction of the ray caster
        direction_y: Y direction of the ray caster
    """

    super().__init__(
        asset_entity=asset_entity,
        name=name,
        origin_x=origin_x,
        origin_y=origin_y,
        origin_z=360,
        direction_x=direction_x,
        direction_y=direction_y,
        direction_z=0,
        impact_radius=20,
        impact_color=QtCore.Qt.cyan,
    )

    self.lidar_id = LidarSensor.nb_lidar_sensors
    LidarSensor.nb_lidar_sensors += 1

Sensor #

Bases: QObject

Base class for all sensors.

The sensors are based on QRayCaster. It casts a ray and detects collisions with obstacles. Detected collision is represented using a ImpactEntity object.

Attributes:

Name Type Description
obstacles list[QEntity]

Class attribute recording all entities that should be detected
all_sensors list[Sensor]

Class attribute recording all sensors
hit QRayCasterHit | None

Last hit of this sensor
Source code in cogip/entities/sensor.py 
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
class Sensor(QtCore.QObject):
    """
    Base class for all sensors.

    The sensors are based on [QRayCaster](https://doc.qt.io/qtforpython-6/PySide6/Qt3DRender/QRayCaster.html).
    It casts a ray and detects collisions with obstacles.
    Detected collision is represented using a [ImpactEntity][cogip.entities.impact.ImpactEntity] object.

    Attributes:
        obstacles: Class attribute recording all entities that should be detected
        all_sensors: Class attribute recording all sensors
        hit: Last hit of this sensor
    """

    obstacles: list[Qt3DCore.QEntity] = []
    all_sensors: list["Sensor"] = []
    hit: Qt3DRender.QRayCasterHit | None = None

    def __init__(
        self,
        asset_entity: AssetEntity,
        name: str,
        origin_x: int,
        origin_y: int,
        origin_z: int,
        direction_x: int,
        direction_y: int,
        direction_z: int,
        impact_radius: float = 50,
        impact_color: QtCore.Qt.GlobalColor = QtCore.Qt.red,
    ):
        """
        Class constructor.

        Origin and direction are relative to the parent entity (the robot).

        Arguments:
            asset_entity: Entity containing the sensor
            name: Name of the sensor
            origin_x: X origin of the ray caster
            origin_y: Y origin of the ray caster
            origin_z: Z origin of the ray caster
            direction_x: X direction of the ray caster
            direction_y: Y direction of the ray caster
            direction_z: Z direction of the ray caster
            impact_radius: Radius of the `ImpactEntity` representing the collision
            impact_color: Color of the `ImpactEntity` representing the collision
        """
        super().__init__()

        Sensor.all_sensors.append(self)

        self.origin_x = origin_x
        self.origin_y = origin_y
        self.asset_entity = asset_entity
        self.name = name

        self.ray_caster = Qt3DRender.QRayCaster()
        self.ray_caster.setEnabled(False)  # Start casting only when the first obstacle is registered
        self.ray_caster.setLength(0)  # Infinite
        self.ray_caster.setRunMode(Qt3DRender.QAbstractRayCaster.Continuous)
        self.ray_caster.setFilterMode(Qt3DRender.QAbstractRayCaster.AcceptAnyMatchingLayers)
        self.ray_caster.setOrigin(QtGui.QVector3D(float(origin_x), float(origin_y), float(origin_z)))
        self.ray_caster.setDirection(QtGui.QVector3D(direction_x, direction_y, direction_z))
        self.asset_entity.addComponent(self.ray_caster)

        # Add layers for obstacles already present
        for obstacle in self.obstacles:
            self.add_obstacle_layer(obstacle)

        # Add impact entity
        self.impact_entity = ImpactEntity(radius=impact_radius, color=impact_color)
        self.impact_entity.setParent(self.asset_entity.parent().parent().parent())

    @qtSlot()
    def update_hit(self):
        """
        Qt Slot

        Compute the distance with the closest detected obstacle.
        """
        distances = [hit for hit in self.ray_caster.hits() if hit.distance() != 0.0]
        self.hit = None
        if len(distances):
            self.hit = min(distances, key=lambda x: x.distance())

        self.update_impact()

    def update_impact(self):
        """
        Display the impact entity at the collision point.
        """
        if self.hit:
            self.impact_entity.setEnabled(True)
            self.impact_entity.transform.setTranslation(self.hit.worldIntersection())
        else:
            self.impact_entity.setEnabled(False)

    @classmethod
    def add_obstacle(cls, obstacle: Qt3DCore.QEntity):
        """
        Class method.

        Register an obstacle added on the table.

        Arguments:
            obstacle: The obstacle to register
        """
        cls.obstacles.append(obstacle)
        for sensor in cls.all_sensors:
            sensor.add_obstacle_layer(obstacle)

    def add_obstacle_layer(self, obstacle: Qt3DCore.QEntity):
        """
        Add the obstacle layer to the ray caster.
        This allows the obstacle to be detected by the ray caster.

        Arguments:
            obstacle: The obstacle to detect
        """
        self.ray_caster.addLayer(obstacle.layer)
        # Activate if not already done
        self.ray_caster.trigger()

    @property
    def distance(self) -> int:
        """
        Last sensor hit distance if any.
        """
        dist = 65535
        if self.hit:
            dist = self.hit.distance()
            dist += math.dist((0, 0), (self.origin_x, self.origin_y))
        return int(dist)

distance: int property #

Last sensor hit distance if any.

__init__(asset_entity, name, origin_x, origin_y, origin_z, direction_x, direction_y, direction_z, impact_radius=50, impact_color=QtCore.Qt.red) #

Class constructor.

Origin and direction are relative to the parent entity (the robot).

Parameters:

Name Type Description Default
asset_entity AssetEntity

Entity containing the sensor

 required
name str

Name of the sensor

 required
origin_x int

X origin of the ray caster

 required
origin_y int

Y origin of the ray caster

 required
origin_z int

Z origin of the ray caster

 required
direction_x int

X direction of the ray caster

 required
direction_y int

Y direction of the ray caster

 required
direction_z int

Z direction of the ray caster

 required
impact_radius float

Radius of the ImpactEntity representing the collision

 50
impact_color GlobalColor

Color of the ImpactEntity representing the collision

 red
Source code in cogip/entities/sensor.py 
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
def __init__(
    self,
    asset_entity: AssetEntity,
    name: str,
    origin_x: int,
    origin_y: int,
    origin_z: int,
    direction_x: int,
    direction_y: int,
    direction_z: int,
    impact_radius: float = 50,
    impact_color: QtCore.Qt.GlobalColor = QtCore.Qt.red,
):
    """
    Class constructor.

    Origin and direction are relative to the parent entity (the robot).

    Arguments:
        asset_entity: Entity containing the sensor
        name: Name of the sensor
        origin_x: X origin of the ray caster
        origin_y: Y origin of the ray caster
        origin_z: Z origin of the ray caster
        direction_x: X direction of the ray caster
        direction_y: Y direction of the ray caster
        direction_z: Z direction of the ray caster
        impact_radius: Radius of the `ImpactEntity` representing the collision
        impact_color: Color of the `ImpactEntity` representing the collision
    """
    super().__init__()

    Sensor.all_sensors.append(self)

    self.origin_x = origin_x
    self.origin_y = origin_y
    self.asset_entity = asset_entity
    self.name = name

    self.ray_caster = Qt3DRender.QRayCaster()
    self.ray_caster.setEnabled(False)  # Start casting only when the first obstacle is registered
    self.ray_caster.setLength(0)  # Infinite
    self.ray_caster.setRunMode(Qt3DRender.QAbstractRayCaster.Continuous)
    self.ray_caster.setFilterMode(Qt3DRender.QAbstractRayCaster.AcceptAnyMatchingLayers)
    self.ray_caster.setOrigin(QtGui.QVector3D(float(origin_x), float(origin_y), float(origin_z)))
    self.ray_caster.setDirection(QtGui.QVector3D(direction_x, direction_y, direction_z))
    self.asset_entity.addComponent(self.ray_caster)

    # Add layers for obstacles already present
    for obstacle in self.obstacles:
        self.add_obstacle_layer(obstacle)

    # Add impact entity
    self.impact_entity = ImpactEntity(radius=impact_radius, color=impact_color)
    self.impact_entity.setParent(self.asset_entity.parent().parent().parent())

add_obstacle(obstacle) classmethod #

Class method.

Register an obstacle added on the table.

Parameters:

Name Type Description Default
obstacle QEntity

The obstacle to register

 required
Source code in cogip/entities/sensor.py 
113
114
115
116
117
118
119
120
121
122
123
124
125
@classmethod
def add_obstacle(cls, obstacle: Qt3DCore.QEntity):
    """
    Class method.

    Register an obstacle added on the table.

    Arguments:
        obstacle: The obstacle to register
    """
    cls.obstacles.append(obstacle)
    for sensor in cls.all_sensors:
        sensor.add_obstacle_layer(obstacle)

add_obstacle_layer(obstacle) #

Add the obstacle layer to the ray caster. This allows the obstacle to be detected by the ray caster.

Parameters:

Name Type Description Default
obstacle QEntity

The obstacle to detect

 required
Source code in cogip/entities/sensor.py 
127
128
129
130
131
132
133
134
135
136
137
def add_obstacle_layer(self, obstacle: Qt3DCore.QEntity):
    """
    Add the obstacle layer to the ray caster.
    This allows the obstacle to be detected by the ray caster.

    Arguments:
        obstacle: The obstacle to detect
    """
    self.ray_caster.addLayer(obstacle.layer)
    # Activate if not already done
    self.ray_caster.trigger()

update_hit() #

Qt Slot

Compute the distance with the closest detected obstacle.

Source code in cogip/entities/sensor.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
@qtSlot()
def update_hit(self):
    """
    Qt Slot

    Compute the distance with the closest detected obstacle.
    """
    distances = [hit for hit in self.ray_caster.hits() if hit.distance() != 0.0]
    self.hit = None
    if len(distances):
        self.hit = min(distances, key=lambda x: x.distance())

    self.update_impact()

update_impact() #

Display the impact entity at the collision point.

Source code in cogip/entities/sensor.py 
103
104
105
106
107
108
109
110
111
def update_impact(self):
    """
    Display the impact entity at the collision point.
    """
    if self.hit:
        self.impact_entity.setEnabled(True)
        self.impact_entity.transform.setTranslation(self.hit.worldIntersection())
    else:
        self.impact_entity.setEnabled(False)

ToFSensor #

Bases: Sensor

Specialized ToF sensor.

It is represented by a small red cube placed at the origin of the ray caster.

Its impact entity is represented by a small red sphere.

Source code in cogip/entities/sensor.py 
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
class ToFSensor(Sensor):
    """
    Specialized ToF sensor.

    It is represented by a small red cube placed at the origin of the ray caster.

    Its impact entity is represented by a small red sphere.
    """

    nb_tof_sensors = 0

    def __init__(
        self,
        asset_entity: AssetEntity,
        name: str,
        origin_x: int,
        origin_y: int,
    ):
        """
        Class constructor.

        Arguments:
            asset_entity: Entity containing the sensor
            name: Name of the sensor
            origin_x: X origin of the ray caster
            origin_y: Y origin of the ray caster
        """
        super().__init__(
            asset_entity=asset_entity,
            name=name,
            origin_x=origin_x,
            origin_y=origin_y,
            origin_z=60,
            direction_x=origin_x,
            direction_y=origin_y,
            direction_z=0,
            impact_radius=50,
            impact_color=QtCore.Qt.red,
        )
        self.tof_id = ToFSensor.nb_tof_sensors
        ToFSensor.nb_tof_sensors += 1

        rotation = math.degrees(math.acos(origin_x / math.dist((0, 0), (origin_x, origin_y))))

        self.entity = Qt3DCore.QEntity()
        self.entity.setParent(self.asset_entity)

        self.mesh = Qt3DExtras.QCuboidMesh()
        self.mesh.setXExtent(10)
        self.mesh.setYExtent(10)
        self.mesh.setZExtent(10)
        self.entity.addComponent(self.mesh)

        self.material = Qt3DExtras.QPhongMaterial()
        self.material.setDiffuse(QtGui.QColor(QtCore.Qt.red))
        self.entity.addComponent(self.material)

        self.transform = Qt3DCore.QTransform()
        self.transform.setTranslation(QtGui.QVector3D(origin_x, origin_y, 60))
        self.transform.setRotationZ(rotation)
        self.entity.addComponent(self.transform)

__init__(asset_entity, name, origin_x, origin_y) #

Class constructor.

Parameters:

Name Type Description Default
asset_entity AssetEntity

Entity containing the sensor

 required
name str

Name of the sensor

 required
origin_x int

X origin of the ray caster

 required
origin_y int

Y origin of the ray caster

 required
Source code in cogip/entities/sensor.py 
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
def __init__(
    self,
    asset_entity: AssetEntity,
    name: str,
    origin_x: int,
    origin_y: int,
):
    """
    Class constructor.

    Arguments:
        asset_entity: Entity containing the sensor
        name: Name of the sensor
        origin_x: X origin of the ray caster
        origin_y: Y origin of the ray caster
    """
    super().__init__(
        asset_entity=asset_entity,
        name=name,
        origin_x=origin_x,
        origin_y=origin_y,
        origin_z=60,
        direction_x=origin_x,
        direction_y=origin_y,
        direction_z=0,
        impact_radius=50,
        impact_color=QtCore.Qt.red,
    )
    self.tof_id = ToFSensor.nb_tof_sensors
    ToFSensor.nb_tof_sensors += 1

    rotation = math.degrees(math.acos(origin_x / math.dist((0, 0), (origin_x, origin_y))))

    self.entity = Qt3DCore.QEntity()
    self.entity.setParent(self.asset_entity)

    self.mesh = Qt3DExtras.QCuboidMesh()
    self.mesh.setXExtent(10)
    self.mesh.setYExtent(10)
    self.mesh.setZExtent(10)
    self.entity.addComponent(self.mesh)

    self.material = Qt3DExtras.QPhongMaterial()
    self.material.setDiffuse(QtGui.QColor(QtCore.Qt.red))
    self.entity.addComponent(self.material)

    self.transform = Qt3DCore.QTransform()
    self.transform.setTranslation(QtGui.QVector3D(origin_x, origin_y, 60))
    self.transform.setRotationZ(rotation)
    self.entity.addComponent(self.transform)