diff --git a/bin/caspian b/bin/caspian new file mode 120000 --- /dev/null +++ b/bin/caspian @@ -0,0 +1 @@ +../scripts/setup/manage_caspian.php \ No newline at end of file diff --git a/scripts/setup/manage_caspian.php b/scripts/setup/manage_caspian.php new file mode 100755 --- /dev/null +++ b/scripts/setup/manage_caspian.php @@ -0,0 +1,21 @@ +#!/usr/bin/env php +setTagline(pht('manage Caspian control systems')); +$args->setSynopsis(<<parseStandardArguments(); + +$workflows = id(new PhutilClassMapQuery()) + ->setAncestorClass('CaspianWorkflow') + ->execute(); +$workflows[] = new PhutilHelpArgumentWorkflow(); +$args->parseWorkflows($workflows); diff --git a/src/__phutil_library_map__.php b/src/__phutil_library_map__.php --- a/src/__phutil_library_map__.php +++ b/src/__phutil_library_map__.php @@ -275,6 +275,17 @@ 'BulkTokenizerParameterType' => 'applications/transactions/bulk/type/BulkTokenizerParameterType.php', 'CalendarTimeUtil' => 'applications/calendar/util/CalendarTimeUtil.php', 'CalendarTimeUtilTestCase' => 'applications/calendar/__tests__/CalendarTimeUtilTestCase.php', + 'CaspianAnalyzer' => 'applications/caspian/CaspianAnalyzer.php', + 'CaspianDriveWorkflow' => 'applications/caspian/CaspianDriveWorkflow.php', + 'CaspianDriver' => 'applications/caspian/CaspianDriver.php', + 'CaspianLIDARAnalyzer' => 'applications/caspian/CaspianLIDARAnalyzer.php', + 'CaspianLIDARSensor' => 'applications/caspian/CaspianLIDARSensor.php', + 'CaspianLIDARView' => 'applications/caspian/CaspianLIDARView.php', + 'CaspianSensor' => 'applications/caspian/CaspianSensor.php', + 'CaspianSimulation' => 'applications/caspian/CaspianSimulation.php', + 'CaspianSoftwareLIDARSensor' => 'applications/caspian/CaspianSoftwareLIDARSensor.php', + 'CaspianState' => 'applications/caspian/CaspianState.php', + 'CaspianWorkflow' => 'applications/caspian/CaspianWorkflow.php', 'CelerityAPI' => 'applications/celerity/CelerityAPI.php', 'CelerityDarkModePostprocessor' => 'applications/celerity/postprocessor/CelerityDarkModePostprocessor.php', 'CelerityDefaultPostprocessor' => 'applications/celerity/postprocessor/CelerityDefaultPostprocessor.php', @@ -5594,6 +5605,17 @@ 'BulkTokenizerParameterType' => 'BulkParameterType', 'CalendarTimeUtil' => 'Phobject', 'CalendarTimeUtilTestCase' => 'PhabricatorTestCase', + 'CaspianAnalyzer' => 'Phobject', + 'CaspianDriveWorkflow' => 'CaspianWorkflow', + 'CaspianDriver' => 'Phobject', + 'CaspianLIDARAnalyzer' => 'CaspianAnalyzer', + 'CaspianLIDARSensor' => 'CaspianSensor', + 'CaspianLIDARView' => 'Phobject', + 'CaspianSensor' => 'Phobject', + 'CaspianSimulation' => 'Phobject', + 'CaspianSoftwareLIDARSensor' => 'CaspianLIDARSensor', + 'CaspianState' => 'Phobject', + 'CaspianWorkflow' => 'PhabricatorManagementWorkflow', 'CelerityAPI' => 'Phobject', 'CelerityDarkModePostprocessor' => 'CelerityPostprocessor', 'CelerityDefaultPostprocessor' => 'CelerityPostprocessor', diff --git a/src/applications/caspian/CaspianAnalyzer.php b/src/applications/caspian/CaspianAnalyzer.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianAnalyzer.php @@ -0,0 +1,19 @@ +name = $name; + return $this; + } + + final public function getName() { + return $this->name; + } + +} diff --git a/src/applications/caspian/CaspianDriveWorkflow.php b/src/applications/caspian/CaspianDriveWorkflow.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianDriveWorkflow.php @@ -0,0 +1,19 @@ +setName('drive') + ->setExamples('**drive** [__options__]') + ->setSynopsis(pht('Drive a control system.')) + ->setArguments(array()); + } + + public function execute(PhutilArgumentParser $args) { + id(new CaspianDriver()) + ->start(); + } + +} diff --git a/src/applications/caspian/CaspianDriver.php b/src/applications/caspian/CaspianDriver.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianDriver.php @@ -0,0 +1,83 @@ +setName('Left-Mounted LIDAR Sensor') + ->setPosition('L'); + + // Right-mounted LIDAR. + $sensors[] = id(new CaspianSoftwareLIDARSensor()) + ->setName('Right-Mounted LIDAR Sensor') + ->setPosition('R'); + + $analyzers = array(); + $analyzers[] = id(new CaspianLIDARAnalyzer()) + ->setName('LIDAR Analyzer'); + + $simulation = new CaspianSimulation(); + + $state = null; + $control = 'n'; + while (true) { + + $update = false; + if (!$state) { + $state = id(new CaspianState()) + ->setEpoch(0); + $update = true; + } else { + if ($control == 'n') { + if ($state->getNext()) { + $state = $state->getNext(); + } else { + $state = $state->newState(); + $update = true; + } + } else { + $state = $state->getPrevious(); + } + } + + if ($update) { + foreach ($sensors as $sensor) { + $state->readSensor($sensor); + } + + foreach ($analyzers as $analyzer) { + $state->analyzeSensors($analyzer); + } + } + + echo $simulation->newView($state); + + while (true) { + echo '[N]ext, [p]rev, [s]top?'; + $i = fgets(STDIN); + $i = strtolower(trim($i)); + + switch ($i) { + case '': + $control = 'n'; + break 2; + case 'n': + case 'p': + case 's': + $control = $i; + break 2; + } + } + + if ($control == 's') { + echo "Stopped at epoch ".$state->getEpoch().".\n"; + break; + } + } + } + +} diff --git a/src/applications/caspian/CaspianLIDARAnalyzer.php b/src/applications/caspian/CaspianLIDARAnalyzer.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianLIDARAnalyzer.php @@ -0,0 +1,54 @@ +getSensorState() as $sensor_state) { + if ($sensor_state['sensor'] instanceof CaspianLIDARSensor) { + $lidars[] = $sensor_state; + } + } + + $lidar_result = array(); + for ($ii = 0; $ii < 12; $ii++) { + $readings = array(); + foreach ($lidars as $lidar_reading) { + $state = $lidar_reading['state']; + if ($state[$ii] !== null) { + $readings[] = $state[$ii]; + } + } + + // Combine the values and certainties. This is just a rough approximation + // that produces output in vaguely the right shape. + + $values = array(); + $certainties = array(); + foreach ($readings as $reading) { + $values[] = $reading['reading']; + $certainties[] = $reading['certainty']; + } + + $value = array_sum($values) / count($values); + $certainty = array_sum($certainties) / (count($certainties) + 1); + + $lidar_result[] = array( + 'reading' => $value, + 'certainty' => $certainty, + ); + } + + return $lidar_result; + } + + public function newView(array $state) { + return id(new CaspianLIDARView())->newView( + $this->getName(), + $state); + } + + +} diff --git a/src/applications/caspian/CaspianLIDARSensor.php b/src/applications/caspian/CaspianLIDARSensor.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianLIDARSensor.php @@ -0,0 +1,10 @@ +newView($this->getName(), $state); + } + +} diff --git a/src/applications/caspian/CaspianLIDARView.php b/src/applications/caspian/CaspianLIDARView.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianLIDARView.php @@ -0,0 +1,37 @@ + '.$name."\n"; + + for ($ii = 0; $ii < count($state); $ii++) { + $out[] = sprintf( + '% 6s [% -4s]', + sprintf('%4.2f', $state[$ii]['reading']), + sprintf('%0.2f', $state[$ii]['certainty'])); + + switch ($ii) { + case 3: + case 5: + case 7: + case 11: + $out[] = "\n"; + break; + case 4: + case 6: + $out[] = ' '; + break; + default: + $out[] = ' '; + break; + } + } + + return implode('', $out); + } + +} diff --git a/src/applications/caspian/CaspianSensor.php b/src/applications/caspian/CaspianSensor.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianSensor.php @@ -0,0 +1,18 @@ +name = $name; + return $this; + } + + final public function getName() { + return $this->name; + } + +} diff --git a/src/applications/caspian/CaspianSimulation.php b/src/applications/caspian/CaspianSimulation.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianSimulation.php @@ -0,0 +1,25 @@ +getEpoch()); + + foreach ($state->getSensorState() as $sensor_state) { + $output[] = $sensor_state['sensor'] + ->newView($sensor_state['state']); + } + + foreach ($state->getAnalyzerState() as $analyzer_state) { + $output[] = $analyzer_state['analyzer'] + ->newView($analyzer_state['analysis']); + } + + return implode("\n", $output); + } + +} diff --git a/src/applications/caspian/CaspianSoftwareLIDARSensor.php b/src/applications/caspian/CaspianSoftwareLIDARSensor.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianSoftwareLIDARSensor.php @@ -0,0 +1,98 @@ +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; + } + +} diff --git a/src/applications/caspian/CaspianState.php b/src/applications/caspian/CaspianState.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianState.php @@ -0,0 +1,71 @@ +previous = $previous; + return $this; + } + + public function getPrevious() { + return $this->previous; + } + + public function setEpoch($epoch) { + $this->epoch = $epoch; + return $this; + } + + public function getEpoch() { + return $this->epoch; + } + + public function setNext(CaspianState $next) { + $this->next = $next; + return $this; + } + + public function getNext() { + return $this->next; + } + + public function newState() { + $next = id(new self()) + ->setPrevious($this) + ->setEpoch($this->getEpoch() + 1); + + $this->setNext($next); + + return $next; + } + + public function readSensor(CaspianSensor $sensor) { + $this->sensorState[] = array( + 'sensor' => $sensor, + 'state' => $sensor->readState(), + ); + } + + public function analyzeSensors(CaspianAnalyzer $analyzer) { + $this->analyzerState[] = array( + 'analyzer' => $analyzer, + 'analysis' => $analyzer->analyzeSensors($this), + ); + } + + public function getSensorState() { + return $this->sensorState; + } + + public function getAnalyzerState() { + return $this->analyzerState; + } + +} diff --git a/src/applications/caspian/CaspianWorkflow.php b/src/applications/caspian/CaspianWorkflow.php new file mode 100644 --- /dev/null +++ b/src/applications/caspian/CaspianWorkflow.php @@ -0,0 +1,4 @@ +