From e7ea75b6999d546766fc86c9218a646623c5cc9f Mon Sep 17 00:00:00 2001 From: Marten Lohstroh Date: Mon, 4 Apr 2022 18:15:58 -0700 Subject: [PATCH] Remove experimental directory --- experimental/C/src/AnytimePrime.lf | 68 --- experimental/C/src/Autoware/Autoware.lf | 292 ---------- experimental/C/src/Autoware/parse_trace.sh | 6 - experimental/C/src/Cutter.lf | 80 --- experimental/C/src/FurutaPendulum.lf | 120 ---- .../C/src/FurutaPendulumSimulation.lf | 132 ----- .../C/src/FurutaPendulumWithOutput.lf | 39 -- .../C/src/Intersection/Intersection.lf | 153 ----- experimental/C/src/Logic/Logic.lf | 54 -- experimental/C/src/Microsteps/Anomaly.lf | 54 -- .../BehaviorTrees/robohub_example_advanced.lf | 141 ----- .../BehaviorTrees/robohub_example_simple.lf | 116 ---- .../ModalModels/Motivation/Chrono/Chrono.lf | 158 ----- .../Motivation/SineAvgMax/sine_max_avg.lf | 130 ----- .../Motivation/SineAvgMax/sine_max_avg_v2.lf | 66 --- .../ModalModels/ReflexGame/ModalReflexGame.lf | 150 ----- experimental/C/src/PowerTrain/PowerTrain.lf | 243 -------- experimental/C/src/PowerTrain/README.md | 1 - experimental/C/src/ScatterGather.lf | 128 ----- experimental/C/src/SieveOfEratosthenes.lf | 137 ----- .../C/src/SpatAnalysis/MQTTPublisher.lf | 183 ------ .../C/src/SpatAnalysis/MQTTSubscriber.lf | 268 --------- experimental/C/src/SpatAnalysis/README.md | 58 -- .../C/src/SpatAnalysis/influxWrite.lf | 130 ----- experimental/C/src/SpatAnalysis/package.json | 36 -- .../C/src/SpatAnalysis/spatReceiver.lf | 93 --- .../C/src/SpatAnalysis/spatRecommender.lf | 165 ------ experimental/C/src/build.sh | 15 - experimental/C/src/pendulum.gnuplot | 19 - .../Intersection/Carla/CarlaIntersection.lf | 91 --- .../Python/src/Intersection/Carla/CarlaSim.lf | 72 --- .../src/Intersection/Carla/ROS/.gitignore | 3 - .../src/Intersection/Carla/ROS/README.md | 31 - .../launch/intersection_demo.launch.py | 52 -- .../Carla/ROS/carla_intersection/package.xml | 23 - .../resource/carla_intersection | 0 .../Carla/ROS/carla_intersection/setup.cfg | 4 - .../Carla/ROS/carla_intersection/setup.py | 28 - .../ROS/carla_intersection/src/__init__.py | 0 .../ROS/carla_intersection/src/carla_sim.py | 144 ----- .../carla_intersection/src/carla_sim_node.py | 86 --- .../ROS/carla_intersection/src/constants.py | 12 - .../src/launch_parameters.py | 29 - .../ROS/carla_intersection/src/ros_utils.py | 4 - .../Carla/ROS/carla_intersection/src/rsu.py | 92 --- .../ROS/carla_intersection/src/rsu_node.py | 61 -- .../Carla/ROS/carla_intersection/src/utils.py | 69 --- .../ROS/carla_intersection/src/vehicle.py | 186 ------ .../carla_intersection/src/vehicle_node.py | 81 --- .../carla_intersection_msgs/CMakeLists.txt | 35 -- .../ROS/carla_intersection_msgs/msg/Grant.msg | 4 - .../carla_intersection_msgs/msg/Request.msg | 3 - .../msg/VehicleCommand.msg | 3 - .../ROS/carla_intersection_msgs/package.xml | 24 - .../Python/src/Intersection/Carla/RSU.lf | 53 -- .../Python/src/Intersection/Carla/Vehicle.lf | 70 --- .../src/Intersection/Carla/run-carla.sh | 1 - .../Python/src/Intersection/Intersection.lf | 543 ------------------ experimental/README.md | 3 - 59 files changed, 5042 deletions(-) delete mode 100644 experimental/C/src/AnytimePrime.lf delete mode 100644 experimental/C/src/Autoware/Autoware.lf delete mode 100755 experimental/C/src/Autoware/parse_trace.sh delete mode 100644 experimental/C/src/Cutter.lf delete mode 100644 experimental/C/src/FurutaPendulum.lf delete mode 100644 experimental/C/src/FurutaPendulumSimulation.lf delete mode 100644 experimental/C/src/FurutaPendulumWithOutput.lf delete mode 100644 experimental/C/src/Intersection/Intersection.lf delete mode 100644 experimental/C/src/Logic/Logic.lf delete mode 100644 experimental/C/src/Microsteps/Anomaly.lf delete mode 100644 experimental/C/src/ModalModels/BehaviorTrees/robohub_example_advanced.lf delete mode 100644 experimental/C/src/ModalModels/BehaviorTrees/robohub_example_simple.lf delete mode 100644 experimental/C/src/ModalModels/Motivation/Chrono/Chrono.lf delete mode 100644 experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg.lf delete mode 100644 experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg_v2.lf delete mode 100644 experimental/C/src/ModalModels/ReflexGame/ModalReflexGame.lf delete mode 100644 experimental/C/src/PowerTrain/PowerTrain.lf delete mode 100644 experimental/C/src/PowerTrain/README.md delete mode 100644 experimental/C/src/ScatterGather.lf delete mode 100644 experimental/C/src/SieveOfEratosthenes.lf delete mode 100644 experimental/C/src/SpatAnalysis/MQTTPublisher.lf delete mode 100644 experimental/C/src/SpatAnalysis/MQTTSubscriber.lf delete mode 100644 experimental/C/src/SpatAnalysis/README.md delete mode 100644 experimental/C/src/SpatAnalysis/influxWrite.lf delete mode 100644 experimental/C/src/SpatAnalysis/package.json delete mode 100644 experimental/C/src/SpatAnalysis/spatReceiver.lf delete mode 100644 experimental/C/src/SpatAnalysis/spatRecommender.lf delete mode 100755 experimental/C/src/build.sh delete mode 100644 experimental/C/src/pendulum.gnuplot delete mode 100644 experimental/Python/src/Intersection/Carla/CarlaIntersection.lf delete mode 100644 experimental/Python/src/Intersection/Carla/CarlaSim.lf delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/.gitignore delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/README.md delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/package.xml delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/resource/carla_intersection delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.cfg delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/__init__.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/CMakeLists.txt delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Grant.msg delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Request.msg delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/VehicleCommand.msg delete mode 100644 experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/package.xml delete mode 100644 experimental/Python/src/Intersection/Carla/RSU.lf delete mode 100644 experimental/Python/src/Intersection/Carla/Vehicle.lf delete mode 100755 experimental/Python/src/Intersection/Carla/run-carla.sh delete mode 100644 experimental/Python/src/Intersection/Intersection.lf delete mode 100644 experimental/README.md diff --git a/experimental/C/src/AnytimePrime.lf b/experimental/C/src/AnytimePrime.lf deleted file mode 100644 index 2fe50bef10..0000000000 --- a/experimental/C/src/AnytimePrime.lf +++ /dev/null @@ -1,68 +0,0 @@ -/** - * This program is a concept demonstration showing how the check_deadline() - * function works. This program performs "anytime computation" (https://en.wikipedia.org/wiki/Anytime_algorithm) - * to find the largest prime within the given deadline. The check_deadline() - * function is called after checking whether each number is prime. If - * check_deadline() is called after the deadline has passed, the function calls - * the deadline handler and returns true. Then the reaction outputs the largest - * prime found by that time and exits. - * - * For more discussion on check_deadline(), see: https://github.com/lf-lang/lingua-franca/issues/403 - * - * @author Hokeun Kim (hokeunkim@berkeley.edu) - * @author Edward A. Lee (eal@berkeley.edu) - * @author Marten Lohstroh (marten@berkeley.edu) - */ -target C { - fast: true, - files: ["/lib/c/reactor-c/core/utils/vector.h", - "/lib/c/reactor-c/core/utils/vector.c"], - cmake-include: ["/lib/c/reactor-c/core/utils/vector.cmake"] -}; - -preamble {= -#include "vector.h" -=} - -reactor Prime { - output out: {=long long=}; - reaction(startup) -> out {= - int num_primes = 1; - long long current_num = 2; - vector_t primes = vector_new(10000); - vector_push(&primes, (void*)2); - - while (!check_deadline(self, true)) { - current_num++; - int i = 0; - for (i = 0; i < num_primes; i++) { - if (current_num % (long long)primes.start[i] == 0) { - break; - } - } - if (i == num_primes) { - // Add the prime to vector. - vector_push(&primes, (void*)current_num); - num_primes++; - } - } - - // Output the largest prime found. - SET(out, (long long)primes.start[num_primes - 1]); - =} deadline (3 sec) {= - info_print("Deadline handler called!"); - =} -} - -reactor Print { - input in:{=long long=}; - reaction(in) {= - printf("Largest prime found within the deadline: %d\n", in->value); - =} -} - -main reactor AnytimePrime { - p = new Prime(); - d = new Print(); - p.out -> d.in; -} diff --git a/experimental/C/src/Autoware/Autoware.lf b/experimental/C/src/Autoware/Autoware.lf deleted file mode 100644 index 442e7c3920..0000000000 --- a/experimental/C/src/Autoware/Autoware.lf +++ /dev/null @@ -1,292 +0,0 @@ -// A lingua franca program with Autoware's general graph structure and constraints -// The tentative constraints are: -// - Sensors: Camera(phase=3, period=33) and LIDAR(phase=10, period=10) -// - LET=30ms or alternatively, the relative deadline is 30 -// - Sensor Fusion causes merge (choke) points that collapse multiple logical timelines into one. - -target C { - threads: 2, - keepalive: false -}; - -// Send a periodic image out -reactor Camera(offset:time(0), period:time(1 sec)) { - output image:string; - timer t(offset, period); - - reaction(t) -> image {= - SET(image, "kitty"); - =} -} - -// Send a periodic LIDAR pointcloud out -reactor LIDAR(offset:time(0), period:time(1 sec)) { - output pointcloud:string; - timer t(offset, period); - - reaction(t) -> pointcloud {= - SET(pointcloud, "INTERSECTION"); - =} -} - -// Simulate object detection on GPU -reactor ImageObjectDetection { - input image:string; - output object:string; - - - state thread_id:pthread_t(0); - - preamble {= - #include - - void camera_callback(void* a) { - // cudaDeviceSynchronize() and copy back goes here. - schedule(a, 0); - } - // Simulate time passing before a callback occurs instead of executing on GPU. - void* camera_take_time(void* a) { - struct timespec sleep_time = {(time_t) 0, (long)5000000}; // WCET for GPU is 5msec - struct timespec remaining_time; - nanosleep(&sleep_time, &remaining_time); - camera_callback(a); - return NULL; - } - pthread_t threadId; - =} - - physical action CUDA(100 msec); - reaction(image) -> CUDA {= - // cudaMalloc(&y_state_d, SIZE); - // cudaMemcpy(&y_state_d, y_state, SIZE, cudaMemcpyHosttoDevice); - // kernel<<<1,1>>>(y_state_d); - - /* Busy wait for 2 msecs for now instead of calling CUDA kernels */ - interval_t sleep_time = MSEC(2); - instant_t start_time = get_physical_time(); - while (get_physical_time() < start_time + sleep_time) {}; - - pthread_create(&self->thread_id, NULL, &camera_take_time, CUDA); - =} - - reaction(CUDA) -> object {= - // cudaMalloc(&y_out_d, SIZE); - - // cudaMemcpy(&y_out, results_d, SIZE, cudaMemcpyDevicetoHost); - - // cudaDeviceSynchronize(); - - - - SET(object, "DUMMY Results"); - =} -} - -// Simulate LIDAR object detection on GPU -reactor LIDARObjectDetection { - input pointcloud:string; - output object:string; - - - state thread_id:pthread_t(0); - - preamble {= - #include - - void LIDAR_callback(void* a) { - // cudaDeviceSynchronize() and copy back goes here. - schedule(a, 0); - } - // Simulate time passing before a callback occurs instead of executing on GPU. - void* LIDAR_take_time(void* a) { - struct timespec sleep_time = {(time_t) 0, (long)12000000}; // WCET for LIDAR GPU is 12 msec - struct timespec remaining_time; - nanosleep(&sleep_time, &remaining_time); - LIDAR_callback(a); - return NULL; - } - pthread_t threadId; - =} - - logical action CUDA(100 msec, 20 msec); - reaction(pointcloud) -> CUDA {= - // self->y_state = y_in; - // cudaMalloc(&y_state_d, SIZE); - // cudaMemcpy(&y_state_d, y_state, SIZE, cudaMemcpyHosttoDevice); - // kernel<<<1,1>>>(y_state_d); - //printf("LIDAR triggered at %llu\n", get_physical_time()); - - /* Busy wait for 2 msecs for now instead of calling CUDA kernels */ - interval_t sleep_time = MSEC(2); - instant_t start_time = get_physical_time(); - while (get_physical_time() < start_time + sleep_time) {}; - - pthread_create(&self->thread_id, NULL, &LIDAR_take_time, CUDA); - =} - - reaction(CUDA) -> object {= - // cudaMalloc(&y_out_d, SIZE); - //printf("LIDAR triggered at %llu\n", get_physical_time()); - // printf("LIDAR triggered at %llu\n", get_logical_time()); - - // cudaMemcpy(&y_out, results_d, SIZE, cudaMemcpyDevicetoHost); - - // cudaDeviceSynchronize(); - - SET(object, "Hey look there is a kitty!"); - =} -} - - -// Fuse LIDAR and Camera detected objects -reactor DataFusion(threshold:time(20 msec)) { - input imageobject:string; - input LIDARobject:string; - output object:string; - logical action both_ports_are_present(0); - - state tmpImageobject:string(""); - state tmpImageobjectTag:time(0); - state tmpLIDARobject:string(""); - state tmpLIDARobjectTag:time(0); - - - // Handle two ports - reaction(imageobject, LIDARobject) -> both_ports_are_present {= - if(imageobject->is_present) - { - self->tmpImageobject = imageobject->value; - self->tmpImageobjectTag = get_logical_time(); // Store the tag - - instant_t window = self->tmpLIDARobjectTag - get_logical_time(); - - if(LIDARobject->is_present) - { - schedule(both_ports_are_present, 0); - } - - else if(window < (long)1000000) - { - schedule(both_ports_are_present, 0); - } - else - { - // instant_t elapsed = get_physical_time() - get_logical_time(); // How much time has passed - soft - instant_t elapsed = self->threshold; // How much time has passed - hard - instant_t remaining = (long)30000000 - elapsed; // How much time is left - instant_t schedule_in = remaining - (long)8000000; // Combined WCET of the remaining reactions is 8 msec - schedule(both_ports_are_present, schedule_in); - } - // printf("Received image object at %llu physical and %llu logical.\n", get_physical_time(), get_logical_time()); - } - else if(LIDARobject->is_present) - { - self->tmpLIDARobject = LIDARobject->value; - self->tmpLIDARobjectTag = get_logical_time(); // Store the tag - - instant_t window = self->tmpImageobjectTag - get_logical_time(); - - if( window < (long)1000000) - { - schedule(both_ports_are_present, 0); - } - else - { - // instant_t elapsed = get_physical_time() - get_logical_time(); // How much time has passed - soft - instant_t elapsed = self->threshold; // How much time has passed - hard - instant_t remaining = (long)30000000 - elapsed; // How much time is left - instant_t schedule_in = remaining - (long)8000000; // Combined WCET of the remaining reactions is 8 msec - schedule(both_ports_are_present, schedule_in); - } - // printf("Received LIDAR object at %llu physical and %llu logical.\n", get_physical_time(), get_logical_time()); - } - - - - =} deadline(threshold) {= - printf("Deadline violation detected.\n"); - =} - - // Fuse - reaction(both_ports_are_present) -> object - {= - - printf("Fusion scheduled at: ( %llu , %llu ).\n", get_physical_time(), get_logical_time()); - //printf("Fusion: %llu\n", get_physical_time()); - /* Busy wait for 2 msecs for now instead of calling CUDA kernels */ - interval_t sleep_time = MSEC(2); - instant_t start_time = get_physical_time(); - while (get_physical_time() < start_time + sleep_time) {}; - SET(object, "fused"); - =} -} - -// Make driving semantic decisions -reactor Semantics { - input fusedObject:string; - output actuation:int; - - reaction(fusedObject) -> actuation {= - struct timespec sleep_time = {(time_t) 0, (long)6000000}; - struct timespec remaining_time; - nanosleep(&sleep_time, &remaining_time); - SET(actuation, 5); - =} -} - -reactor Actuator(threshold:time(33 msec)){ - input actuation:int; - - reaction(actuation) {= - // Do nothing; - printf("Actuator scheduled at: ( %llu , %llu ).\n", get_physical_time(), get_logical_time()); - // printf("Actuator: %llu\n", get_physical_time()); - =} deadline(threshold) {= - printf("Deadline violation detected in Actuator.\n"); - =} -} - -/* An alternative path exists between the LIDAR and the actuator for the safety break system */ -reactor SafetyBreak { - input LIDARPointCloud:string; - output actuation:int; - - reaction(LIDARPointCloud) -> actuation {= - SET(actuation, 5); - =} -} - - -// End-to-end daedline should be 33ms -// LET must be enforced with an output period of 33ms -// GPU is used -// Sub-deadlines (i.e., reaction deadlines) must not be violated - -main reactor Autoware { - c = new Camera(offset = 3 msec, period = 33 msec); // Camera has a phase (startup time) of 3 msec and a period of 33 msec - l = new LIDAR(offset = 10 msec, period = 10 msec); // Lidar has a phase (spooling up time) of 10 msec and a period of 10 msec - - iobjectdetection = new ImageObjectDetection(); - c.image -> iobjectdetection.image; - - lobjectdetection = new LIDARObjectDetection(); - l.pointcloud -> lobjectdetection.pointcloud; - - // Choke point - fuse = new DataFusion(); - iobjectdetection.object -> fuse.imageobject; - lobjectdetection.object -> fuse.LIDARobject; - - - sem = new Semantics(); - fuse.object -> sem.fusedObject; - - ac = new Actuator(); - sem.actuation -> ac.actuation; - - // An alternative path that activates the safety break system based on LIDAR input - sb = new SafetyBreak(); - l.pointcloud -> sb.LIDARPointCloud; - // sb.actuation -> ac.actuation; // Cannot construct the alternative path because actuation may only be connected to a single upstream port - -} diff --git a/experimental/C/src/Autoware/parse_trace.sh b/experimental/C/src/Autoware/parse_trace.sh deleted file mode 100755 index 926cbf8de7..0000000000 --- a/experimental/C/src/Autoware/parse_trace.sh +++ /dev/null @@ -1,6 +0,0 @@ -# This script will generate 4 timelines for the important components in Autoware -# based on the stored output of Autoware (e.g., './bin/Autoware > trace' and 'parse_trace.sh trace'). -grep -nre "Fusion" $1 | awk '{print $5}' | awk '{if (NR==1) {first_row = $1} printf "%s", ($1 - first_row)/1000000; printf "\n";}' > fusion_physical_timeline # Physical time -grep -nre "Actuator" $1 | awk '{print $5}' | awk '{if (NR==1) {first_row = $1} printf "%s", ($1 - first_row)/1000000; printf "\n";}' > actuator_physical_timeline # Physical time -grep -nre "Fusion" $1 | awk '{print $7}' | awk '{if (NR==1) {first_row = $1} printf "%s", ($1 - first_row)/1000000; printf "\n";}' > fusion_logical_timeline # Logical time -grep -nre "Actuator" $1 | awk '{print $7}' | awk '{if (NR==1) {first_row = $1} printf "%s", ($1 - first_row)/1000000; printf "\n";}' > actutator_logical_timeline # Logical time diff --git a/experimental/C/src/Cutter.lf b/experimental/C/src/Cutter.lf deleted file mode 100644 index f9d5747c7f..0000000000 --- a/experimental/C/src/Cutter.lf +++ /dev/null @@ -1,80 +0,0 @@ -target C { - workers: 2, - keepalive: true, - files: [ - "/lib/c/reactor-c/util/sensor_simulator.c", - "/lib/c/reactor-c/util/sensor_simulator.h", - ], - cmake-include: [ - "/lib/c/reactor-c/util/sensor_simulator.cmake" - ], - build-type: RelWithDebInfo // Release with debug info -} -preamble {= - #include "sensor_simulator.h" - char* messages[] = {"Cutter Simulator"}; - int num_messages = 1; -=} -reactor ButtonController( - key:char('\0'), - description:string("Button") -) { - physical action pushed:char*; - output push:bool; - reaction(startup) -> pushed {= - register_sensor_key(self->key, pushed); - if (self->key == '\0') { - info_print("%s with any key", self->description); - } else { - info_print("%s with '%c'", self->description, self->key); - } - =} - reaction(pushed) -> push {= - SET(push, true); - =} -} -reactor MachineController { - input emergencyStop:bool; - input run:bool; - initial mode Off { - reaction(run) -> Running {= - SET_MODE(Running); - =} - } - mode Running { - timer t(0, 100 msec); - reaction(emergencyStop) -> Off {= - if (emergencyStop) SET_MODE(Off); - =} deadline (1 msec) {= - info_print("DEADLINE VIOLATION!"); - SET_MODE(Off); - =} - // With centralized coordination, time cannot advance to trigger - // this reaction unless null messages have arrived on emergencyStop. - reaction(t) -> Off {= - show_tick("*"); - =} deadline (10 msec) {= - SET_MODE(Off); - =} - } -} -main reactor { - eStop = new ButtonController(key = 'e', description="Emergency stop"); - run = new ButtonController(key = 'r', description="Run"); - m = new MachineController(); - - eStop.push -> m.emergencyStop; - run.push -> m.run; - - physical action stop:char*; - - reaction(startup) -> stop {= - // This has to be done exactly once. - start_sensor_simulator(messages, num_messages, 16, NULL, LOG_LEVEL_INFO); - register_sensor_key('x', stop); - info_print("Exit with 'x'"); - =} - reaction(stop) {= - request_stop(); - =} -} \ No newline at end of file diff --git a/experimental/C/src/FurutaPendulum.lf b/experimental/C/src/FurutaPendulum.lf deleted file mode 100644 index 705d0092f3..0000000000 --- a/experimental/C/src/FurutaPendulum.lf +++ /dev/null @@ -1,120 +0,0 @@ -target C; -preamble {= - #include - #define PI 3.14159265 - double sign(double x) { - return (x > 0.0) - (x < 0.0); - } - double restrictAngle(double theta) { - return((fmod(fabs(theta) + PI, 2 * PI) - PI) * sign(theta)); - } -=} -reactor Sensor { - output angle:double; - output d_angle:double; -} -reactor Actuator { - input control:double; -} -reactor Controller( - h:double(0.005), // Sample interval - w0:double(6.3), - k:double(0.5), // Energy multiplier to swing up. - n:double(0.5), // Bound on swing up control magnitude. - region1:double(0.1), // Region to exit SwingUp. - region2:double(0.2), // Region to exit Stabilize. - max_speed:double(0.05), // Speed to exit Catch. - ci1:double(-1.04945717118225), - ci2:double(-0.20432286791216), - ci3:double(-0.00735846749875), - ci4:double(-0.00735846749875), - si1:double(-1.70871686211144), - si2:double(-0.30395427746831), - si3:double(-0.03254225945714), - si4:double(-0.05808270221773), - phi2:double(-7.0124562) -) { - input theta:double; - input d_theta:double; - input phi:double; - input d_phi:double; - - output control:double; - output modeID:double; - output energy:double; - - state phi0:double(0.0); - - initial mode SwingUp { - reaction(theta, d_theta) d_phi -> control, modeID, energy {= - double th = restrictAngle(theta->value); - double E = 0.5 * d_theta->value * d_theta->value / (self->w0 * self->w0) - + cos(th) - 1.0; - double c = sign(d_theta->value * cos(th)); - double out = sign(E) * MIN(fabs(self->k * E), self->n) * c; - SET(control, out); - SET(energy, E); - SET(modeID, -1); - =} - reaction(theta) -> Catch {= - if (fabs(theta->value) < self->region1) { - SET_MODE(Catch); - } - =} - } - - mode Catch { - reaction(theta, d_theta, phi, d_phi) -> control, modeID, energy {= - double th = restrictAngle(theta->value); - SET(control, -1.0 * ( - th * self->ci1 - + d_theta->value * self->ci2 - + (phi->value - self->phi2) * self->ci3 - + d_phi->value * self->ci4 - )); - SET(modeID, 0); - double E = 0.5 * d_theta->value * d_theta->value / (self->w0 * self->w0) - + cos(th) - 1.0; - SET(energy, E); - =} - reaction(phi, d_phi) -> Stabilize {= - if (fabs(d_phi->value) < self->max_speed) { - SET_MODE(Stabilize); - self->phi0 = phi->value; - } - =} - } - - mode Stabilize { - reaction(theta, d_theta, phi, d_phi) -> control, modeID, energy {= - double th = restrictAngle(theta->value); - SET(control, -1.0 * ( - th * self->si1 - + d_theta->value * self->si2 - + (phi->value - self->phi0) * self->si3 - + d_phi->value * self->si4 - )); - double E = 0.5 * d_theta->value * d_theta->value / (self->w0 * self->w0) - + cos(th) - 1.0; - SET(energy, E); - SET(modeID, 1); - =} - reaction(theta) -> SwingUp {= - double th = restrictAngle(theta->value); - if (fabs(th) > self->region2) { - SET_MODE(SwingUp); - } - =} - } -} -main reactor { - s1 = new Sensor(); - s2 = new Sensor(); - a = new Actuator(); - c = new Controller(); - - s1.angle, s1.d_angle -> c.theta, c.d_theta; - s2.angle, s2.d_angle -> c.phi, c.d_phi; - - c.control -> a.control; -} \ No newline at end of file diff --git a/experimental/C/src/FurutaPendulumSimulation.lf b/experimental/C/src/FurutaPendulumSimulation.lf deleted file mode 100644 index 9c2d1e4e7a..0000000000 --- a/experimental/C/src/FurutaPendulumSimulation.lf +++ /dev/null @@ -1,132 +0,0 @@ -target C { - timeout: 3 secs -} -import Controller from "FurutaPendulum.lf"; - -preamble {= - #include -=} - -/** - * Pendulum model based on Ptolemy II model by Johan Eker. - * This is a simple forward-Euler simulation, unlike the - * Ptolemy II model, which uses an RK-45 solver. - * This outputs its state every `sample_period`. - * It updates the state before outputting it - * using the most recently received control input, - * except on the first input, where it just outputs - * the initial state. - */ -reactor PendulumModel( - sample_period:time(5 msec), // Sample period. - g:double(9.81), // Acceleration of gravity. - alpha:double(0.00260569), - beta:double(0.05165675), - gamma:double(9.7055e-4), - epsilon:double(0.08103060) -){ - input u:double; // Control input. - - output theta:double; // Pendulum angle. - output d_theta:double; // Pendulum angular velocity. - output phi:double; // Arm angle. - output d_phi:double; // Arm angular velocity. - - state x:double[4](-3.0, 0.0, 0.0, 0.0); - state first:bool(true); - state latest_u:double(0.0); - - timer t(0, sample_period); - - reaction(t) -> theta, d_theta, phi, d_phi {= - if (!self->first) { - // Update the state. - double x0_dot = self->x[1]; - double x1_dot = 1.0/( - self->alpha * self->beta - + pow(self->alpha * sin(self->x[0]), 2.0) - - pow(self->gamma * cos(self->x[0]), 2.0) - ) * ( - (self->alpha * self->beta + pow(self->alpha * sin(self->x[0]), 2.0)) - * pow(self->x[3], 2.0) - * sin(self->x[0]) - * cos(self->x[0]) - - - pow(self->gamma * self->x[1], 2.0) - * sin(self->x[0]) - * cos(self->x[0]) - + - 2.0 - * self->alpha - * self->gamma\ - * self->x[1] - * self->x[3] - * sin(self->x[0]) - * pow(cos(self->x[0]), 2.0) - - - self->gamma - * cos(self->x[0]) - * self->g - * self->latest_u - + - (self->alpha * self->beta + pow(self->alpha * sin(self->x[0]), 2.0)) - * self->epsilon / self->alpha * sin(self->x[0]) - ); - double x2_dot = self->x[3]; - double x3_dot = (1.0 / ( - self->alpha * self->beta - + pow(self->alpha * sin(self->x[0]), 2.0) - - pow(self->gamma * cos(self->x[0]), 2.0) - )) * ( - -self->gamma - * self->alpha - * pow(self->x[3], 2.0) - * sin(self->x[0]) - * pow(cos(self->x[1]), 2.0) - - - self->gamma - * self->epsilon - * sin(self->x[0]) - * cos(self->x[0]) - + - self->gamma - * self->alpha - * pow(self->x[1], 2.0) - * sin(self->x[0]) - - - 2 - * pow(self->alpha, 2.0) - * self->x[1] - * self->x[3] - * sin(self->x[0]) - * cos(self->x[0]) - + - self->alpha - * self->g - * self->latest_u - ); - double sample_period = self->sample_period * 1e-9; - self->x[0] += x0_dot * sample_period; - self->x[1] += x1_dot * sample_period; - self->x[2] += x2_dot * sample_period; - self->x[3] += x3_dot * sample_period; - } else { - self->first = false; - } - // Output the state. - SET(theta, self->x[0]); - SET(d_theta, self->x[1]); - SET(phi, self->x[2]); - SET(d_phi, self->x[3]); - =} - reaction(u) {= - self->latest_u = u->value; - =} -} -main reactor { - p = new PendulumModel(); - c = new Controller(); - p.phi, p.d_phi -> c.phi, c.d_phi; - p.theta, p.d_theta -> c.theta, c.d_theta; - c.control -> p.u; -} \ No newline at end of file diff --git a/experimental/C/src/FurutaPendulumWithOutput.lf b/experimental/C/src/FurutaPendulumWithOutput.lf deleted file mode 100644 index 340fb7c2f7..0000000000 --- a/experimental/C/src/FurutaPendulumWithOutput.lf +++ /dev/null @@ -1,39 +0,0 @@ -target C { - timeout: 3 secs, - fast: true, - build: "./build.sh FurutaPendulumWithOutput" -} -import Controller from "FurutaPendulum.lf"; -import PendulumModel from "FurutaPendulumSimulation.lf"; - -reactor Print(filename:string("output.data")) { - input y:double; - state file:FILE*({=NULL=}); - reaction(startup) {= - self->file = fopen(self->filename, "w"); - if(self->file == NULL) { - error_print_and_exit("Failed to open file: %s", self->filename); - } - =} - reaction(y) {= - double t = get_elapsed_logical_time() / 1.0e9; - fprintf(self->file, "%f %f\n", t, y->value); - =} - reaction(shutdown) {= - fclose(self->file); - =} -} - -main reactor { - p = new PendulumModel(sample_period = 5 msec); - c = new Controller(); - pr_pendulum = new Print(filename = "pendulum.data"); - pr_mode = new Print(filename = "mode.data"); - pr_energy = new Print(filename = "energy.data"); - p.phi, p.d_phi -> c.phi, c.d_phi; - p.theta, p.d_theta -> c.theta, c.d_theta; - c.control -> p.u; - c.control -> pr_pendulum.y; - c.modeID -> pr_mode.y; - c.energy -> pr_energy.y; -} \ No newline at end of file diff --git a/experimental/C/src/Intersection/Intersection.lf b/experimental/C/src/Intersection/Intersection.lf deleted file mode 100644 index 7bdb7425d0..0000000000 --- a/experimental/C/src/Intersection/Intersection.lf +++ /dev/null @@ -1,153 +0,0 @@ -/** - * Model of a smart intersection with a road-side unit (RSU) - * that regulates the flow of automated vehicles through the - * intersection. Vehicles that are approaching the intersection - * send an initial message to the RSU with their speed and - * distance to the intersection. The RSU responds with a - * reservation for when the vehicle can enter the intersection - * and what its average speed through the intersection should be. - * - * This is meant as a supervisory controller, and it assumes that - * the vehicle is equipped with a low-level controller (or a human) - * that is responsible for lane keeping, collision avoidance, etc. - * - * This is a very rough starting point that needs a lot of work. - */ -target C { - timeout: 5 sec -} - -preamble {= - typedef struct { - double speed; - double distance; - } request_message_t; - - typedef struct { - // Average speed vehicle should maintain in the intersection. - double target_speed; // FIXME: Deadline. = t/w - // Time at which the vehicle can enter the intersection. - instant_t arrival_time; - } grant_message_t; - - // Table of offsets by vehicle bank_index: - interval_t timer_offsets[] = { - 0LL, - MSEC(200), - MSEC(400), - MSEC(600) - }; - // Table of periods by vehicle bank_index: - interval_t timer_periods[] = { - SEC(4), - SEC(8), - SEC(16), - SEC(32) - }; -=} - -reactor Vehicle ( - offset:time(0), - period:time(1 sec), - speed:double(42.0), // in km per hour. About 11.7 m/sec - distance:double(42.0) // in meters. About 4 sec to traverse. -) { - input grant:grant_message_t; - - output request:request_message_t; - - logical action delay; - - reaction(startup) -> request, delay {= - if (timer_offsets[self->bank_index] == 0LL) { - // Need to send a message at the start time. - request_message_t message; - message.speed = self->speed; - message.distance = self->distance; - SET(request, message); - schedule(delay, timer_periods[self->bank_index]); - } else { - schedule(delay, timer_offsets[self->bank_index]); - } - =} - - reaction(delay) -> request, delay {= - request_message_t message; - message.speed = self->speed; - message.distance = self->distance; - SET(request, message); - schedule(delay, timer_periods[self->bank_index]); - =} - - reaction(grant) {= - info_print("Granted access at elapsed logical time %lld. Physical time is %lld", - get_elapsed_logical_time(), - get_elapsed_physical_time() - ); - =} -} - -reactor RSU ( - num_entries:int(4), - intersection_width:double(42.0), // in meters. - // If the vehicle is told to slow down, then its target - // average speed in the intersection should be at least this. - nominal_speed_in_intersection:double(10.0) // In km/hr. 2.8 m/sec. 15 sec to traverse. -) { - input[num_entries] request:request_message_t; - output[num_entries] grant:grant_message_t; - - state earliest_free:time(0); - - reaction(request) -> grant {= - for (int i = 0; i < self->num_entries; i++) { - if (request[i]->is_present) { - // Calculate the time it will take the approaching vehicle to - // arrive at its current speed. Note that this is - // time from the time the vehicle sends the message - // according to the arriving vehicle's clock. - double speed_in_m_per_sec = request[i]->value.speed * 1000.0 / 3600.0; - double arrival_in = request[i]->value.distance / speed_in_m_per_sec; - - instant_t time_message_sent = get_logical_time(); - - // Convert the time interval to nsec (it is seconds). - interval_t arrival_time_ns = time_message_sent + (interval_t) (arrival_in * BILLION); - - grant_message_t response; - if (arrival_time_ns >= self->earliest_free) { - // Vehicle can maintain speed. - response.target_speed = request[i]->value.speed; - response.arrival_time = arrival_time_ns; - } else { - // Vehicle has to slow down and maybe stop. - response.arrival_time = self->earliest_free; - // Could be smarter than this, but just send the nominal speed in intersection. - response.target_speed = self->nominal_speed_in_intersection; - } - SET(grant[i], response); - // Update earliest free on the assumption that the vehicle - // maintains its target speed (on average) within the intersection. - interval_t time_in_intersection - = (interval_t)(BILLION * self->intersection_width * 3600 - / (1000 * response.target_speed) - ); - self->earliest_free = response.arrival_time + time_in_intersection; - - info_print("*** Grant access to vehicle %d to enter at time %lld. Next available time is %lld", - i, - response.arrival_time - start_time, - self->earliest_free - start_time - ); - } - } - =} -} - -main reactor { - vehicles = new[4] Vehicle(offset = 0); - - rsu = new RSU(); - vehicles.request -> rsu.request; - rsu.grant -> vehicles.grant; -} \ No newline at end of file diff --git a/experimental/C/src/Logic/Logic.lf b/experimental/C/src/Logic/Logic.lf deleted file mode 100644 index f123a1c327..0000000000 --- a/experimental/C/src/Logic/Logic.lf +++ /dev/null @@ -1,54 +0,0 @@ -target C {timeout: 200 msec}; - -// Test illustrating that the current implementation of schedule leads to unexpected behavior. - -reactor Source(name:string("source"), even:bool(true)) { - output out:bool; - state on:bool(false); - state count:int(0); - logical action next; - - reaction(startup) -> out {= - if (!self->even) { - self->on = true; - } - =} - - reaction(startup, next) -> next, out {= - if (self->on) { - SET(out, true); - } - - self->on = !self->on; - - if (self->count < 9) { - self->count++; - schedule(next, 0); - } else { - schedule(next, MSEC(100)); - self->count = 0; - } - =} -} - -reactor LogicalAnd { - input a:bool; - input b:bool; - - reaction(a, b) {= - printf("Output: %d, tag: (%lld, %u)\n", - a->is_present & b->is_present, get_elapsed_logical_time(), get_microstep() - ); - =} -} - -main reactor Logic { - x = new Source(name="x", even=true); - y = new Source(name="y", even=false); - z = new LogicalAnd(); - - x.out -> z.a after 1 msec; - y.out -> z.b after 1 msec; - //x.out -> z.a; - //y.out -> z.b; -} \ No newline at end of file diff --git a/experimental/C/src/Microsteps/Anomaly.lf b/experimental/C/src/Microsteps/Anomaly.lf deleted file mode 100644 index e35189c142..0000000000 --- a/experimental/C/src/Microsteps/Anomaly.lf +++ /dev/null @@ -1,54 +0,0 @@ - /** - * This program illustrates the strangeness of the way microsteps are currently handled - * (at least as of February, 2021) when scheduling in the future. The two outputs of the - * Source reactor s are never simultaneous. At the Destination reactor d1, they are also - * never simultaneous, but at d2, they are always simultaneous. - * - * @author Edward A. Lee - */ -target C { - timeout: 50 msec, -} -reactor Source { - output out1:int; - output out2:int; - logical action redo; - state count:int(0); - reaction(startup, redo) -> out1, out2, redo {= - if (self->count++ < 4) { - if (self->count % 2 == 0) { - SET (out2, self->count); - } else { - SET(out1, self->count); - } - schedule(redo, 0); - } else { - self->count = 0; - // The following resets the microstep to 0. - // If it were to not do that, then microsteps would - // grow monotonically as execution advances. - schedule(redo, MSEC(10)); - } - =} -} -reactor Destination(name:string("dest")) { - input in1:int; - input in2:int; - reaction (in1, in2) {= - if (in1->is_present) { - printf("%s: Tag (%lld, %d): in1 received %d\n", self->name, get_elapsed_logical_time(), get_microstep(), in1->value); - } - if (in2->is_present) { - printf("%s: Tag (%lld, %d): in2 received %d\n", self->name, get_elapsed_logical_time(), get_microstep(), in2->value); - } - =} -} -main reactor Anomaly { - s = new Source(); - d1 = new Destination(name = "d1"); - d2 = new Destination(name = "d2"); - s.out1 -> d1.in1; - s.out2 -> d1.in2; - s.out1 -> d2.in1 after 5 msec; - s.out2 -> d2.in2 after 5 msec; -} \ No newline at end of file diff --git a/experimental/C/src/ModalModels/BehaviorTrees/robohub_example_advanced.lf b/experimental/C/src/ModalModels/BehaviorTrees/robohub_example_advanced.lf deleted file mode 100644 index 25c703a253..0000000000 --- a/experimental/C/src/ModalModels/BehaviorTrees/robohub_example_advanced.lf +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Implements a modal LF version of the hierarchical statemachine - * for the behavior tree in presented in this article: - * https://robohub.org/introduction-to-behavior-trees/ - * - * Compared to the simple variant this uses modes more extensively, which - * results in the correct behavior. - * Moreover, modeling the sequence in Nominal as modal enables the potential - * use of a history transition that could allow modeling the continuation - * of the task sequence at the point where it was left when the battery ran out. - */ -target C { -// logging: debug -} - -reactor GenericTask(name:string("")) { - output success:bool - output failure:bool - - initial mode Running { - // Just for testing - timer work(0, 250msec) - timer finish(1sec, 1sec) - - reaction(work) {= - printf("%s\n", self->name); - =} - - reaction(finish) -> success, Succeeded, failure, Failed {= - int r = rand() % 6; - if (r == 0) { - SET(failure, true); - SET_MODE(Failed); - } else { - SET(success, true); - SET_MODE(Succeeded); - } - =} - } - - mode Succeeded {} - mode Failed {} -} - -reactor NominalBehavior { - input BatteryOK:bool - - output success:bool - output failure:bool - - initial mode MoveToObj { - MoveToObjTask = new GenericTask(name="MoveToObj") - - MoveToObjTask.failure -> failure - - reaction(MoveToObjTask.success) -> CloseGrip {= - SET_MODE(CloseGrip); - =} - } - - mode CloseGrip { - CloseGripTask = new GenericTask(name="CloseGrip") - - CloseGripTask.failure -> failure - - reaction(CloseGripTask.success) -> MoveHome {= - SET_MODE(MoveHome); - =} - } - - mode MoveHome { - MoveHomeTask = new GenericTask(name="MoveHome") - - MoveHomeTask.failure -> failure - - reaction(MoveHomeTask.success) -> success {= - SET(success, true); - =} - } -} - -reactor Robot { - input BatteryOK:bool - - output success:bool - output failure:bool - - initial mode Nominal { - NominalBehavior = new NominalBehavior() - - NominalBehavior.success -> success - NominalBehavior.failure -> failure - - reaction(BatteryOK) -> Charging {= - if (!BatteryOK->value) { - SET_MODE(Charging); - printf("Battery empty\n"); - } - =} - } - - mode Charging { - GoCharge = new GenericTask(name="GoCharge") - - GoCharge.failure -> failure - - reaction(BatteryOK, GoCharge.success) -> Nominal {= - // Assumes simultaneous presence - if (BatteryOK->value && GoCharge.success->value) { - SET_MODE(Nominal); - printf("Battery charged\n"); - } - =} - } -} - -main reactor { - timer Battery(1sec, 1sec) - state battery_state:int(1) - - robot = new Robot() - - reaction(Battery) -> robot.BatteryOK {= - self->battery_state--; - SET(robot.BatteryOK, self->battery_state > 0); - if (self->battery_state <= 0) { - self->battery_state = 5; - } - =} - - reaction(robot.success) {= - printf("Total success\n"); - request_stop(); - =} - - reaction(robot.failure) {= - printf("Utter failure\n"); - request_stop(); - =} - -} \ No newline at end of file diff --git a/experimental/C/src/ModalModels/BehaviorTrees/robohub_example_simple.lf b/experimental/C/src/ModalModels/BehaviorTrees/robohub_example_simple.lf deleted file mode 100644 index e8937eb1e5..0000000000 --- a/experimental/C/src/ModalModels/BehaviorTrees/robohub_example_simple.lf +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Implements a modal LF version of the hierarchical statemachine - * for the behavior tree in presented in this article: - * https://robohub.org/introduction-to-behavior-trees/ - * - * It implements the core behavior sequence by chaining up reactors. - * However, this currently does not work correctly with modes because - * there is no support for reacting to entering a mode. - * Hence, when switching to Charging the task is not started and if - * switching back to Nominal the sequence would not restart. - */ -target C; - -reactor GenericTask(name:string("")) { - input start:bool - output success:bool - output failure:bool - - logical action continue_task - - reaction(start, continue_task) -> continue_task, success, failure {= - printf("%s\n", self->name); - int r = rand() % 10; - if (r == 0) { - SET(failure, true); - } else if (r >= 6) { - SET(success, true); - } else { - schedule(continue_task, MSEC(250)); - } - =} -} - -reactor Robot { - input start:bool - input BatteryOK:bool - output success:bool - output failure:bool - - initial mode Nominal { - MoveToObj = new GenericTask(name="MoveToObj") - CloseGrip = new GenericTask(name="CloseGrip") - MoveHome = new GenericTask(name="MoveHome") - - start -> MoveToObj.start // No resume after charging - MoveToObj.success -> CloseGrip.start - CloseGrip.success -> MoveHome.start - MoveHome.success -> success - - MoveToObj.failure -> failure - CloseGrip.failure -> failure - MoveHome.failure -> failure - - // Potential solution for resuming after charging -// reaction(entry) -> MoveToObj.start {= -// // PROBLEM!! -// SET(MoveToObj.start, true); -// =} - - reaction(BatteryOK) -> Charging {= - if (!BatteryOK->value) { - SET_MODE(Charging); - printf("Battery empty\n"); - } - =} - } - - mode Charging { - GoCharge = new GenericTask(name="GoCharge") - - GoCharge.failure -> failure - - // Potential solution for starting task when mode is entered because no start event is provided -// reaction(entry) -> GoCharge.start {= -// SET(GoCharge.start, true); -// =} - - reaction(BatteryOK, GoCharge.success) -> Nominal {= - // Assumes simultaneous presence - if (BatteryOK->value && GoCharge.success->value) { - SET_MODE(Nominal); - printf("Battery charged\n"); - } - =} - } -} - -main reactor { - timer Battery(1sec, 1sec) - state battery_state:int(1) - - robot = new Robot() - - reaction(startup) -> robot.start {= - SET(robot.start, true); - =} - - reaction(Battery) -> robot.BatteryOK {= - self->battery_state--; - SET(robot.BatteryOK, self->battery_state > 0); - if (self->battery_state <= 0) { - self->battery_state = 5; - } - =} - - reaction(robot.success) {= - printf("Total success\n"); - request_stop(); - =} - - reaction(robot.failure) {= - printf("Utter failure\n"); - request_stop(); - =} - -} \ No newline at end of file diff --git a/experimental/C/src/ModalModels/Motivation/Chrono/Chrono.lf b/experimental/C/src/ModalModels/Motivation/Chrono/Chrono.lf deleted file mode 100644 index a7d6236fe3..0000000000 --- a/experimental/C/src/ModalModels/Motivation/Chrono/Chrono.lf +++ /dev/null @@ -1,158 +0,0 @@ -/** - * This example illustrates a first idea for the use of modes. - * - * It is inspired by the Chronometer example by - * Jean-Louis Colaço, Bruno Pagano, Marc Pouzet, - * A conservative extension of synchronous data-flow with state machines, - * EMSOFT 2005. - * - * The code was originally derived from ReflexGame.lf - * - * @author Alexander Schulz-Rosengarten - * @author Reinhard von Hanxleden - */ -target C { - threads: 1, - keepalive: true -}; - -/* POSSIBLE NEW SYNTAX FOR DESCRIBING MODES AT LF-LEVEL - * Still open questions include - * Q1: Do we want hierarchy, ie, mixing state/dataflow across levels as in Ptolemy/SCADE? - modes Stop, Start; // Q2: This is actually redundant - still useful? - - mode Stop() { ... // Q3: Should possible successor states ("Start") be listed here as well? - reaction(enter) {= - printf("Entered Stop!"); - } - - reaction(stst) -> Start {= // Q4: Should transitions be encoded as reactions? - changemode(Start); // Q5: Should this be hostcode? Should this be instantaneous? - =} - } - - mode Start() { ... - timer dTimer(10 msec, 10 msec); - - reaction(enter) {= - printf("Entered Start!"); - } - - reaction(dTimer) -> d, s, m {= ... =} - - reaction(stst) -> Stop {= - changemode(Stop); - =} - } - -*/ - -reactor ChronoLogic { - input stst:int; - - // time output - output m:int; - output s:int; - output d:int; - - // time state - state dState:int(0); - state sState:int(0); - state mState:int(0); - - // active state - state ststState:int(0); // 0 = STOP, 1 = START - - // provide base time - timer dTimer(10 msec, 10 msec); - - reaction(startup) {= - printf("'x' + 'Enter' kills the program.\n"); - printf("Just 'Enter' alternates between modes STOP and START.\n\n"); - =} - - reaction(stst) {= - self->ststState = 1 - self->ststState; - if (self->ststState) { - printf("Entered START!\n"); - } else { - printf("Entered STOP!\n"); - } - =} - - reaction(dTimer) -> d, s, m {= - if (self->ststState) { - self->dState = (self->dState + 1) % 100; - if (self->dState == 0) { - self->sState = (self->sState + 1) % 60; - if (self->sState == 0) { - self->mState = (self->mState + 1) % 60; - } - } - // Only create outputs for changes - SET(d, self->dState); - SET(s, self->sState); - SET(m, self->mState); - } - =} -} - -reactor GetUserInput { - preamble {= - void* read_input(void* user_input) { - int c; - while(1) { - c = getchar(); - schedule_copy(user_input, 0, &c, 1); - if (c == EOF) break; - } - return NULL; - } - =} - - physical action user_input:char; - output stst:int; - - reaction(startup) -> user_input {= - // Start the thread that listens for Enter or Return. - pthread_t thread_id; - pthread_create(&thread_id, NULL, &read_input, user_input); - =} - - reaction(user_input) -> stst {= - if (user_input->value == 'x') { - request_stop(); - } else { - SET(stst, 42); - } - =} -} - -reactor PrintOutput { - input m:int; - input s:int; - input d:int; - - reaction(m) {= - printf("m = %d, ", m->value); - =} - - reaction(s) {= - printf("s = %d, ", s->value); - =} - - reaction(d) {= - printf("d = %d\n", d->value); - =} - } - -main reactor Chrono { - in = new GetUserInput(); - chrono = new ChronoLogic(); - out = new PrintOutput(); - - in.stst -> chrono.stst; - chrono.m -> out.m; - chrono.s -> out.s; - chrono.d -> out.d; -} diff --git a/experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg.lf b/experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg.lf deleted file mode 100644 index f5883c1fa0..0000000000 --- a/experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg.lf +++ /dev/null @@ -1,130 +0,0 @@ -/** - * Ptolemy II Example for Modes. - * An SDF model where the ModalModel requires more than one token - * on its input in order to fire. - * http://ptolemy.org/systems/models/modal/ModalSDF/index.html - * - * @author Alexander Schulz-Rosengarten - */ -target Python{ - threads: 0 -}; - -reactor Sinewave(sample_rate(125 usec), frequency(440), phase(0)) { - output data; - - timer rate(0, sample_rate); - - state ramp({=itertools.count(0)=}) - - preamble {= - import math - import itertools - =} - - reaction(rate) -> data {= - x = next(self.ramp) * ((self.frequency * 2 * self.math.pi) * (self.sample_rate / SEC(1))) + self.phase - #print("Sinewave (%f, %f)" % (x, self.math.sin(x))) - data.set(self.math.sin(x)) - =} -} - -reactor ModalModel(sample_size(10)) { - input data; - output out; - - state sample({=[None] * sample_size=}); - state count(0); - - state _mode(0); # Only present w/o mode support - - // These actions only mimic the Ptolemy structure - logical action processAVG - logical action processMAX - - -// initial mode AVG { - - /** @label Mode AVG: Collect */ - reaction(data) -> processAVG {= - if self._mode == 0: # Only present w/o mode support - self.sample[self.count] = data.value - self.count += 1 - if self.count == self.sample_size: - self.count = 0 - processAVG.schedule(0) - =} - - /** @label Mode AVG: Process and Transition */ - reaction(processAVG) -> out {= - if self._mode == 0: # Only present w/o mode support - #print("Processing: ", self.sample) - out.set(sum(self.sample) / self.sample_size) - # Transition to MAX - self._mode = 1 # set_mode(MAX) - =} - -// } -// mode MAX { - - /** @label Mode MAX: Collect */ - reaction(data) -> processMAX {= - if self._mode == 1: # Only present w/o mode support - self.sample[self.count] = data.value - self.count += 1 - if self.count == self.sample_size: - self.count = 0 - processMAX.schedule(0) - =} - - /** @label Mode MAX: Process and Transition */ - reaction(processMAX) -> out {= - if self._mode == 1: # Only present w/o mode support - #print("Processing: ", self.sample) - out.set(max(self.sample)) - # Transition to MAX - self._mode = 0 # set_mode(AVG) - =} -// } -} - -reactor Plotter { - input data; - - state plot_data({=[]=}) - - preamble {= - import matplotlib.pyplot as plt - =} - - reaction(startup) {= - self.plt.ion() - self.plt.title("Plot") - - # No idea why multiple draws are required but it works this way - self.plt.draw() - self.plt.pause(0.00000001) - self.plt.draw() - self.plt.pause(0.00000001) - =} - - reaction(data) {= - #print("Plotting ", data.value) - self.plot_data.append(data.value) - self.plt.stem(self.plot_data) - - # No idea why multiple draws are required but it works this way - self.plt.draw() - self.plt.pause(0.00000001) - self.plt.draw() - self.plt.pause(0.00000001) - =} -} - -main reactor { - s = new Sinewave(sample_rate = 125 msec, frequency = 0.44) - m = new ModalModel() - p = new Plotter() - s.data -> m.data - m.out -> p.data -} diff --git a/experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg_v2.lf b/experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg_v2.lf deleted file mode 100644 index 1e734ff01f..0000000000 --- a/experimental/C/src/ModalModels/Motivation/SineAvgMax/sine_max_avg_v2.lf +++ /dev/null @@ -1,66 +0,0 @@ -/** - * Ptolemy II Example for Modes. - * An SDF model where the ModalModel requires more than one token - * on its input in order to fire. - * http://ptolemy.org/systems/models/modal/ModalSDF/index.html - * - * @author Alexander Schulz-Rosengarten - */ -target Python{ - threads: 0 -}; - -import Plotter, Sinewave from "./sine_max_avg.lf" - -reactor ModalModel(sample_size(10)) { - input data; - output out; - - state sample({=[None] * sample_size=}); - state count(0); - - state _mode(0); # Only present w/o mode support - - logical action process - - reaction(data) -> process {= - if self._mode == 0: # Only present w/o mode support - self.sample[self.count] = data.value - self.count += 1 - if self.count == self.sample_size: - self.count = 0 - process.schedule(0) - =} - -// initial mode AVG { - - /** @label Mode AVG: Process and Transition */ - reaction(process) -> out {= - if self._mode == 0: # Only present w/o mode support - print("Processing: ", self.sample) - out.set(sum(self.sample) / self.sample_size) - # Transition to MAX - self._mode = 1 # set_mode(MAX) - =} - -// } -// mode MAX { - - /** @label Mode MAX: Process and Transition */ - reaction(process) -> out {= - if self._mode == 1: # Only present w/o mode support - print("Processing: ", self.sample) - out.set(max(self.sample)) - # Transition to MAX - self._mode = 0 # set_mode(AVG) - =} -// } -} - -main reactor { - s = new Sinewave(sample_rate = 125 msec, frequency = 0.44) - m = new ModalModel() - p = new Plotter() - s.data -> m.data - m.out -> p.data -} diff --git a/experimental/C/src/ModalModels/ReflexGame/ModalReflexGame.lf b/experimental/C/src/ModalModels/ReflexGame/ModalReflexGame.lf deleted file mode 100644 index a307bc26f7..0000000000 --- a/experimental/C/src/ModalModels/ReflexGame/ModalReflexGame.lf +++ /dev/null @@ -1,150 +0,0 @@ -/** - * This example illustrates the use of logical and physical actions, - * asynchronous external inputs, the use of startup and shutdown - * reactions, and the use of actions with values. - * - * The example is fashioned after an Esterel implementation given by - * Berry and Gonthier in "The ESTEREL synchronous programming language: - * design, semantics, implementation," Science of Computer Programming, - * 19(2) pp. 87-152, Nov. 1992, DOI: 10.1016/0167-6423(92)90005-V. - * - * Version that uses modes! - * - * @author Edward A. Lee - * @author Marten Lohstroh - * @author Alexander Schulz-Rosengarten - */ -target C { - threads: 1, - keepalive: true -}; - -/** - * Produce a counting sequence at random times with a minimum - * and maximum time between outputs specified as parameters. - * - * @param min_time The minimum time between outputs. - * @param max_time The maximum time between outputs. - */ -reactor RandomSource(min_time:time(2 sec), max_time:time(8 sec)) { - preamble {= - // Generate a random additional delay over the minimum. - // Assume millisecond precision is enough. - interval_t additional_time(interval_t min_time, interval_t max_time) { - int interval_in_msec = (max_time - min_time) / MSEC(1); - return (rand() % interval_in_msec) * MSEC(1); - } - =} - input another:int; - output out:int; - logical action prompt(min_time); - state count:int(0); - - reaction(startup) -> prompt {= - printf("***********************************************\n"); - printf("Watch for the prompt, then hit Return or Enter.\n"); - printf("Type Control-D (EOF) to quit.\n\n"); - - // Random number functions are part of stdlib.h, which is included by reactor.h. - // Set a seed for random number generation based on the current time. - srand(time(0)); - - // Schedule the first event. - schedule(prompt, additional_time(0, self->max_time - self->min_time)); - =} - reaction(prompt) -> out {= - self->count++; - printf("%d. Hit Return or Enter!", self->count); - fflush(stdout); - SET(out, self->count); - =} - reaction(another) -> prompt {= - // Schedule the next event. - schedule(prompt, additional_time(0, self->max_time - self->min_time)); - =} -} -/** - * Upon receiving a prompt, record the time of the prompt, - * then listen for user input. When the user hits return, - * then schedule a physical action that records the time - * of this event and then report the response time. - */ -reactor GetUserInput { - preamble {= - // Thread to read input characters until an EOF is received. - // Each time a newline is received, schedule a user_response action. - void* read_input(void* user_response) { - int c; - while(1) { - while((c = getchar()) != '\n') { - if (c == EOF) break; - } - schedule_copy(user_response, 0, &c, 1); - if (c == EOF) break; - } - return NULL; - } - =} - - physical action user_response:char; - state prompt_time:time(0); - state total_time_in_ms:int(0); - state count:int(0); - - input prompt:int; - output another:int; - - reaction(startup) -> user_response {= - // Start the thread that listens for Enter or Return. - lf_thread_t thread_id; - lf_thread_create(&thread_id, &read_input, user_response); - =} - - initial mode Preparing { - reaction(prompt) -> Waiting {= - self->prompt_time = get_logical_time(); - // Switch to mode waiting for user input - SET_MODE(Waiting); - =} - - reaction(user_response) {= - printf("YOU CHEATED!\n"); - request_stop(); - =} - } - mode Waiting { - reaction(user_response) -> another, Preparing {= - int time_in_ms = (get_logical_time() - self->prompt_time) / 1000000LL; - printf("Response time in milliseconds: %d\n", time_in_ms); - self->count++; - self->total_time_in_ms += time_in_ms; - // In the original the time was set to zero to encode the mode. This is no longer necessary. - // self->prompt_time = 0LL; - // Trigger another prompt. - SET(another, 42); - // Switch to mode waiting for next prompt - SET_MODE(Preparing); - =} - } - - reaction(user_response) {= - if (user_response->value == EOF) { - request_stop(); - return; - } - =} - - reaction(shutdown) {= - if (self->count > 0) { - printf("\n**** Average response time: %d.\n", self->total_time_in_ms/self->count); - } else { - printf("\n**** No attempts.\n"); - } - =} -} -main reactor { - p = new RandomSource(); - g = new GetUserInput(); - p.out -> g.prompt; - g.another -> p.another; -} \ No newline at end of file diff --git a/experimental/C/src/PowerTrain/PowerTrain.lf b/experimental/C/src/PowerTrain/PowerTrain.lf deleted file mode 100644 index acef834948..0000000000 --- a/experimental/C/src/PowerTrain/PowerTrain.lf +++ /dev/null @@ -1,243 +0,0 @@ -target C {threads: 1, keepalive: true, flags: "-lncurses"}; - -/** - * FIXME: there are still two problems with this implementation: - * 1. Use of a global variable to let the ncurses thread find the actions - * that it should schedule. It would be helpful to have some kind of generic - * framework for setting up interactive tests. - * 2. When the program runs out of space on the terminal, newlines are ignored. - * It is not clear to me why this is the case, but I've only just now started - * using ncurses, which is a pretty rich toolkit. More on ncurses here: - * http://web.mit.edu/barnowl/src/ncurses/ncurses-5.4/doc/html/ncurses-intro.html - * - * @author Marten Lohstroh - */ -preamble {= - #include - #include - #include - - struct { - void* brake; - void* accelerate; - } pedals; - - int calc_brake_force(int angle) { // ~0-600 Nm in Tesla Model 3 - return 110 * angle; - } - - int calc_motor_torque(int angle) { - return 100 * angle; - } - - void* read_input(void* arg) { - initscr(); - noecho(); // Don't echo input - cbreak(); // Don't interrupt for user input - timeout(1); // Wait for key press in ms - printw("***************************************************************\n"); - printw("Press '1-6' to change the angle of the accelerator\n"); - printw("Press 'q-y' to change the angle of the brake pedal\n"); - char c = 0; // Command: [1-6|q-y] - int v = 0; - while (true) { - bool skip = false; // Whether to schedule and event or not - c = getch(); - switch(c) { - case 'q': - v = 0; - break; - case 'w': - v = 1; - break; - case 'e': - v = 2; - break; - case 'r': - v = 3; - break; - case 't': - v = 4; - break; - case 'y': - v = 5; - break; - default: - skip = true; - break; - } - - if (!skip) { - schedule_int(pedals.brake, 0, v); - } - - skip = false; - switch(c) { - case '1': - v = 0; - break; - case '2': - v = 1; - break; - case '3': - v = 2; - break; - case '4': - v = 3; - break; - case '5': - v = 4; - break; - case '6': - v = 5; - break; - default: - skip = true; - break; - } - - if (!skip) { - schedule_int(pedals.accelerate, 0, v); - } - } - endwin(); - return 0; - } - pthread_t curses_thread; - bool initialized = false; - void init_sensors() { - if (!initialized) { - pthread_create(&curses_thread, NULL, &read_input, NULL); - initialized = true; - } - } -=} - -reactor MotorControl { - - input brkOn:bool; - input angle:int; - output torque:int; - state braking:bool(true); - - //@label Set torque to zero if car is braking - reaction(brkOn) -> torque {= - self->braking = brkOn->value; - SET(torque, 0); - =} - - //@label Adjust torque unless car is braking - reaction(angle) -> torque {= - if (!self->braking) { - SET(torque, calc_motor_torque(angle->value)); - } else if (angle->value > 0){ - printw("Cannot accelerate; release brake pedal first.\n"); - } - =} -} - -reactor BrakePedal { - output angle:int; - output applied:bool; - - physical action a(0, 1 msec, "replace"):int; - state last:int(1); - - // @label Setup callback - reaction(startup) -> a {= - pedals.brake = a; - init_sensors(); - =} - - // @label Output reported angle - reaction(a) -> angle, applied {= - if (self->last != a->value) { - if (self->last == 0) { - SET(applied, true); // zero to nonzero - } else if (a->value == 0) { - SET(applied, false); // nonzero to zero - } - - self->last = a->value; - - SET(angle, a->value); - } - =} -} - -reactor Accelerator { - - state pedal:int(-1); - physical action a(0, 2 msec, "replace"):int; - output angle:int; - state last:int(0); - - // @label Setup callback - reaction(startup) -> a {= - pedals.accelerate = a; - init_sensors(); - =} - - // @label Output reported angle - reaction(a) -> angle {= - if (self->last != a->value) { - SET(angle, a->value); - self->last = a->value; - } - =} - -} - -// @label Apply the requested force -reactor Brakes { - input force:int; - - // @label Reaction with deadline - reaction(force) {= - printw("Adjusting brake power to %dN; on time!\n", force->value); - =} deadline (2 msec) {= - printw("Adjusting brake power to %dN; too late!\n", force->value); - =} -} - -// @label Apply the requested torque -reactor Motor { - input torque:int; - - // @label Reaction with deadline - reaction(torque) {= - printw("Adjusting engine torque to %dNm; on time!\n", torque->value); - =} deadline (3 msec) {= - printw("Adjusting engine torque to %dNm; too late!\n", torque->value); - =} -} - -// @label Adjust the force -reactor BrakeControl { - input angle:int; - output force:int; - - reaction(angle) -> force {= - SET(force, calc_brake_force(angle->value)); - =} -} - -/** - * Reactor that implements a simplified power train control module. - */ -main reactor PowerTrain { - - bp = new BrakePedal(); - a = new Accelerator(); - bc = new BrakeControl(); - mc = new MotorControl(); - b = new Brakes(); - m = new Motor(); - - bp.angle -> bc.angle; - bc.force -> b.force; - bp.applied -> mc.brkOn; - mc.torque -> m.torque; - a.angle -> mc.angle; - -} diff --git a/experimental/C/src/PowerTrain/README.md b/experimental/C/src/PowerTrain/README.md deleted file mode 100644 index 4822829806..0000000000 --- a/experimental/C/src/PowerTrain/README.md +++ /dev/null @@ -1 +0,0 @@ -This example does not compile yet, but eventually will illustrate an automotive power train. \ No newline at end of file diff --git a/experimental/C/src/ScatterGather.lf b/experimental/C/src/ScatterGather.lf deleted file mode 100644 index c73ebcf298..0000000000 --- a/experimental/C/src/ScatterGather.lf +++ /dev/null @@ -1,128 +0,0 @@ -/** - * This program is a concept demonstration showing how higher-order - * combinators could be defined in the C target. This example has - * a ScatterGather reactor that, upon receiving an input array, - * creates as many instances of a Worker reactor as there are elements - * in the array, distributes the elements of the array to those workers, - * and puts onto the reaction queue all reactions in the workers that are - * sensitive to those inputs. When the workers are finished executing, - * a second reaction in ScatterGather will execute. That second reaction - * collects all the results of the workers into an array and sends the - * result to the output. - * - * This program should not be used as a model for creating designs. - * It uses internal, implementation-specific details that will eventually - * be abstracted out into an API. - */ -target C { - threads: 4 -}; -reactor Source { - output out:int[]; - reaction(startup) -> out {= - SET_NEW_ARRAY(out, 8); - for (int i=0; i < 8; i++) { - out->value[i] = i; - } - =} -} -reactor Worker(id:int(2)) { - input worker_in:int; - output worker_out:int; - reaction(worker_in) -> worker_out {= - printf("Worker received in first reaction%d\n", worker_in->value); - SET(worker_out, worker_in->value * self->id); - =} - reaction(worker_in) {= - printf("Worker received second %d\n", worker_in->value); - =} -} -reactor ScatterGather2 { - input in:int[]; - output out:int[]; - - // Create a state variable to pass the workers created in - // the first reaction to the second reaction. This is an array - // of pointers to the self struct of the worker. - // The type of a self struct is the reactor class name, converted to - // lower case, followed by _self_t. - state workers:worker_self_t**({=NULL=}); - - // The data type of the upstream source does not match the - // data type of the workers' inputs, so we have to create an - // array of places to store the input data. - state worker_inputs:worker_worker_in_t*({=NULL=}); - - // Create a template worker, which ensures that downstream - // levels are correct. The template worker could have any - // number of reactions sensitive to the input we will provide. - template_worker = new Worker(); - reaction(in) -> template_worker.worker_in {= - SET(template_worker.worker_in, in->value[0]); - // SCATTER(in, worker_in, Worker, self->workers, id); - // Create an array of pointers to work self structs. - self->workers = malloc(in->length * sizeof(worker_self_t*)); - // The data type of the upstream source does not match the - // data type of the workers' inputs, so we have to create an - // array of places to store the input data. - self->worker_inputs = malloc(in->length * sizeof(worker_worker_in_t)); - pthread_mutex_lock(&mutex); - for (int i=1; i < in->length; i++) { - printf("Initializing worker %d\n", i); - self->workers[i] = new_Worker(); - self->workers[i]->id = i; - printf("HERE %d\n", i); - self->workers[i]->_lf_worker_in = &self->worker_inputs[i]; - self->worker_inputs[i].is_present = true; - self->worker_inputs[i].value = in->value[i]; // Copies the value or pointer. - self->worker_inputs[i].num_destinations = 1; - trigger_t worker_trigger = self->workers[i]->_lf__worker_in; - for(int j = 0; j < worker_trigger.number_of_reactions; j++) { - worker_trigger.reactions[j]->index = self->_lf__reaction_0.index + j + 1; - // The chain_id is the same for each worker, which ensures that the - // second reaction below will not be invoked before all the workers - // have finished. The second reaction below has a larger level than - // the workers, and since the chain_id overlaps, the workers must - // finish before the reaction will execute. - worker_trigger.reactions[j]->chain_id = self->_lf__reaction_0.chain_id; - pqueue_insert(reaction_q, worker_trigger.reactions[j]); - } - printf("Initialized worker %d\n", i); - } - // Signal all the worker threads. - pthread_cond_broadcast(&reaction_q_changed); - pthread_mutex_unlock(&mutex); - =} - reaction(in, template_worker.worker_out) -> out {= - SET_NEW_ARRAY(out, in->length); - // FIXME: We should be checking template_worker.worker_out_is_present. - // But what do we do if it is not present? - out->value[0] = template_worker.worker_out->value; - for (int i=1; i < in->length; i++) { - printf("Gather received %d at index %d.\n", self->workers[i]->_lf_worker_out.value, i); - out->value[i] = self->workers[i]->_lf_worker_out.value; - } - // FIXME: Invoke the destructor for each of the workers, once - // they have a destructor. - =} -} -reactor Print { - input in:int[]; - reaction(in) {= - printf("["); - for(int i = 0; i < in->length; i++) { - printf("%d", in->value[i]); - if (i < in->length - 1) { - printf(", "); - } - } - printf("]\n"); - =} -} -main reactor { - s = new Source(); - g = new ScatterGather2(); - p = new Print(); - s.out -> g.in; - g.out -> p.in; -} \ No newline at end of file diff --git a/experimental/C/src/SieveOfEratosthenes.lf b/experimental/C/src/SieveOfEratosthenes.lf deleted file mode 100644 index fdb89853bd..0000000000 --- a/experimental/C/src/SieveOfEratosthenes.lf +++ /dev/null @@ -1,137 +0,0 @@ -/** - * This program is a concept demonstration showing how higher-order - * combinators could be defined in the C target. This example has - * an Sieve reactor that, upon receiving an integer input, feeds - * it through a growing chain of Filter reactors, each of which filters - * out from the stream multiples of one prime number. When a number - * emerges from the end of the chain, it is prime. This number triggers - * a reaction in the Sieve reactor that forwards the prime to its output - * and then splices in a new Filter reactor to filter out multiples of - * this primte. The Sieve starts with one Filter which filters out - * multiples of 2. - * - * This program should not be used as a model for creating designs. - * It uses internal, implementation-specific details that will eventually - * be abstracted out into an API. - * - * @author Edward A. Lee - */ -target C { - fast: true -} -reactor Count { - timer t(0, 1 sec); - output out:int; - // Start at 3 because 1 and 2 are taken care of. - state c:int(3); - reaction(t) -> out {= - SET(out, self->c); - // printf("Count sent %d\n", self->c); - self->c++; - =} -} -reactor Filter(prime:int(2)) { - input in:int; - output out:int; - - reaction(in) -> out {= - // printf("Filter for prime %d received %d\n", self->prime, in->value); - if (in->value % self->prime != 0) { - // printf("Filter forwarding %d\n", in->value); - SET(out, in->value); - } - =} -} -reactor Sieve { - input in:int; - output out:int; - - // Create the first filter. - filter2 = new Filter(prime = 2); - in -> filter2.in; - - // React to a new prime number. - reaction(filter2.out) -> out {= - // Forward the new prime number. - // printf("Sieve received prime: %d\n", filter2.out->value); - SET(out, filter2.out->value); - - // Splice in a new Filter. - filter_self_t* filter = new_Filter(); - filter->prime = filter2.out->value; - filter->_lf_out_width = -2; // FIXME: Why isn't this done by the constructor? - filter->_lf_in_width = -2; // FIXME: Why isn't this done by the constructor? - filter->_lf__reaction_0.num_outputs = 1; - filter->_lf__reaction_0.triggers = (trigger_t***)calloc(1, sizeof(trigger_t**)); - filter->_lf__reaction_0.triggered_sizes = (int*)calloc(1, sizeof(int)); - filter->_lf__reaction_0.output_produced = (bool**)calloc(1, sizeof(bool*)); - filter->_lf__reaction_0.output_produced[0] = &filter->_lf_out.is_present; - - // Get a pointer to the self struct of the last reactor in the chain. - assert(self->_lf__reaction_0.last_enabling_reaction == &last->_lf__reaction_0); - filter_self_t *last = (filter_self_t*)self->_lf__reaction_0.last_enabling_reaction->self; - - // Adjust last_enabling_reaction for optimized execution of a chain. - self->_lf__reaction_0.last_enabling_reaction = &filter->_lf__reaction_0; - filter->_lf__reaction_0.last_enabling_reaction = &last->_lf__reaction_0; - - filter->_lf_out.num_destinations = 1; - - filter->_lf__reaction_0.triggered_sizes[0] = 1; - trigger_t** trigger_array = (trigger_t**)calloc(1, sizeof(trigger_t*)); - filter->_lf__reaction_0.triggers[0] = trigger_array; - - // Inherit destination triggers from the last filter in the chain. - filter->_lf__reaction_0.triggers[0][0] = last->_lf__reaction_0.triggers[0][0]; - - // Update the triggers of the last reactor to point to the new filter... - last->_lf__reaction_0.triggers[0][0] = &filter->_lf__in; - - // The input data of the new filter comes from the output of the last filter. - filter->_lf_in = (filter_in_t*)&last->_lf_out; - - // The container's reaction gets its data from the new filter from now on. - self->_lf_filter2.out = (filter_out_t*)&filter->_lf_out; - - // Unfortunately, need to reallocate the arrays that indicate the is_present - // fields because now there are two more. - // FIXME: This should be done in large chunks rather than on each call? - bool** new_memory = (bool**)realloc(_lf_is_present_fields, (_lf_is_present_fields_size + 1) * sizeof(bool*)); - if (new_memory == NULL) { - error_print("Out of memory!"); - request_stop(); - } else { - _lf_is_present_fields = new_memory; - - new_memory = (bool**)realloc(_lf_is_present_fields_abbreviated, (_lf_is_present_fields_size + 1) * sizeof(bool*)); - if (new_memory == NULL) { - error_print("Out of memory!"); - request_stop(); - } else { - _lf_is_present_fields_abbreviated = new_memory; - } - _lf_is_present_fields_size += 1; - - _lf_is_present_fields[_lf_is_present_fields_size - 1] = &filter->_lf_out.is_present; - - // NOTE: The level of the reaction in the newly created reactor should be set - // and downstream reactions incremented, but levels have no effect on this system. - // In fact, the reaction queue never has more than one item on it. - } - =} -} -reactor Print(stop_after:int(10000)) { - input in:int; - state count:int(1); - reaction(in) {= - printf("Prime %d: %d\n", self->count++, in->value); - if (self->count > self->stop_after) request_stop(); - =} -} -main reactor { - c = new Count(); - s = new Sieve(); - p = new Print(); - c.out -> s.in; - s.out -> p.in; -} \ No newline at end of file diff --git a/experimental/C/src/SpatAnalysis/MQTTPublisher.lf b/experimental/C/src/SpatAnalysis/MQTTPublisher.lf deleted file mode 100644 index 2ac705662b..0000000000 --- a/experimental/C/src/SpatAnalysis/MQTTPublisher.lf +++ /dev/null @@ -1,183 +0,0 @@ -/** - * Reactor that publishes strings to a specified MQTT topic. - * See MQTTPhysical and MQTTLogical for prerequisites and an example usage. - * - * @author Ravi Akella - * @author Edward A. Lee - */ -target C; - -/** - * Reactor that publishes strings to a specified MQTT topic. - * This publisher appends to the end of the message the - * timestamp of the message at the publisher. The receiving - * end adds to this timestamp a specified offset, and if that - * offset is larger than the current physical time at which it - * receives the message, assigns that incremented timestamp - * to the message. If the offset is always greater than or - * equal to the transport latency plus the clock synchronization - * error, then the overall program remains deterministic. - * - * This publisher also ensures in-order delivery messages to - * subscribers. If an attempt is made to send a message before - * the delivery of the previous message has completed, then the reaction - * that sends the message (the reaction to an input 'in') will - * block until the previous delivery has completed. - * - * @param topic The topic name on which to publish. - * @param address The IP address of the MQTT broker. - * @param clientID The name of the client instance. - * These names are required to be unique. - * @param include_physical_timestamp If true, include - * in the message the time of the local physical clock - * when the message is sent. - * @see MQTTSubscriber. - */ -reactor MQTTPublisher ( - topic:string("DefaultTopic"), - address:string("tcp://localhost:1883"), - clientID:string("DefaultPublisher"), - include_physical_timestamp:int(0) -) { - preamble {= - #include "MQTTClient.h" - #include "core/util.h" - - // Timeout for completion of message sending in milliseconds. - #define TIMEOUT 10000L - - // Connection options for the client. - // Making this global means that all instances of this reactor have - // the same connection options. - MQTTClient_connectOptions pub_connect_options = MQTTClient_connectOptions_initializer; - - // Struct type used to keep track of messages in flight between reactions. - typedef struct inflight_t { - bool message_in_flight; - MQTTClient_deliveryToken delivery_token; - char* message; - } inflight_t; - - // Callback invoked once delivery is complete. - void pub_delivered(void *inflight, MQTTClient_deliveryToken dt) { - // printf("DEBUG: Message with token value %d delivery confirmed\n", dt); - ((inflight_t*)inflight)->message_in_flight = false; - free(((inflight_t*)inflight)->message); - } - // Callback invoked if the connection is lost. - void pub_connection_lost(void *context, char *cause) { - printf("\nMQTTPublisher: Connection lost\n"); - printf(" cause: %s\n", cause); - } - =} - /** - * Input type char* instead of string is used for dynamically - * allocated character arrays (as opposed to static constant strings). - */ - input in:char*; - - /** State variable that keeps track of a message in flight. */ - state inflight:inflight_t({={false, 0, NULL}=}); - - /** The client object. */ - state client:MQTTClient({=NULL=}); - - /** The message object. */ - state mqtt_msg:MQTTClient_message({=MQTTClient_message_initializer=}); - - /** Connect to the broker. Exit if this fails. */ - reaction(startup){= - MQTTClient_create(&self->client, self->address, self->clientID, MQTTCLIENT_PERSISTENCE_NONE, NULL); - pub_connect_options.keepAliveInterval = 20; - pub_connect_options.cleansession = 1; - - // Set up callback functions. - // Second to last argument should be a pointer to a function - // to handle notification of delivery of a message. - // But this reactor isn't sending any messages. - // Second argument is a pointer to context that will be passed to pub_delivered, - // which in this case is a pointer to the inflight state variable. - MQTTClient_setCallbacks(self->client, &self->inflight, pub_connection_lost, NULL, pub_delivered); - - // Connect to the broker. - int rc; // response code. - if ((rc = MQTTClient_connect(self->client, &pub_connect_options)) != MQTTCLIENT_SUCCESS) { - fprintf(stderr, "MQTTPublisher: Failed to connect to MQTT broker.\n"); - fprintf(stderr, "Perhaps one is not running? Return code: %d\n", rc); - exit(EXIT_FAILURE); - } - // printf("DEBUG: MQTTPublisher connected to broker.\n"); - =} - - /** - * React to an input by sending a message with the value of the input as the payload. - * If delivery has not yet completed for a previously sent message, then wait for - * it to complete before proceeding (blocking this reaction). - * This copies the message from the input into a buffer, so the input can - * freed upon return of this reaction (LF will automatically decrement its - * reference count). - */ - reaction(in) {= - if(self->inflight.message_in_flight) { - // Wait for message delivery to be complete. - // printf("DEBUG: Waiting for publication of previous message\n"); - int rc = MQTTClient_waitForCompletion(self->client, self->inflight.delivery_token, TIMEOUT); - if (rc != MQTTCLIENT_SUCCESS) { - fprintf(stderr, "ERROR: Message delivery failed with error code %d.\n", rc); - fprintf(stderr, "Message: %s\n", in->value); - fprintf(stderr, "On topic '%s' for publisher with ClientID: %s\n", self->topic, self->clientID); - } - } - //printf("DEBUG: Publishing message: %s\n", in->value); - // printf("DEBUG: on topic '%s' for publisher with ClientID: %s\n", self->topic, self->clientID); - - // Allocate memory for a copy of the message. - // The length includes the null-terminator of the string and 8 bytes for the timestamp. - int length = strlen(in->value) + 1 + sizeof(instant_t); - if (self->include_physical_timestamp) { - length += sizeof(instant_t); - } - self->inflight.message = malloc(sizeof(char) * length); - memcpy(self->inflight.message, in->value, strlen(in->value) + 1); - - // Append the current timestamp to the message. - // This is always last, after the physical timestamp if it is included. - encode_ll(get_logical_time(), - (unsigned char*)(self->inflight.message + length - sizeof(instant_t)) - ); - // printf("DEBUG: Timestamp of sending message: %lld.\n", *timestamp); - - self->mqtt_msg.payload = self->inflight.message; - self->mqtt_msg.payloadlen = length; - - // QoS 2 means that the message will be delivered exactly once. - self->mqtt_msg.qos = 2; - - // Retained messages are held by the server and sent to future new subscribers. - // Specify that this message should not be retained. - // It will be sent only to subscribers currently subscribed. - self->mqtt_msg.retained = 0; - - // As close as possible to the publishing of the message, insert - // the physical timestamp if it has been requested. - if (self->include_physical_timestamp) { - encode_ll(get_physical_time(), - (unsigned char*)(self->inflight.message + length - 2 * sizeof(instant_t)) - ); - } - //For Dashboard, echo physical time stamp - else { - printf("EVENT: sender_pts: %lld\n", get_physical_time()); - } - - MQTTClient_publishMessage(self->client, self->topic, &self->mqtt_msg, &self->inflight.delivery_token); - self->inflight.message_in_flight = true; - =} - - /** Disconnect the client. */ - reaction(shutdown) {= - printf("MQTTPublisher: Client ID %s disconnecting.\n", self->clientID); - MQTTClient_disconnect(self->client, 10000); - MQTTClient_destroy(&self->client); - =} -} diff --git a/experimental/C/src/SpatAnalysis/MQTTSubscriber.lf b/experimental/C/src/SpatAnalysis/MQTTSubscriber.lf deleted file mode 100644 index f556e4cb52..0000000000 --- a/experimental/C/src/SpatAnalysis/MQTTSubscriber.lf +++ /dev/null @@ -1,268 +0,0 @@ -/** - * Reactor that subscribes to a specified MQTT topic on which - * string messages are published. - * See MQTTPhysical and MQTTLogical for prerequisites and an example usage. - * - * @author Ravi Akella - * @author Edward A. Lee - */ -target C; - -/** - * Reactor that subscribes to a specified MQTT topic on which - * string messages are published. - * This reactor extracts the sender's timestamp from the message - * and adds to this timestamp the specified offset. If that - * offset is larger than the current physical time at which this - * reactor receives the message, then it assigns that incremented timestamp - * to the message. Otherwise, the received message gets a timestamp - * equal to the physical time at which the message is received. - * - * In addition, if the corresponding MQTTPublisher has its - * parameter include_physical_timestamp set to non-zero, then - * this reactor measures the physical transport time from the - * time of sending (as reported by the physical clock at the sender) - * and the time at which this reactor's reaction is invoked - * (as reported by the physical clock at this receiver). - * - * If the offset is always greater than or equal to the sum of - * transport latency, clock synchronization error, and execution - * times between creation of the timestamped event at the sender - * and its reception at the receiver, then the overall program - * remains deterministic. - * Hence, a good practice, if the application can tolerate the - * latency, is to set the offset to be larger than this expected - * communication latency between a publisher and a subscriber. - * To determine whether an execution was deterministic in this - * sense, set include_physical_timestamp = 0 at the sender, - * set a non-zero offset at the receiver, and see if the reported - * average latency equals the reported maximum latency. If they - * are equal, then execution was deterministic. - * - * By default, the offset is 0, so the assigned timestamp will - * always be the physical time at which the message is received, - * assuming the clock synchronization error is less than the - * transport latency. With this setting, this timestamp increment - * also measures the total communication latency. If the sender has - * include_physical_timestamp set to zero, then this reactor - * reports at the end of execution the average and - * maximum timestamp increments rather than physical latencies. - * - * @param topic The topic name on which to publish. - * @param address The IP address of the MQTT broker. - * @param clientID The name of the client instance. - * These names are required to be unique. - * @param offset The offset to add to the sender's timestamp. - * @see MQTTPublisher. - */ - -reactor MQTTSubscriber ( - address:string("tcp://localhost:1883"), - clientID:string("DefaultSubscriber"), - topic:string("DefaultTopic"), - offset:time(0 msec) -) { - preamble {= - #include "MQTTClient.h" - #include "core/util.h" - #include - - #define QOS 2 - #define TIMEOUT 10000L - - // Connection options for the client. - // Making this global means that all instances of this reactor have - // the same connection options. - MQTTClient_connectOptions sub_connect_options = MQTTClient_connectOptions_initializer; - - // Callback function invoked by MQTT when a message arrives. - int message_arrived( - void *incoming_message, - char *topicName, - int topicLen, - MQTTClient_message *message - ) { - instant_t receive_physical_time = get_physical_time(); - // If a physical timestamp was sent, report the transport time. - size_t string_length = strlen((char*)message->payload); // Assumes null-terminated string. - if (message->payloadlen == string_length + 1 + 2*sizeof(instant_t)) { - instant_t* physical_timestamp = (instant_t*)((char*)message->payload + string_length + 1); - // printf("DEBUG: MQTTReceiver.message_arrived: Received message after measured latency of %lld nsec (assuming synchronized clocks).\n", receive_physical_time - *physical_timestamp); - } - - // printf("DEBUG: MQTTSubscriber: Message arrived on topic %s: %s\n", topicName, (char*)message->payload); - - // Extract the timestamp and calculate delay from current_time to that timestamp. - // Note that if this subscriber's current time is ahead of current time - // at the publisher (something that should not happen even in a distributed - // implementation), then this extra delay may be negative. If it becomes - // less than -offset, then the schedule_copy() function will complain - // by printing a warning to stderr and revise the timestamp to current time. - // First acquire the mutex lock. Otherwise, logical time could have a big - // jump between this calculation and the call to schedule, resulting in a long delay. - - // NOTE: Since schedule_copy also acquires this lock, we assume here that the - // pthreads library correctly implements recursive mutex locks to unlock all - // locks held by the current thread when waiting for signals. - pthread_mutex_lock(&mutex); - - instant_t timestamp = extract_ll((unsigned char*)message->payload + message->payloadlen - sizeof(instant_t)); - interval_t delay = timestamp - get_logical_time(); - // printf("DEBUG: MQTTSubscriber.message_arrived: received timestamp that is %lld ahead of current_time %lld.\n", *timestamp - start_time, current_time); - // printf("DEBUG: MQTTSubscriber.message_arrived: physical time is ahead of current logical time by: %lld.\n", receive_physical_time - current_time); - - // Schedule the event. Since incoming_message is a physical action, - // the offset specified in the second argument will be added to current_time - // and to the min_delay in the action and then compared to physical time. - // If the sum is greater than physical time - // (i.e. if the offset + min_delay is large enough), then the event will be scheduled at - // exactly the logical time at the publisher plus the offset. - // Otherwise, it will be scheduled at the current physical time. - // The incoming message is in dynamically allocated memory. - // We copy the message using schedule_copy() because, unfortunately, Paho MQTT uses its own - // version of malloc() and free() (defined in Heap.h and Heap.c). - // We could modify Paho MQTT to use the generic malloc() and free(), - // and then we could use schedule_value() to avoid the copy. - // Note that the last 8 bytes of the message are the sender's timestamp. - // We include that in the copy so that the reaction to the physical action - // can measure the latency. - schedule_copy(incoming_message, delay, (char*)message->payload, message->payloadlen); - - pthread_mutex_unlock(&mutex); - - // MQTTClient_freeMessage() also frees the memory allocated to the payload, - // which is why we have to copy the message here. - MQTTClient_freeMessage(&message); - MQTTClient_free(topicName); - - // Return true to indicate that the message has been successfully handled. - return 1; - } - - /** Callback invoked if the connection is lost. */ - void sub_connection_lost(void *incoming_message, char *cause) { - printf("\nConnection lost\n"); - printf(" cause: %s\n", cause); - } - =} - - /** - * Output for sending the incoming MQTT message. - * Use type char* rather than string because it is not - * a static string, but rather dynamically allocated memory. - */ - output message:char*; - - /** - * Action that is triggered when there is an incoming MQTT message. - * Use a physical action here so that the logical time when the action - * is triggered is the physical time of arrival of the message if - * the offset is 0. If the offset is larger than the communication - * latency plus the clock synchronization error, - * then the incoming message will have have a timestamp deterministically - * larger than the sender's timestamp by the offset. - */ - physical action incoming_message(offset):char*; - - /** - * State variable storing the MQTT client created for each instance of this reactor. - */ - state client:MQTTClient({=NULL=}); - - /** - * Maximum observed latency from the originator of the message to here. - */ - state max_latency:time(0); - - /** - * Sum of all observed latencies. - */ - state latencies:time(0); - - /** - * Count of messages. - */ - state count:int(0); - - reaction(startup) -> incoming_message {= - MQTTClient_create(&self->client, self->address, self->clientID, MQTTCLIENT_PERSISTENCE_NONE, NULL); - sub_connect_options.keepAliveInterval = 20; - sub_connect_options.cleansession = 1; - - // Set up callback functions. - // Last argument should be a pointer to a function to - // handle notification of delivery of a sent message. - // But this reactor isn't sending any messages. - MQTTClient_setCallbacks(self->client, incoming_message, sub_connection_lost, message_arrived, NULL); - - // Connect to the broker. - int rc; // response code. - if ((rc = MQTTClient_connect(self->client, &sub_connect_options)) != MQTTCLIENT_SUCCESS) { - fprintf(stderr, "MQTTSubscriber: Failed to connect to MQTT broker.\n"); - fprintf(stderr, "Perhaps one is not running? Return code: %d\n", rc); - exit(EXIT_FAILURE); - } - - MQTTClient_subscribe(self->client, self->topic, QOS); - =} - - reaction(incoming_message) -> message {= - self->count++; - - // The incoming_message action contains a token that we can just forward. - // The allocated memory will be freed when the token's reference count hits 0. - // Note that this token will still contain the sender's timestamp. - SET_TOKEN(message, incoming_message->token); - - // Get the sender's timestamp. - instant_t* timestamp = (instant_t*)( - incoming_message->token->value + incoming_message->token->length - sizeof(instant_t) - ); - //printf("DEBUG: Received message carrying timestamp %lld.\n", self->count, *timestamp); - - // If the offset is 0, then the latency will be a measure of - // the physical latency between creation of the message at the sender - // and its receipt here, offset by the clock synchronization error, - // assuming that the sender sent the message at a physical time matching its - // logical timestamp. - instant_t logical_timestamp = get_logical_time(); - interval_t latency = logical_timestamp - *timestamp; - // printf("DEBUG: MQTTSubscriber.reaction: Received timestamp is larger than sent timestamp by: %lld.\n", latency); - printf("EVENT: message_id: %d\n", self->count); - printf("EVENT: sender_lts: %lld\n", *timestamp); - printf("EVENT: receiver_lts: %lld\n", logical_timestamp); - printf("EVENT: logical_latency: %lld\n", latency); - - // If a physical timestamp was sent, use that to collect - // latency stats instead of the logical time increment. - size_t string_length = strlen(incoming_message->value); // Assumes null-terminated string. - if (incoming_message->token->length == string_length + 1 + 2*sizeof(instant_t)) { - instant_t receive_physical_time = get_physical_time(); - instant_t physical_timestamp = extract_ll((unsigned char*)(incoming_message->value + string_length + 1)); - latency = receive_physical_time - physical_timestamp; - printf("EVENT: sender_pts: %lld\n", physical_timestamp); - printf("EVENT: receiver_pts: %lld\n", receive_physical_time); - printf("EVENT: physical_latency: %lld\n", latency); - //printf("EVENT: MQTTReceiver.reaction: Reacted to message after measured latency of %lld nsec (assuming synchronized clocks).\n", latency); - } - else{ //physical timestamp was not sent, echo physical timestamp for dashboard - printf("EVENT: receiver_pts: %lld\n", get_physical_time()); - } - - self->latencies += latency; - - if (latency > self->max_latency) { - self->max_latency = latency; - } - =} - - reaction(shutdown) {= - printf("MQTTSubscriber: Maximum latency measured at receiver (in nsec): %lld.\n", self->max_latency); - if (self->count > 0) { - printf("MQTTSubscriber: Average latency measured at receiver (in nsec): %lld.\n", self->latencies/self->count); - } - printf("MQTTSubscriber: Client ID %s disconnecting.\n", self->clientID); - MQTTClient_disconnect(self->client, 10000); - MQTTClient_destroy(&self->client); - =} -} diff --git a/experimental/C/src/SpatAnalysis/README.md b/experimental/C/src/SpatAnalysis/README.md deleted file mode 100644 index bea2e7a9bd..0000000000 --- a/experimental/C/src/SpatAnalysis/README.md +++ /dev/null @@ -1,58 +0,0 @@ -## Logging and analysis of Lingua Franca programs - -This example demonstrates logging and analysis of the timing of publish-and-subscribe messages sent between Lingua Franca programs using MQTT. - -### Requirements: -This example has quite a few dependencies that must be installed and run: - - [MQTT C Client library](https://github.com/eclipse/paho.mqtt.c) (used by the MQTT publisher and subscriber reactors) - - MQTT broker, such as [mosquitto](https://mosquitto.org). This needs to be running after installation. - - [InfluxDB](https://docs.influxdata.com/influxdb/v1.8/introduction/install/) , a time-series database. This needs to be started after installation: - - Start `influxd` in the background or run `brew services start influxdb`. (`brew services list` will show that InfluxDB is running.) - - Drop into influx shell using `influx` on terminal and create a database using 'CREATE DATABASE '. I used LF_EVENTS as the database name. - - databaseName: check if this database exists from influx shell using `SHOW DATABASES` - - [Grafana](https://grafana.com/docs/grafana/latest/), a browser-based data visualizer. This too needs to be started after installation (e.g. using `brew services start grafana` on MacOS). - - Check that you can access the frontend from browser over the default port http://localhost:3000. You change the default port as in here (https://grafana.com/docs/grafana/latest/administration/configuration/) - - [InfluxBD client for node](https://github.com/node-influx/node-influx): `npm install --save influx` - - This is needed by the `InfluxWrite` reactor, which reads the traces and writes them to the InfluxDB database. - - Check the following in this reactor - - measurementName: default "event_times". If you change this, the Grafana query in the following steps will change as well - - traceFilePath: "/relative/path/to/trace/my_trace" - - schema: Modify the data schema of your Influx database record based on the `EVENT: : ` pattern used in the trace file. - -### Steps: - - Run the program to create a trace file (eg:"my_trace"). The publisher and the subscriber need to be run separately: - - Terminal 1$: `lfc spatRecommender.lf; bin/spatRecommender` - - Terminal 2$: `lfc spatReceiver.lf; bin/spatReceiver > my_trace` -- Then run the "influxWrite" reactor to route the trace data from the file to InfluxDB: - - Terminal $: `lfc influxWrite.js; node influxWrite/dist/influxWrite.js` - - **Note**: This reactor has a number of variables defined in it that have to match what you did above: - - `databaseName`: This is `LF_EVENTS` above. - - `measurementName`: This is `event_times` to extract those events. - - `traceFilePath`: This is `../../my_trace` above (sadly, it is relative to the `dist` directory). - - `schema`: This needs to match how your events are written to the trace file (see below). -- Configure Grafana dashboard to include influxDS as datasource (see also: https://grafana.com/docs/grafana/latest/datasources/influxdb/ . However, that page seems out of date): - - Point your browser to `http://localhost:3000/datasources` - - Click the gear icon in the side menu and select `Data Sources` - - Click on `Add data source` and then select `InfluxDB` - - Configure this page with the `admin` password that you specified when setting up grafana and pointer to `http://localhost:8086`. -- Create a new panel and select `InfluxDB` in query tab. We create two queries for physical and logical times on the measurement named "event_times" as below: - -B -FROM default event_times WHERE -SELECT field (logical_latency) math (/1000000000) -GROUP BY -FORMAT AS Time series -ALIAS BY - -A -FROM default event_times WHERE -SELECT field (physical_latency) math (/1000000000) -GROUP BY -FORMAT AS Time series -ALIAS BY - -Under panel settings, customize axes, legends - -### How it Works: - - Lingua Franca program should include printf statements in "EVENT: : " format. This stdout including "Field" and "Value" are later parsed for insertion into a time series database. diff --git a/experimental/C/src/SpatAnalysis/influxWrite.lf b/experimental/C/src/SpatAnalysis/influxWrite.lf deleted file mode 100644 index 999113c815..0000000000 --- a/experimental/C/src/SpatAnalysis/influxWrite.lf +++ /dev/null @@ -1,130 +0,0 @@ -target TypeScript; - -/* - * Uses another package @ https://github.com/node-influx/node-influx/tree/master/examples/express_response_times - */ - -main reactor { - - preamble{= - // @ts-ignore - import * as Influx from "influx"; - import * as os from "os"; - - import * as fs from "fs"; - import * as path from "path"; - - const databaseName = "LF_EVENTS"; - const measurementName = "event_times"; - const traceFilePath = "../../my_trace"; - - - const influx = new Influx.InfluxDB({ - host: "localhost", - database: databaseName, - schema: [ - { - measurement: measurementName, - fields: { - message_id: Influx.FieldType.INTEGER, - sender_lts: Influx.FieldType.FLOAT, - receiver_lts: Influx.FieldType.FLOAT, - logical_latency: Influx.FieldType.INTEGER, - sender_pts: Influx.FieldType.FLOAT, - receiver_pts: Influx.FieldType.FLOAT, - physical_latency: Influx.FieldType.INTEGER, - residual: Influx.FieldType.INTEGER, - phase: Influx.FieldType.INTEGER, - deadline_miss: Influx.FieldType.INTEGER, - }, - tags: [] - }, - ], - }); - =} - - reaction(startup){= - influx - .getDatabaseNames() - // @ts-ignore - .then((names) => { - if (!names.includes(databaseName)) { - return influx.createDatabase(databaseName); - } - }) - .then(() => { - console.log("Created new database if not exists"); - - }) - // @ts-ignore - .catch((err) => { - console.error(`Error creating Influx database!`); - }); - =} - - timer t(0, 0 msec); - - reaction(t) {= - - - let fieldRecord: {[key:string] : number} = {}; - - var lines; - try { - - const data = fs.readFileSync(path.resolve(__dirname, traceFilePath), "utf8") - lines = data.split(/\r?\n/); - - - lines.forEach((line) => { - if(line.indexOf("EVENT") < 0){ - return; - } - var mod_line = line.replace("EVENT: ", ""); - var split_line = mod_line.split(": "); - //console.log(split_line); - var _timestamp = ""; - if(split_line[0] == "sender_pts") - _timestamp = split_line[1]; - fieldRecord[split_line[0]] = parseInt(split_line[1]); - if(split_line[0] == "deadline_miss"){ - influx - .writePoints([ - { - measurement: measurementName, - tags: {}, - fields: fieldRecord, - timestamp: _timestamp, - }, - ]) - // @ts-ignore - .catch((err) => { - console.error(`Error saving data to InfluxDB! ${err.stack}`); - }); - } - }); - } catch (err) { - console.error(err) - } - =} - - reaction(shutdown){= - influx - .query( - ` - select * from measurementName - where host = ${Influx.escape.stringLit(os.hostname())} - order by time desc - limit 10 - ` - ) - // @ts-ignore - .then((result) => { - console.log(result); - }) - // @ts-ignore - .catch((err) => { - console.log(err.stack); - }); - =} -} diff --git a/experimental/C/src/SpatAnalysis/package.json b/experimental/C/src/SpatAnalysis/package.json deleted file mode 100644 index 6442e68f52..0000000000 --- a/experimental/C/src/SpatAnalysis/package.json +++ /dev/null @@ -1,36 +0,0 @@ -{ - "name": "LinguaFrancaDefault", - "version": "0.0.1", - "description": "A default Lingua Franca project for the TypeScript target", - "repository": { - "type": "git", - "url": "https://github.com/icyphy/lingua-franca" - }, - "license": "BSD-2-Clause", - "dependencies": { - "command-line-args": "^5.1.1", - "command-line-usage": "^6.1.0", - "influx": "^5.6.3", - "microtime": "^3.0.0", - "ulog": "^2.0.0-beta.7" - }, - "devDependencies": { - "@babel/cli": "^7.8.4", - "@babel/core": "^7.8.7", - "@babel/node": "^7.8.7", - "@babel/plugin-proposal-class-properties": "^7.8.3", - "@babel/plugin-proposal-object-rest-spread": "^7.8.3", - "@babel/plugin-proposal-optional-chaining": "^7.8.3", - "@babel/plugin-transform-modules-commonjs": "^7.8.3", - "@babel/preset-env": "^7.8.7", - "@babel/preset-typescript": "^7.8.3", - "@types/google-protobuf": "^3.7.2", - "@types/node": "^13.13.30", - "ts-protoc-gen": "^0.12.0", - "typescript": "^3.8.3" - }, - "scripts": { - "check-types": "tsc", - "build": "rm -rf dist && babel src --out-dir dist --extensions .ts" - } -} diff --git a/experimental/C/src/SpatAnalysis/spatReceiver.lf b/experimental/C/src/SpatAnalysis/spatReceiver.lf deleted file mode 100644 index 6a8b7f775d..0000000000 --- a/experimental/C/src/SpatAnalysis/spatReceiver.lf +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Example PTIDES application for connected vehicle application - * Demo Tile: RSU Coordinated Speed alignment - * This is the vehicle side application that - * - Requests RSU side application to provide speed recommendations based on its SPAT - * - Uses MQTT pub/sub - * @author Ravi Akella - */ - -target C { - threads: 1, // Must use threaded implementation so schedule is thread safe. - flags: "-I/usr/local/include -L/usr/local/lib -g -lpaho-mqtt3c src-gen/core/util.c ", - timeout: 60 secs, - keepalive: true -}; - - -import MQTTSubscriber from "MQTTSubscriber.lf"; - - -/** - * Reactor that prints an incoming string. - * @param prefix A prefix for the message. - * @input message The message. - */ -reactor recvSpatMessage { - preamble {= - #define BILLION 1000000000LL - typedef struct spat_status{ - int signal_phase; //integer code for signal 1-Red, 2-Green, 3-Yellow - long long unsigned residual; // remaining time for phase change - } spat_status; - =} - - input message:spat_status*; - reaction(message) {= - printf("Received Phase=\"%d\", residual=%llu.\n", - message->value->signal_phase, message->value->residual - ); - =} -} - - -reactor recvSpatMessage2 { - input message:char*; - state signal_phase:int(0); - state residual:time(0 nsec); - - reaction(message) {= - /*printf("PrintMessage: At (elapsed) time %lld, subscriber receives: %s\n", - get_elapsed_logical_time(), - message->value - );*/ - - sscanf(message->value, "%d %llu", &self->signal_phase, &self->residual); - printf("EVENT: residual: %llu\n", self->residual); - printf("EVENT: phase: %d\n", self->signal_phase); - printf("spatReceiver: At phy time %lld, Phase:%d residual:%llu nsec.\n", get_physical_time(), self->signal_phase, self->residual); - =} deadline (10 msec) {= - printf("EVENT: deadline_miss: true\n"); - =} -} - - -/** - * Reactor that prints an incoming string. - * @param prefix A prefix for the message. - * @input message The message. - */ -reactor PrintMessage { - input message:char*; - reaction(message) {= - printf("PrintMessage: At (elapsed) time %lld, subscriber receives: %s\n", - get_elapsed_logical_time(), - message->value - ); - =} -} - -main reactor spatReceiver { - - rsuMsg = new recvSpatMessage2(); - sub = new MQTTSubscriber( - //address = "tcp://host.docker.internal:1883", - address = "tcp://localhost:1883", - clientID = "Vehicle 760", - topic = "spat/rsu101", - offset = 10 msec - ); - - - sub.message->rsuMsg.message; -} diff --git a/experimental/C/src/SpatAnalysis/spatRecommender.lf b/experimental/C/src/SpatAnalysis/spatRecommender.lf deleted file mode 100644 index 8cdad76e4e..0000000000 --- a/experimental/C/src/SpatAnalysis/spatRecommender.lf +++ /dev/null @@ -1,165 +0,0 @@ -/* - * Example PTIDES application for connected vehicle application - * Demo Tile: RSU Coordinated Speed alignment - * This is the Road side unit application that - * - Accepts requests to provide speed recommendations (based on its SPAT) from vehicles - * - Uses MQTT pub/sub - * @author Ravi Akella - */ - -target C { - - threads: 1, // Must use threaded implementation so schedule is thread safe. - flags: "-I/usr/local/include -L/usr/local/lib -g -lpaho-mqtt3c src-gen/core/util.c", - timeout: 60 secs, - keepalive: true -}; - -import MQTTPublisher from "MQTTPublisher.lf"; - - -reactor phaseChanger{ - - preamble {= - #define BILLION 1000000000LL - typedef struct spat_status{ - int signal_phase; //integer code for signal 1-Red, 2-Green, 3-Yellow - long long unsigned residual; // remaining time for phase change - } spat_status; - - =} - - input in:int; - output out:char*; - output outStruct:spat_status; - - state status:spat_status(1, 0); //(signal_phase, residual time)) - - //Internal state variables - state phase_start_time:time(0 msec); - state phase_duration:time(0 msec); - - logical action red:int; - logical action green:int; - logical action yellow:int; - - reaction(startup) -> red {= - schedule_int(red, MSEC(100), 1); - self->phase_start_time = get_logical_time(); - =} - - reaction(red) -> green, out, outStruct {= - self->status.signal_phase = red->value; - self->phase_duration = MSEC(5000); - self->phase_start_time = get_logical_time(); - - self->status.residual = self->phase_duration; - int length = snprintf(NULL, 0, "%d %llu", self->status.signal_phase, self->status.residual) + 1; - // Dynamically allocate memory for the output. - SET_NEW_ARRAY(out, length); - // Populate the output string and increment the count. - snprintf(out->value, length, "%d %llu", self->status.signal_phase, self->status.residual); - printf("spatRecommender: At phy time %lld, phase change message: %s\n", - get_physical_time(), - out->value - ); - - - SET(outStruct, self->status); - schedule_int(green, self->phase_duration, 2); - =} - reaction(green) -> yellow, out {= - self->status.signal_phase = green->value; - self->phase_duration = MSEC(3000); - self->phase_start_time = get_logical_time(); - - self->status.residual = self->phase_duration; - int length = snprintf(NULL, 0, "%d %llu", self->status.signal_phase, self->status.residual) + 1; - // Dynamically allocate memory for the output. - SET_NEW_ARRAY(out, length); - // Populate the output string and increment the count. - snprintf(out->value, length, "%d %llu", self->status.signal_phase, self->status.residual); - printf("spatRecommender: At phy time %lld, phase change message: %s\n", - get_physical_time(), - out->value - ); - - schedule_int(yellow, self->phase_duration, 3); - =} - - reaction(yellow)->red, out {= - self->status.signal_phase = yellow->value; - self->phase_duration = MSEC(1000); - self->phase_start_time = get_logical_time(); - - self->status.residual = self->phase_duration; - int length = snprintf(NULL, 0, "%d %llu", self->status.signal_phase, self->status.residual) + 1; - // Dynamically allocate memory for the output. - SET_NEW_ARRAY(out, length); - // Populate the output string and increment the count. - snprintf(out->value, length, "%d %llu", self->status.signal_phase, self->status.residual); - printf("spatRecommender: At phy time %lld, phase change message: %s\n", - get_physical_time(), - out->value - ); - - schedule_int(red, self->phase_duration, 1); - =} - - timer t(0, 500 msec); - reaction(t)->out{= - - self->status.residual = self->phase_duration - get_logical_time() + self->phase_start_time; - int length = snprintf(NULL, 0, "%d %llu", self->status.signal_phase, self->status.residual) + 1; - // Dynamically allocate memory for the output. - SET_NEW_ARRAY(out, length); - // Populate the output string and increment the count. - snprintf(out->value, length, "%d %llu", self->status.signal_phase, self->status.residual); - printf("spatRecommender: At phy time %lld, publish message: %s\n", - get_physical_time(), - out->value - ); - =} -} - -/** - * Reactor that prints an incoming string. - * @param prefix A prefix for the message. - * @input message The message. - */ -reactor recvSpatMessage { - input message:char*; - reaction(message) {= - printf("Received (phase, residual in nsec) %s:\n", message->value); - - /*printf("Received Phase=\"%d\", residual=%llu.\n", - message->signal_phase, message->residual - ); - */ - =} -} - -// expected parameter is for testing. -reactor Print { - input in:char*; - - reaction(in) {= - printf("Received: name = %d, value = %lld\n", in->value.signal_phase, in->value.residual); - =} -} - - -main reactor spatRecommender { - signal = new phaseChanger(); - - pub = new MQTTPublisher( - topic = "spat/rsu101", - //address = "tcp://host.docker.internal:1883", - address = "tcp://localhost:1883", - clientID = "Intersection Signal 101", - include_physical_timestamp = 0 - ); - - signal.out -> pub.in; - -} diff --git a/experimental/C/src/build.sh b/experimental/C/src/build.sh deleted file mode 100755 index 6577cf9f97..0000000000 --- a/experimental/C/src/build.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -# Build the generated code. -cd ${LF_SOURCE_GEN_DIRECTORY} -cmake . -make - -# Move the executable to the bin directory. -mv $1 ${LF_BIN_DIRECTORY} - -# Invoke the executable. -${LF_BIN_DIRECTORY}/$1 - -# Plot the results, which have appeared in the src-gen directory. -gnuplot ${LF_SOURCE_DIRECTORY}/pendulum.gnuplot -open pendulum.pdf diff --git a/experimental/C/src/pendulum.gnuplot b/experimental/C/src/pendulum.gnuplot deleted file mode 100644 index 73d77d9a83..0000000000 --- a/experimental/C/src/pendulum.gnuplot +++ /dev/null @@ -1,19 +0,0 @@ -# Gnuplot commands for the FurutaPendulumWithOutput.lf program. - -set title 'Pendulum' # Set graph title - -set xrange ["0":"3"] # Set x-axis range of values -set yrange [-2:2.5] # Set y-axis range of values - -set xlabel "Time (seconds)" - -set terminal pdf size 5, 3.5 # Set the output format to PDF -set output 'pendulum.pdf' # Set output file. - -set label "SwingUp" at 0.2, -1.2 -set label "Catch" at 1.1, -0.2 -set label "Stabilize" at 1.2, 1.1 - -plot 'pendulum.data' using 1:2 with lines linetype 1 linewidth 2 title 'control', \ - 'energy.data' using 1:2 with lines linetype 2 linewidth 2 title 'energy', \ - 'mode.data' using 1:2 with lines linetype 3 linewidth 2 title 'mode' \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf b/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf deleted file mode 100644 index 2bb3296e24..0000000000 --- a/experimental/Python/src/Intersection/Carla/CarlaIntersection.lf +++ /dev/null @@ -1,91 +0,0 @@ -/** - * Simulate an intersection scenario using Carla. - * - * To run this example, install Carla. Then, use - * the provided bash script (if applicable) to run Carla. - * - * Compile the LF program using lfc: - * - * ` lfc CarlaIntersection.lf - * - * Locate the generated CarlaIntersection.py and run it: - * - * python3 ../../../src-gen/Intersection/Carla/CarlaIntersection/CarlaIntersection.py - * - * **Note:** This example assumes that Carla is installed in - * '/opt/carla-simulator'. Please change CARLA_INSTALL_DIR in the - * preamble to reflect your install location of the simulator. - * - */ -target Python { - timeout: 40 sec, - files: [ - "ROS/carla_intersection/src/launch_parameters.py", - "ROS/carla_intersection/src/constants.py" - ] - // fast: true // You can enable fast to get a much faster simulation - // logging: DEBUG -}; - -import Vehicle from "Vehicle.lf"; -import RSU from "RSU.lf"; -import CarlaSim from "CarlaSim.lf"; - -preamble {= - from launch_parameters import SPAWN_POINTS, INITIAL_POSITIONS, INITIAL_VELOCITIES, \ - INTERSECTION_WIDTH, NOMINAL_SPEED_IN_INTERSECTION, INTERSECTION_POSITION -=} - -reactor Relay(duration_in_seconds(0)) { - preamble {= - from time import sleep - =} - input i; - output o; - logical action t; - - reaction(i) -> t {= - t.schedule(self.duration_in_seconds, i.value) - =} - - reaction(t) -> o {= - o.set(t.value) - =} -} - -main reactor ( - num_entries(4), - spawn_points({=SPAWN_POINTS=}), - initial_velocities({=INITIAL_VELOCITIES=}), - initial_positions({=INITIAL_POSITIONS=}), - intersection_width({=INTERSECTION_WIDTH=}), - nominal_speed_in_intersection({=NOMINAL_SPEED_IN_INTERSECTION=}), - intersection_position({=INTERSECTION_POSITION=}) -) { - carla = new[num_entries] CarlaSim( - interval = 16 msec, - initial_velocities = initial_velocities, - spawn_points = spawn_points - ); - - vehicle = new[num_entries] Vehicle( - initial_velocities = initial_velocities, - initial_positions = initial_positions - ); - - rsu = new RSU( - num_entries = num_entries, - intersection_width = intersection_width, - nominal_speed_in_intersection = nominal_speed_in_intersection, - intersection_position = intersection_position - ); - - carla.velocity_ -> vehicle.velocity_; - carla.position_ -> vehicle.position_; - vehicle.control_ -> carla.command_; - - vehicle.request_ -> rsu.request_; - rsu.grant_ -> vehicle.grant_; - - -} diff --git a/experimental/Python/src/Intersection/Carla/CarlaSim.lf b/experimental/Python/src/Intersection/Carla/CarlaSim.lf deleted file mode 100644 index 88dd413142..0000000000 --- a/experimental/Python/src/Intersection/Carla/CarlaSim.lf +++ /dev/null @@ -1,72 +0,0 @@ -target Python { - files: [ - "ROS/carla_intersection/src/carla_sim.py", - "ROS/carla_intersection/src/utils.py" - ] -}; - -preamble {= - from utils import make_coordinate, make_spawn_point - from carla_sim import CarlaSim -=} - -# TODO: Figure out why this warning persists | -# v -# WARNING: World::ApplySettings: After 30 attemps, the settings were not correctly set. Please check that everything is consistent. -# -# as well as why the velocity of the vehicle shoots through the roof... which causes CARLA to crash. -reactor CarlaSim( - interval(16 msec), - bank_index(0), - initial_velocities({=None=}), - vehicle_type("vehicle.tesla.model3"), - spawn_points({=None=}) -) { - input command_; - output velocity_; - output position_; - logical action world_is_ready; - state carla_sim; - timer timer_(10 msec, interval); - - preamble {= - class Logger: - def __init__(self, vehicle_id): - self.vehicle_id = vehicle_id - - def info(self, *args): - print(f"{get_logical_time()} - carla_sim_{self.vehicle_id}: ", args) - =} - - reaction(startup) -> world_is_ready {= - print("initial velocity: ", self.initial_velocities[self.bank_index]) - self.carla_sim = CarlaSim(interval=self.interval, - vehicle_type=self.vehicle_type, - initial_velocity=make_coordinate(self.initial_velocities[self.bank_index]), - spawn_point=make_spawn_point(self.spawn_points[self.bank_index]), - logger=self.Logger(self.bank_index)) - self.carla_sim.connect_to_carla() - if self.bank_index == 0: - self.carla_sim.initialize_world(self.interval / SEC(1)) - world_is_ready.schedule(MSEC(5)) - =} - - reaction(world_is_ready) {= - self.carla_sim.get_world() - self.carla_sim.initialize_vehicle() - =} - - reaction(timer_) -> velocity_, position_ {= - self.carla_sim.tick() - print("setting velocity to ", self.carla_sim.get_vehicle_velocity()) - velocity_.set(self.carla_sim.get_vehicle_velocity()) - p = self.carla_sim.get_vehicle_position() - coordinate = make_coordinate([p.latitude, p.longitude, p.altitude]) - position_.set(coordinate) - =} - - reaction(command_) {= - cmd = command_.value - self.carla_sim.apply_control(cmd.throttle, cmd.brake) - =} -} \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/.gitignore b/experimental/Python/src/Intersection/Carla/ROS/.gitignore deleted file mode 100644 index 3387172b3d..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -install -__pycache__ -log diff --git a/experimental/Python/src/Intersection/Carla/ROS/README.md b/experimental/Python/src/Intersection/Carla/ROS/README.md deleted file mode 100644 index fbb1c36f21..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/README.md +++ /dev/null @@ -1,31 +0,0 @@ -# ROS implementation of the Carla Intersection Experiment - -To run the experiment, run CARLA: -```bash -~$ ../run-carla.sh -``` - -Then, use `colcon` to build both `carla_intersection` and `carla_intersection_msgs`: -```bash -~$ cd carla_intersection_msgs -~$ colcon build -~$ cd .. -~$ cd carla_intersection -~$ colcon build -``` - -Finally, launch the ros program using the launch file: -```bash -~$ ros2 launch launch/intersection_demo.launch.py -``` - - -## Noteworthy Observations / Comparisons with Lingua Franca Implementation - -- Goal - - Preventing the vehicle from issueing another request before the RSU replies with a grant. -- Implementation - - ROS - - An additional flag is needed to track a pending request. - - LF (Centralized Coordination) - - Vehicle's logical time cannot advance before receiving a grant, so the goal is achieved by default. diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py deleted file mode 100644 index 6845a36579..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/launch/intersection_demo.launch.py +++ /dev/null @@ -1,52 +0,0 @@ -import launch -import launch.actions -import launch.substitutions -from launch_ros.actions import Node -import sys -from os import path -sys.path.insert(0, path.dirname(path.dirname(__file__))) -from src.launch_parameters import SPAWN_POINTS, INITIAL_POSITIONS, INITIAL_VELOCITIES, \ - INTERSECTION_WIDTH, NOMINAL_SPEED_IN_INTERSECTION, INTERSECTION_POSITION - -def generate_launch_description(): - nodes = [] - nodes.append( - Node( - package='carla_intersection', - executable='rsu_node', - parameters=[ - {"intersection_position": INTERSECTION_POSITION}, - {"intersection_width": INTERSECTION_WIDTH}, - {"nominal_speed_in_intersection": NOMINAL_SPEED_IN_INTERSECTION} - ] - ) - ) - - number_of_vehicles = 4 - assert 1 <= number_of_vehicles <= 4 - - for i in range(number_of_vehicles): - nodes.append( - Node( - package='carla_intersection', - executable='vehicle_node', - parameters=[ - {"vehicle_id": i}, - {"initial_velocity": INITIAL_VELOCITIES[i]}, - {"initial_position": INITIAL_POSITIONS[i]} - ] - ) - ) - nodes.append( - Node( - package='carla_intersection', - executable='carla_sim_node', - parameters=[ - {"vehicle_id": i}, - {"initial_velocity": INITIAL_VELOCITIES[i]}, - {"spawn_point": SPAWN_POINTS[i]} - ] - ) - ) - - return launch.LaunchDescription(nodes) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/package.xml b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/package.xml deleted file mode 100644 index b80a840fec..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - carla_intersection - 0.0.0 - TODO: Package description - soroush - TODO: License declaration - - rclpy - carla_intersection_msgs - geometry_msgs - ros2launch - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/resource/carla_intersection b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/resource/carla_intersection deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.cfg b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.cfg deleted file mode 100644 index 4b84a06202..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/carla_intersection -[install] -install_scripts=$base/lib/carla_intersection \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py deleted file mode 100644 index f82cee7a27..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/setup.py +++ /dev/null @@ -1,28 +0,0 @@ -from setuptools import setup, find_packages - -package_name = 'carla_intersection' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='soroush', - maintainer_email='soroosh129@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'rsu_node = src.rsu_node:main', - 'vehicle_node = src.vehicle_node:main', - 'carla_sim_node = src.carla_sim_node:main' - ], - }, -) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/__init__.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py deleted file mode 100644 index b5dd10e347..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim.py +++ /dev/null @@ -1,144 +0,0 @@ -# Set up sys.path such that imports work across LF and ROS -import sys -from os import path -if path.dirname(__file__) not in sys.path: - sys.path.insert(0, path.dirname(__file__)) - -# Other libraries -import glob -import os -try: - import queue -except ImportError: - import Queue as queue - -# Constants -from constants import CARLA_INSTALL_DIR - -# Set up Carla -try: - sys.path.append(glob.glob(CARLA_INSTALL_DIR + "/PythonAPI/carla/dist/carla-*%d.%d-%s.egg" % ( - sys.version_info.major, - sys.version_info.minor, - "win-amd64" if os.name == "nt" else "linux-x86_64"))[0]) -except IndexError: - pass -try: - import carla -except ImportError: - sys.stderr.write("ERROR: Could not find the Carla .egg file.\nPlease make sure that" - " CARLA_INSTALL_DIR in CarlaIntersection.lf points to the correct location.\n") - - -class CarlaSim: - def __init__(self, interval, vehicle_type, initial_velocity, spawn_point, logger): - # State variables initialization - self.world = None - self.client = None - self.vehicle = None - self.interval = interval - self.vehicle_type = vehicle_type - self.initial_velocity = initial_velocity - self.spawn_point = spawn_point - self.logger = logger - - def connect_to_carla(self): - # initialize Carla - self.client = carla.Client("localhost", 2000) - self.client.set_timeout(10.0) # seconds - - def initialize_world(self, fixed_delta_seconds): - self.world = self.client.load_world("Town05") - settings = self.world.get_settings() - settings.fixed_delta_seconds = fixed_delta_seconds - settings.substepping = True - settings.max_substep_delta_time = settings.fixed_delta_seconds / 10 - settings.max_substeps = 10 - settings.synchronous_mode = True # Enables synchronous mode - self.world.apply_settings(settings) - self.set_weather() - self.set_spectator_camera() - - def set_weather(self): - # Set the weather - weather = carla.WeatherParameters( - cloudiness=80.0, - precipitation=30.0, - sun_altitude_angle=70.0) - self.world.set_weather(weather) - - def set_spectator_camera(self): - # Set the spectator (camera) position - transform = carla.Transform(carla.Location(x=-126.163864, y=3, z=67), \ - carla.Rotation(pitch=-90, yaw=-180, roll=180)) - self.world.get_spectator().set_transform(transform) - - def get_vehicle_velocity(self): - return self.vehicle.get_velocity() - - def get_vehicle_position(self): - return self.gps_queue.get() - - def apply_control(self, throttle: float, brake: float): - self.vehicle.apply_control(carla.VehicleControl(throttle=throttle, brake=brake)) - - def get_world(self): - self.world = self.client.get_world() - - def tick(self): - self.world.tick() - - def initialize_vehicle(self): - blueprint_library = self.world.get_blueprint_library() - - sensors_bp = {} - sensors = {} - sensors_to_spawn = { \ - "gps": "sensor.other.gnss", \ - "imu": "sensor.other.imu" - } - - spawn_point = self.spawn_point - # Spawn the vehicle - vehicle_bp = blueprint_library.find(self.vehicle_type) - transform = carla.Transform(carla.Location( \ - x = spawn_point.x, \ - y = spawn_point.y, \ - z = spawn_point.z \ - ), carla.Rotation(yaw = spawn_point.yaw)) - self.vehicle = self.world.spawn_actor(vehicle_bp, transform) - - for key in sensors_to_spawn.keys(): - sensors_bp[key] = blueprint_library.find(sensors_to_spawn[key]) - if key == "gps": - # Spawn the GPS sensor that is attached to the vehicle - relative_transform = carla.Transform(carla.Location( \ - x = 1.0, \ - y = 0.0, \ - z = 2.0), carla.Rotation()) - - elif key == "imu": - # Spawn the imu unit - relative_transform = carla.Transform(carla.Location( \ - x = 2.0, \ - y = 0.0, \ - z = 2.0), carla.Rotation()) - else: - relative_transform = carla.Transform(carla.Location(), carla.Rotation()) - - sensors[key] = self.world.spawn_actor( \ - sensors_bp[key], \ - relative_transform, \ - attach_to=self.vehicle, \ - attachment_type=carla.AttachmentType.Rigid \ - ) - - self.gps = sensors["gps"] - self.gps_queue = queue.Queue() - self.gps.listen(self.gps_queue.put) - - # Set the initial velocity - target_speed = self.initial_velocity - self.vehicle.set_target_velocity(carla.Vector3D(x=target_speed.x, y=target_speed.y, z=target_speed.z)) - - self.logger.info("Spawned vehicle") \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py deleted file mode 100644 index 5cb1714529..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/carla_sim_node.py +++ /dev/null @@ -1,86 +0,0 @@ -# ROS2 libraries -import rclpy -from rclpy.node import Node - -# ROS2 messages -from carla_intersection_msgs.msg import VehicleCommand -from std_msgs.msg import Bool -from geometry_msgs.msg import Vector3 - -# Other libraries -from src.utils import make_coordinate, make_spawn_point -from src.ros_utils import make_Vector3 -from src.carla_sim import CarlaSim - -# The Carla Simulator -class CarlaSimNode(Node): - def __init__(self): - super().__init__("carla_sim_node") - - # Parameters declaration - self.declare_parameter('interval', 16) # msec - self.declare_parameter('vehicle_id', 0) - self.declare_parameter('initial_velocity', [0.0, 0.0, 0.0]) - self.declare_parameter('vehicle_type', 'vehicle.tesla.model3') - self.declare_parameter('spawn_point', [0.0, 0.0, 0.0, 0.0]) - - # State variables initialization - self.interval = self.get_parameter('interval').value - self.vehicle_id = self.get_parameter('vehicle_id').value - self.vehicle_type = self.get_parameter('vehicle_type').value - self.initial_velocity = make_coordinate(self.get_parameter('initial_velocity').value) - self.spawn_point = make_spawn_point(self.get_parameter('spawn_point').value) - self.world_is_ready = False - - # pubsub for input and output ports - self.velocity_ = self.create_publisher(Vector3, f"vehicle_velocity_{self.vehicle_id}", 10) - self.position_ = self.create_publisher(Vector3, f"vehicle_position_{self.vehicle_id}", 10) - self.command_ = self.create_subscription(VehicleCommand, f"control_to_command_{self.vehicle_id}", self.command_callback, 10) - - self.carla_sim = CarlaSim(interval=self.interval, vehicle_type=self.vehicle_type, - initial_velocity=self.initial_velocity, spawn_point=self.spawn_point, - logger=self.get_logger()) - self.carla_sim.connect_to_carla() - if self.vehicle_id == 0: - self.world_is_ready_ = self.create_publisher(Bool, "world_is_ready", 10) - self.carla_sim.initialize_world(self.interval / 1000.0) - self.world_is_ready_.publish(Bool()) - self.world_is_ready_callback() - else: - self.world_is_ready_ = self.create_subscription(Bool, "world_is_ready", self.world_is_ready_callback, 10) - - # timer (should be after connect_to_carla() is called) - self.timer_ = self.create_timer(self.interval / 1000.0, self.timer_callback) - - def command_callback(self, command): - self.carla_sim.apply_control(command.throttle, command.brake) - - def timer_callback(self): - if not self.world_is_ready: - return - self.carla_sim.tick() - self.velocity_.publish(make_Vector3(self.carla_sim.get_vehicle_velocity())) - position = self.carla_sim.get_vehicle_position() - coordinate = make_coordinate([position.latitude, position.longitude, position.altitude]) - self.position_.publish(make_Vector3(coordinate)) - - def world_is_ready_callback(self, _=None): - self.carla_sim.get_world() - self.world_is_ready = True - self.carla_sim.initialize_vehicle() - - -def main(args=None): - rclpy.init(args=args) - - ego_vehicle = CarlaSimNode() - - rclpy.spin(ego_vehicle) - - # Destroy the node explicitly - ego_vehicle.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py deleted file mode 100644 index 0cc92ce634..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/constants.py +++ /dev/null @@ -1,12 +0,0 @@ -BILLION = 1_000_000_000 - -CARLA_INSTALL_DIR = "/opt/carla-simulator" - -# The speed limit of vehicles in m/s -SPEED_LIMIT = 14.0 - -# The distance (in meters) at which the controller assumes it has reached its goal -GOAL_REACHED_THRESHOLD = 14.0 - -# The time threshold at which the vehicle has reached its time-based goal -GOAL_REACHED_THRESHOLD_TIME = (GOAL_REACHED_THRESHOLD / SPEED_LIMIT) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py deleted file mode 100644 index 1c30742721..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/launch_parameters.py +++ /dev/null @@ -1,29 +0,0 @@ -SPAWN_POINTS = [ - # x y z yaw - [-122.0, 39.6, 0.3, -90.0], - [-177.77, 6.48, 0.3, 0.0], - [-132.77, -40, 0.3, 90.0], - [-80.77, -4.5, 0.3, 180.0] -] - -INITIAL_POSITIONS = [ - # x y z - [0.000038, -0.000674, 2.794825], # /|\ - [-0.000501, -0.001084, 2.794891], # -> - [-0.000060, -0.001510, 2.794854], # \|/ - [0.000367, -0.001185, 2.794846] # <- -] - -INITIAL_VELOCITIES = [ - # x y z - [ 0.0, -8.0, 0.0], - [ 8.0, 0.0, 0.0], - [ 0.0, 8.0, 0.0], - [-8.0, 0.0, 0.0] -] - -INTERSECTION_WIDTH = 40 - -NOMINAL_SPEED_IN_INTERSECTION = 14.0 - -INTERSECTION_POSITION = [-0.000007632,-0.001124366,2.792485] \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py deleted file mode 100644 index b744bf823b..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/ros_utils.py +++ /dev/null @@ -1,4 +0,0 @@ -from geometry_msgs.msg import Vector3 - -def make_Vector3(coordinate): - return Vector3(x=coordinate.x, y=coordinate.y, z=coordinate.z) \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py deleted file mode 100644 index e85f5a5eca..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu.py +++ /dev/null @@ -1,92 +0,0 @@ -# Set up sys.path such that imports work across LF and ROS -import sys -from os import path -if path.dirname(__file__) not in sys.path: - sys.path.insert(0, path.dirname(__file__)) - -# Other libraries -from utils import distance, dotdict - -# Constants -from constants import BILLION - -class RSU: - def __init__(self, intersection_width, nominal_speed_in_intersection, intersection_position, clock, logger): - self.intersection_width = intersection_width - self.nominal_speed_in_intersection = nominal_speed_in_intersection - self.intersection_position = intersection_position - self.earliest_free = 0 # nsec - self.active_participants = [0] * 20 - self.clock = clock - self.logger = logger - - - def receive_request(self, request) -> dotdict: - ''' - Handles the request to enter the intersection from a vehicle. - - Parameters - ---------- - request: Class - A request to enter the intersection from a - vehicle with the following structure: - - requestor_id : int - - speed : float - - position : coordinate - - Returns - ---------- - dotdict - A dotted dictionary with the following structure: - - grant - - requestor_id : int - - intersection_position : coordinate - - target_speed : float - - arrival_time : int - ''' - pub_packets = dotdict() - self.active_participants[request.requestor_id] = 1 - if request.speed == 0.0: - # Avoid division by zero - request.speed = 0.001 - # Calculate the time it will take the approaching vehicle to - # arrive at its current speed. Note that this is - # time from the time the vehicle sends the message - # according to the arriving vehicle's clock. - speed_in_m_per_sec = request.speed - dr = distance(self.intersection_position, request.position) - time_to_intersection = dr / speed_in_m_per_sec - - self.logger.info("*** RSU: Vehicle {}'s distance to intersection is {}. ".format(request.requestor_id, dr, self.intersection_position, request.position, time_to_intersection)) - - current_time = self.clock.get_current_time_in_ns() - - # Convert the time interval to nsec (it is in seconds). - arrival_time_ns = int(current_time + (time_to_intersection * BILLION)) - - pub_packets.grant = dotdict() - pub_packets.grant.requestor_id = request.requestor_id - pub_packets.grant.intersection_position = self.intersection_position - if arrival_time_ns >= self.earliest_free: - # Vehicle can maintain speed. - pub_packets.grant.target_speed = request.speed - pub_packets.grant.arrival_time = arrival_time_ns - else: - # Could be smarter than this, but just send the nominal speed in intersection. - pub_packets.grant.target_speed = self.nominal_speed_in_intersection - # Vehicle has to slow down and maybe stop. - pub_packets.grant.arrival_time = self.earliest_free - - # Update earliest free on the assumption that the vehicle - # maintains its target speed (on average) within the intersection. - time_in_intersection_ns = int((BILLION * self.intersection_width) / (pub_packets.grant.target_speed)) - self.earliest_free = pub_packets.grant.arrival_time + time_in_intersection_ns - - self.logger.info("*** RSU: Granted access to vehicle {} to enter at " - "time {} ns with average target velocity {} m/s. Next available time is {}".format( - pub_packets.grant.requestor_id, - pub_packets.grant.arrival_time, - pub_packets.grant.target_speed, - self.earliest_free) - ) - return pub_packets \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py deleted file mode 100644 index b83efbfe24..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/rsu_node.py +++ /dev/null @@ -1,61 +0,0 @@ -# ROS2 libraries -import rclpy -from rclpy.node import Node - -# ROS2 messages -from carla_intersection_msgs.msg import Request, Grant - -# Other libraries -from src.utils import make_coordinate, ROSClock -from src.ros_utils import make_Vector3 -from src.rsu import RSU - -class RSUNode(Node): - def __init__(self): - super().__init__("rsu") - - # Parameters declaration - self.declare_parameter('intersection_width', 0) - self.declare_parameter('nominal_speed_in_intersection', 0.0) - self.declare_parameter('intersection_position', [0.0, 0.0, 0.0]) - - # State variables initialization - self.intersection_width = self.get_parameter('intersection_width').value - self.nominal_speed_in_intersection = self.get_parameter('nominal_speed_in_intersection').value - self.intersection_position = make_coordinate(self.get_parameter('intersection_position').value) - - # pubsub for input / output ports - self.grant_ = self.create_publisher(Grant, "grant", 10) - self.request_ = self.create_subscription(Request, "request", self.request_callback, 10) - self.rsu = RSU(intersection_width = self.intersection_width, - nominal_speed_in_intersection = self.nominal_speed_in_intersection, - intersection_position = self.intersection_position, - clock = ROSClock(self.get_clock()), - logger = self.get_logger()) - - - def request_callback(self, request): - pub_packets = self.rsu.receive_request(request) - if pub_packets.grant != None: - grant = Grant() - grant.requestor_id = pub_packets.grant.requestor_id - grant.intersection_position = make_Vector3(pub_packets.grant.intersection_position) - grant.target_speed = pub_packets.grant.target_speed - grant.arrival_time = pub_packets.grant.arrival_time - self.grant_.publish(grant) - - -def main(args=None): - rclpy.init(args=args) - - rsu = RSUNode() - - rclpy.spin(rsu) - - # Destroy the node explicitly - rsu.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py deleted file mode 100644 index cd89ff4955..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/utils.py +++ /dev/null @@ -1,69 +0,0 @@ -# Set up sys.path such that imports work across LF and ROS -import sys -from os import path -if path.dirname(__file__) not in sys.path: - sys.path.insert(0, path.dirname(__file__)) - -# Other libraries -from math import sin, cos, sqrt, atan2, radians -from constants import BILLION - -class dotdict(dict): - """dot.notation access to dictionary attributes""" - __getattr__ = dict.get - __setattr__ = dict.__setitem__ - __delattr__ = dict.__delitem__ - - -class Coordinate: - def __init__(self, x, y, z): - self.x = x - self.y = y - self.z = z - -class GenericClock: - def get_current_time_in_ns(self): - assert False, "subclass must override this method" - -class ROSClock(GenericClock): - def __init__(self, clock): - self.clock = clock - - def get_current_time_in_ns(self): - current_time = self.clock.now().to_msg() - return current_time.sec * BILLION + current_time.nanosec - - -def distance(coordinate1, coordinate2): - """ - Calculate the great circle distance between two points - on the earth (specified in decimal degrees) - Taken from: https://stackoverflow.com/a/15737218/783868 - """ - # Currently ignores altitude - # Convert decimal degrees to radians - lat1 = radians(coordinate1.x) - lon1 = radians(coordinate1.y) - lat2 = radians(coordinate2.x) - lon2 = radians(coordinate2.y) - - # Haversine formula - dlon = lon2 - lon1 - dlat = lat2 - lat1 - a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 - c = 2 * atan2(sqrt(a), sqrt(1 - a)) - # Radius of earth in kilometers is 6371 - km = 6371.0 * c - m = km * 1000.0 - return m - - -def make_coordinate(list): - return Coordinate(x=list[0], y=list[1], z=list[2]) - - -def make_spawn_point(list): - return dotdict({"x": list[0], "y": list[1], "z": list[2], "yaw": list[3]}) - -def make_speed(velocity): - return sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py deleted file mode 100644 index 5488b1b23d..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle.py +++ /dev/null @@ -1,186 +0,0 @@ -# Set up sys.path such that imports work across LF and ROS -import sys -from os import path -if path.dirname(__file__) not in sys.path: - sys.path.insert(0, path.dirname(__file__)) - -# Other libraries -from utils import make_speed, dotdict, distance, Coordinate -from constants import BILLION, GOAL_REACHED_THRESHOLD, GOAL_REACHED_THRESHOLD_TIME, SPEED_LIMIT - -class Vehicle: - def __init__(self, vehicle_id, initial_position, initial_velocity, clock, logger): - self.vehicle_id = vehicle_id - self.position = initial_position - self.velocity = initial_velocity - self.speed = make_speed(initial_velocity) - self.goal_reached = False - self.granted_time_to_enter = 0 - self.intersection_position = None - self.clock = clock - self.logger = logger - - def set_velocity(self, new_velocity: Coordinate) -> None: - self.velocity = new_velocity - self.speed = make_speed(new_velocity) - # Prevent divisions by zero - if self.speed == 0.0: - self.speed = 0.001 - - def set_position(self, new_position: Coordinate) -> None: - self.position = new_position - - def get_velocity(self) -> Coordinate: - ''' - Returns the current velocity of the vehicle as a coordinate. - ''' - return self.velocity - - def get_position(self) -> Coordinate: - ''' - Returns the current position of the vehicle as a coordinate. - ''' - return self.position - - def get_speed(self) -> float: - ''' - Returns the current speed of the vehicle. - ''' - return self.speed - - def grant(self, arrival_time: int, intersection_position: Coordinate) -> None: - ''' - Grants the vehicle access to the intersection. - ''' - self.logger.info("Vehicle {} Granted access".format(self.vehicle_id) + - "to enter the intersection at elapsed logical time {:d}.\n".format( - arrival_time - ) - ) - self.granted_time_to_enter = arrival_time - self.intersection_position = intersection_position - - def receive_velocity_from_simulator(self, vehicle_stat: Coordinate) -> dotdict: - ''' - React to the velocity from the simulator - - Parameters - ---------- - vehicle_stat: Coordinate - The updated velocity of the vehicle. - - Returns - ---------- - dotdict - A dotted dictionary with the following structure: - - request - - speed : float - - position : Coordinate - - cmd - - throttle : float - - brake : float - ''' - pub_packets = dotdict() - if self.goal_reached: - return pub_packets - # Record the speed - self.set_velocity(vehicle_stat) - - # Send a new request to the RSU if no time to enter - # the intersection is granted - if self.granted_time_to_enter == 0: - pub_packets.request = dotdict() - pub_packets.request.speed = self.get_speed() - pub_packets.request.position = self.get_position() - - # Stop the vehicle - pub_packets.cmd = dotdict() - pub_packets.cmd.throttle = 0.0 - pub_packets.cmd.brake = 1.0 - else: - # We have a granted time from the RSU - # All we need to do is adjust our velocity - # to enter the intersection at the allocated - # time - - # First, how far are we from the intersection - distance_remaining = distance(self.intersection_position, self.get_position()) - current_time = self.clock.get_current_time_in_ns() - time_remaining = (self.granted_time_to_enter - current_time) / BILLION - - self.logger.info("########################################") - self.logger.info("Vehicle {}: Distance to intersection: {}m.".format(self.vehicle_id, distance_remaining)) - self.logger.info("Vehicle {}: Time to intersection: {}s.".format(self.vehicle_id, time_remaining)) - self.logger.info("Vehicle {}: Current speed: {}m/s.".format(self.vehicle_id, self.speed)) - - target_speed = 0.0 - # target_speed = distance_remaining/time_remaining - - if distance_remaining <= GOAL_REACHED_THRESHOLD and \ - time_remaining <= GOAL_REACHED_THRESHOLD_TIME : - # Goal reached - # At this point, a normal controller should stop the vehicle until - # it receives a new goal. However, for the purposes of this demo, - # it will set the target speed to the speed limit so that vehicles - # can leave the intersection (otherwise, they will just stop at the - # intersection). - target_speed = SPEED_LIMIT - # Simulation is over - self.goal_reached = True - - self.logger.info("\n\n*************************************************************\n\n".format(self.vehicle_id)) - self.logger.info("************* Vehicle {}: Reached intersection! *************".format(self.vehicle_id)) - self.logger.info("\n\n*************************************************************\n\n".format(self.vehicle_id)) - - elif time_remaining < (distance_remaining / SPEED_LIMIT): - # No time to make it to the intersection even if we - # were going at the speed limit. - # Ask the RSU again - self.granted_time_to_enter = 0 - # Apply the brake since we ran out of time - target_speed = 0 - else: - # Has not reached the goal - # target_speed = ((2 * distance_remaining) / (time_remaining)) - self.speed - target_speed = distance_remaining / time_remaining - - self.logger.info("Vehicle {}: Calculated target speed: {}m/s.".format(self.vehicle_id, target_speed)) - - if (target_speed - SPEED_LIMIT) > 0: - self.logger.info("Warning: target speed exceeds the speed limit") - target_speed = 0 - self.granted_time_to_enter = 0 - - if target_speed <= 0: - self.logger.info("Warning: target speed negative or zero") - target_speed = 0.001 - self.granted_time_to_enter = 0 - - brake = 0.0 - throttle = 0.0 - - if target_speed >= self.speed: - # Calculate a proportional throttle (0.0 < throttle < 1.0) - throttle = min((target_speed - self.speed)/target_speed, 1.0) - # throttle = 1.0 - brake = 0.0 - # throttle = min(abs(target_speed / self.speed), 1) - else: - # Need to apply the brake - brake = min((self.speed - target_speed)/self.speed, 1.0) - # brake = 1.0 - throttle = 0.0 - - # Check throttle boundaries - if throttle < 0: - self.logger.info("Error: negative throttle") - throttle = 0.0 - - # Prepare and send the target velocity as a vehicle command - pub_packets.cmd = dotdict() - pub_packets.cmd.throttle = throttle - pub_packets.cmd.brake = brake - self.logger.info("Vehicle {}: Throttle: {}. Brake: {}".format(self.vehicle_id, throttle, brake)) - return pub_packets - - diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py deleted file mode 100644 index 1ae94d2ec7..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection/src/vehicle_node.py +++ /dev/null @@ -1,81 +0,0 @@ -# ROS2 libraries -import rclpy -from rclpy.node import Node - -# ROS2 messages -from carla_intersection_msgs.msg import Request, Grant, VehicleCommand -from geometry_msgs.msg import Vector3 - -# Other libraries -from src.utils import make_coordinate, ROSClock -from src.ros_utils import make_Vector3 -from src.vehicle import Vehicle - -# Constants -from src.constants import BILLION - -class VehicleNode(Node): - def __init__(self): - super().__init__(f"vehicle") - - # Parameters declaration - self.declare_parameter('vehicle_id', 0) - self.declare_parameter('initial_velocity', [0.0, 0.0, 0.0]) - self.declare_parameter('initial_position', [0.0, 0.0, 0.0]) - - # State variables initialization - self.vehicle_id = self.get_parameter('vehicle_id').value - self.initial_position = make_coordinate(self.get_parameter('initial_position').value) - self.initial_velocity = make_coordinate(self.get_parameter('initial_velocity').value) - self.asking_for_grant = False - self.vehicle = Vehicle(vehicle_id = self.vehicle_id, - initial_position = self.initial_position, - initial_velocity = self.initial_velocity, - clock = ROSClock(self.get_clock()), - logger = self.get_logger()) - - # pubsub for input output ports - self.velocity_ = self.create_subscription(Vector3, f"vehicle_velocity_{self.vehicle_id}", self.velocity_callback, 10) - self.position_ = self.create_subscription(Vector3, f"vehicle_position_{self.vehicle_id}", self.position_callback, 10) - self.control_ = self.create_publisher(VehicleCommand, f"control_to_command_{self.vehicle_id}", 10) - self.grant_ = self.create_subscription(Grant, "grant", self.grant_callback, 10) - self.request_ = self.create_publisher(Request, "request", 10) - - def position_callback(self, new_position): - self.vehicle.set_position(new_position) - - def velocity_callback(self, new_velocity): - pub_packets = self.vehicle.receive_velocity_from_simulator(new_velocity) - if pub_packets.cmd != None: - cmd = VehicleCommand() - cmd.throttle = pub_packets.cmd.throttle - cmd.brake = pub_packets.cmd.brake - self.control_.publish(cmd) - if pub_packets.request != None and not self.asking_for_grant: - request = Request() - request.requestor_id = self.vehicle_id - request.speed = pub_packets.request.speed - request.position = make_Vector3(pub_packets.request.position) - self.request_.publish(request) - self.asking_for_grant = True - - def grant_callback(self, grant): - if grant.requestor_id != self.vehicle_id: - return - self.vehicle.grant(grant.arrival_time, grant.intersection_position) - self.asking_for_grant = False - -def main(args=None): - rclpy.init(args=args) - - ego_vehicle = VehicleNode() - - rclpy.spin(ego_vehicle) - - # Destroy the node explicitly - ego_vehicle.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/CMakeLists.txt b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/CMakeLists.txt deleted file mode 100644 index e55aa2d975..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(carla_intersection_msgs) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -find_package(rosidl_default_generators REQUIRED) -find_package(geometry_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Request.msg" - "msg/Grant.msg" - "msg/VehicleCommand.msg" - DEPENDENCIES geometry_msgs -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Grant.msg b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Grant.msg deleted file mode 100644 index 4311de7aed..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Grant.msg +++ /dev/null @@ -1,4 +0,0 @@ -int64 requestor_id -float64 target_speed -uint64 arrival_time -geometry_msgs/Vector3 intersection_position \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Request.msg b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Request.msg deleted file mode 100644 index e87f9d4e41..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/Request.msg +++ /dev/null @@ -1,3 +0,0 @@ -int64 requestor_id -float64 speed -geometry_msgs/Vector3 position \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/VehicleCommand.msg b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/VehicleCommand.msg deleted file mode 100644 index a10669a5c0..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/msg/VehicleCommand.msg +++ /dev/null @@ -1,3 +0,0 @@ -int64 vehicle_id -float64 throttle -float64 brake \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/package.xml b/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/package.xml deleted file mode 100644 index 096e97cde9..0000000000 --- a/experimental/Python/src/Intersection/Carla/ROS/carla_intersection_msgs/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - carla_intersection_msgs - 0.0.0 - TODO: Package description - soroush - TODO: License declaration - - ament_cmake - - rosidl_default_generators - - rosidl_default_runtime - - rosidl_interface_packages - - ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/RSU.lf b/experimental/Python/src/Intersection/Carla/RSU.lf deleted file mode 100644 index 96194aea48..0000000000 --- a/experimental/Python/src/Intersection/Carla/RSU.lf +++ /dev/null @@ -1,53 +0,0 @@ -target Python { - files: [ - "ROS/carla_intersection/src/rsu.py", - "ROS/carla_intersection/src/utils.py" - ] -}; - -preamble {= - from rsu import RSU - from utils import GenericClock, dotdict, make_coordinate -=} - -reactor RSU( - num_entries(0), - intersection_width(0), - nominal_speed_in_intersection(0.0), - intersection_position(0, 0, 0) -) { - input[num_entries] request_; - output[num_entries] grant_; - - preamble {= - class Logger: - def info(self, *args): - print(f"{get_logical_time()} - rsu: ", args) - - class LFClock(GenericClock): - def get_current_time_in_ns(self): - return get_logical_time() - =} - - reaction(startup) {= - self.rsu = RSU(intersection_width = self.intersection_width, - nominal_speed_in_intersection = self.nominal_speed_in_intersection, - intersection_position = make_coordinate(self.intersection_position), - clock = self.LFClock(), - logger = self.Logger()) - =} - - reaction(request_) -> grant_ {= - for i in range(self.num_entries): - if not request_[i].is_present: - continue - request = request_[i].value - pub_packets = self.rsu.receive_request(request) - if pub_packets.grant != None: - grant = dotdict() - grant.intersection_position = pub_packets.grant.intersection_position - grant.target_speed = pub_packets.grant.target_speed - grant.arrival_time = pub_packets.grant.arrival_time - grant_[i].set(grant) - =} -} \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/Vehicle.lf b/experimental/Python/src/Intersection/Carla/Vehicle.lf deleted file mode 100644 index fcde445ee6..0000000000 --- a/experimental/Python/src/Intersection/Carla/Vehicle.lf +++ /dev/null @@ -1,70 +0,0 @@ -target Python { - files: [ - "ROS/carla_intersection/src/vehicle.py", - "ROS/carla_intersection/src/utils.py" - ] -}; - -preamble {= - from vehicle import Vehicle - from utils import GenericClock, dotdict, make_coordinate -=} - -reactor Vehicle( - bank_index(0), - initial_velocities({=None=}), - initial_positions({=None=}) -) { - input velocity_; - input position_; - input grant_; - output control_; - output request_; - state vehicle; - - preamble {= - class Logger: - def __init__(self, vehicle_id): - self.vehicle_id = vehicle_id - - def info(self, *args): - print(f"{get_logical_time()} - vehicle_{self.vehicle_id}: ", args) - - class LFClock(GenericClock): - def get_current_time_in_ns(self): - return get_logical_time() - =} - - reaction(startup) {= - self.vehicle = Vehicle(vehicle_id = self.bank_index, - initial_position = make_coordinate(self.initial_positions[self.bank_index]), - initial_velocity = make_coordinate(self.initial_velocities[self.bank_index]), - clock = self.LFClock(), - logger = self.Logger(self.bank_index)) - =} - - reaction(position_) {= - self.vehicle.set_position(position_.value) - =} - - reaction(velocity_) -> control_, request_ {= - new_velocity = velocity_.value - pub_packets = self.vehicle.receive_velocity_from_simulator(new_velocity) - if pub_packets.cmd != None: - cmd = dotdict() - cmd.throttle = pub_packets.cmd.throttle - cmd.brake = pub_packets.cmd.brake - control_.set(cmd) - if pub_packets.request != None: - request = dotdict() - request.requestor_id = self.bank_index - request.speed = pub_packets.request.speed - request.position = pub_packets.request.position - request_.set(request) - =} - - reaction(grant_) {= - grant = grant_.value - self.vehicle.grant(grant.arrival_time, grant.intersection_position) - =} -} \ No newline at end of file diff --git a/experimental/Python/src/Intersection/Carla/run-carla.sh b/experimental/Python/src/Intersection/Carla/run-carla.sh deleted file mode 100755 index 55698beeb2..0000000000 --- a/experimental/Python/src/Intersection/Carla/run-carla.sh +++ /dev/null @@ -1 +0,0 @@ -/opt/carla-simulator/CarlaUE4.sh --opengl #-quality-level=Low diff --git a/experimental/Python/src/Intersection/Intersection.lf b/experimental/Python/src/Intersection/Intersection.lf deleted file mode 100644 index 4e487d8be2..0000000000 --- a/experimental/Python/src/Intersection/Intersection.lf +++ /dev/null @@ -1,543 +0,0 @@ -/** - * Model of a smart intersection with a road-side unit (RSU) - * that regulates the flow of automated vehicles through the - * intersection. Vehicles that are approaching the intersection - * send an initial message to the RSU with their speed and - * distance to the intersection. The RSU responds with a - * reservation for when the vehicle can enter the intersection - * and what its average speed through the intersection should be. - * - * This is meant as a supervisory controller, and it assumes that - * the vehicle is equipped with a low-level controller (or a human) - * that is responsible for lane keeping, collision avoidance, etc. - * - * This is a very rough starting point that needs a lot of work. - * - * Note: - * The 'Vehicle' controller reactor relies on an external vehicle interface - * that provides a 'vehicle_status' and a 'vehicle_position' (see the - * preamble below). To make this example interactive, a toy Simulator - * reactor is provided that outputs an initial velocity and position - * for each vehicle, and updates these values continuously using an - * input vehicle command (throttle and brake) from the vehicle controller. - */ -target Python; - -preamble {= -from math import sin, cos, sqrt, atan2, radians, pi -import time -import random - -try: - from math import isclose -except ImportError: - def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): - return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol) - -class coordinate: - """ - Represent a GPS coordinate in the form of x (lat), - y (lon), and z (alt). - """ - - def __init__(self, x = 0.0, y = 0.0, z = 0.0): - self.x = x - self.y = y - self.z = z - def distance(self, coordinate2): - """ - Calculate the great circle distance between two points - on the earth (specified in decimal degrees) - Taken from: https://stackoverflow.com/a/15737218/783868 - """ - # Currently ignores altitude - # Convert decimal degrees to radians - lat1 = radians(self.x) - lon1 = radians(self.y) - lat2 = radians(coordinate2.x) - lon2 = radians(coordinate2.y) - - # Haversine formula - dlon = lon2 - lon1 - dlat = lat2 - lat1 - a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2 - c = 2 * atan2(sqrt(a), sqrt(1 - a)) - # Radius of earth in kilometers is 6371 - km = 6371.0 * c - m = km * 1000.0 - return m - -class intersection_request: - """ - Represent a request to enter the intersection. - Sent by vehicles attempting to enter to the - Road-Side Unit (RSU) - """ - def __init__(self, speed = 0.0, current_pos = coordinate(0,0,0)): - self.speed = float(speed) - self.current_pos = current_pos # Current GPS position of the vehicle - -class vehicle_grant: - """ - Represent a grant issued by the Road-Side Unit (RSU) - to vehicles allowing them to enter the intersection - at a given 'arrival_time' with a given optional - 'target_speed'. - """ - - def __init__(self, target_speed = 0.0, arrival_time = None, intersection_pos = None): - self.target_speed = target_speed - self.arrival_time = arrival_time - self.intersection_pos = intersection_pos - -class vehicle_command: - """ - Command sent by the vehicle controller to the vehicle - interface. - """ - - def __init__(self, throttle = 0.0, brake = 0.0): - self.throttle = throttle - self.brake = brake - -class vehicle_status: - """ - Current status of the vehicle received from the - vehicle interface. Currently only contains - 'velocity'. - """ - - def __init__(self, velocity): - self.velocity = velocity # Velocity of the vehicle in m/s - # Fetching acceleration and orientation is also possible, - # but both involve very complex structures. - -class vehicle_position: - """ - Current GPS position of the vehicle, received from - the vehicle interface or a separate GNSS sensor. - """ - - def __init__(self, current_pos): - self.current_pos = current_pos # Current GPS position of the vehicle - - -class vehicle_velocity: - """ - Represent the current 3D velocity of the vehicle in a - .x, .y, .z format. - """ - def __init__(self, x=0.0, y=0.0, z=0.0): - self.x = x - self.y = y - self.z = z - -# The speed limit of vehicles in m/s -speed_limit = 14.0 -# The distance (in meters) at which the controller assumes it has reached its goal -goal_reached_threshold = 14.0 -# The time threshold at which the vehicle has reached its time-based goal -goal_reached_threshold_time = (goal_reached_threshold/speed_limit) -=} - -reactor Vehicle (vehicle_id(0)){ - input vehicle_stat; - input vehicle_pos; - - input grant; - - output request; - - output control; - - output goal_reached; - - logical action delay; - - state current_pos({=coordinate(0.0, 0.0, 0.0)=}); - - state last_pos; - - state granted_time_to_enter(0); - state intersection_pos; - state goal_reached(false); - state velocity(0.0); - - reaction(vehicle_pos) {= - self.current_pos = vehicle_pos.value.current_pos; - =} - reaction(vehicle_stat) -> request, control, goal_reached {= - if self.goal_reached: - # Nothing to do here - return - - # Record the speed - velocity_3d = vehicle_stat.value.velocity - linear_speed = sqrt(velocity_3d.x**2 + velocity_3d.y**2 + velocity_3d.z**2) - self.velocity = linear_speed - - if self.velocity == 0: - # Prevent divisions by zero - self.velocity = 0.001 - - # Check if we have received an initial pos - if self.current_pos.distance(coordinate(0.0, 0.0, 0.0)) <= 0.00000001: - print("Warning: Have not received initial pos yet.") - return - - # Send a new request to the RSU if no time to enter - # the intersection is granted - if self.granted_time_to_enter == 0: - message = intersection_request(speed = self.velocity, current_pos = self.current_pos) - request.set(message) - # Stop the vehicle - cmd = vehicle_command(throttle = 0, brake = 1) - control.set(cmd) - else: - # We have a granted time from the RSU - # All we need to do is adjust our velocity - # to enter the intersection at the allocated - # time - - # First, how far are we from the intersection - distance_remaining = self.intersection_pos.distance(self.current_pos) - time_remaining = (self.granted_time_to_enter - get_logical_time()) / (BILLION * 1.0) - - print("########################################") - print("Vehicle {}: Distance to intersection: {}m.".format(self.vehicle_id + 1, distance_remaining)) - print("Vehicle {}: Time to intersection: {}s.".format(self.vehicle_id + 1, time_remaining)) - print("Vehicle {}: Current speed: {}m/s.".format(self.vehicle_id + 1, self.velocity)) - - target_speed = 0.0 - # target_speed = distance_remaining/time_remaining - - if distance_remaining <= goal_reached_threshold and \ - time_remaining <= goal_reached_threshold_time : - # Goal reached - # At this point, a normal controller should stop the vehicle until - # it receives a new goal. However, for the purposes of this demo, - # it will set the target speed to the speed limit so that vehicles - # can leave the intersection (otherwise, they will just stop at the - # intersection). - target_speed = speed_limit - # Simulation is over - self.goal_reached = True - - print("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) - print("************* Vehicle {}: Reached intersection! *************".format(self.vehicle_id + 1)) - print("\n\n*************************************************************\n\n".format(self.vehicle_id + 1)) - - goal_reached.set(True) - elif time_remaining < (distance_remaining / speed_limit): - # No time to make it to the intersection even if we - # were going at the speed limit. - # Ask the RSU again - self.granted_time_to_enter = 0 - # Apply the brake since we ran out of time - target_speed = 0 - else: - # Has not reached the goal - # target_speed = ((2 * distance_remaining) / (time_remaining)) - self.velocity - target_speed = distance_remaining / time_remaining - - print("Vehicle {}: Calculated target speed: {}m/s.".format(self.vehicle_id + 1, target_speed)) - - if (target_speed - speed_limit) > 0: - print("Warning: target speed exceeds the speed limit") - target_speed = 0 - self.granted_time_to_enter = 0 - - if target_speed <= 0: - print("Warning: target speed negative or zero") - target_speed = 0.001 - self.granted_time_to_enter = 0 - - brake = 0.0 - throttle = 0.0 - - if target_speed >= self.velocity: - # Calculate a proportional throttle (0.0 < throttle < 1.0) - throttle = min((target_speed - self.velocity)/target_speed, 1) - # throttle = 1.0 - brake = 0.0 - # throttle = min(abs(target_speed / self.velocity), 1) - else: - # Need to apply the brake - brake = min((self.velocity - target_speed)/self.velocity, 1) - # brake = 1.0 - throttle = 0.0 - - # Check throttle boundaries - if throttle < 0: - print("Error: negative throttle") - throttle = 0 - - # Prepare and send the target velocity as a vehicle command - cmd = vehicle_command(throttle = throttle, brake = brake) - control.set(cmd) - - print("Vehicle {}: Throttle: {}. Brake: {}".format(self.vehicle_id + 1, throttle, brake)) - =} - - reaction(grant) {= - print("Vehicle {} Granted access".format(self.vehicle_id + 1), - "to enter the intersection at elapsed logical time {:d}.\n".format( - int(grant.value.arrival_time) - get_start_time() - ), - "Current elapsed logical time: {:d}, Current physical time is {:d}.".format( - get_elapsed_logical_time(), - get_elapsed_physical_time()) - ) - - self.granted_time_to_enter = grant.value.arrival_time - self.intersection_pos = grant.value.intersection_pos - self.goal_reached = False - =} deadline (5 sec) {= - # Ignore the grant. Will ask for another one - print("Received the grant late.") - self.granted_time_to_enter = 0 - =} -} - -reactor RSU ( - num_entries(4), - intersection_width(42.0), // in meters. - // If the vehicle is told to slow down, then its target - // average speed in the intersection should be at least this. - nominal_speed_in_intersection(2.8), // In m/sec 0.6 sec to traverse. - intersection_pos({=coordinate(0.0, 0.0, 0.0)=}) // GPS coordinates for the intersection -) { - input[num_entries] request; - input[num_entries] vehicle_reached_intersection; - output[num_entries] grant; - - state earliest_free(0 msec); - state active_participants({=[0] * 20=}); - - reaction(request) -> grant {= - for i in range(self.num_entries): - if request[i].is_present: - self.active_participants[i] = 1 - if request[i].value.speed == 0: - # Avoid division by zero - request[i].value.speed = 0.001 - # Calculate the time it will take the approaching vehicle to - # arrive at its current speed. Note that this is - # time from the time the vehicle sends the message - # according to the arriving vehicle's clock. - speed_in_m_per_sec = request[i].value.speed - dr = self.intersection_pos.distance(request[i].value.current_pos) - print("*** RSU: Vehicle {}'s distance to intersection is {}.".format(i+1, dr)) - arrival_in = dr / speed_in_m_per_sec - - time_message_sent = get_logical_time() - - # Convert the time interval to nsec (it is in seconds). - arrival_time_ns = time_message_sent + (arrival_in * BILLION) - - response = vehicle_grant() - if arrival_time_ns >= self.earliest_free: - # Vehicle can maintain speed. - response.target_speed = request[i].value.speed - response.arrival_time = arrival_time_ns - else: - # Could be smarter than this, but just send the nominal speed in intersection. - response.target_speed = self.nominal_speed_in_intersection - # Vehicle has to slow down and maybe stop. - response.arrival_time = self.earliest_free - - response.intersection_pos = self.intersection_pos - grant[i].set(response) - # Update earliest free on the assumption that the vehicle - # maintains its target speed (on average) within the intersection. - time_in_intersection = (BILLION * self.intersection_width) / (response.target_speed) - self.earliest_free = response.arrival_time + time_in_intersection - - print("*** RSU: Granted access to vehicle {} to enter at " - "time {} with average target velocity {} m/s. Next available time is {}".format( - i + 1, - response.arrival_time - get_start_time(), - response.target_speed, - self.earliest_free - get_start_time()) - ) - =} - - reaction(vehicle_reached_intersection) {= - sum_of_active_participants = 0 - for i in range(len(vehicle_reached_intersection)): - if vehicle_reached_intersection[i].is_present: - self.active_participants[i] = 0 - sum_of_active_participants += self.active_participants[i] - if sum_of_active_participants == 0: - # End the simulation if all vehicles have reached the intersection - request_stop() - print("\n********* SUCCESS: All vehicles have reached the intersection. *********\n") - =} -} - -main reactor ( - num_vehicles(4), - positions({= [ # Direction (velocity vector) - coordinate(0.000038,-0.000674,2.794825), # /|\ - coordinate(-0.000501,-0.001084,2.794891), # -> - coordinate(-0.000060,-0.001510,2.794854), # \|/ - coordinate(0.000367,-0.001185,2.794846), # <- - ]=}), - initial_speeds({= \ - [ \ - vehicle_velocity(y = -8.0), vehicle_velocity(x = 8.0), \ - vehicle_velocity(y = 8.0), vehicle_velocity(x = -8.0) \ - ] - =}) -) { - vehicles = new[num_vehicles] Vehicle(); - - rsu = new RSU( - num_entries = num_vehicles, - intersection_pos = {=coordinate(-0.000007632,-0.001124366,2.792485)=}, - intersection_width = 28, - nominal_speed_in_intersection = 14 - ); - vehicles.request -> rsu.request; - // Simulation will end once all vehicles have reached the intersection - vehicles.goal_reached -> rsu.vehicle_reached_intersection; - rsu.grant -> vehicles.grant; - - // Handle simulation - simulators = new[num_vehicles] Simulator( - initial_speeds = initial_speeds, - initial_speed = {=lambda self: self.initial_speeds[self.bank_index]=}, - positions = positions, - start_pos={=lambda self: self.positions[self.bank_index]=} - ) - - simulators.vehicle_stat -> vehicles.vehicle_stat; - simulators.vehicle_pos -> vehicles.vehicle_pos; - vehicles.control -> simulators.vehicle_command; -} - -/** - * A simulator that interacts with the vehicle controllers. - * This enables this example to be self-contained, without a need - * for a simulator such as Carla. - */ -reactor Simulator( - interval(1 sec), // Note: For demonstration purposes, - // this interval is set to a very low frequency. - // Set the interval to 16 msecs for a more - // realistic simulation. - initial_speeds({=[]=}), - initial_speed({=vehicle_velocity(x = 12.0)=}), - positions({=[]=}), - start_pos({=coordinate(0.000042,-0.000701,2.794825)=}), - max_acceleration(4), // m/s/s - drag_acceleration({=-0.2=}) -){ - - input vehicle_command; - output vehicle_stat; - output vehicle_pos; - - state current_pos; - state current_velocity; - state current_throttle(0); - state current_brake(0); - state velocity_vector( - {={"x":0, "y":0}=} - ); - - reaction(startup) -> vehicle_stat, vehicle_pos {= - # Give initial values to the vehicle controller - self.current_pos = self.start_pos(self) - self.current_velocity = self.initial_speed(self) - - # For this simulation, we would like the vehicles to - # move in a straight line. Therefore, we calculate a - # velocity vector here and will keep it throughout the - # simulation. An improvement would be for the vehicles - # to be able to change directions at the intersection. - if not isclose(self.current_velocity.x, 0): - self.velocity_vector["x"] = self.current_velocity.x / abs(self.current_velocity.x) # Keep the sign - - if not isclose(self.current_velocity.y, 0): - self.velocity_vector["y"] = self.current_velocity.y / abs(self.current_velocity.y) # Keep the sign - - """ - print("Simulator: Vehicle {}:\n".format(self.bank_index + 1), - "Initial position: x={:f}, y={:f}, z={:f}\n".format( - self.current_pos.x, - self.current_pos.y, - self.current_pos.z - ), - "Initial velocity: x={:f}, y={:f}, z={:f}\n".format( - self.current_velocity.x, - self.current_velocity.y, - self.current_velocity.z - ), - "Velocity vector: x={:f}, y={:f}".format( - self.velocity_vector["x"], - self.velocity_vector["y"] - ) - ) - """ - - vehicle_pos.set(vehicle_position(self.current_pos)) - vehicle_stat.set(vehicle_status(self.current_velocity)) - =} - - timer tick(0, interval) - reaction(tick) -> vehicle_stat, vehicle_pos {= - previous_velocity = self.current_velocity - - # Linearly calculate an acceleration based on the value of throttle and brake - current_acceleration = self.current_throttle * self.max_acceleration - current_acceleration -= self.current_brake * self.max_acceleration - - # Apply a constant drag - current_acceleration -= self.drag_acceleration - - print("Simulator: Vehicle {}:\n".format(self.bank_index + 1), - "Current position: x={:f}, y={:f}, z={:f}\n".format( - self.current_pos.x, - self.current_pos.y, - self.current_pos.z - ), - "Current velocity: x={:f}, y={:f}, z={:f}\n".format( - self.current_velocity.x, - self.current_velocity.y, - self.current_velocity.z - ), - "Current acceleration: {:f}\n".format( - current_acceleration - ), - "Velocity vector: x={:f}, y={:f}".format( - self.velocity_vector["x"], - self.velocity_vector["y"] - ) - ) - - # Apply acceleration - self.current_velocity.x += (current_acceleration * (self.interval / BILLION)) \ - * self.velocity_vector["x"] # Keep the direction - self.current_velocity.y += (current_acceleration * (self.interval / BILLION)) \ - * self.velocity_vector["y"] # Keep the direction - - vehicle_stat.set(vehicle_status(self.current_velocity)) - - # Change the GPS position of the vehicle - x_move_meters = previous_velocity.x * self.interval / BILLION - y_move_meters = previous_velocity.y * self.interval / BILLION - - self.current_pos.x = self.current_pos.x + (180/pi)*(x_move_meters/6378137) - self.current_pos.y = self.current_pos.y + (180/pi)*(y_move_meters/6378137) - - vehicle_pos.set(vehicle_position(self.current_pos)) - =} - - - reaction(vehicle_command) {= - # Update throttle and brake values - self.current_throttle = vehicle_command.value.throttle - self.current_brake = vehicle_command.value.brake - =} -} diff --git a/experimental/README.md b/experimental/README.md deleted file mode 100644 index a7d8f8fa1a..0000000000 --- a/experimental/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Experimental - -This directory contains a collection of programs and notes associated with Lingua Franca topics that are immature and under discussion. Some of these Lingua Franca programs do not compile, but rather are meant to illustrate possible future development ideas.