Changeset View
Changeset View
Standalone View
Standalone View
src/applications/caspian/CaspianSoftwareLIDARSensor.php
- This file was added.
<?php | |||||
final class CaspianSoftwareLIDARSensor | |||||
extends CaspianLIDARSensor { | |||||
private $position; | |||||
public function setPosition($position) { | |||||
$this->position = $position; | |||||
return $this; | |||||
} | |||||
public function getPosition() { | |||||
return $this->position; | |||||
} | |||||
public function readState() { | |||||
switch ($this->position) { | |||||
case 'L': | |||||
// Left-mounted LIDAR. Sees ahead, behind, and to the left of the | |||||
// vehicle. Less reliable at the edges. | |||||
$fov = array( | |||||
0.9, | |||||
0.8, | |||||
0.7, | |||||
null, | |||||
0.9, | |||||
null, | |||||
0.9, | |||||
null, | |||||
0.9, | |||||
0.8, | |||||
0.7, | |||||
null, | |||||
); | |||||
break; | |||||
case 'R': | |||||
// Right-mounted LIDAR. | |||||
$fov = array( | |||||
null, | |||||
0.7, | |||||
0.8, | |||||
0.9, | |||||
null, | |||||
0.9, | |||||
null, | |||||
0.9, | |||||
null, | |||||
0.7, | |||||
0.8, | |||||
0.9, | |||||
); | |||||
break; | |||||
} | |||||
$distances = array( | |||||
2, | |||||
6, | |||||
6, | |||||
2, | |||||
2, | |||||
2, | |||||
2, | |||||
2, | |||||
2, | |||||
99, | |||||
99, | |||||
2, | |||||
); | |||||
$result = array(); | |||||
for ($ii = 0; $ii < count($distances); $ii++) { | |||||
// This LIDAR can not see in this direction. | |||||
if ($fov[$ii] === null) { | |||||
$result[] = null; | |||||
continue; | |||||
} | |||||
// True distance to obstacle. | |||||
$reading = $distances[$ii]; | |||||
// Add uncertainty to the sensor reading. | |||||
$error_abs = ($reading * (1 - $fov[$ii])); | |||||
$error_rel = (mt_rand(-500, 500) / 1000.0); | |||||
$reading = $reading + ($error_abs * $error_rel); | |||||
// Sensor reading is a combination of actual value and sensor | |||||
// uncertainty. | |||||
$result[] = array( | |||||
'reading' => $reading, | |||||
'certainty' => $fov[$ii], | |||||
); | |||||
} | |||||
return $result; | |||||
} | |||||
} |