├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── mainConfig.yaml ├── perception.yaml ├── sensors.yaml └── vehicleModel.yaml ├── doc ├── ARWO_2024_presentation_slides.pdf ├── demo.gif ├── getting_started.md ├── logo_full.png └── logo_small.png ├── include ├── VehicleModel │ ├── VehicleModelInterface.hpp │ └── deadTime.hpp ├── competitionLogic.hpp ├── configParser.hpp ├── logger.hpp ├── quaternion.hpp ├── reportWriter.hpp ├── ros2Helpers.hpp ├── sensorModels │ ├── gnssSensor.hpp │ ├── imuSensor.hpp │ ├── perceptionSensor.hpp │ ├── scalarValueSensor.hpp │ ├── sensorBase.hpp │ └── wheelsSensor.hpp ├── track │ ├── gripMap.hpp │ └── trackLoader.hpp ├── transform.hpp └── types.hpp ├── launch └── example.launch.py ├── msg ├── GNSS.msg ├── Landmark.msg ├── PerceptionDetections.msg ├── StampedScalar.msg ├── Track.msg └── Wheels.msg ├── package.xml ├── scripts ├── convert_fssim_sdf_to_yaml.py └── report_evaluation.py ├── src ├── VehicleModel │ └── VehicleModelBicycle.cpp ├── competitionLogic.cpp ├── configParser.cpp ├── pacsim_main.cpp ├── quaternion.cpp ├── reportWriter.cpp ├── ros2Helpers.cpp ├── sensorModels │ ├── gnssSensor.cpp │ ├── imuSensor.cpp │ ├── perceptionSensor.cpp │ ├── scalarValueSensor.cpp │ └── wheelsSensor.cpp ├── track │ ├── gripMap.cpp │ └── trackLoader.cpp ├── transform.cpp └── types.cpp ├── srv ├── ClockTriggerAbsolute.srv └── ClockTriggerRelative.srv ├── track_editor ├── drawView.py ├── guiLogic.py ├── icons │ ├── compass.png │ ├── coneBigOrange.png │ ├── coneBlue.png │ ├── coneGreen.png │ ├── coneInvisible.png │ ├── coneOrange.png │ ├── coneUnknown.png │ ├── coneYellow.png │ ├── gnss.png │ ├── icon.png │ ├── laneConnectIcon.png │ ├── rulesIcon.png │ ├── start.png │ └── timeKeeping.png ├── main.py └── mapFile.py ├── tracks ├── FSE23.yaml ├── FSE24.yaml ├── FSG23.yaml ├── FSG24.yaml ├── FSS22_V1.yaml ├── FSS22_V2.yaml ├── acceleration.yaml ├── gripMap.yaml └── skidpad.yaml └── urdf ├── Model_without_Steering_Tires.stl ├── Steering_Wheel.stl ├── Tires ├── FL_Inside.stl ├── FL_Outside.stl ├── FR_Inside.stl ├── RL_Inside.stl └── RR_Inside.stl └── separate_model.xacro /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: WebKit 2 | IndentWidth: 4 3 | SeparateDefinitionStyle: SDS_Always 4 | UseTabStyle: UT_Always 5 | BreakBeforeBraces: Allman 6 | ColumnLimit: 120 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | __pycache__/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(pacsim) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 8 | add_compile_options(-Wall -Wextra -Wpedantic) 9 | endif() 10 | 11 | find_package(ament_cmake_auto REQUIRED) 12 | find_package(ament_cmake REQUIRED) 13 | find_package(visualization_msgs REQUIRED) 14 | find_package(std_msgs REQUIRED) 15 | find_package(std_srvs REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(tf2 REQUIRED) 18 | find_package(tf2_ros REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | find_package(rosidl_default_generators REQUIRED) 22 | find_package(builtin_interfaces REQUIRED) 23 | 24 | find_package(rclcpp_components REQUIRED) 25 | find_package(yaml-cpp REQUIRED) 26 | 27 | find_package(Eigen3 REQUIRED) 28 | 29 | 30 | rosidl_generate_interfaces( 31 | ${PROJECT_NAME} 32 | "msg/StampedScalar.msg" 33 | "msg/Wheels.msg" 34 | "msg/Landmark.msg" 35 | "msg/PerceptionDetections.msg" 36 | "msg/GNSS.msg" 37 | "msg/Track.msg" 38 | "srv/ClockTriggerAbsolute.srv" 39 | "srv/ClockTriggerRelative.srv" 40 | DEPENDENCIES 41 | geometry_msgs 42 | std_msgs 43 | sensor_msgs 44 | ) 45 | 46 | file(GLOB_RECURSE pacsim_SRCS 47 | "${PROJECT_SOURCE_DIR}/src/*.cpp" 48 | "${PROJECT_SOURCE_DIR}/src/*.c" 49 | ) 50 | 51 | 52 | add_executable(${PROJECT_NAME}_node ${pacsim_SRCS}) 53 | ament_target_dependencies(${PROJECT_NAME}_node rclcpp std_srvs visualization_msgs geometry_msgs tf2_ros yaml-cpp) 54 | ament_export_dependencies(std_msgs geometry_msgs rosidl_default_runtime) 55 | 56 | rosidl_target_interfaces(${PROJECT_NAME}_node 57 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 58 | 59 | target_include_directories(${PROJECT_NAME}_node PUBLIC 60 | ${EIGEN3_INCLUDE_DIRS} 61 | "$" 62 | "$") 63 | 64 | target_link_libraries( 65 | ${PROJECT_NAME}_node 66 | ${YAML_CPP_LIBRARIES} 67 | ) 68 | 69 | 70 | install(TARGETS 71 | ${PROJECT_NAME}_node 72 | DESTINATION lib/${PROJECT_NAME} 73 | ) 74 | 75 | file(GLOB_RECURSE pacsim_TRACKS 76 | "${PROJECT_SOURCE_DIR}/tracks/*.yaml" 77 | ) 78 | file(GLOB_RECURSE pacsim_CONFIGS 79 | "${PROJECT_SOURCE_DIR}/config/*.yaml" 80 | ) 81 | file(GLOB_RECURSE modells 82 | "${PROJECT_SOURCE_DIR}/urdf/*" 83 | ) 84 | install(FILES ${modells} 85 | DESTINATION share/${PROJECT_NAME}/urdf 86 | ) 87 | install(FILES ${pacsim_TRACKS} 88 | DESTINATION tracks/${PROJECT_NAME} 89 | ) 90 | install(FILES ${pacsim_CONFIGS} 91 | DESTINATION config/${PROJECT_NAME} 92 | ) 93 | ament_auto_package(INSTALL_TO_SHARE 94 | tracks 95 | config 96 | launch 97 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 PacSim 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Project logo](doc/logo_full.png) 2 | # PacSim 3 | PacSim (Planning and Controls Simuluator) is a simulator for Formula Student Driverless competitions originally developed at [Elbflorace](https://elbflorace.de/). 4 | 5 | Example of a pipeline running in PacSim with visualizations in Foxglove: 6 | 7 | ![Demo gif](doc/demo.gif) 8 | 9 | # Features 10 | * Closed loop simulation of the autonomous system. 11 | * Simulation of vehicle dynamics 12 | * Simulation of all the relevant sensors (except for raw perception data) for autonomous operation. All sensors simulated are specified by at least their rate, dead time (delay), noise and some sensor-specific additional attributes. 13 | * IMU, wheel speeds, wheel motor torques, steering angle, GNSS 14 | * Mock model of the detected cones by the perception system 15 | * Competition logic 16 | * Track lap times, evaluate penalties and detect DNF 17 | * Create report at the end of the run 18 | * Configurable grip map (different friction coefficients for track segments) 19 | * Additional scripts and tools 20 | * Track editor (track_editor directory) 21 | * Converter from [FSSIM](https://github.com/AMZ-Driverless/fssim) sdf to our track format. 22 | 23 | # Prerequisites 24 | This package is developed and tested on Ubuntu 22.04 with ROS2 Iron. 25 | 26 | Install dependencies: 27 | 28 | `sudo apt install ros-iron-desktop ros-iron-xacro` 29 | 30 | # How to get started 31 | 32 | To get started with using PacSim, take a look at the [Getting Started README](https://github.com/PacSim/pacsim/blob/master/doc/getting_started.md) here. 33 | 34 | # Contributing 35 | Contributions in any form (reports, feedback, requests, submissions) are welcome. Preferably create an Issue or Pull request for that. 36 | 37 | The project also has a [discord server](https://discord.gg/Ay3XzB5p33). 38 | 39 | # Known issues 40 | * In Foxglove Studio (in contrast to RViz) the mesh of the car is displayed incorrectly using default settings. To fix this go to the left sidebar of the 3d panel -> Scene -> Mesh up axis and select Z-up. Restart to apply the change. 41 | * Currently the track editor can't handle the skidpad and acceleration tracks (lanesFirstWithLastConnected = False) properly. 42 | * Currently there exists no editor and visualization for the grip map. 43 | 44 | # Acknowledgements 45 | The initial version was developed at Elbflorace by: 46 | * Alexander Phieler (Main development) 47 | * Niklas Leukroth (Track and Config file parser) 48 | * Sergio Antuna (Artwork) 49 | * Tim Hanel (3d car model integration) 50 | -------------------------------------------------------------------------------- /config/mainConfig.yaml: -------------------------------------------------------------------------------- 1 | pacsim: 2 | timeouts: 3 | start: 60 # time to trigger first timekeeping gate 4 | acceleration: 25 5 | autocross: 300 6 | skidpad: 90 7 | trackdrive_first: 300 8 | trackdrive_total: 1000 9 | oc_detect: true 10 | doo_detect: true 11 | uss_detect: true 12 | finish_validate: true 13 | 14 | cog_frame_id_pipeline: "cog" # name of the cog frame in the autnomous pipeline 15 | broadcast_sensors_tf2: true 16 | 17 | pre_transform_track: true # transform track such that starting pose also seems to be the origin of the track -------------------------------------------------------------------------------- /config/perception.yaml: -------------------------------------------------------------------------------- 1 | perception_sensors: 2 | - sensor: 3 | name: "livox_front" 4 | enabled: true 5 | rate: 10 6 | delay: 7 | mean: 0.2 8 | pose: 9 | position: [0.0,0.0,0.0] 10 | orientation: [0.0,0.0,0.0] 11 | observation: 12 | min_range: 1 13 | max_range: 45 14 | min_angle_horizontal: -1.0472 15 | max_angle_horizontal: 1.0472 16 | min_angle_vertical: -0.2181662 17 | max_angle_vertical: 0.2181662 18 | 19 | detection: 20 | prob_min_dist: 0.99 21 | prob_decrease_dist_linear: 0.01 22 | prob_decrease_dist_quadratic: 0.00012 23 | min_prob: 0.1 24 | 25 | classification: 26 | max_dist: 15 27 | prob_min_dist: 0.99 28 | prob_decrease_dist_linear: 0.01 29 | prob_decrease_dist_quadratic: 0.000125 30 | min_prob: 0.2 # 0.2 is equivalent to random guessing (5 classes) 31 | 32 | noise: 33 | cartesian: #x, y, z 34 | mean: [0.0, 0.0, 0.0] 35 | sigma: [0.01, 0.01, 0.01] 36 | angle: # horizontal, vertical 37 | mean: [0.0, 0.0] 38 | sigma: [0.001, 0.001] 39 | range: 40 | mean: 0.0 41 | sigma: 0.02 42 | range_relative: 43 | mean: 0.0 44 | sigma: 0.0001 45 | - sensor: 46 | name: "livox_left2" 47 | enabled: true 48 | rate: 10 49 | delay: 50 | mean: 0.1 51 | pose: 52 | position: [-1.7533289, 0.59746433, 0.07546455] 53 | orientation: [0.12472367, -0.00889492, 1.21845561] 54 | observation: 55 | min_range: 1 56 | max_range: 45 57 | min_angle_horizontal: -1.0472 58 | max_angle_horizontal: 1.0472 59 | min_angle_vertical: -0.2181662 60 | max_angle_vertical: 0.2181662 61 | 62 | detection: 63 | prob_min_dist: 0.99 64 | prob_decrease_dist_linear: 0.01 65 | prob_decrease_dist_quadratic: 0.00012 66 | min_prob: 0.1 67 | 68 | classification: 69 | max_dist: 15 70 | prob_min_dist: 0.99 71 | prob_decrease_dist_linear: 0.01 72 | prob_decrease_dist_quadratic: 0.000125 73 | min_prob: 0.2 # 0.2 is equivalent to random guessing (5 classes) 74 | 75 | noise: 76 | cartesian: #x, y, z 77 | mean: [0.0, 0.0, 0.0] 78 | sigma: [0.01, 0.01, 0.01] 79 | angle: # horizontal, vertical 80 | mean: [0.0, 0.0] 81 | sigma: [0.001, 0.001] 82 | range: 83 | mean: 0.0 84 | sigma: 0.02 85 | range_relative: 86 | mean: 0.0 87 | sigma: 0.0001 88 | - sensor: 89 | name: "livox_right2" 90 | enabled: true 91 | rate: 10 92 | delay: 93 | mean: 0.1 94 | pose: 95 | position: [-1.73520207, -0.61862863, 0.04065186] 96 | orientation: [-0.08730302, 0.06381912, -1.23762976] 97 | observation: 98 | min_range: 1 99 | max_range: 45 100 | min_angle_horizontal: -1.0472 101 | max_angle_horizontal: 1.0472 102 | min_angle_vertical: -0.2181662 103 | max_angle_vertical: 0.2181662 104 | 105 | detection: 106 | prob_min_dist: 0.99 107 | prob_decrease_dist_linear: 0.01 108 | prob_decrease_dist_quadratic: 0.00012 109 | min_prob: 0.1 110 | 111 | classification: 112 | max_dist: 15 113 | prob_min_dist: 0.99 114 | prob_decrease_dist_linear: 0.01 115 | prob_decrease_dist_quadratic: 0.000125 116 | min_prob: 0.2 # 0.2 is equivalent to random guessing (5 classes) 117 | 118 | noise: 119 | cartesian: #x, y, z 120 | mean: [0.0, 0.0, 0.0] 121 | sigma: [0.01, 0.01, 0.01] 122 | angle: # horizontal, vertical 123 | mean: [0.0, 0.0] 124 | sigma: [0.001, 0.001] 125 | range: 126 | mean: 0.0 127 | sigma: 0.02 128 | range_relative: 129 | mean: 0.0 130 | sigma: 0.0001 -------------------------------------------------------------------------------- /config/sensors.yaml: -------------------------------------------------------------------------------- 1 | sensors: 2 | steering_front: 3 | rate: 200 4 | dead_time: 0.005 5 | error: 6 | mean: 0.0 7 | sigma: 0.00001 8 | steering_rear: 9 | rate: 200 10 | dead_time: 0.005 11 | error: 12 | mean: 0.0 13 | sigma: 0.00001 14 | wheelspeeds: 15 | rate: 200 16 | dead_time: 0.005 17 | error: 18 | mean: 0.0 19 | sigma: 0.1 20 | torques: 21 | rate: 200 22 | dead_time: 0.005 23 | error: 24 | mean: 0.0 25 | sigma: 0.01 26 | voltage_ts: 27 | rate: 200 28 | dead_time: 0.005 29 | error: 30 | mean: 0.0 31 | sigma: 0.00001 32 | current_ts: 33 | rate: 200 34 | dead_time: 0.005 35 | error: 36 | mean: 0.0 37 | sigma: 0.00001 38 | imus: 39 | - imu: 40 | name: "cog_imu" 41 | frame: "cog_static" 42 | pose: 43 | position: [0.0,0.0,0.0] 44 | orientation: [0.0,0.0,0.0] 45 | rate: 200 46 | dead_time: 0.005 47 | error_acc: 48 | mean: 0.0 49 | sigma: 0.05 50 | error_rot: 51 | mean: 0.0 52 | sigma: 0.001 53 | gnssSensors: 54 | - gnss: 55 | name: "gnss_front" 56 | frame: "gnss_front" 57 | pose: 58 | position: [0.66,0.0,0.64] 59 | orientation: [0.0,0.0,0.0] 60 | rate: 20 61 | delay: 62 | mean: 0.05 63 | features: 64 | velocity: true 65 | orientation: 0 # 0 : no orientaiton, 1 : heading only, 2 : full orientation 66 | noise: 67 | position: # enu plane 68 | mean: [0.0, 0.0, 0.0] 69 | sigma: [0.03, 0.03, 1.3] 70 | velocity: # enu plane 71 | mean: [0.0, 0.0, 0.0] 72 | sigma: [0.02, 0.02, 0.02] 73 | orientation: # enu plane 74 | mean: [0.0, 0.0, 0.0] 75 | sigma: [0.02, 0.02, 0.02] -------------------------------------------------------------------------------- /config/vehicleModel.yaml: -------------------------------------------------------------------------------- 1 | vehicle_model: 2 | simple_bicycle_model: 3 | # some made up values 4 | kinematics: 5 | lr: 0.72 6 | lf: 0.72 7 | sf: 1.15 8 | sr: 1.15 9 | tire: 10 | Blat: 9.63 11 | Clat: -1.39 12 | Dlat: 1.6 13 | Elat: 1.0 14 | aero: 15 | cla: 3.7 16 | cda: 1.1 17 | aeroArea: 1.1 18 | m: 178.0 19 | Izz: 111.0 20 | wheelRadius: 0.206 21 | gearRatio: 12.23 22 | innerSteeringRatio: 0.255625 23 | outerSteeringRatio: 0.20375 24 | nominalVoltageTS: 550.0 25 | powerGroundForce: 700.0 26 | powertrainEfficiency: 0.95 27 | -------------------------------------------------------------------------------- /doc/ARWO_2024_presentation_slides.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/doc/ARWO_2024_presentation_slides.pdf -------------------------------------------------------------------------------- /doc/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/doc/demo.gif -------------------------------------------------------------------------------- /doc/getting_started.md: -------------------------------------------------------------------------------- 1 | # Getting started with PacSim 2 | 3 | ## Interfaces 4 | To use PacSim, you need to create a message converter node to match your own interfaces with the simulator. You will need to advertise and publish to some topics. Note that some topics are not directly used in the simulator with the default vehicle model provided. 5 | 6 | ### Inputs 7 | 8 | | TOPIC | MESSAGE TYPE | DESCRIPTION | USED WITH DEFAULT MODEL? | 9 | |------------------------------|----------------------------|---------------------------------------------------------------------------------------|---| 10 | | /pacsim/steering_setpoint | pacsim::msg::StampedScalar | Target steering angle at the steering wheel/sensor (rad) | YES | 11 | | /pacsim/powerground_setpoint | pacsim::msg::StampedScalar | Powered ground multiplier. 0 for no powered aero, 1 for full capacity (Dimensionless) | YES | 12 | | /pacsim/wheelspeed_setpoints | pacsim::msg::Wheels | Target wheel speeds at each wheel's motor (RPM) | NO | 13 | | /pacsim/torques_min | pacsim::msg::Wheels | Lower bound torque value at each wheel's motor (Nm) | NO | 14 | | /pacsim/torques_max | pacsim::msg::Wheels | Upper bound torque value at each wheel's motor (Nm) | YES | 15 | 16 | ### Outputs 17 | 18 | TBD 19 | 20 | 21 | ## Other notes 22 | 23 | The simulation's interfaces are provided so that motor controllers with an internal control loop for the wheel speeds (e.g. AMK or Electrophorus Engineering) can be fully modelled in the simulator. The default model does not contain this control loop. The values provided for `torques_max` are the actual torques applied to the wheels. 24 | 25 | The example launch file (example.launch.py) shows an example of how to start the simulator node and the robot_state_publisher for the 3D visualisation of the car. 26 | 27 | The sensors and vehicle model are configured using config files. Examples are provided in the config folder. Things such as the discipline or the path of the track file or config files are defined using ROS2 parameters. 28 | 29 | The default vehicle model provided is rather simple and just meant to be a starting point. You are encouraged to integrate your own vehicle model by implementing the `IVehicleModel` class. 30 | -------------------------------------------------------------------------------- /doc/logo_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/doc/logo_full.png -------------------------------------------------------------------------------- /doc/logo_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/doc/logo_small.png -------------------------------------------------------------------------------- /include/VehicleModel/VehicleModelInterface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMVEHICLEMODELINTERFACE_HPP 2 | #define PACSIMVEHICLEMODELINTERFACE_HPP 3 | 4 | #include "configParser.hpp" 5 | #include "types.hpp" 6 | #include 7 | 8 | class IVehicleModel 9 | { 10 | public: 11 | // pure virtual function 12 | virtual Eigen::Vector3d getPosition() = 0; 13 | virtual Eigen::Vector3d getOrientation() = 0; 14 | virtual Eigen::Vector3d getAngularVelocity() = 0; 15 | virtual Eigen::Vector3d getAngularAcceleration() = 0; 16 | virtual Eigen::Vector3d getVelocity() = 0; 17 | virtual Eigen::Vector3d getAcceleration() = 0; 18 | virtual Wheels getSteeringAngles() = 0; 19 | virtual Wheels getWheelspeeds() = 0; 20 | virtual Wheels getTorques() = 0; 21 | virtual Wheels getWheelOrientations() = 0; 22 | virtual double getSteeringWheelAngle() = 0; 23 | virtual double getVoltageTS() = 0; 24 | virtual double getCurrentTS() = 0; 25 | virtual std::array getWheelPositions() = 0; 26 | virtual bool readConfig(ConfigElement& config) = 0; 27 | 28 | virtual void setTorques(Wheels in) = 0; 29 | virtual void setRpmSetpoints(Wheels in) = 0; 30 | virtual void setMinTorques(Wheels in) = 0; 31 | virtual void setMaxTorques(Wheels in) = 0; 32 | virtual void setSteeringSetpointFront(double in) = 0; 33 | virtual void setSteeringSetpointRear(double in) = 0; 34 | virtual void setPowerGroundSetpoint(double in) = 0; 35 | 36 | virtual void setPosition(Eigen::Vector3d position) = 0; 37 | virtual void setOrientation(Eigen::Vector3d orientation) = 0; 38 | 39 | virtual void forwardIntegrate(double dt, Wheels frictionCoefficients) = 0; 40 | 41 | protected: 42 | // states 43 | Eigen::Vector3d position; 44 | Eigen::Vector3d orientation; 45 | 46 | Eigen::Vector3d velocity; 47 | Eigen::Vector3d acceleration; 48 | 49 | Eigen::Vector3d angularVelocity; 50 | Eigen::Vector3d angularAcceleration; 51 | 52 | Wheels wheelOrientations; 53 | Wheels wheelspeeds; 54 | // inputs 55 | Wheels torques; 56 | Wheels steeringAngles; 57 | 58 | double time; 59 | }; 60 | 61 | #endif /* PACSIMVEHICLEMODELINTERFACE_HPP */ -------------------------------------------------------------------------------- /include/VehicleModel/deadTime.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMDEADTIME_HPP 2 | #define PACSIMDEADTIME_HPP 3 | 4 | #include 5 | #include 6 | 7 | template class DeadTime 8 | { 9 | public: 10 | DeadTime(double deadtime) { this->deadTime = deadtime; } 11 | 12 | DeadTime(const DeadTime& in) 13 | { 14 | std::lock_guard l(_mutex); 15 | this->deadTime = in.deadTime; 16 | this->deadTimeQueue = in.deadTimeQueue; 17 | this->times = in.times; 18 | } 19 | 20 | DeadTime& operator=(const DeadTime& in) 21 | { 22 | std::lock_guard l(_mutex); 23 | this->deadTime = in.deadTime; 24 | this->deadTimeQueue = in.deadTimeQueue; 25 | this->times = in.times; 26 | return *this; 27 | } 28 | 29 | void updateDeadTime(double time) { this->deadTime = time; } 30 | 31 | T getOldest() 32 | { 33 | std::lock_guard l(_mutex); 34 | T elem = this->deadTimeQueue.front(); 35 | this->deadTimeQueue.pop(); 36 | this->times.pop(); 37 | return elem; 38 | } 39 | 40 | bool availableDeadTime(double time) 41 | { 42 | bool ret = false; 43 | std::lock_guard l(_mutex); 44 | if (this->deadTimeQueue.size() >= 1) 45 | { 46 | T elem = this->deadTimeQueue.front(); 47 | if (time >= (this->times.front() + this->deadTime)) 48 | { 49 | ret = true; 50 | } 51 | } 52 | return ret; 53 | } 54 | 55 | void addVal(T val, double currTime) 56 | { 57 | std::lock_guard l(_mutex); 58 | this->deadTimeQueue.push(val); 59 | this->times.push(currTime); 60 | } 61 | 62 | private: 63 | std::mutex _mutex; 64 | double deadTime; 65 | std::queue deadTimeQueue; 66 | std::queue times; 67 | }; 68 | 69 | #endif /* PACSIMDEADTIME_HPP */ -------------------------------------------------------------------------------- /include/competitionLogic.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COMPETITIONLOGIC_HPP 2 | #define COMPETITIONLOGIC_HPP 3 | 4 | #include "logger.hpp" 5 | #include "types.hpp" 6 | #include 7 | #include 8 | 9 | class CompetitionLogic 10 | { 11 | public: 12 | enum PENALTY_TYPE 13 | { 14 | DOO, 15 | OC, 16 | USS 17 | }; 18 | 19 | struct Penalty 20 | { 21 | int lap; 22 | double occurence_time; 23 | PENALTY_TYPE reason; 24 | double penalty_time; 25 | Eigen::Vector3d position; 26 | }; 27 | 28 | CompetitionLogic(std::shared_ptr logger, Track& track, MainConfig config); 29 | 30 | bool evaluateOffCourse(Track& track, double time, Eigen::Vector3d& position, Eigen::Vector3d& orientation); 31 | 32 | void evaluateConeHit(Track& track, double time, Eigen::Vector3d& position, Eigen::Vector3d& orientation); 33 | 34 | int timeKeepingStatus( 35 | Eigen::Vector3d lm1, Eigen::Vector3d lm2, Eigen::Vector3d& position, Eigen::Vector3d& orientation); 36 | 37 | void evaluateTimeKeepings(Track& track, Eigen::Vector3d& position, Eigen::Vector3d& orientation, double time); 38 | 39 | bool checkFinishConditionsMet(double time); 40 | 41 | bool checkUSS(Track track, double time, Eigen::Vector3d position); 42 | 43 | bool checkDNF(Track track, double time, Eigen::Vector3d position); 44 | 45 | bool performAllChecks(Track& track, double time, Eigen::Vector3d& position, Eigen::Vector3d& orientation); 46 | 47 | void setFinish(bool val); 48 | 49 | void fillReport(Report& report, double time); 50 | 51 | private: 52 | bool pointInTriangle(Eigen::Vector2d a, Eigen::Vector2d b, Eigen::Vector2d c, Eigen::Vector2d point); 53 | 54 | std::vector pointsInTrackConnected(Track& track, std::vector points); 55 | 56 | bool pointInPolygon(std::vector polyPoints, Eigen::Vector2d point); 57 | 58 | std::pair rayIntersectLineSegment( 59 | Eigen::Vector2d a, Eigen::Vector2d b, Eigen::Vector2d rayOrigin, Eigen::Vector2d rayDirection); 60 | 61 | double cross2d(Eigen::Vector2d a, Eigen::Vector2d b); 62 | 63 | std::vector pointsInTrackNotConnected(Track& track, std::vector points); 64 | 65 | bool carConePolyIntersect(std::vector carPoly, std::vector conePoly); 66 | 67 | double determinantLinePoint(Eigen::Vector2d a, Eigen::Vector2d b, Eigen::Vector2d c); 68 | 69 | bool inLineSegement(Eigen::Vector2d a, Eigen::Vector2d b, Eigen::Vector2d position); 70 | 71 | void evaluateTimeKeepingGateTrigger(Track track, double time, int index); 72 | 73 | std::string discipline2str(Discipline d); 74 | 75 | std::string penalty2str(PENALTY_TYPE p); 76 | 77 | bool checkTimeout(double time); 78 | 79 | std::vector timeKeepingStatuses; 80 | 81 | std::vector timeKeepingFirstTriggerStatuses; 82 | 83 | std::vector> triggerTimes; 84 | 85 | bool started; 86 | double startedTime; 87 | int lapCount; 88 | 89 | double lastTriggerTime; 90 | 91 | std::vector lapTimes; 92 | std::vector currentSectorTimes; 93 | std::vector> sectorTimes; 94 | Discipline discipline; 95 | double finishConditionsMetFirstTime; 96 | bool finishConditionsMet; 97 | bool alreadyOC; 98 | double Off_Course_Start; 99 | bool isDNF; 100 | std::string dnf_reason = ""; 101 | std::vector penalties; 102 | bool ussTriggered; 103 | bool finishSignal; 104 | double timeout_start; 105 | double timeout_acceleration; 106 | double timeout_autocross; 107 | double timeout_skidpad; 108 | double timeout_trackdrive_first; 109 | double timeout_trackdrive_total; 110 | bool properTrack; 111 | }; 112 | 113 | #endif /* COMPETITIONLOGIC_HPP */ -------------------------------------------------------------------------------- /include/configParser.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONFIGPARSER_HPP 2 | #define CONFIGPARSER_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "types.hpp" 6 | #include "yaml-cpp/yaml.h" 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | using namespace YAML; 13 | 14 | class ConfigElement 15 | { 16 | public: 17 | ConfigElement(Node node) 18 | : node(node) 19 | , type(node.Type()) 20 | { 21 | } 22 | 23 | NodeType::value getType(); 24 | Node getNode(); 25 | bool hasElement(string elementName); 26 | vector getElements(); 27 | ConfigElement getElement(string elementName); 28 | bool getElement(ConfigElement* element, string elementName); 29 | bool getElements(vector* vec); 30 | ConfigElement operator[](string elementName) 31 | { 32 | if (this->type != NodeType::Map) 33 | { 34 | throw runtime_error("Not a map but a " + to_string(this->type)); 35 | } 36 | return this->node[elementName]; 37 | } 38 | ConfigElement operator[](int i) 39 | { 40 | if (this->type != NodeType::Sequence) 41 | { 42 | throw runtime_error("Not a sequence but a " + to_string(this->type)); 43 | } 44 | return this->node[i]; 45 | } 46 | template inline T getElement(string elementName) { return this->node[elementName].as(); }; 47 | template inline bool getElement(T* res, string elementName) 48 | { 49 | *res = this->node[elementName].as(); 50 | return true; 51 | } 52 | 53 | protected: 54 | Node node; 55 | NodeType::value type; 56 | }; 57 | 58 | class Config : public ConfigElement 59 | { 60 | public: 61 | Config(string path) 62 | : ConfigElement(LoadFile(path)) 63 | { 64 | } 65 | }; 66 | 67 | #endif // CONFIGPARSER_HPP -------------------------------------------------------------------------------- /include/logger.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSCIMLOGGER_HPP 2 | #define PACSCIMLOGGER_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include 6 | 7 | class Logger 8 | { 9 | public: 10 | void logInfo(std::string in) { RCLCPP_INFO_STREAM(rclcpp::get_logger("pacsim_logger"), in); } 11 | 12 | void logWarning(std::string in) { RCLCPP_WARN_STREAM(rclcpp::get_logger("pacsim_logger"), in); } 13 | 14 | void logError(std::string in) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("pacsim_logger"), in); } 15 | 16 | void logFatal(std::string in) { RCLCPP_FATAL_STREAM(rclcpp::get_logger("pacsim_logger"), in); } 17 | }; 18 | 19 | #endif /* PACSCIMLOGGER_HPP */ -------------------------------------------------------------------------------- /include/quaternion.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMQUATERNION_HPP 2 | #define PACSIMQUATERNION_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | struct quaternion 10 | { 11 | double w = 1.0; 12 | double x = 0.0; 13 | double y = 0.0; 14 | double z = 0.0; 15 | }; 16 | 17 | double quatNorm(quaternion a); 18 | 19 | quaternion quatMult(quaternion a, quaternion b); 20 | 21 | quaternion quatInversion(quaternion a); 22 | 23 | quaternion quatNormalize(quaternion a); 24 | 25 | quaternion getRelativeQuat(quaternion a, quaternion b); 26 | 27 | quaternion quatFromEulerAngles(Eigen::Vector3d a); 28 | 29 | quaternion quatFromRotationMatrix(Eigen::Matrix3d mat); 30 | 31 | Eigen::Matrix3d rotationMatrixFromQuaternion(quaternion a); 32 | 33 | Eigen::Vector3d eulerAnglesFromQuat(quaternion a); 34 | 35 | #endif /* PACSIMQUATERNION_HPP */ -------------------------------------------------------------------------------- /include/reportWriter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef REPORTWRITER_HPP 2 | #define REPORTWRITER_HPP 3 | 4 | #include "types.hpp" 5 | #include "yaml-cpp/yaml.h" 6 | #include 7 | #include 8 | #include 9 | 10 | bool reportToFile(Report report, std::string dir); 11 | 12 | #endif /* REPORTWRITER_HPP */ -------------------------------------------------------------------------------- /include/ros2Helpers.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMROS2HELPERS_HPP 2 | #define PACSIMROS2HELPERS_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | #include "geometry_msgs/msg/transform_stamped.hpp" 7 | #include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" 8 | #include "pacsim/msg/gnss.hpp" 9 | #include "pacsim/msg/perception_detections.hpp" 10 | #include "pacsim/msg/track.hpp" 11 | #include "sensor_msgs/msg/imu.hpp" 12 | #include "sensor_msgs/msg/joint_state.hpp" 13 | #include "types.hpp" 14 | #include "visualization_msgs/msg/marker_array.hpp" 15 | #include 16 | #include 17 | #include 18 | 19 | class LandmarksMarkerWrapper 20 | { 21 | public: 22 | LandmarksMarkerWrapper(double alpha, std::string nameSpace); 23 | 24 | visualization_msgs::msg::MarkerArray markerFromLMs(Track& in, std::string frame, double time); 25 | 26 | visualization_msgs::msg::MarkerArray deleteAllMsg(std::string frame); 27 | 28 | private: 29 | double alpha = 0.3; 30 | std::string nameSpace = "pacsim"; 31 | int lastMaxId; 32 | }; 33 | 34 | pacsim::msg::Landmark LandmarkToRosMessage(const Landmark& lm, std::string frameId, double time); 35 | 36 | pacsim::msg::PerceptionDetections LandmarkListToRosMessage( 37 | const LandmarkList& sensorLms, std::string frameId, double time); 38 | 39 | sensor_msgs::msg::Imu createRosImuMsg(const ImuData& data); 40 | 41 | geometry_msgs::msg::TwistWithCovarianceStamped createRosTwistMsg( 42 | const Eigen::Vector3d& vel, const Eigen::Vector3d& rot, const std::string& frame, double time); 43 | 44 | geometry_msgs::msg::TransformStamped createRosTransformMsg(const Eigen::Vector3d& trans, const Eigen::Vector3d& rot, 45 | const std::string& frame, const std::string& child_frame, double time); 46 | 47 | geometry_msgs::msg::TransformStamped createStaticTransform( 48 | const std::string& frame, const std::string& child_frame, double time); 49 | 50 | sensor_msgs::msg::JointState createRosJointMsg( 51 | const std::vector& joint_names, const std::vector& joint_vals, double time); 52 | 53 | pacsim::msg::GNSS createRosGnssMessage(const GnssData& data); 54 | 55 | pacsim::msg::Track createRosTrackMessage(const Track& data, std::string frameId, double time); 56 | #endif /* PACSIMROS2HELPERS_HPP */ -------------------------------------------------------------------------------- /include/sensorModels/gnssSensor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMGNSSSENSOR_HPP 2 | #define PACSIMGNSSSENSOR_HPP 3 | 4 | #include "configParser.hpp" 5 | #include "sensorBase.hpp" 6 | #include "transform.hpp" 7 | #include "types.hpp" 8 | #include 9 | #include 10 | 11 | class GnssSensor : public SensorBase 12 | { 13 | public: 14 | GnssSensor(); 15 | 16 | void readConfig(ConfigElement& config); 17 | 18 | std::string getName(); 19 | 20 | std::string getFrameId(); 21 | 22 | bool RunTick(Eigen::Vector3d& gnssOrigin, Eigen::Vector3d& enuToTrackRotation, Eigen::Vector3d& trans, 23 | Eigen::Vector3d& rot, double time, Eigen::Vector3d velocity, Eigen::Vector3d omega, 24 | Eigen::Vector3d start_position, Eigen::Vector3d start_orientation, bool trackPreTransformed); 25 | 26 | private: 27 | std::string name; 28 | std::string frame_id; 29 | 30 | int noiseSeed; 31 | 32 | Eigen::Vector3d errorSigmaPosition; 33 | Eigen::Vector3d errorMeanPosition; 34 | 35 | Eigen::Vector3d errorSigmaVelocity; 36 | Eigen::Vector3d errorMeanVelocity; 37 | 38 | Eigen::Vector3d errorSigmaOrientation; 39 | Eigen::Vector3d errorMeanOrientation; 40 | 41 | bool outputVelocity; 42 | int orientationMode; 43 | }; 44 | 45 | #endif /* PACSIMGNSSSENSOR_HPP */ -------------------------------------------------------------------------------- /include/sensorModels/imuSensor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMIMUSENSOR_HPP 2 | #define PACSIMIMUSENSOR_HPP 3 | 4 | #include "configParser.hpp" 5 | #include "sensorBase.hpp" 6 | #include "transform.hpp" 7 | #include "types.hpp" 8 | #include 9 | #include 10 | 11 | class ImuSensor : public SensorBase 12 | { 13 | public: 14 | ImuSensor(double rate, double deadTime); 15 | 16 | void readConfig(ConfigElement& config); 17 | 18 | ImuData process(ImuData& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time); 19 | 20 | bool RunTick(ImuData& in, Eigen::Vector3d& alpha, double time); 21 | 22 | ImuData applyError(ImuData input); 23 | 24 | std::string getName(); 25 | 26 | private: 27 | double error_mean_acc; 28 | double error_sigma_acc; 29 | double error_mean_rot; 30 | double error_sigma_rot; 31 | std::string name; 32 | std::string frame; 33 | }; 34 | 35 | #endif /* PACSIMIMUSENSOR_HPP */ -------------------------------------------------------------------------------- /include/sensorModels/perceptionSensor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMPERCEPTIONSENSOR_HPP 2 | #define PACSIMPERCEPTIONSENSOR_HPP 3 | 4 | #include "configParser.hpp" 5 | #include "sensorBase.hpp" 6 | #include "transform.hpp" 7 | #include "types.hpp" 8 | #include 9 | #include 10 | 11 | class PerceptionSensor : public SensorBase 12 | { 13 | public: 14 | PerceptionSensor(); 15 | 16 | void readConfig(ConfigElement& config); 17 | 18 | LandmarkList process(LandmarkList& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time); 19 | 20 | std::vector filterFoV(std::vector& in); 21 | 22 | std::vector filterTypeAndDOO(std::vector& in); 23 | 24 | std::vector addClassProbailities(std::vector& in); 25 | 26 | std::vector handleFalsePositivesAndNegatives(std::vector& in); 27 | 28 | std::vector addNoise(std::vector& in); 29 | 30 | std::string getName(); 31 | 32 | std::string getFrameId(); 33 | 34 | bool RunTick(LandmarkList& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time); 35 | 36 | private: 37 | std::string name; 38 | std::string frame_id; 39 | double minRange; 40 | double maxRange; 41 | double minAngleHorizontal; 42 | double maxAngleHorizontal; 43 | double minAngleVertical; 44 | double maxAngleVertical; 45 | Eigen::Vector3d errorSigmaXYZ; 46 | Eigen::Vector3d errorMeanXYZ; 47 | 48 | double errorSigmaRange; 49 | double errorMeanRange; 50 | double errorSigmaRangeRelative; 51 | double errorMeanRangeRelative; 52 | Eigen::Vector2d errorSigmaAngle; 53 | Eigen::Vector2d errorMeanAngle; 54 | 55 | double detection_prob_min_dist; 56 | double detection_prob_decrease_dist_linear; 57 | double detection_prob_decrease_dist_quadratic; 58 | double min_detection_prob; 59 | 60 | double classification_max_distance; 61 | double classification_prob_min_dist; 62 | double classification_prob_decrease_dist_linear; 63 | double classification_prob_decrease_dist_quadratic; 64 | double min_classification_prob; 65 | }; 66 | 67 | #endif /* PACSIMPERCEPTIONSENSOR_HPP */ -------------------------------------------------------------------------------- /include/sensorModels/scalarValueSensor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SCALARVALUESENSOR_HPP 2 | #define SCALARVALUESENSOR_HPP 3 | 4 | #include "configParser.hpp" 5 | #include "sensorBase.hpp" 6 | #include "types.hpp" 7 | #include 8 | #include 9 | 10 | class ScalarValueSensor : public SensorBase 11 | { 12 | public: 13 | ScalarValueSensor(double rate, double deadTime); 14 | 15 | void readConfig(ConfigElement& config); 16 | 17 | bool RunTick(StampedScalar& in, double time); 18 | 19 | StampedScalar applyError(StampedScalar input); 20 | 21 | private: 22 | double error_mean; 23 | double error_sigma; 24 | }; 25 | 26 | #endif /* SCALARVALUESENSOR_HPP */ -------------------------------------------------------------------------------- /include/sensorModels/sensorBase.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMISENSORBASE_HPP 2 | #define PACSIMISENSORBASE_HPP 3 | 4 | #include 5 | 6 | template class SensorBase 7 | { 8 | public: 9 | double getRate() { return this->rate; } 10 | 11 | T getOldest() 12 | { 13 | T elem = this->deadTimeQueue.front(); 14 | this->deadTimeQueue.pop(); 15 | return elem; 16 | } 17 | 18 | bool availableDeadTime(double time) 19 | { 20 | if (this->deadTimeQueue.size() >= 1) 21 | { 22 | T elem = this->deadTimeQueue.front(); 23 | if (time >= (elem.timestamp + this->deadTime)) 24 | { 25 | return true; 26 | } 27 | } 28 | return false; 29 | } 30 | 31 | bool sampleReady(double time) { return (time >= (this->lastSampleTime + 1 / this->rate)); } 32 | 33 | void registerSampling() 34 | { 35 | this->lastSampleTime += 1.0 / this->rate; 36 | return; 37 | } 38 | 39 | Eigen::Vector3d getPosition() { return this->position; } 40 | 41 | Eigen::Vector3d getOrientation() { return this->orientation; } 42 | 43 | protected: 44 | Eigen::Vector3d position; 45 | Eigen::Vector3d orientation; 46 | 47 | double rate; 48 | double lastSampleTime; 49 | double deadTime; 50 | std::queue deadTimeQueue; 51 | int numFrames; 52 | }; 53 | 54 | #endif /* PACSIMISENSORBASE_HPP */ -------------------------------------------------------------------------------- /include/sensorModels/wheelsSensor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMWHEELSSENSOR_HPP 2 | #define PACSIMWHEELSSENSOR_HPP 3 | 4 | #include "configParser.hpp" 5 | #include "sensorBase.hpp" 6 | #include "types.hpp" 7 | #include 8 | #include 9 | 10 | class WheelsSensor : public SensorBase 11 | { 12 | public: 13 | WheelsSensor(double rate, double deadTime); 14 | 15 | void readConfig(ConfigElement& config); 16 | 17 | bool RunTick(Wheels& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time); 18 | 19 | Wheels applyError(Wheels input); 20 | 21 | private: 22 | double error_mean; 23 | double error_sigma; 24 | }; 25 | 26 | #endif /* PACSIMWHEELSSENSOR_HPP */ -------------------------------------------------------------------------------- /include/track/gripMap.hpp: -------------------------------------------------------------------------------- 1 | #include "configParser.hpp" 2 | #include "logger.hpp" 3 | #include "transform.hpp" 4 | #include "types.hpp" 5 | #include 6 | 7 | class gripMap 8 | { 9 | public: 10 | gripMap(std::shared_ptr logger); 11 | void loadConfig(std::string path); 12 | Wheels getGripValues(std::array in); 13 | void transformPoints(Eigen::Vector3d trans, Eigen::Vector3d rot); 14 | 15 | private: 16 | std::shared_ptr logger; 17 | double total_scale = 1.0; 18 | std::vector> mapPoints; 19 | }; 20 | -------------------------------------------------------------------------------- /include/track/trackLoader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TRACKLOADER_HPP 2 | #define TRACKLOADER_HPP 3 | 4 | #include "types.hpp" 5 | #include 6 | #include 7 | 8 | Track loadMap(std::string mapPath, Eigen::Vector3d& start_position, Eigen::Vector3d& start_orientation); 9 | 10 | #endif /* TRACKLOADER_HPP */ -------------------------------------------------------------------------------- /include/transform.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMTRANSFORM_HPP 2 | #define PACSIMTRANSFORM_HPP 3 | 4 | #include "types.hpp" 5 | #include 6 | #include 7 | 8 | Eigen::Vector3d rigidBodyVelocity(Eigen::Vector3d v, Eigen::Vector3d r, Eigen::Vector3d omega); 9 | 10 | Eigen::Vector3d rigidBodyAcceleration( 11 | Eigen::Vector3d a, Eigen::Vector3d r, Eigen::Vector3d omega, Eigen::Vector3d alpha); 12 | 13 | Eigen::Matrix3d eulerAnglesToRotMat(Eigen::Vector3d& angles); 14 | 15 | Eigen::Vector3d inverseTranslation(Eigen::Vector3d trans, Eigen::Vector3d rot); 16 | 17 | LandmarkList transformLmList(LandmarkList& in, Eigen::Vector3d trans, Eigen::Vector3d rot); 18 | 19 | Track transformTrack(Track& in, Eigen::Vector3d trans, Eigen::Vector3d rot); 20 | 21 | #endif /* PACSIMTRANSFORM_HPP */ -------------------------------------------------------------------------------- /include/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PACSIMTYPES_HPP 2 | #define PACSIMTYPES_HPP 3 | 4 | #include "quaternion.hpp" 5 | #include 6 | #include 7 | 8 | enum LandmarkType 9 | { 10 | BLUE = 0, 11 | YELLOW = 1, 12 | ORANGE = 2, 13 | BIG_ORANGE = 3, 14 | TIMEKEEPING = 4, 15 | INVISIBLE = 5, 16 | // convention: unknown is last element, that way we know the number of classes 17 | UNKNOWN = 6 18 | }; 19 | 20 | LandmarkType stringToLandmarkType(const std::string& in); 21 | 22 | // data structure to store everything for each Landmark (without color probabilities) 23 | struct Landmark 24 | { 25 | int id; 26 | Eigen::Vector3d position; 27 | Eigen::Matrix3d cov; 28 | bool beenHit = false; 29 | LandmarkType type = LandmarkType::UNKNOWN; 30 | double typeWeights[LandmarkType::UNKNOWN + 1] = { 0.0 }; 31 | double detection_probability = 1.0; 32 | }; 33 | 34 | struct Map 35 | { 36 | std::vector landmarks; 37 | }; 38 | 39 | struct Track 40 | { 41 | bool lanesFirstWithLastConnected = true; 42 | std::vector left_lane; 43 | std::vector right_lane; 44 | std::vector unknown; 45 | std::vector> time_keeping_gates; 46 | Eigen::Vector3d gnssOrigin; 47 | Eigen::Vector3d enuToTrackRotation; 48 | }; 49 | struct LandmarkList 50 | { 51 | std::vector list; 52 | std::vector fov_polygon; 53 | std::string source_type; 54 | 55 | std::string frame_id; 56 | double timestamp; 57 | }; 58 | 59 | // TODO get rid of that 60 | LandmarkList trackToLMList(Track& in); 61 | 62 | Track lmListToTrack(LandmarkList& in); 63 | 64 | // data structure to store everything for the Pose 65 | struct Pose 66 | { 67 | Eigen::Vector3d mean; 68 | Eigen::Matrix3d cov; 69 | double timestamp; 70 | 71 | std::string frame_id; 72 | }; 73 | struct Wheels 74 | { 75 | double FL; 76 | double FR; 77 | double RL; 78 | double RR; 79 | 80 | double timestamp; 81 | }; 82 | 83 | struct ImuData 84 | { 85 | Eigen::Vector3d acc; 86 | Eigen::Vector3d rot; 87 | 88 | Eigen::Matrix3d acc_cov; 89 | Eigen::Matrix3d rot_cov; 90 | 91 | double timestamp; 92 | std::string frame; 93 | }; 94 | 95 | struct GnssData 96 | { 97 | double latitude; 98 | double longitude; 99 | double altitude; 100 | Eigen::Matrix3d position_covariance; 101 | 102 | double vel_east; 103 | double vel_north; 104 | double vel_up; 105 | Eigen::Matrix3d velocity_covariance; 106 | 107 | quaternion orientation; 108 | Eigen::Matrix3d orientation_covariance; 109 | 110 | enum FixStatus 111 | { 112 | NO_FIX = -1, 113 | FIX = 0 114 | }; 115 | 116 | FixStatus fix_status; 117 | 118 | double timestamp; 119 | std::string frame; 120 | }; 121 | 122 | struct StampedScalar 123 | { 124 | double data; 125 | 126 | double timestamp; 127 | }; 128 | 129 | struct Report 130 | { 131 | std::string creation_time = ""; 132 | std::string track_name = ""; 133 | std::string perception_config_file = ""; 134 | std::string sensors_config_file = ""; 135 | std::string vehicle_config_file = ""; 136 | int seed = 0; 137 | 138 | std::string discipline = ""; 139 | bool success; 140 | std::string dnf_reason = ""; 141 | double final_time = 0.0; 142 | double final_time_raw = 0.0; 143 | double total_sim_time = 0.0; 144 | 145 | bool off_course_detect = true; 146 | bool cone_hit_detect = true; 147 | bool uss_detect = true; 148 | bool dnf_detect = true; 149 | bool finish_validate = true; 150 | double timeout_total = 300; 151 | double timeout_first_lap = 100; 152 | struct LapTime 153 | { 154 | double time; 155 | std::vector sector_times; 156 | }; 157 | std::vector lap_times; 158 | // std::vector> sector_times; 159 | 160 | struct Penalty 161 | { 162 | int lap; 163 | double penalty_time; 164 | double occurence_time; 165 | std::string reason; 166 | Eigen::Vector3d position; 167 | }; 168 | std::vector penalties; 169 | }; 170 | 171 | enum Discipline 172 | { 173 | AUTOCROSS, 174 | TRACKDRIVE, 175 | ACCELERATION, 176 | SKIDPAD 177 | }; 178 | 179 | struct MainConfig 180 | { 181 | double timeout_start; 182 | double timeout_acceleration; 183 | double timeout_autocross; 184 | double timeout_skidpad; 185 | double timeout_trackdrive_first; 186 | double timeout_trackdrive_total; 187 | bool oc_detect; 188 | bool doo_detect; 189 | bool uss_detect; 190 | bool finish_validate; 191 | Discipline discipline; 192 | std::string cog_frame_id_pipeline; 193 | bool broadcast_sensors_tf2; 194 | bool pre_transform_track; 195 | }; 196 | 197 | Discipline stringToDiscipline(const std::string& disciplineStr); 198 | 199 | #endif /* PACSIMTYPES_HPP */ -------------------------------------------------------------------------------- /launch/example.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.substitutions import Command 7 | from launch.event_handlers import OnProcessExit 8 | from launch.actions import RegisterEventHandler, LogInfo, EmitEvent 9 | from launch.events import Shutdown 10 | 11 | def getFullFilePath(name, dir): 12 | ret = os.path.join( 13 | get_package_share_directory('pacsim'), 14 | dir, 15 | name 16 | ) 17 | return ret 18 | 19 | 20 | def generate_launch_description(): 21 | track_name = "FSE23.yaml" 22 | track_frame = "map" 23 | realtime_ratio = 1.0 24 | discipline = "autocross" 25 | 26 | xacro_file_name = 'separate_model.xacro' 27 | xacro_path = getFullFilePath(xacro_file_name, "urdf") 28 | 29 | 30 | nodePacsim = Node( 31 | package='pacsim', 32 | namespace='pacsim', 33 | executable='pacsim_node', 34 | name='pacsim_node', 35 | parameters = [{'use_sim_time':True}, {"track_name" : getFullFilePath(track_name, "tracks")}, {"grip_map_path" : getFullFilePath("gripMap.yaml", "tracks")}, {"track_frame" : track_frame}, {"realtime_ratio" : realtime_ratio}, 36 | {"report_file_dir" : "/tmp"}, {"main_config_path" : getFullFilePath("mainConfig.yaml", dir="config")}, {"perception_config_path" : getFullFilePath("perception.yaml", dir="config")}, 37 | {"sensors_config_path" : getFullFilePath("sensors.yaml", dir="config")}, {"vehicle_model_config_path" : getFullFilePath("vehicleModel.yaml", dir="config")}, {"discipline" : discipline}], 38 | output="screen", 39 | emulate_tty=True) 40 | nodePacsimShutdownEventHandler = RegisterEventHandler( 41 | OnProcessExit( 42 | target_action=nodePacsim, 43 | on_exit=[ 44 | LogInfo(msg=('Pacsim closed')), 45 | EmitEvent(event=Shutdown( 46 | reason='Pacsim closed, shutdown whole launchfile')), 47 | ] 48 | ) 49 | ) 50 | robot_state_publisher= Node( 51 | package='robot_state_publisher', 52 | executable='robot_state_publisher', 53 | name='robot_state_publisher', 54 | output='screen', 55 | parameters=[{'use_sim_time':True}, {'publish_frequency':float(1000), 56 | 'robot_description': Command(['xacro',' ',xacro_path]) 57 | }], 58 | arguments=[xacro_path]) 59 | return LaunchDescription([nodePacsim, nodePacsimShutdownEventHandler, robot_state_publisher]) -------------------------------------------------------------------------------- /msg/GNSS.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # for unknown entries, set -1 on main diagonal 4 | 5 | int8 STATUS_NO_FIX = -1 6 | int8 STATUS_FIX = 0 7 | int8 status 8 | 9 | # Latitude [degrees] 10 | float64 latitude 11 | # Longitude [degrees] 12 | float64 longitude 13 | # Altitude [m]. Positive is above the WGS 84 ellipsoid 14 | float64 altitude 15 | # covariance in ENU coordinates 16 | float64[9] position_covariance 17 | 18 | # relative to ENU plane 19 | geometry_msgs/Quaternion orientation 20 | float64[9] orientation_covariance 21 | 22 | float64 velocity_east 23 | float64 velocity_north 24 | float64 velocity_up 25 | float64[9] velocity_covariance -------------------------------------------------------------------------------- /msg/Landmark.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/PoseWithCovariance pose 3 | 4 | uint8 CLASS_UNKNOWN = 0 5 | uint8 CLASS_INVISIBLE = 1 6 | uint8 CLASS_BLUE = 2 7 | uint8 CLASS_YELLOW = 3 8 | uint8 CLASS_ORANGE = 4 9 | uint8 CLASS_BIGORANGE = 5 10 | uint8 CLASS_TIMEKEEPING = 6 11 | 12 | float64[7] class_probabilities # highest probability -> "detection class" 13 | float64 detection_probability # 1.0 -> for sure true positive : maybe false positive 14 | int32 id # used for ground truth data associations -------------------------------------------------------------------------------- /msg/PerceptionDetections.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | pacsim/Landmark[] detections 3 | -------------------------------------------------------------------------------- /msg/StampedScalar.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | float64 value -------------------------------------------------------------------------------- /msg/Track.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | bool lanes_first_with_last_connected 4 | 5 | pacsim/Landmark[] left_lane 6 | pacsim/Landmark[] right_lane 7 | pacsim/Landmark[] unknown 8 | 9 | pacsim/Landmark[] time_keeping_gates -------------------------------------------------------------------------------- /msg/Wheels.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | float64 fl 3 | float64 fr 4 | float64 rl 5 | float64 rr -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pacsim 4 | 0.0.0 5 | The pacsim package 6 | 7 | 8 | 9 | 10 | Alexander Phieler 11 | MIT 12 | ament_cmake 13 | rosidl_default_generators 14 | ament_cmake_auto 15 | message_generation 16 | message_runtime 17 | rclcpp 18 | rcutils 19 | rcutrclcpp_componentsils 20 | std_msgs 21 | std_srvs 22 | tf2 23 | tf2_ros 24 | geometry_msgs 25 | visualization_msgs 26 | rosidl_default_runtime 27 | rclpy 28 | 29 | rosidl_interface_packages 30 | 31 | 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /scripts/convert_fssim_sdf_to_yaml.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as Et 2 | import sys 3 | from pathlib import Path 4 | from ruamel.yaml import YAML 5 | # from collections import OrderedDict 6 | from ruamel.yaml.comments import CommentedMap as OrderedDict 7 | 8 | class My_Yaml_Dump: 9 | def __init__(self, f) -> None: 10 | self.f = f 11 | 12 | 13 | def write(self, s): 14 | self.f.write(self.__clean_yaml(s.decode('utf-8'))) 15 | 16 | 17 | def __clean_yaml(self, yaml_file: str) -> str: 18 | yaml_file = yaml_file.replace("'", "") 19 | return yaml_file 20 | 21 | 22 | def get_child_with_tag(root: Et.Element, tag: str) -> Et.Element: 23 | for child in root: 24 | if child.tag == tag: 25 | return child 26 | return None 27 | 28 | 29 | def get_children_with_tag(root: Et.Element, tag: str) -> list: 30 | result = [] 31 | for child in root: 32 | if child.tag == tag: 33 | result.append(child) 34 | 35 | return result 36 | 37 | 38 | def convert_to_object_class(uri: str) -> str: 39 | if uri == "cone_blue": 40 | return "blue" 41 | if uri == "cone_yellow": 42 | return "yellow" 43 | if uri == "time_keeping": 44 | return "timekeeping" 45 | if uri == "cone_orange": 46 | return "small-orange" 47 | if uri == "cone_orange_big": 48 | return "big-orange" 49 | return "unknown" 50 | 51 | 52 | def extract_cones(elements: list) -> dict: 53 | left = [] 54 | right = [] 55 | time_keeping = [] 56 | unknown = [] 57 | 58 | for element in elements: 59 | pose = get_child_with_tag(element, "pose") 60 | url = get_child_with_tag(element, "uri") 61 | name = get_child_with_tag(element, "name") 62 | 63 | x = float(pose.text.rsplit(" ")[0]) 64 | y = float(pose.text.rsplit(" ")[1]) 65 | z = float(pose.text.rsplit(" ")[2]) 66 | 67 | object_class = convert_to_object_class(str(url.text.rsplit("/")[-1])) 68 | 69 | if "cone_right" in name.text: 70 | right.append((x, y, z, object_class)) 71 | elif "cone_left" in name.text: 72 | left.append((x, y, z, object_class)) 73 | elif "tk_device" in name.text: 74 | time_keeping.append((x, y, z, object_class)) 75 | else: 76 | unknown.append((x, y, z, object_class)) 77 | 78 | return {"left": left, "right": right, "time_keeping": time_keeping, "unknown": unknown} 79 | 80 | 81 | def convert_cones_to_yaml(cones: list) -> list: 82 | result = [] 83 | for cone in cones: 84 | result.append({"position": f"[{cone[0]}, {cone[1]}, {cone[2]}]", "class": f"{cone[3]}"}) 85 | 86 | return result 87 | 88 | 89 | def write_to_yaml(cones: dict, file_path: str) -> None: 90 | left = convert_cones_to_yaml(cones["left"]) 91 | right = convert_cones_to_yaml(cones["right"]) 92 | time_keeping = convert_cones_to_yaml(cones["time_keeping"]) 93 | unknown = convert_cones_to_yaml(cones["unknown"]) 94 | start_position = (0.0, 0.0, 0.0) 95 | start_orientation = (0.0, 0.0, 0.0) 96 | 97 | yaml_file = OrderedDict({ 98 | "track": OrderedDict({ 99 | "version": 1.0, 100 | "lanesFirstWithLastConnected": True, 101 | "start": OrderedDict({ 102 | "position": f'[{start_position[0]}, {start_position[1]}, {start_position[2]}]', 103 | "orientation": f'[{start_orientation[0]}, {start_orientation[1]}, {start_orientation[2]}]'}), 104 | "earthToTrack": OrderedDict({ 105 | "position": f'[{0.0}, {0.0}, {0.0}]', 106 | "orientation": f'[{0.0}, {0.0}, {0.0}]'}), 107 | "left": left, 108 | "right": right, 109 | "time_keeping": time_keeping, 110 | "unknown": unknown 111 | }) 112 | }) 113 | 114 | with open(Path(file_path).with_suffix('.yaml'), "w+") as f: 115 | f.write("# Map file for PacSim\n") 116 | yaml_dumper = My_Yaml_Dump(f) 117 | yaml = YAML() 118 | yaml.dump(yaml_file, yaml_dumper) 119 | 120 | 121 | def main(file_path: str) -> None: 122 | tree = Et.parse(file_path) 123 | root = tree.getroot() 124 | model = get_child_with_tag(root, "model") 125 | objects = get_children_with_tag(model, "include") 126 | cones = extract_cones(objects) 127 | write_to_yaml(cones, file_path) 128 | 129 | 130 | if __name__ == '__main__': 131 | if len(sys.argv) != 2: 132 | print("Call with python convert_sdf_to_yaml.py ") 133 | else: 134 | main(sys.argv[1]) 135 | -------------------------------------------------------------------------------- /scripts/report_evaluation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import yaml 3 | import sys 4 | import os 5 | 6 | 7 | def main(path): 8 | with open(path) as f: 9 | report = yaml.full_load(f) 10 | success = evaluate(report) 11 | output(report, success, os.path.splitext(path)[0]) 12 | return success 13 | 14 | 15 | def evaluate(report: dict): 16 | if "report" not in report.keys(): 17 | raise RuntimeError( 18 | "The specified report does not follow the standard formatting") 19 | report_contents = report["report"] 20 | if "status" not in report_contents.keys(): 21 | raise RuntimeError( 22 | "The specified report does not have a status associated") 23 | status = report_contents["status"] 24 | if "success" not in status: 25 | raise RuntimeError( 26 | "The specified report does not provide success information") 27 | return bool(status["success"]) 28 | 29 | 30 | def output(report, success, file_path): 31 | report = report["report"] 32 | f = open(file_path + ".txt", "w+") 33 | 34 | if "discipline" in report["status"]: 35 | f.write(f"Discipline: {report['status']['discipline']}\n") 36 | if "track_file" in report: 37 | f.write(f"Track: {report['track_file']}\n") 38 | f.write(f"Success: {':white_check_mark:' if success else ':x:'}\n") 39 | if "final_time_raw" in report['status']: 40 | f.write(f"Driven Time: {report['status']['final_time_raw']}\n") 41 | 42 | penalty_time = 0 43 | for penalty in report["penalties"]: 44 | if penalty["penalty"]["time"]: 45 | penalty_time += penalty["penalty"]["time"] 46 | 47 | f.write(f"Penalty time: {penalty_time}") 48 | 49 | 50 | if __name__ == "__main__": 51 | try: 52 | path = str(sys.argv[1]) 53 | main(path) 54 | except IndexError: 55 | raise IndexError( 56 | "Path to the report file not specified, expected call: report_evaluation.py {path}") 57 | -------------------------------------------------------------------------------- /src/VehicleModel/VehicleModelBicycle.cpp: -------------------------------------------------------------------------------- 1 | #include "VehicleModel/VehicleModelInterface.hpp" 2 | 3 | #include "transform.hpp" 4 | class VehicleModelBicycle : public IVehicleModel 5 | { 6 | 7 | public: 8 | VehicleModelBicycle() 9 | { 10 | this->position = Eigen::Vector3d(0.0, 0.0, 0.0); 11 | this->orientation = Eigen::Vector3d(0.0, 0.0, 0.0); 12 | this->velocity = Eigen::Vector3d(0.0, 0.0, 0.0); 13 | this->angularVelocity = Eigen::Vector3d(0.0, 0.0, 0.0); 14 | this->acceleration = Eigen::Vector3d(0.0, 0.0, 0.0); 15 | 16 | this->torques = { 0.0, 0.0, 0.0, 0.0 }; 17 | this->steeringAngles = { 0.0, 0.0, 0.0, 0.0 }; 18 | this->wheelOrientations = { 0.0, 0.0, 0.0, 0.0 }; 19 | this->wheelspeeds = { 0.0, 0.0, 0.0, 0.0 }; 20 | } 21 | 22 | bool readConfig(ConfigElement& config) 23 | { 24 | auto configModel = config["simple_bicycle_model"]; 25 | configModel["kinematics"].getElement(&this->lf, "lf"); 26 | configModel["kinematics"].getElement(&this->lr, "lr"); 27 | configModel["kinematics"].getElement(&this->sf, "sf"); 28 | configModel["kinematics"].getElement(&this->sr, "sr"); 29 | 30 | configModel["tire"].getElement(&this->Blat, "Blat"); 31 | configModel["tire"].getElement(&this->Clat, "Clat"); 32 | configModel["tire"].getElement(&this->Dlat, "Dlat"); 33 | configModel["tire"].getElement(&this->Elat, "Elat"); 34 | 35 | configModel["aero"].getElement(&this->cla, "cla"); 36 | configModel["aero"].getElement(&this->cda, "cda"); 37 | configModel["aero"].getElement(&this->aeroArea, "aeroArea"); 38 | 39 | configModel.getElement(&this->m, "m"); 40 | configModel.getElement(&this->Izz, "Izz"); 41 | configModel.getElement(&this->wheelRadius, "wheelRadius"); 42 | configModel.getElement(&this->gearRatio, "gearRatio"); 43 | configModel.getElement(&this->innerSteeringRatio, "innerSteeringRatio"); 44 | configModel.getElement(&this->outerSteeringRatio, "outerSteeringRatio"); 45 | configModel.getElement(&this->nominalVoltageTS, "nominalVoltageTS"); 46 | configModel.getElement(&this->powerGroundForce, "powerGroundForce"); 47 | configModel.getElement(&this->powertrainEfficiency, "powertrainEfficiency"); 48 | 49 | return true; 50 | } 51 | 52 | Eigen::Vector3d getPosition() 53 | { 54 | Eigen::Vector3d ret = this->position; 55 | return ret; 56 | } 57 | 58 | Eigen::Vector3d getOrientation() { return this->orientation; } 59 | 60 | Eigen::Vector3d getVelocity() { return this->velocity; } 61 | 62 | Eigen::Vector3d getAcceleration() { return this->acceleration; } 63 | 64 | Eigen::Vector3d getAngularVelocity() { return this->angularVelocity; } 65 | 66 | Eigen::Vector3d getAngularAcceleration() { return this->angularAcceleration; } 67 | 68 | Wheels getSteeringAngles() { return this->steeringAngles; } 69 | 70 | double getSteeringWheelAngle() 71 | { 72 | return (this->steeringAngles.FL > 0) ? this->steeringAngles.FL / this->innerSteeringRatio 73 | : this->steeringAngles.FL / this->outerSteeringRatio; 74 | } 75 | 76 | double getVoltageTS() { return this->nominalVoltageTS; } 77 | 78 | double getCurrentTS() 79 | { 80 | double powerCoeff = 1.0 / 9.55; 81 | double powerFL = this->torques.FL * this->wheelspeeds.FL * powerCoeff; 82 | double powerFR = this->torques.FR * this->wheelspeeds.FR * powerCoeff; 83 | double powerRL = this->torques.RL * this->wheelspeeds.RL * powerCoeff; 84 | double powerRR = this->torques.RR * this->wheelspeeds.RR * powerCoeff; 85 | double totalPower = (powerFL + powerFR + powerRL + powerRR) / powertrainEfficiency; 86 | 87 | return (totalPower / this->nominalVoltageTS); 88 | } 89 | 90 | Wheels getWheelspeeds() { return this->wheelspeeds; } 91 | 92 | Wheels getWheelOrientations() { return this->wheelOrientations; } 93 | 94 | Wheels getTorques() { return this->torques; } 95 | 96 | std::array getWheelPositions() 97 | { 98 | auto rotMat = eulerAnglesToRotMat(this->orientation).transpose(); 99 | Eigen::Vector3d FL = rotMat * Eigen::Vector3d(this->lf, this->sf * 0.5, 0.0) + this->position; 100 | Eigen::Vector3d FR = rotMat * Eigen::Vector3d(this->lf, -this->sf * 0.5, 0.0) + this->position; 101 | Eigen::Vector3d RL = rotMat * Eigen::Vector3d(-this->lr, this->sr * 0.5, 0.0) + this->position; 102 | Eigen::Vector3d RR = rotMat * Eigen::Vector3d(-this->lr, -this->sr * 0.5, 0.0) + this->position; 103 | std::array ret { FL, FR, RL, RR }; 104 | return ret; 105 | } 106 | 107 | void setTorques(Wheels in) { this->torques = in; } 108 | 109 | void setRpmSetpoints(Wheels in) { this->rpmSetpoints = in; } 110 | 111 | void setMaxTorques(Wheels in) { this->maxTorques = in; } 112 | 113 | void setMinTorques(Wheels in) { this->minTorques = in; } 114 | 115 | void setSteeringSetpointFront(double in) { setSteeringFront(in); } 116 | 117 | void setSteeringSetpointRear(double in) { return; } 118 | 119 | void setPowerGroundSetpoint(double in) { this->powerGroundSetpoint = std::min(std::max(in, 0.0), 1.0); } 120 | 121 | void setSteeringFront(double in) 122 | { 123 | double avgRatio = 0.5 * (this->innerSteeringRatio + this->outerSteeringRatio); 124 | if (in > 0) 125 | { 126 | this->steeringAngles.FL = this->innerSteeringRatio * in; 127 | this->steeringAngles.FR = this->outerSteeringRatio * in; 128 | } 129 | else 130 | { 131 | this->steeringAngles.FL = this->outerSteeringRatio * in; 132 | this->steeringAngles.FR = this->innerSteeringRatio * in; 133 | } 134 | return; 135 | } 136 | 137 | void setPosition(Eigen::Vector3d position) { this->position = position; } 138 | void setOrientation(Eigen::Vector3d orientation) { this->orientation = orientation; } 139 | 140 | double processSlipAngleLat(double alpha) 141 | { 142 | return std::sin(Clat * std::atan(Blat * alpha - Elat * (Blat * alpha - std::atan(Blat * alpha)))); 143 | } 144 | 145 | // ax, ay, rdot 146 | Eigen::Vector3d getDynamicStates(double dt, Wheels frictionCoefficients) 147 | { 148 | double l = this->lr + this->lf; 149 | double vx = this->velocity.x(); 150 | double vy = this->velocity.y(); 151 | double ax = this->acceleration.x(); 152 | double ay = this->acceleration.y(); 153 | double r = this->angularVelocity.z(); 154 | // Downforce 155 | double F_aero_downforce 156 | = 0.5 * 1.29 * this->aeroArea * this->cla * (vx * vx) + this->powerGroundSetpoint * this->powerGroundForce; 157 | double F_aero_drag = 0.5 * 1.29 * this->aeroArea * this->cda * (vx * vx); 158 | double g = 9.81; 159 | double steeringFront = 0.5 * (this->steeringAngles.FL + this->steeringAngles.FR); 160 | 161 | // max because lifted tire makes no forces 162 | double Fz_Front = std::max(0.0, ((m * g + F_aero_downforce) * 0.5 * this->lr / l)); 163 | double Fz_Rear = std::max(0.0, ((m * g + F_aero_downforce) * 0.5 * this->lf / l)); 164 | 165 | Eigen::Vector3d vCog = this->velocity; 166 | Eigen::Vector3d omega = this->angularVelocity; 167 | 168 | Eigen::Vector3d rFL = Eigen::Vector3d(lf, 0.5 * sf, 0.0); 169 | Eigen::Vector3d rFR = Eigen::Vector3d(lf, -0.5 * sf, 0.0); 170 | Eigen::Vector3d rRL = Eigen::Vector3d(-lr, 0.5 * sr, 0.0); 171 | Eigen::Vector3d rRR = Eigen::Vector3d(-lr, -0.5 * sr, 0.0); 172 | Eigen::Vector3d rFront = Eigen::Vector3d(lf, 0.0, 0.0); 173 | Eigen::Vector3d rRear = Eigen::Vector3d(-lf, 0.0, 0.0); 174 | 175 | Eigen::Vector3d vFL = vCog + omega.cross(rFL); 176 | Eigen::Vector3d vFR = vCog + omega.cross(rFR); 177 | Eigen::Vector3d vRL = vCog + omega.cross(rRL); 178 | Eigen::Vector3d vRR = vCog + omega.cross(rRR); 179 | Eigen::Vector3d vFront = vCog + omega.cross(rFront); 180 | Eigen::Vector3d vRear = vCog + omega.cross(rRear); 181 | 182 | double rpm2ms = this->wheelRadius * 2.0 * M_PI / (this->gearRatio * 60.0); 183 | 184 | bool stillstand = (vCog.norm() < 0.1) && (std::abs(this->angularVelocity.z()) < 0.001); 185 | 186 | // tire side slip angles 187 | double eps = 0.00001; 188 | 189 | double kappaFront = std::atan2(vFront.y(), std::max(std::abs(vFront.x()), eps)) - steeringFront; 190 | double kappaRear = std::atan2(vRear.y(), std::max(std::abs(vRear.x()), eps)); 191 | 192 | // don't steer when vehicle doesn't move 193 | if (stillstand) 194 | { 195 | kappaFront = 0.0; 196 | kappaRear = 0.0; 197 | } 198 | 199 | // old calculation as backup for now 200 | double Fx_FL = this->gearRatio * this->torques.FL / this->wheelRadius; 201 | double Fx_FR = this->gearRatio * this->torques.FR / this->wheelRadius; 202 | double Fx_RL = this->gearRatio * this->torques.RL / this->wheelRadius; 203 | double Fx_RR = this->gearRatio * this->torques.RR / this->wheelRadius; 204 | 205 | Fx_FL *= (((this->torques.FL) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0; 206 | Fx_FR *= (((this->torques.FR) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0; 207 | Fx_RL *= (((this->torques.RL) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0; 208 | Fx_RR *= (((this->torques.RR) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0; 209 | 210 | double Dlat_Front = 0.5 * (frictionCoefficients.FL + frictionCoefficients.FR) * this->Dlat * Fz_Front; 211 | double Dlat_Rear = 0.5 * (frictionCoefficients.RL + frictionCoefficients.RR) * this->Dlat * Fz_Rear; 212 | 213 | double Fy_Front = Dlat_Front * processSlipAngleLat(kappaFront); 214 | double Fy_Rear = Dlat_Rear * processSlipAngleLat(kappaRear); 215 | 216 | // todo convert speed 217 | this->wheelspeeds.FL = vFL.x() / rpm2ms; 218 | this->wheelspeeds.FR = vFR.x() / rpm2ms; 219 | this->wheelspeeds.RL = vRL.x() / rpm2ms; 220 | this->wheelspeeds.RR = vRR.x() / rpm2ms; 221 | 222 | double axTires = (std::cos(this->steeringAngles.FL) * Fx_FL + std::cos(this->steeringAngles.FR) * Fx_FR + Fx_RL 223 | + Fx_RR - std::sin(steeringFront) * Fy_Front) 224 | / m; 225 | double axModel = axTires - F_aero_drag / m; 226 | 227 | double ayTires = (std::sin(this->steeringAngles.FL) * Fx_FL + std::sin(this->steeringAngles.FR) * Fx_FR 228 | + std::cos(steeringFront) * Fy_Front + Fy_Rear) 229 | / m; 230 | double ayModel = (ayTires); 231 | 232 | double rdotFx 233 | = 0.5 * this->sf * (-Fx_FL * std::cos(this->steeringAngles.FL) + Fx_FR * std::cos(this->steeringAngles.FR)) 234 | + this->lf * (Fx_FL * std::sin(this->steeringAngles.FL) + Fx_FR * std::sin(this->steeringAngles.FR)) 235 | + 0.5 * this->sr * (Fx_RR * std::cos(this->steeringAngles.RR) - Fx_RL * std::cos(this->steeringAngles.RL)) 236 | - this->lr * (Fx_RL * std::sin(this->steeringAngles.RL) + Fx_RR * std::sin(this->steeringAngles.RR)); 237 | double rdotFy = this->lf * (Fy_Front * std::cos(steeringFront)) - this->lr * (Fy_Rear); 238 | double rdot = (1 / Izz * (rdotFx + rdotFy)); 239 | 240 | Eigen::Vector3d ret(axModel, ayModel, rdot); 241 | return ret; 242 | } 243 | 244 | void forwardIntegrate(double dt, Wheels frictionCoefficients) 245 | { 246 | Eigen::Vector3d friction(std::min(200.0, 2000.0 * std::abs(this->velocity.x())), 247 | std::min(200.0, 2000.0 * std::abs(this->velocity.y())), 248 | std::min(200.0, 2000.0 * std::abs(this->velocity.z()))); 249 | friction[0] = (this->velocity.x() > 0) ? friction.x() : -friction.x(); 250 | friction[1] = (this->velocity.y() > 0) ? friction.y() : -friction.y(); 251 | friction[2] = (this->velocity.z() > 0) ? friction.z() : -friction.z(); 252 | 253 | Eigen::AngleAxisd yawAngle(this->orientation.z(), Eigen::Vector3d::UnitZ()); 254 | this->position += (yawAngle.matrix() * this->velocity) * dt; 255 | 256 | this->torques = this->maxTorques; 257 | 258 | Eigen::Vector3d xdotdyn = getDynamicStates(dt, frictionCoefficients); 259 | 260 | this->orientation += Eigen::Vector3d(0.0, 0.0, dt * angularVelocity.z()); 261 | 262 | this->acceleration = Eigen::Vector3d(xdotdyn[0] - friction.x() / m, xdotdyn[1], 0.0); 263 | 264 | this->angularVelocity = (this->angularVelocity + Eigen::Vector3d(0.0, 0.0, xdotdyn[2] * dt)); 265 | 266 | this->angularAcceleration = Eigen::Vector3d(0.0, 0.0, xdotdyn[2]); 267 | 268 | this->velocity += dt * (this->acceleration - this->angularVelocity.cross(this->velocity)); 269 | this->wheelOrientations.FL = std::fmod( 270 | this->wheelOrientations.FL + (this->wheelspeeds.FL / (60.0 * this->gearRatio)) * dt * 2.0 * M_PI, 271 | 2.0 * M_PI); 272 | this->wheelOrientations.FR = std::fmod( 273 | this->wheelOrientations.FR + (this->wheelspeeds.FR / (60.0 * this->gearRatio)) * dt * 2.0 * M_PI, 274 | 2.0 * M_PI); 275 | this->wheelOrientations.RL = std::fmod( 276 | this->wheelOrientations.RL + (this->wheelspeeds.RL / (60.0 * this->gearRatio)) * dt * 2.0 * M_PI, 277 | 2.0 * M_PI); 278 | this->wheelOrientations.RR = std::fmod( 279 | this->wheelOrientations.RR + (this->wheelspeeds.RR / (60.0 * this->gearRatio)) * dt * 2.0 * M_PI, 280 | 2.0 * M_PI); 281 | } 282 | 283 | private: 284 | double lr = 0.72; 285 | double lf = 0.78; 286 | double sf = 1.15; // track width front 287 | double sr = 1.15; // track width rear 288 | 289 | double Blat = 9.63; 290 | double Clat = -1.39; 291 | double Dlat = 1.6; 292 | double Elat = 1.0; 293 | 294 | double cla = 3.7; 295 | double cda = 1.1; 296 | double aeroArea = 1.1; 297 | double m = 178.0; 298 | double Izz = 111.0; 299 | double wheelRadius = 0.206; 300 | double gearRatio = 12.23; 301 | double innerSteeringRatio = 0.255625; 302 | double outerSteeringRatio = 0.20375; 303 | double nominalVoltageTS = 550.0; 304 | double powerGroundSetpoint = 0.0; 305 | double powerGroundForce = 700.0; 306 | double powertrainEfficiency = 1.0; 307 | 308 | Wheels minTorques = { -0.0, -0.0, -0.0, -0.0 }; 309 | Wheels maxTorques = { 0.0, 0.0, 0.0, 0.0 }; 310 | Wheels rpmSetpoints = { 0.0, 0.0, 0.0, 0.0 }; 311 | Wheels currentFx = { 0.0, 0.0, 0.0, 0.0 }; 312 | Wheels currentFy = { 0.0, 0.0, 0.0, 0.0 }; 313 | }; 314 | -------------------------------------------------------------------------------- /src/configParser.cpp: -------------------------------------------------------------------------------- 1 | #include "configParser.hpp" 2 | 3 | NodeType::value ConfigElement::getType() { return this->type; } 4 | 5 | Node ConfigElement::getNode() { return this->node; } 6 | 7 | vector ConfigElement::getElements() 8 | { 9 | if (this->type != NodeType::Sequence) 10 | { 11 | throw runtime_error("Not a sequence but a " + to_string(this->type)); 12 | } 13 | 14 | vector res; 15 | for (const_iterator it = this->node.begin(); it != this->node.end(); ++it) 16 | { 17 | Node child = *it; 18 | res.push_back(ConfigElement(child)); 19 | } 20 | return res; 21 | } 22 | 23 | ConfigElement ConfigElement::getElement(string elementName) 24 | { 25 | if (this->type != NodeType::Map) 26 | { 27 | throw runtime_error("Not a map but a " + to_string(this->type)); 28 | } 29 | 30 | if (!this->hasElement(elementName)) 31 | { 32 | throw runtime_error(elementName + " is not a member of this config item"); 33 | } 34 | return this->node[elementName]; 35 | } 36 | 37 | bool ConfigElement::hasElement(string elementName) 38 | { 39 | if (this->type != NodeType::Map && this->type != NodeType::Scalar) 40 | { 41 | return false; 42 | } 43 | if (!this->node[elementName]) 44 | { 45 | return false; 46 | } 47 | return true; 48 | } 49 | 50 | bool ConfigElement::getElement(ConfigElement* element, string elementName) 51 | { 52 | if (this->type != NodeType::Map && this->type != NodeType::Scalar) 53 | { 54 | return false; 55 | } 56 | *element = this->node[elementName]; 57 | return true; 58 | } 59 | 60 | bool ConfigElement::getElements(vector* vec) 61 | { 62 | if (this->type != NodeType::Sequence) 63 | { 64 | return false; 65 | } 66 | for (const_iterator it = this->node.begin(); it != this->node.end(); ++it) 67 | { 68 | Node child = *it; 69 | vec->push_back(ConfigElement(child)); 70 | } 71 | return true; 72 | } 73 | -------------------------------------------------------------------------------- /src/quaternion.cpp: -------------------------------------------------------------------------------- 1 | #include "quaternion.hpp" 2 | 3 | double quatNorm(quaternion a) 4 | { 5 | double ret; 6 | ret = std::sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z); 7 | return ret; 8 | } 9 | 10 | quaternion quatMult(quaternion a, quaternion b) 11 | { 12 | quaternion ret; 13 | ret.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; 14 | ret.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; 15 | ret.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x; 16 | ret.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w; 17 | return ret; 18 | } 19 | 20 | quaternion quatInversion(quaternion a) 21 | { 22 | quaternion ret; 23 | ret.w = a.w; 24 | ret.x = -a.x; 25 | ret.y = -a.y; 26 | ret.z = -a.z; 27 | return ret; 28 | } 29 | 30 | quaternion quatNormalize(quaternion a) 31 | { 32 | double norm = quatNorm(a); 33 | quaternion ret; 34 | ret.w = a.w / norm; 35 | ret.x = a.x / norm; 36 | ret.y = a.y / norm; 37 | ret.z = a.z / norm; 38 | return ret; 39 | } 40 | 41 | quaternion getRelativeQuat(quaternion a, quaternion b) 42 | { 43 | quaternion ret; 44 | ret = quatMult(a, quatInversion(b)); 45 | return ret; 46 | } 47 | 48 | quaternion quatFromEulerAngles(Eigen::Vector3d a) 49 | { 50 | double x = a.x(); 51 | double y = a.y(); 52 | double z = a.z(); 53 | 54 | quaternion quatX; 55 | quatX.w = std::cos(x / 2); 56 | quatX.x = std::sin(x / 2); 57 | quatX.y = 0.0; 58 | quatX.z = 0.0; 59 | 60 | quaternion quatY; 61 | quatY.w = std::cos(y / 2); 62 | quatY.x = 0.0; 63 | quatY.y = std::sin(y / 2); 64 | quatY.z = 0.0; 65 | 66 | quaternion quatZ; 67 | quatX.w = std::cos(z / 2); 68 | quatX.x = 0.0; 69 | quatX.y = 0.0; 70 | quatX.z = std::sin(z / 2); 71 | 72 | quaternion ret; 73 | ret.w = 1.0; 74 | ret.x = 0.0; 75 | ret.y = 0.0; 76 | ret.z = 0.0; 77 | 78 | ret = quatMult(quatX, ret); 79 | ret = quatMult(quatY, ret); 80 | ret = quatMult(quatZ, ret); 81 | return ret; 82 | } 83 | 84 | quaternion quatFromRotationMatrix(Eigen::Matrix3d mat) 85 | { 86 | Eigen::Quaterniond q(mat); 87 | quaternion ret; 88 | ret.w = q.w(); 89 | ret.x = q.x(); 90 | ret.y = q.y(); 91 | ret.z = q.z(); 92 | return ret; 93 | } 94 | 95 | Eigen::Matrix3d rotationMatrixFromQuaternion(quaternion a) 96 | { 97 | Eigen::Quaterniond q; 98 | q.w() = a.w; 99 | q.x() = a.x; 100 | q.y() = a.y; 101 | q.z() = a.z; 102 | Eigen::Matrix3d ret = q.toRotationMatrix(); 103 | return ret; 104 | } 105 | 106 | Eigen::Vector3d eulerAnglesFromQuat(quaternion a) 107 | { 108 | double sinr_cosp = 2 * (a.w * a.x + a.y * a.z); 109 | double cosr_cosp = 1 - 2 * (a.x * a.x + a.y * a.y); 110 | double roll = std::atan2(sinr_cosp, cosr_cosp); 111 | 112 | double sinp = 2 * (a.w * a.y - a.z * a.x); 113 | double pitch = std::asin(sinp); 114 | 115 | double siny_cosp = 2 * (a.w * a.z + a.x * a.y); 116 | double cosy_cosp = 1 - 2 * (a.y * a.y + a.z * a.z); 117 | double yaw = std::atan2(siny_cosp, cosy_cosp); 118 | 119 | Eigen::Vector3d ret(roll, pitch, yaw); 120 | return ret; 121 | } -------------------------------------------------------------------------------- /src/reportWriter.cpp: -------------------------------------------------------------------------------- 1 | #include "reportWriter.hpp" 2 | 3 | bool reportToFile(Report report, std::string dir) 4 | { 5 | YAML::Emitter out; 6 | out << YAML::BeginMap; 7 | out << YAML::Key << "report"; 8 | out << YAML::BeginMap; 9 | out << YAML::Key << "version"; 10 | out << YAML::Value << "0.9"; 11 | out << YAML::Key << "creation_time"; 12 | time_t rawtime; 13 | 14 | struct tm* timeinfo; 15 | char buffer[80]; 16 | 17 | time(&rawtime); 18 | timeinfo = localtime(&rawtime); 19 | 20 | strftime(buffer, 80, "%F %T", timeinfo); 21 | out << YAML::Value << buffer; 22 | out << YAML::Key << "seed"; 23 | out << YAML::Value << report.seed; 24 | out << YAML::Key << "track_file"; 25 | out << YAML::Value << report.track_name; 26 | out << YAML::Key << "perception_config_file"; 27 | out << YAML::Value << report.perception_config_file; 28 | out << YAML::Key << "sensors_config_file"; 29 | out << YAML::Value << report.sensors_config_file; 30 | out << YAML::Key << "vehicle_config_file"; 31 | out << YAML::Value << report.vehicle_config_file; 32 | 33 | out << YAML::Key << "status"; 34 | out << YAML::BeginMap; 35 | out << YAML::Key << "discipline"; 36 | out << YAML::Value << report.discipline; 37 | out << YAML::Key << "success"; 38 | out << YAML::Value << report.success; 39 | out << YAML::Key << "reason"; 40 | out << YAML::Value << report.dnf_reason; 41 | out << YAML::Key << "final_time"; 42 | out << YAML::Value << report.final_time; 43 | out << YAML::Key << "final_time_raw"; 44 | out << YAML::Value << report.final_time_raw; 45 | out << YAML::Key << "total_sim_time"; 46 | out << YAML::Value << report.total_sim_time; 47 | out << YAML::EndMap; 48 | 49 | out << YAML::Key << "settings"; 50 | out << YAML::BeginMap; 51 | out << YAML::Key << "timeout-first-lap"; 52 | out << YAML::Value << report.timeout_first_lap; 53 | out << YAML::Key << "timeout-total"; 54 | out << YAML::Value << report.timeout_total; 55 | out << YAML::Key << "oc_detect"; 56 | out << YAML::Value << report.off_course_detect; 57 | out << YAML::Key << "doo_detect"; 58 | out << YAML::Value << report.cone_hit_detect; 59 | out << YAML::Key << "uss_detect"; 60 | out << YAML::Value << report.uss_detect; 61 | out << YAML::Key << "dnf_detect"; 62 | out << YAML::Value << report.dnf_detect; 63 | out << YAML::Key << "finish_validate"; 64 | out << YAML::Value << report.finish_validate; 65 | out << YAML::EndMap; 66 | 67 | out << YAML::Key << "laps"; 68 | out << YAML::BeginSeq; 69 | for (int i = 0; i < report.lap_times.size(); ++i) 70 | { 71 | out << YAML::BeginMap; 72 | out << YAML::Key << "lap"; 73 | out << YAML::BeginMap; 74 | out << YAML::Key << "time"; 75 | out << YAML::Value << report.lap_times[i].time; 76 | out << YAML::Key << "sectors"; 77 | out << YAML::Flow << YAML::BeginSeq; 78 | for (int j = 0; j < report.lap_times[i].sector_times.size(); ++j) 79 | { 80 | out << report.lap_times[i].sector_times[j]; 81 | } 82 | out << YAML::EndSeq; 83 | out << YAML::EndMap; 84 | out << YAML::EndMap; 85 | } 86 | out << YAML::EndSeq; 87 | 88 | out << YAML::Key << "penalties"; 89 | out << YAML::BeginSeq; 90 | for (int i = 0; i < report.penalties.size(); ++i) 91 | { 92 | out << YAML::BeginMap; 93 | out << YAML::Key << "penalty"; 94 | out << YAML::BeginMap; 95 | out << YAML::Key << "lap"; 96 | out << YAML::Value << report.penalties[i].lap; 97 | out << YAML::Key << "time"; 98 | out << YAML::Value << report.penalties[i].penalty_time; 99 | out << YAML::Key << "occurence_time"; 100 | out << YAML::Value << report.penalties[i].occurence_time; 101 | out << YAML::Key << "reason"; 102 | out << YAML::Value << report.penalties[i].reason; 103 | out << YAML::Key << "car_position"; 104 | out << YAML::Flow << YAML::BeginSeq; 105 | out << report.penalties[i].position.x() << report.penalties[i].position.y() << report.penalties[i].position.z(); 106 | out << YAML::EndSeq; 107 | out << YAML::EndMap; 108 | out << YAML::EndMap; 109 | } 110 | out << YAML::EndSeq; 111 | 112 | out << YAML::EndMap; 113 | out << YAML::EndMap; 114 | strftime(buffer, 80, "%F-%H-%M-%S", timeinfo); 115 | std::string path = dir + "/report-" + std::string(buffer) + ".yaml"; 116 | std::ofstream fout(path); 117 | fout << out.c_str(); 118 | 119 | return true; 120 | } -------------------------------------------------------------------------------- /src/sensorModels/gnssSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorModels/gnssSensor.hpp" 2 | 3 | #include "quaternion.hpp" 4 | 5 | GnssSensor::GnssSensor() 6 | { 7 | this->rate = rate; 8 | this->lastSampleTime = 0.0; 9 | this->deadTime = deadTime; 10 | this->noiseSeed = 0; 11 | } 12 | 13 | void GnssSensor::readConfig(ConfigElement& config) 14 | { 15 | config.getElement(&this->name, "name"); 16 | config.getElement(&this->frame_id, "name"); 17 | config.getElement(&this->rate, "rate"); 18 | config["delay"].getElement(&this->deadTime, "mean"); 19 | std::vector position; 20 | config["pose"].getElement>(&position, "position"); 21 | this->position = Eigen::Vector3d(position[0], position[1], position[2]); 22 | std::vector orientation; 23 | config["pose"].getElement>(&orientation, "orientation"); 24 | this->orientation = Eigen::Vector3d(orientation[0], orientation[1], orientation[2]); 25 | 26 | std::vector sigmaPosition; 27 | config["noise"]["position"].getElement>(&sigmaPosition, "sigma"); 28 | this->errorSigmaPosition = Eigen::Vector3d(sigmaPosition[0], sigmaPosition[1], sigmaPosition[2]); 29 | std::vector meanPosition; 30 | config["noise"]["position"].getElement>(&meanPosition, "mean"); 31 | this->errorMeanPosition = Eigen::Vector3d(meanPosition[0], meanPosition[1], meanPosition[2]); 32 | 33 | std::vector sigmaVelocity; 34 | config["noise"]["velocity"].getElement>(&sigmaVelocity, "sigma"); 35 | this->errorSigmaVelocity = Eigen::Vector3d(sigmaVelocity[0], sigmaVelocity[1], sigmaVelocity[2]); 36 | std::vector meanVelocity; 37 | config["noise"]["velocity"].getElement>(&meanVelocity, "mean"); 38 | this->errorMeanVelocity = Eigen::Vector3d(meanVelocity[0], meanVelocity[1], meanVelocity[2]); 39 | 40 | std::vector sigmaOrientation; 41 | config["noise"]["orientation"].getElement>(&sigmaOrientation, "sigma"); 42 | this->errorSigmaOrientation = Eigen::Vector3d(sigmaOrientation[0], sigmaOrientation[1], sigmaOrientation[2]); 43 | std::vector meanOrientation; 44 | config["noise"]["orientation"].getElement>(&meanOrientation, "mean"); 45 | this->errorMeanOrientation = Eigen::Vector3d(meanOrientation[0], meanOrientation[1], meanOrientation[2]); 46 | 47 | config["features"].getElement(&this->outputVelocity, "velocity"); 48 | config["features"].getElement(&this->orientationMode, "orientation"); 49 | 50 | return; 51 | } 52 | 53 | Eigen::Vector3d wgs84toEcef(double lat, double lon, double alt) 54 | { 55 | // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion 56 | double deg2rad = M_PI / 180.0; 57 | double a = 6378137.0; 58 | double b = 6356752.314; 59 | double h = alt; 60 | double esqr = 1 - std::pow(b / a, 2); 61 | double N = a / (sqrt(1 - esqr * std::pow(std::sin(lat * deg2rad), 2))); 62 | double X = (N + h) * std::cos(lat * deg2rad) * std::cos(lon * deg2rad); 63 | double Y = (N + h) * std::cos(lat * deg2rad) * std::sin(lon * deg2rad); 64 | double Z = std::sin(lat * deg2rad) * ((1 - esqr) * N + h); 65 | return Eigen::Vector3d(X, Y, Z); 66 | } 67 | 68 | double getRn(double latitude) 69 | { 70 | double a = 6378137.0; 71 | double b = 6356752.314; 72 | double esqr = 1 - std::pow(b / a, 2); 73 | 74 | double N = a / (sqrt(1 - esqr * std::pow(std::sin(latitude), 2))); 75 | return N; 76 | } 77 | 78 | Eigen::Vector3d ecefToWgs84(double x, double y, double z) 79 | { 80 | // https://www.oc.nps.edu/oc2902w/coord/coordcvt.pdf 81 | double lon = std::atan2(y, x); 82 | double p = std::hypot(x, y); 83 | double deg2rad = M_PI / 180.0; 84 | double a = 6378137.0; 85 | double b = 6356752.314; 86 | 87 | double esqr = 1 - std::pow(b / a, 2); 88 | // there exists no closed form solution 89 | // iterative method to get height and latitude 90 | double h = 0.0; 91 | double lat = std::atan2(p, z); 92 | // 7 iterarations gave me error which rounds to 0 on doubles 93 | for (int i = 0; i < 7; ++i) 94 | { 95 | double rn = getRn(lat); 96 | h = (p / std::cos(lat)) - rn; 97 | lat = std::atan((z / p) / (1 - esqr * rn / (rn + h))); 98 | } 99 | lon = lon / deg2rad; 100 | lat = lat / deg2rad; 101 | return Eigen::Vector3d(lat, lon, h); 102 | } 103 | 104 | Eigen::Matrix3d getEnuToEcefRotMat(double lat, double lon) 105 | { 106 | // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ENU_to_ECEF 107 | double deg2rad = M_PI / 180.0; 108 | lat = lat * deg2rad; 109 | lon = lon * deg2rad; 110 | Eigen::Matrix3d ret; 111 | ret << -sin(lon), -sin(lat) * cos(lon), cos(lat) * cos(lon), cos(lon), -sin(lat) * sin(lon), cos(lat) * sin(lon), 112 | 0.0, cos(lat), sin(lat); 113 | return ret; 114 | } 115 | 116 | std::string GnssSensor::getName() { return this->name; } 117 | 118 | std::string GnssSensor::getFrameId() { return this->frame_id; } 119 | 120 | bool GnssSensor::RunTick(Eigen::Vector3d& gnssOrigin, Eigen::Vector3d& enuToTrackRotation, Eigen::Vector3d& trans, 121 | Eigen::Vector3d& rot, double time, Eigen::Vector3d velocity, Eigen::Vector3d omega, Eigen::Vector3d start_position, 122 | Eigen::Vector3d start_orientation, bool trackPreTransformed) 123 | { 124 | 125 | if (this->sampleReady(time)) 126 | { 127 | double originLat = gnssOrigin.x(); 128 | double originLon = gnssOrigin.y(); 129 | double originAlt = gnssOrigin.z(); 130 | auto originEcef = wgs84toEcef(originLat, originLon, originAlt); 131 | Eigen::Vector3d ecefRef = Eigen::Vector3d(originEcef.x(), originEcef.y(), originEcef.z()); 132 | auto rotEnuToEcef = getEnuToEcefRotMat(originLat, originLon); 133 | Eigen::Vector3d rotTrackToENU(enuToTrackRotation.x(), enuToTrackRotation.y(), enuToTrackRotation.z()); 134 | Eigen::Matrix3d rotMatTrackToEnu = eulerAnglesToRotMat(rotTrackToENU).transpose(); 135 | Eigen::Vector3d actualTrackCar = start_position; 136 | // if the track was transformed, we need to account for the changed track origin 137 | if (trackPreTransformed) 138 | { 139 | actualTrackCar = eulerAnglesToRotMat(start_orientation).transpose() * trans + start_position; 140 | } 141 | Eigen::Vector3d enuCar = rotMatTrackToEnu * actualTrackCar; 142 | 143 | std::default_random_engine generator(noiseSeed); 144 | 145 | std::normal_distribution distX(errorMeanPosition.x(), errorSigmaPosition.x()); 146 | std::normal_distribution distY(errorMeanPosition.y(), errorSigmaPosition.y()); 147 | std::normal_distribution distZ(errorMeanPosition.z(), errorSigmaPosition.z()); 148 | 149 | std::normal_distribution distXOr(errorMeanOrientation.x(), errorSigmaOrientation.x()); 150 | std::normal_distribution distYOr(errorMeanOrientation.y(), errorSigmaOrientation.y()); 151 | std::normal_distribution distZOr(errorMeanOrientation.z(), errorSigmaOrientation.z()); 152 | 153 | std::normal_distribution distXVel(errorMeanVelocity.x(), errorSigmaVelocity.x()); 154 | std::normal_distribution distYVel(errorMeanVelocity.y(), errorSigmaVelocity.y()); 155 | std::normal_distribution distZVel(errorMeanVelocity.z(), errorSigmaVelocity.z()); 156 | 157 | enuCar.x() += distX(generator); 158 | enuCar.y() += distY(generator); 159 | enuCar.z() += distZ(generator); 160 | Eigen::Vector3d ecefCar = rotEnuToEcef * enuCar + ecefRef; 161 | 162 | Eigen::Vector3d positionWgs = ecefToWgs84(ecefCar.x(), ecefCar.y(), ecefCar.z()); 163 | 164 | Eigen::Vector3d velENU 165 | = rotMatTrackToEnu * eulerAnglesToRotMat(rot) * rigidBodyVelocity(velocity, this->position, omega); 166 | if (trackPreTransformed) 167 | { 168 | velENU = rotMatTrackToEnu * eulerAnglesToRotMat(start_orientation) * eulerAnglesToRotMat(rot) 169 | * rigidBodyVelocity(velocity, this->position, omega); 170 | } 171 | 172 | // sensor->car->track->enu 173 | quaternion q0 = quatFromEulerAngles(this->orientation); 174 | quaternion q1 = quatFromEulerAngles(rot); 175 | quaternion q11 = quatFromEulerAngles(start_orientation); 176 | quaternion qCar = q1; 177 | // if the track was transformed, we need to account for the changed track origin 178 | if (trackPreTransformed) 179 | { 180 | qCar = quatMult(q11, q1); 181 | } 182 | quaternion q2 = quatFromEulerAngles(rotTrackToENU); 183 | quaternion q3 = quatMult(q2, quatMult(qCar, q0)); 184 | // apply noise as additional rotation 185 | quaternion q4 186 | = quatFromEulerAngles(Eigen::Vector3d(distXOr(generator), distYOr(generator), distZOr(generator))); 187 | quaternion q5 = quatMult(q4, q3); 188 | 189 | quaternion outQuat = quatFromEulerAngles(Eigen::Vector3d(0.0, 0.0, 0.0)); 190 | if (this->orientationMode == 1) 191 | { 192 | Eigen::Matrix3d testMat = rotationMatrixFromQuaternion(q5); 193 | Eigen::Vector3d testVec = testMat * Eigen::Vector3d(1, 0, 0); 194 | double angleN = std::atan2(testVec.y(), testVec.x()); 195 | outQuat = quatFromEulerAngles(Eigen::Vector3d(0.0, 0.0, angleN)); 196 | } 197 | else if (this->orientationMode == 2) 198 | { 199 | outQuat = q5; 200 | } 201 | 202 | GnssData value; 203 | value.latitude = positionWgs.x(); 204 | value.longitude = positionWgs.y(); 205 | value.altitude = positionWgs.z(); 206 | value.position_covariance = this->errorSigmaPosition.asDiagonal(); 207 | value.position_covariance = value.position_covariance.array().square(); 208 | 209 | if (this->outputVelocity) 210 | { 211 | value.vel_east = velENU.x() + distXVel(generator); 212 | value.vel_north = velENU.y() + distYVel(generator); 213 | value.vel_up = velENU.z() + distZVel(generator); 214 | value.velocity_covariance = this->errorSigmaVelocity.asDiagonal(); 215 | value.velocity_covariance = value.velocity_covariance.array().square(); 216 | } 217 | else 218 | { 219 | value.vel_east = 0.0; 220 | value.vel_north = 0.0; 221 | value.vel_up = 0.0; 222 | value.velocity_covariance(0, 0) = -1.0; 223 | value.velocity_covariance(1, 1) = -1.0; 224 | value.velocity_covariance(2, 2) = -1.0; 225 | } 226 | 227 | value.orientation = outQuat; 228 | value.orientation_covariance = this->errorSigmaOrientation.asDiagonal(); 229 | value.orientation_covariance = value.orientation_covariance.array().square(); 230 | 231 | value.fix_status = GnssData::FixStatus::FIX; 232 | 233 | value.timestamp = time; 234 | value.frame = this->frame_id; 235 | 236 | noiseSeed += 1; 237 | 238 | this->deadTimeQueue.push(value); 239 | this->registerSampling(); 240 | } 241 | return availableDeadTime(time); 242 | } -------------------------------------------------------------------------------- /src/sensorModels/imuSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorModels/imuSensor.hpp" 2 | 3 | ImuSensor::ImuSensor(double rate, double deadTime) 4 | { 5 | this->rate = rate; 6 | this->lastSampleTime = 0.0; 7 | this->deadTime = deadTime; 8 | } 9 | 10 | void ImuSensor::readConfig(ConfigElement& config) 11 | { 12 | config.getElement(&this->rate, "rate"); 13 | config.getElement(&this->deadTime, "dead_time"); 14 | config["error_acc"].getElement(&this->error_mean_acc, "mean"); 15 | config["error_acc"].getElement(&this->error_sigma_acc, "sigma"); 16 | config["error_rot"].getElement(&this->error_mean_rot, "mean"); 17 | config["error_rot"].getElement(&this->error_sigma_rot, "sigma"); 18 | config.getElement(&this->name, "name"); 19 | config.getElement(&this->frame, "frame"); 20 | 21 | std::vector position; 22 | config["pose"].getElement>(&position, "position"); 23 | this->position = Eigen::Vector3d(position[0], position[1], position[2]); 24 | std::vector orientation; 25 | config["pose"].getElement>(&orientation, "orientation"); 26 | this->orientation = Eigen::Vector3d(orientation[0], orientation[1], orientation[2]); 27 | } 28 | 29 | bool ImuSensor::RunTick(ImuData& in, Eigen::Vector3d& alpha, double time) 30 | { 31 | // TODO rigid body tranform of accleration 32 | if (this->sampleReady(time)) 33 | { 34 | ImuData value = this->applyError(in); 35 | value.frame = "cog_static"; 36 | this->deadTimeQueue.push(value); 37 | this->registerSampling(); 38 | } 39 | return availableDeadTime(time); 40 | } 41 | 42 | ImuData ImuSensor::applyError(ImuData input) 43 | { 44 | std::default_random_engine generator(numFrames); 45 | std::normal_distribution distAccError(error_mean_acc, error_sigma_acc); 46 | std::normal_distribution distRotError(error_mean_rot, error_sigma_rot); 47 | 48 | input.acc.x() += distAccError(generator); 49 | input.acc.y() += distAccError(generator); 50 | input.acc.z() += distAccError(generator); 51 | 52 | input.acc_cov(0, 0) = error_sigma_acc * error_sigma_acc; 53 | input.acc_cov(1, 1) = error_sigma_acc * error_sigma_acc; 54 | input.acc_cov(2, 2) = error_sigma_acc * error_sigma_acc; 55 | 56 | input.rot.x() += distAccError(generator); 57 | input.rot.y() += distAccError(generator); 58 | input.rot.z() += distAccError(generator); 59 | 60 | input.rot_cov(0, 0) = error_sigma_rot * error_sigma_rot; 61 | input.rot_cov(1, 1) = error_sigma_rot * error_sigma_rot; 62 | input.rot_cov(2, 2) = error_sigma_rot * error_sigma_rot; 63 | 64 | numFrames += 1; 65 | return input; 66 | } 67 | 68 | std::string ImuSensor::getName() { return this->name; } -------------------------------------------------------------------------------- /src/sensorModels/perceptionSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorModels/perceptionSensor.hpp" 2 | 3 | PerceptionSensor::PerceptionSensor() 4 | { 5 | this->name = ""; 6 | this->frame_id = ""; 7 | this->maxRange = 0.0; 8 | this->minRange = 0.0; 9 | this->minAngleHorizontal = 0.0; 10 | this->maxAngleHorizontal = 0.0; 11 | this->rate = rate; 12 | this->lastSampleTime = 0.0; 13 | this->deadTime = deadTime; 14 | this->position = Eigen::Vector3d(0.0, 0.0, 0.0); 15 | this->orientation = Eigen::Vector3d(0.0, 0.0, 0.0); 16 | this->errorMeanXYZ = Eigen::Vector3d::Zero(); 17 | this->errorSigmaXYZ = Eigen::Vector3d::Zero(); 18 | this->errorMeanRange = 0.0; 19 | this->errorSigmaRange = 0.0; 20 | this->errorMeanRangeRelative = 0.0; 21 | this->errorSigmaRangeRelative = 0.0; 22 | this->errorMeanAngle = Eigen::Vector2d::Zero(); 23 | this->errorSigmaAngle = Eigen::Vector2d::Zero(); 24 | this->numFrames = 0; 25 | this->detection_prob_min_dist = 0.99; 26 | this->detection_prob_decrease_dist_linear = 0.01; 27 | this->detection_prob_decrease_dist_quadratic = 0.00012; 28 | this->min_detection_prob = 0.1; 29 | } 30 | 31 | void PerceptionSensor::readConfig(ConfigElement& config) 32 | { 33 | config.getElement(&this->name, "name"); 34 | config.getElement(&this->frame_id, "name"); 35 | config.getElement(&this->rate, "rate"); 36 | config["delay"].getElement(&this->deadTime, "mean"); 37 | std::vector position; 38 | config["pose"].getElement>(&position, "position"); 39 | this->position = Eigen::Vector3d(position[0], position[1], position[2]); 40 | std::vector orientation; 41 | config["pose"].getElement>(&orientation, "orientation"); 42 | this->orientation = Eigen::Vector3d(orientation[0], orientation[1], orientation[2]); 43 | config["observation"].getElement(&this->minRange, "min_range"); 44 | config["observation"].getElement(&this->maxRange, "max_range"); 45 | 46 | config["observation"].getElement(&this->minAngleHorizontal, "min_angle_horizontal"); 47 | config["observation"].getElement(&this->maxAngleHorizontal, "max_angle_horizontal"); 48 | config["observation"].getElement(&this->minAngleVertical, "min_angle_vertical"); 49 | config["observation"].getElement(&this->maxAngleVertical, "max_angle_vertical"); 50 | 51 | std::vector meanErrorXYZ; 52 | config["noise"]["cartesian"].getElement>(&meanErrorXYZ, "mean"); 53 | this->errorMeanXYZ = Eigen::Vector3d(meanErrorXYZ[0], meanErrorXYZ[1], meanErrorXYZ[2]); 54 | 55 | std::vector sigmaErrorXYZ; 56 | config["noise"]["cartesian"].getElement>(&sigmaErrorXYZ, "sigma"); 57 | this->errorSigmaXYZ = Eigen::Vector3d(sigmaErrorXYZ[0], sigmaErrorXYZ[1], sigmaErrorXYZ[2]); 58 | 59 | std::vector meanErrorAngle; 60 | config["noise"]["angle"].getElement>(&meanErrorAngle, "mean"); 61 | this->errorMeanAngle = Eigen::Vector2d(meanErrorAngle[0], meanErrorAngle[1]); 62 | 63 | std::vector sigmaErrorAngle; 64 | config["noise"]["angle"].getElement>(&sigmaErrorAngle, "sigma"); 65 | this->errorSigmaAngle = Eigen::Vector2d(sigmaErrorAngle[0], sigmaErrorAngle[1]); 66 | 67 | config["noise"]["range"].getElement(&this->errorMeanRange, "mean"); 68 | config["noise"]["range"].getElement(&this->errorSigmaRange, "sigma"); 69 | config["noise"]["range_relative"].getElement(&this->errorMeanRangeRelative, "mean"); 70 | config["noise"]["range_relative"].getElement(&this->errorSigmaRangeRelative, "sigma"); 71 | 72 | config["detection"].getElement(&this->detection_prob_min_dist, "prob_min_dist"); 73 | config["detection"].getElement(&this->detection_prob_decrease_dist_linear, "prob_decrease_dist_linear"); 74 | config["detection"].getElement( 75 | &this->detection_prob_decrease_dist_quadratic, "prob_decrease_dist_quadratic"); 76 | config["detection"].getElement(&this->min_detection_prob, "min_prob"); 77 | 78 | config["classification"].getElement(&this->classification_max_distance, "max_dist"); 79 | config["classification"].getElement(&this->classification_prob_min_dist, "prob_min_dist"); 80 | config["classification"].getElement( 81 | &this->classification_prob_decrease_dist_linear, "prob_decrease_dist_linear"); 82 | config["classification"].getElement( 83 | &this->classification_prob_decrease_dist_quadratic, "prob_decrease_dist_quadratic"); 84 | config["classification"].getElement(&this->min_classification_prob, "min_prob"); 85 | } 86 | 87 | LandmarkList PerceptionSensor::process(LandmarkList& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time) 88 | { 89 | LandmarkList transformedLmsVehicle = transformLmList(in, trans, rot); 90 | LandmarkList transformedLms = transformLmList(transformedLmsVehicle, this->position, this->orientation); 91 | std::vector listNew = this->filterFoV(transformedLms.list); 92 | listNew = this->filterTypeAndDOO(listNew); 93 | listNew = this->handleFalsePositivesAndNegatives(listNew); 94 | listNew = this->addClassProbailities(listNew); 95 | listNew = this->addNoise(listNew); 96 | LandmarkList ret; 97 | ret.list = listNew; 98 | ret.timestamp = time; 99 | ret.frame_id = frame_id; 100 | numFrames += 1; 101 | return ret; 102 | } 103 | 104 | std::vector PerceptionSensor::filterFoV(std::vector& in) 105 | { 106 | std::vector listNew; 107 | for (Landmark& lm : in) 108 | { 109 | double dist = lm.position.norm(); 110 | double angleHorizontal = std::atan2(lm.position.y(), lm.position.x()); 111 | // double angleVertical = std::asin(lm.position.z() / dist); 112 | // if (dist >= minRange && dist <= maxRange && angleHorizontal >= minAngleHorizontal 113 | // && angleHorizontal <= maxAngleHorizontal && angleVertical >= minAngleVertical, 114 | // angleVertical <= maxAngleVertical) 115 | if (dist >= minRange && dist <= maxRange && angleHorizontal >= minAngleHorizontal 116 | && angleHorizontal <= maxAngleHorizontal) 117 | { 118 | listNew.push_back(lm); 119 | } 120 | } 121 | return listNew; 122 | } 123 | 124 | std::vector PerceptionSensor::filterTypeAndDOO(std::vector& in) 125 | { 126 | std::vector listNew; 127 | for (Landmark& lm : in) 128 | { 129 | if (lm.type != LandmarkType::INVISIBLE && (!lm.beenHit)) 130 | { 131 | listNew.push_back(lm); 132 | } 133 | } 134 | return listNew; 135 | } 136 | 137 | std::vector PerceptionSensor::addNoise(std::vector& in) 138 | { 139 | std::vector listNew; 140 | // provide numFrames as seed because the pseudo random generator would otherwise output the same values in 141 | // standstill and the noise would be constant 142 | std::default_random_engine generator(numFrames); 143 | std::normal_distribution distX(errorMeanXYZ.x(), errorSigmaXYZ.x()); 144 | std::normal_distribution distY(errorMeanXYZ.y(), errorSigmaXYZ.y()); 145 | std::normal_distribution distZ(errorMeanXYZ.z(), errorSigmaXYZ.z()); 146 | 147 | std::normal_distribution distRange(errorMeanRange, errorSigmaRange); 148 | std::normal_distribution distRangeRelative(errorMeanRangeRelative, errorSigmaRangeRelative); 149 | 150 | std::normal_distribution distTheta(errorMeanAngle.x(), errorSigmaAngle.x()); 151 | std::normal_distribution distPhi(errorMeanAngle.y(), errorSigmaAngle.y()); 152 | 153 | Eigen::Matrix3d rotationJacobian; 154 | 155 | for (Landmark& lm : in) 156 | { 157 | double dist = lm.position.norm(); 158 | double theta = std::acos(lm.position.z() / dist); 159 | double phi = std::atan2(lm.position.y(), lm.position.x()); 160 | 161 | Eigen::Matrix3d covSpherical = Eigen::Matrix3d::Zero(); 162 | covSpherical(0, 0) = errorSigmaAngle.x() * errorSigmaAngle.x(); 163 | covSpherical(1, 1) = errorSigmaAngle.y() * errorSigmaAngle.y(); 164 | covSpherical(2, 2) 165 | = errorSigmaRange * errorSigmaRange + dist * dist * errorSigmaRangeRelative * errorSigmaRangeRelative; 166 | 167 | rotationJacobian(0, 0) = dist * std::cos(theta) * std::cos(phi); 168 | rotationJacobian(0, 1) = dist * std::sin(theta) * -std::sin(phi); 169 | rotationJacobian(0, 2) = std::sin(theta) * std::cos(phi); 170 | 171 | rotationJacobian(1, 0) = dist * std::cos(theta) * std::sin(phi); 172 | rotationJacobian(1, 1) = dist * std::sin(theta) * std::cos(phi); 173 | rotationJacobian(1, 2) = std::sin(theta) * std::sin(phi); 174 | 175 | rotationJacobian(2, 0) = -dist * std::sin(theta); 176 | rotationJacobian(2, 1) = 0.0; 177 | rotationJacobian(2, 2) = std::cos(theta); 178 | 179 | theta += distTheta(generator); 180 | phi += distPhi(generator); 181 | 182 | dist += dist * distRangeRelative(generator); 183 | dist += distRange(generator); 184 | 185 | lm.position.x() = dist * std::sin(theta) * std::cos(phi); 186 | lm.position.y() = dist * std::sin(theta) * std::sin(phi); 187 | lm.position.z() = dist * std::cos(theta); 188 | 189 | lm.position.x() += distX(generator); 190 | lm.position.y() += distY(generator); 191 | lm.position.z() += distZ(generator); 192 | 193 | lm.cov = rotationJacobian * covSpherical * rotationJacobian.transpose(); 194 | lm.cov(0, 0) += errorSigmaXYZ.x() * errorSigmaXYZ.x(); 195 | lm.cov(1, 1) += errorSigmaXYZ.y() * errorSigmaXYZ.y(); 196 | lm.cov(2, 2) += errorSigmaXYZ.z() * errorSigmaXYZ.z(); 197 | 198 | listNew.push_back(lm); 199 | } 200 | return listNew; 201 | } 202 | 203 | std::vector PerceptionSensor::addClassProbailities(std::vector& in) 204 | { 205 | std::vector listNew; 206 | bool detect_big_orange = true; 207 | bool detect_timekeeping = true; 208 | std::default_random_engine random_generator(this->numFrames); 209 | std::uniform_real_distribution unif(0, 1.0); 210 | 211 | std::vector detectionClasses = { LandmarkType::UNKNOWN, LandmarkType::BLUE, LandmarkType::YELLOW, 212 | LandmarkType::ORANGE, LandmarkType::BIG_ORANGE }; 213 | for (Landmark& lm : in) 214 | { 215 | double dist = lm.position.norm(); 216 | double prob = this->classification_prob_min_dist 217 | - (dist - this->minRange) * this->classification_prob_decrease_dist_linear 218 | - std::pow((dist - this->minRange), 2) * this->classification_prob_decrease_dist_quadratic; 219 | prob = std::max(prob, min_classification_prob); 220 | if (dist > this->classification_max_distance) 221 | { 222 | prob = 1.0; 223 | lm.type = LandmarkType::UNKNOWN; 224 | } 225 | // if it's some weird class (like timekeeping), just set as unknown 226 | if (std::find(detectionClasses.begin(), detectionClasses.end(), lm.type) == detectionClasses.end()) 227 | { 228 | lm.type = LandmarkType::UNKNOWN; 229 | } 230 | std::fill(lm.typeWeights, lm.typeWeights + LandmarkType::UNKNOWN, 0); 231 | lm.typeWeights[lm.type] = prob; 232 | for (auto i : detectionClasses) 233 | { 234 | if (i != lm.type) 235 | { 236 | lm.typeWeights[i] = (1 - prob) / ((double)detectionClasses.size() - 1); 237 | } 238 | } 239 | // randomly swap some class with p = prob 240 | double randomNum = unif(random_generator); 241 | double acc = 0; 242 | for (int i = 0; i < (LandmarkType::UNKNOWN + 1); ++i) 243 | { 244 | acc += lm.typeWeights[i]; 245 | if (acc > randomNum) 246 | { 247 | double tmp = lm.typeWeights[i]; 248 | lm.typeWeights[i] = lm.typeWeights[lm.type]; 249 | lm.typeWeights[lm.type] = tmp; 250 | lm.type = static_cast(i); 251 | break; 252 | } 253 | } 254 | 255 | listNew.push_back(lm); 256 | } 257 | return listNew; 258 | } 259 | 260 | std::vector PerceptionSensor::handleFalsePositivesAndNegatives(std::vector& in) 261 | { 262 | std::vector listNew; 263 | 264 | std::default_random_engine random_generator(this->numFrames); 265 | std::uniform_real_distribution unif(0, 1.0); 266 | 267 | for (Landmark& lm : in) 268 | { 269 | double dist = lm.position.norm(); 270 | double detection_prob = this->detection_prob_min_dist 271 | - (dist - this->minRange) * this->detection_prob_decrease_dist_linear 272 | - std::pow((dist - this->minRange), 2) * this->detection_prob_decrease_dist_quadratic; 273 | detection_prob = std::max(this->min_detection_prob, detection_prob); 274 | double random = unif(random_generator); 275 | if (random < detection_prob) 276 | { 277 | lm.detection_probability = detection_prob; 278 | listNew.push_back(lm); 279 | } 280 | } 281 | return listNew; 282 | } 283 | 284 | std::string PerceptionSensor::getName() { return this->name; } 285 | 286 | std::string PerceptionSensor::getFrameId() { return this->frame_id; } 287 | 288 | bool PerceptionSensor::RunTick(LandmarkList& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time) 289 | { 290 | if (this->sampleReady(time)) 291 | { 292 | LandmarkList value = this->process(in, trans, rot, time); 293 | this->deadTimeQueue.push(value); 294 | this->registerSampling(); 295 | } 296 | return availableDeadTime(time); 297 | } 298 | -------------------------------------------------------------------------------- /src/sensorModels/scalarValueSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorModels/scalarValueSensor.hpp" 2 | 3 | ScalarValueSensor::ScalarValueSensor(double rate, double deadTime) 4 | { 5 | this->rate = rate; 6 | this->lastSampleTime = 0.0; 7 | this->deadTime = deadTime; 8 | this->error_mean = 0.0; 9 | this->error_sigma = 0.0; 10 | } 11 | 12 | void ScalarValueSensor::readConfig(ConfigElement& config) 13 | { 14 | config.getElement(&this->rate, "rate"); 15 | config.getElement(&this->deadTime, "dead_time"); 16 | config["error"].getElement(&this->error_mean, "mean"); 17 | config["error"].getElement(&this->error_sigma, "sigma"); 18 | } 19 | 20 | bool ScalarValueSensor::RunTick(StampedScalar& in, double time) 21 | { 22 | if (this->sampleReady(time)) 23 | { 24 | StampedScalar value = applyError(in); 25 | this->deadTimeQueue.push(value); 26 | this->registerSampling(); 27 | } 28 | return availableDeadTime(time); 29 | } 30 | 31 | StampedScalar ScalarValueSensor::applyError(StampedScalar input) 32 | { 33 | std::default_random_engine generator(numFrames); 34 | std::normal_distribution distError(error_mean, error_sigma); 35 | 36 | input.data += distError(generator); 37 | numFrames += 1; 38 | return input; 39 | } -------------------------------------------------------------------------------- /src/sensorModels/wheelsSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorModels/wheelsSensor.hpp" 2 | 3 | WheelsSensor::WheelsSensor(double rate, double deadTime) 4 | { 5 | this->rate = rate; 6 | this->lastSampleTime = 0.0; 7 | this->deadTime = deadTime; 8 | } 9 | 10 | void WheelsSensor::readConfig(ConfigElement& config) 11 | { 12 | config.getElement(&this->rate, "rate"); 13 | config.getElement(&this->deadTime, "dead_time"); 14 | config["error"].getElement(&this->error_mean, "mean"); 15 | config["error"].getElement(&this->error_sigma, "sigma"); 16 | } 17 | 18 | bool WheelsSensor::RunTick(Wheels& in, Eigen::Vector3d& trans, Eigen::Vector3d& rot, double time) 19 | { 20 | if (this->sampleReady(time)) 21 | { 22 | Wheels value = this->applyError(in); 23 | this->deadTimeQueue.push(value); 24 | this->registerSampling(); 25 | } 26 | return availableDeadTime(time); 27 | } 28 | 29 | Wheels WheelsSensor::applyError(Wheels input) 30 | { 31 | std::default_random_engine generator(numFrames); 32 | std::normal_distribution distError(error_mean, error_sigma); 33 | 34 | input.FL += distError(generator); 35 | input.FR += distError(generator); 36 | input.RL += distError(generator); 37 | input.RR += distError(generator); 38 | numFrames += 1; 39 | return input; 40 | } -------------------------------------------------------------------------------- /src/track/gripMap.cpp: -------------------------------------------------------------------------------- 1 | #include "track/gripMap.hpp" 2 | 3 | gripMap::gripMap(std::shared_ptr logger) { this->logger = logger; } 4 | 5 | void gripMap::loadConfig(std::string path) 6 | { 7 | Config cfg(path); 8 | ConfigElement config = cfg.getElement("grip_map"); 9 | config.getElement(&this->total_scale, "total_scale"); 10 | 11 | std::vector gripPoints; 12 | config["points"].getElements(&gripPoints); 13 | 14 | if (gripPoints.size() == 0) 15 | { 16 | this->logger->logFatal("No gripMap points provided!"); 17 | std::abort(); 18 | } 19 | 20 | for (auto& point : gripPoints) 21 | { 22 | double gripValue; 23 | std::vector position; 24 | point.getElement>(&position, "position"); 25 | Eigen::Vector3d positionEigen(position.at(0), position.at(1), position.at(2)); 26 | point.getElement(&gripValue, "value"); 27 | 28 | this->mapPoints.push_back(std::make_pair(positionEigen, gripValue)); 29 | } 30 | } 31 | 32 | Wheels gripMap::getGripValues(std::array in) 33 | { 34 | std::array temp; 35 | for (int i = 0; i < 4; ++i) 36 | { 37 | auto el = in.at(i); 38 | auto bestPoint = std::min_element(std::begin(this->mapPoints), std::end(this->mapPoints), 39 | [&el](const std::pair& a, const std::pair& b) 40 | { return (a.first - el).norm() < (b.first - el).norm(); }); 41 | temp[i] = ((*bestPoint).second); 42 | } 43 | 44 | Wheels ret; 45 | ret.FL = this->total_scale * temp[0]; 46 | ret.FR = this->total_scale * temp[1]; 47 | ret.RL = this->total_scale * temp[2]; 48 | ret.RR = this->total_scale * temp[3]; 49 | 50 | return ret; 51 | } 52 | 53 | void gripMap::transformPoints(Eigen::Vector3d trans, Eigen::Vector3d rot) 54 | { 55 | Eigen::Matrix3d rotationMatrix = eulerAnglesToRotMat(rot); 56 | Eigen::Vector3d transInverse = inverseTranslation(trans, rot); 57 | 58 | for (int i = 0; i < this->mapPoints.size(); ++i) 59 | { 60 | this->mapPoints[i].first = rotationMatrix * this->mapPoints[i].first; 61 | this->mapPoints[i].first += transInverse; 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /src/track/trackLoader.cpp: -------------------------------------------------------------------------------- 1 | #include "track/trackLoader.hpp" 2 | #include "yaml-cpp/yaml.h" 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | using namespace YAML; 8 | 9 | Node fillChildNode(Node parentNode, string tag) 10 | { 11 | if (parentNode[tag]) 12 | { 13 | Node childNode = parentNode[tag]; 14 | return childNode; 15 | } 16 | else 17 | throw runtime_error(string("Failed loading the map, no ") + tag); 18 | } 19 | 20 | void addLandmarks(std::vector* _ret, Node* list, int* _coneCounter) 21 | { 22 | Landmark lm; 23 | for (const_iterator it = list->begin(); it != list->end(); ++it) 24 | { 25 | const Node& position = *it; 26 | vector vi = position["position"].as>(); 27 | lm.id = *_coneCounter; 28 | (*_coneCounter)++; 29 | lm.position = Eigen::Vector3d(vi[0], vi[1], vi[2]); 30 | lm.type = stringToLandmarkType(position["class"].as()); 31 | lm.typeWeights[lm.type] = 1.0; 32 | _ret->push_back(lm); 33 | } 34 | } 35 | 36 | void addTimeKeepings(std::vector>* _ret, Node* list, int* _coneCounter) 37 | { 38 | Landmark lm; 39 | std::vector lms; 40 | for (const_iterator it = list->begin(); it != list->end(); ++it) 41 | { 42 | const Node& position = *it; 43 | vector vi = position["position"].as>(); 44 | lm.id = *_coneCounter; 45 | (*_coneCounter)++; 46 | lm.position = Eigen::Vector3d(vi[0], vi[1], vi[2]); 47 | lm.type = stringToLandmarkType(position["class"].as()); 48 | lm.typeWeights[lm.type] = 1.0; 49 | lms.push_back(lm); 50 | } 51 | for (int i = 0; i < (lms.size() / 2); ++i) 52 | { 53 | _ret->push_back(std::make_pair(lms[i * 2], lms[i * 2 + 1])); 54 | } 55 | } 56 | 57 | Track loadMap(string mapPath, Eigen::Vector3d& start_position, Eigen::Vector3d& start_orientation) 58 | { 59 | Node map = LoadFile(mapPath); 60 | Node track = fillChildNode(map, "track"); 61 | Node left = fillChildNode(track, "left"); 62 | Node right = fillChildNode(track, "right"); 63 | Node time_keeping = fillChildNode(track, "time_keeping"); 64 | Node unknown = fillChildNode(track, "unknown"); 65 | // TODO: Catch pos or orientation size != 3 66 | std::vector start_pos = track["start"]["position"].as>(); 67 | std::vector start_or = track["start"]["orientation"].as>(); 68 | start_position = Eigen::Vector3d(start_pos.data()); 69 | start_orientation = Eigen::Vector3d(start_or.data()); 70 | 71 | Track ret; 72 | std::vector gnss_start_pos = track["earthToTrack"]["position"].as>(); 73 | std::vector gnss_start_or = track["earthToTrack"]["orientation"].as>(); 74 | ret.gnssOrigin = Eigen::Vector3d(gnss_start_pos.data()); 75 | ret.enuToTrackRotation = Eigen::Vector3d(gnss_start_or.data()); 76 | 77 | int coneCounter = 0; 78 | if (track["lanesFirstWithLastConnected"]) 79 | { 80 | ret.lanesFirstWithLastConnected = track["lanesFirstWithLastConnected"].as(); 81 | } 82 | else 83 | { 84 | ret.lanesFirstWithLastConnected = true; 85 | } 86 | 87 | addLandmarks(&ret.left_lane, &left, &coneCounter); 88 | addLandmarks(&ret.right_lane, &right, &coneCounter); 89 | addTimeKeepings(&ret.time_keeping_gates, &time_keeping, &coneCounter); 90 | addLandmarks(&ret.unknown, &unknown, &coneCounter); 91 | 92 | return ret; 93 | } 94 | -------------------------------------------------------------------------------- /src/transform.cpp: -------------------------------------------------------------------------------- 1 | #include "transform.hpp" 2 | 3 | Eigen::Vector3d rigidBodyVelocity(Eigen::Vector3d v, Eigen::Vector3d r, Eigen::Vector3d omega) 4 | { 5 | return (v + omega.cross(r)); 6 | } 7 | 8 | Eigen::Vector3d rigidBodyAcceleration( 9 | Eigen::Vector3d a, Eigen::Vector3d r, Eigen::Vector3d omega, Eigen::Vector3d alpha) 10 | { 11 | return (a + omega.cross(omega.cross(r)) + alpha.cross(r)); 12 | } 13 | 14 | Eigen::Matrix3d eulerAnglesToRotMat(Eigen::Vector3d& angles) 15 | { 16 | Eigen::AngleAxisd rollAngle(-angles.x(), Eigen::Vector3d::UnitX()); 17 | Eigen::AngleAxisd yawAngle(-angles.z(), Eigen::Vector3d::UnitZ()); 18 | Eigen::AngleAxisd pitchAngle(-angles.y(), Eigen::Vector3d::UnitY()); 19 | 20 | // rotation order matches with tf2 21 | Eigen::Quaternion q = rollAngle * pitchAngle * yawAngle; 22 | 23 | Eigen::Matrix3d rotationMatrix = q.matrix(); 24 | return rotationMatrix; 25 | } 26 | 27 | Eigen::Vector3d inverseTranslation(Eigen::Vector3d trans, Eigen::Vector3d rot) 28 | { 29 | Eigen::Matrix3d rotationMatrix = eulerAnglesToRotMat(rot); 30 | 31 | return (-rotationMatrix * trans); 32 | } 33 | 34 | LandmarkList transformLmList(LandmarkList& in, Eigen::Vector3d trans, Eigen::Vector3d rot) 35 | { 36 | Eigen::Matrix3d rotationMatrix = eulerAnglesToRotMat(rot); 37 | Eigen::Vector3d transInverse = inverseTranslation(trans, rot); 38 | 39 | LandmarkList out; 40 | for (auto& lm : in.list) 41 | { 42 | Landmark temp = lm; 43 | temp.position = rotationMatrix * lm.position; 44 | temp.position += transInverse; 45 | temp.id = lm.id; 46 | out.list.push_back(temp); 47 | } 48 | 49 | return out; 50 | } 51 | 52 | Track transformTrack(Track& in, Eigen::Vector3d trans, Eigen::Vector3d rot) 53 | { 54 | Eigen::Matrix3d rotationMatrix = eulerAnglesToRotMat(rot); 55 | Eigen::Vector3d transInverse = inverseTranslation(trans, rot); 56 | 57 | Track out = in; 58 | out.lanesFirstWithLastConnected = in.lanesFirstWithLastConnected; 59 | out.left_lane.clear(); 60 | for (auto& lm : in.left_lane) 61 | { 62 | Landmark temp = lm; 63 | temp.position = rotationMatrix * lm.position; 64 | temp.position += transInverse; 65 | temp.id = lm.id; 66 | out.left_lane.push_back(temp); 67 | } 68 | out.right_lane.clear(); 69 | for (auto& lm : in.right_lane) 70 | { 71 | Landmark temp = lm; 72 | temp.position = rotationMatrix * lm.position; 73 | temp.position += transInverse; 74 | temp.id = lm.id; 75 | out.right_lane.push_back(temp); 76 | } 77 | out.unknown.clear(); 78 | for (auto& lm : in.unknown) 79 | { 80 | Landmark temp = lm; 81 | temp.position = rotationMatrix * lm.position; 82 | temp.position += transInverse; 83 | temp.id = lm.id; 84 | out.unknown.push_back(temp); 85 | } 86 | 87 | out.time_keeping_gates.clear(); 88 | for (auto& gate : in.time_keeping_gates) 89 | { 90 | Landmark temp1 = gate.first; 91 | temp1.position = rotationMatrix * gate.first.position; 92 | temp1.position += transInverse; 93 | temp1.id = gate.first.id; 94 | Landmark temp2 = gate.second; 95 | temp2.position = rotationMatrix * gate.second.position; 96 | temp2.position += transInverse; 97 | temp2.id = gate.second.id; 98 | 99 | out.time_keeping_gates.push_back(std::make_pair(temp1, temp2)); 100 | } 101 | 102 | return out; 103 | } -------------------------------------------------------------------------------- /src/types.cpp: -------------------------------------------------------------------------------- 1 | #include "types.hpp" 2 | 3 | LandmarkType stringToLandmarkType(const std::string& in) 4 | { 5 | LandmarkType ret = LandmarkType::UNKNOWN; 6 | if (in == "blue") 7 | { 8 | ret = LandmarkType::BLUE; 9 | } 10 | else if (in == "yellow") 11 | { 12 | ret = LandmarkType::YELLOW; 13 | } 14 | else if (in == "small-orange" || in == "orange") 15 | { 16 | ret = LandmarkType::ORANGE; 17 | } 18 | else if (in == "big-orange") 19 | { 20 | ret = LandmarkType::BIG_ORANGE; 21 | } 22 | else if (in == "timekeeping") 23 | { 24 | ret = LandmarkType::TIMEKEEPING; 25 | } 26 | else if (in == "invisible") 27 | { 28 | ret = LandmarkType::INVISIBLE; 29 | } 30 | return ret; 31 | } 32 | 33 | LandmarkList trackToLMList(Track& in) 34 | { 35 | LandmarkList ret; 36 | // TODO: no copy for lists 37 | std::vector> lists { in.left_lane, in.right_lane, in.unknown }; 38 | for (auto& list : lists) 39 | { 40 | for (Landmark lm : list) 41 | { 42 | ret.list.push_back(lm); 43 | } 44 | } 45 | for (auto& p : in.time_keeping_gates) 46 | { 47 | ret.list.push_back(p.first); 48 | ret.list.push_back(p.second); 49 | } 50 | return ret; 51 | } 52 | 53 | Track lmListToTrack(LandmarkList& in) 54 | { 55 | Track ret; 56 | for (auto& lm : in.list) 57 | { 58 | ret.unknown.push_back(lm); 59 | } 60 | return ret; 61 | } 62 | 63 | Discipline stringToDiscipline(const std::string& disciplineStr) 64 | { 65 | Discipline discipline = Discipline::AUTOCROSS; 66 | if (disciplineStr == "autocross") 67 | { 68 | discipline = Discipline::AUTOCROSS; 69 | } 70 | else if (disciplineStr == "trackdrive") 71 | { 72 | discipline = Discipline::TRACKDRIVE; 73 | } 74 | else if (disciplineStr == "acceleration") 75 | { 76 | discipline = Discipline::ACCELERATION; 77 | } 78 | else if (disciplineStr == "skidpad") 79 | { 80 | discipline = Discipline::SKIDPAD; 81 | } 82 | return discipline; 83 | } -------------------------------------------------------------------------------- /srv/ClockTriggerAbsolute.srv: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stop_time 2 | --- 3 | bool already_past # whether stop_time was already in the past -------------------------------------------------------------------------------- /srv/ClockTriggerRelative.srv: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Duration runtime 2 | --- 3 | builtin_interfaces/Time stop_time # time at which simulation will stop running -------------------------------------------------------------------------------- /track_editor/guiLogic.py: -------------------------------------------------------------------------------- 1 | from enum import Enum 2 | import mapFile 3 | import numpy as np 4 | class editorMode(Enum): 5 | MOVE = 1 6 | ADD = 2 7 | REMOVE = 3 8 | LANE_CONNECT_LEFT = 4 9 | LANE_CONNECT_RIGHT = 5 10 | TIMEKEEPING_START = 6 11 | TIMEKEEPING_FINISH = 7 12 | TIMEKEEPING_SECTOR = 8 13 | class landmarkType(Enum): 14 | UNDEFINED = 0 15 | BLUE = 1 16 | YELLOW = 2 17 | ORANGE = 3 18 | BIG_ORANGE = 4 19 | INVISIBLE = 5 20 | TIMEKEEPING = 6 21 | 22 | class guiLogic(): 23 | def __init__(self, *args, **kwargs): 24 | self.editorMode = editorMode.ADD 25 | self.landmarkType = landmarkType.UNDEFINED 26 | self.graphicsView = None 27 | self.cones = [] 28 | self.coneColorMap = {} 29 | self.lanesConnectionLeft = [] 30 | self.lanesConnectionRight = [] 31 | self.timeKeepingGates = [] 32 | self.startPosition = np.array([0,0,0]) 33 | self.startOrientation = np.array([0,0,0]) 34 | d2r = np.pi / 180.0 35 | self.originGeodeticCoordinates = np.array([51.001745, 13.641794,210]) 36 | self.originENURotation = d2r * np.array([0,0,45]) 37 | 38 | # https://stackoverflow.com/questions/27161533/find-the-shortest-distance-between-a-point-and-line-segments-not-line 39 | def lineseg_dists(self, p, a, b): 40 | """Cartesian distance from point to line segment 41 | 42 | Edited to support arguments as series, from: 43 | https://stackoverflow.com/a/54442561/11208892 44 | 45 | Args: 46 | - p: np.array of single point, shape (2,) or 2D array, shape (x, 2) 47 | - a: np.array of shape (x, 2) 48 | - b: np.array of shape (x, 2) 49 | """ 50 | # normalized tangent vectors 51 | d_ba = b - a 52 | d = np.divide(d_ba, (np.hypot(d_ba[:, 0], d_ba[:, 1]) 53 | .reshape(-1, 1))) 54 | 55 | # signed parallel distance components 56 | # rowwise dot products of 2D vectors 57 | s = np.multiply(a - p, d).sum(axis=1) 58 | t = np.multiply(p - b, d).sum(axis=1) 59 | 60 | # clamped parallel distance 61 | h = np.maximum.reduce([s, t, np.zeros(len(s))]) 62 | 63 | # perpendicular distance component 64 | # rowwise cross products of 2D vectors 65 | d_pa = p - a 66 | c = d_pa[:, 0] * d[:, 1] - d_pa[:, 1] * d[:, 0] 67 | 68 | return np.hypot(h, c) 69 | 70 | def getClosestInd(self, point, list): 71 | bestInd = 0 72 | bestDist = 999 73 | for i in range(len(list)): 74 | dist = np.linalg.norm(list[i][0]-point) 75 | if(dist < bestDist): 76 | bestDist = dist 77 | bestInd = i 78 | return bestInd 79 | 80 | def getMinTrackWidth(self): 81 | if(len(self.lanesConnectionLeft) == 0 or len(self.lanesConnectionRight) == 0): 82 | return 0 83 | minDistance = 999 84 | offsetLane = 0 85 | toBeckCheckedConesLeft = self.lanesConnectionLeft[:-1] 86 | toBeckCheckedConesRight = self.lanesConnectionRight[:-1] 87 | for i in range(-offsetLane,len(toBeckCheckedConesLeft)-1): 88 | p1 = toBeckCheckedConesLeft[i-2][0] 89 | p2 = toBeckCheckedConesLeft[i-1][0] 90 | p3 = toBeckCheckedConesLeft[i][0] 91 | tangential = p3-p1 92 | tangential = (1/np.linalg.norm(tangential)) * tangential 93 | projectionDistance = 4 94 | normal = np.array([-tangential[1], tangential[0], 0]) 95 | normal = projectionDistance * normal 96 | closestInd = self.getClosestInd(p2 + normal, toBeckCheckedConesRight) 97 | indicesToCheck = np.linspace(closestInd-5, closestInd+5, num = 11, dtype=int) 98 | a = [] 99 | b = [] 100 | for j in indicesToCheck: 101 | a.append(toBeckCheckedConesRight[j % len(toBeckCheckedConesRight)][0][0:2]) 102 | b.append(toBeckCheckedConesRight[(j+1) % len(toBeckCheckedConesRight)][0][0:2]) 103 | minDistance = min(minDistance, min(self.lineseg_dists(p2[0:2], np.array(a), np.array(b)))) 104 | return minDistance 105 | 106 | def getTrackLength(self): 107 | if(len(self.lanesConnectionLeft) == 0 or len(self.lanesConnectionRight) == 0): 108 | return 0 109 | leftLength = 0 110 | rightLength = 0 111 | offsetLane = 0 112 | for i in range(-offsetLane,len(self.lanesConnectionLeft)): 113 | p1 = self.lanesConnectionLeft[i-1][0] 114 | p2 = self.lanesConnectionLeft[i][0] 115 | dist = np.linalg.norm(p1-p2) 116 | leftLength += dist 117 | for i in range(-offsetLane,len(self.lanesConnectionRight)): 118 | p1 = self.lanesConnectionRight[i-1][0] 119 | p2 = self.lanesConnectionRight[i][0] 120 | dist = np.linalg.norm(p1-p2) 121 | rightLength += dist 122 | return 0.5*(leftLength + rightLength) 123 | 124 | def getMaxLaneDistance(self): 125 | if(len(self.lanesConnectionLeft) == 0 or len(self.lanesConnectionRight) == 0): 126 | return 0 127 | maxDist = 0 128 | offsetLane = -1 129 | for i in range(-offsetLane,len(self.lanesConnectionLeft)): 130 | p1 = self.lanesConnectionLeft[i-1][0] 131 | p2 = self.lanesConnectionLeft[i][0] 132 | dist = np.linalg.norm(p1-p2) 133 | maxDist = max(maxDist, dist) 134 | for i in range(-offsetLane,len(self.lanesConnectionRight)): 135 | p1 = self.lanesConnectionRight[i-1][0] 136 | p2 = self.lanesConnectionRight[i][0] 137 | dist = np.linalg.norm(p1-p2) 138 | maxDist = max(maxDist, dist) 139 | return maxDist 140 | 141 | # https://stackoverflow.com/questions/41144224/calculate-curvature-for-3-points-x-y 142 | def getCurvature(self, point1, point2, point3): 143 | # Calculating length of all three sides 144 | len_side_1 = round( np.linalg.norm(point1-point2), 2) 145 | len_side_2 = round( np.linalg.norm(point2-point3), 2) 146 | len_side_3 = round( np.linalg.norm(point1-point3), 2) 147 | 148 | # sp is semi-perimeter 149 | sp = (len_side_1 + len_side_2 + len_side_3) / 2 150 | 151 | # Calculating area using Herons formula 152 | area = np.sqrt(max(sp * (sp - len_side_1) * (sp - len_side_2) * (sp - len_side_3),0.00001)) 153 | 154 | # Calculating curvature using Menger curvature formula 155 | curvature = (4 * area) / max(len_side_1 * len_side_2 * len_side_3,0.0000001) 156 | 157 | return curvature 158 | 159 | def getMinOuterRadius(self): 160 | if(len(self.lanesConnectionLeft) == 0 or len(self.lanesConnectionRight) == 0): 161 | return 0 162 | minRadius = 999 163 | offsetLane = 0 164 | toBeckCheckedConesLeft = self.lanesConnectionLeft[:-1] 165 | toBeckCheckedConesRight = self.lanesConnectionRight[:-1] 166 | for i in range(-offsetLane,len(toBeckCheckedConesLeft)-2): 167 | p1 = toBeckCheckedConesLeft[i-2][0] 168 | p2 = toBeckCheckedConesLeft[i-1][0] 169 | p3 = toBeckCheckedConesLeft[i][0] 170 | tangential = p3-p1 171 | tangential = (1/np.linalg.norm(tangential)) * tangential 172 | projectionDistance = 4 173 | normal = np.array([-tangential[1], tangential[0], 0]) 174 | normal = projectionDistance * normal 175 | closestInd = self.getClosestInd(p2 + normal, self.lanesConnectionRight) 176 | p4 = toBeckCheckedConesLeft[(closestInd-1) % len(toBeckCheckedConesLeft)][0] 177 | p5 = toBeckCheckedConesLeft[(closestInd) % len(toBeckCheckedConesLeft)][0] 178 | p6 = toBeckCheckedConesLeft[(closestInd+1) % len(toBeckCheckedConesLeft)][0] 179 | minRadius = min(minRadius, max(1/self.getCurvature(p1[0:2],p2[0:2],p3[0:2]), 1/self.getCurvature(p4[0:2],p5[0:2],p6[0:2]))) 180 | return minRadius 181 | 182 | def readMapFile(self, path): 183 | self.cones = [] 184 | self.coneColorMap = {} 185 | self.lanesConnectionLeft = [] 186 | self.lanesConnectionRight = [] 187 | self.timeKeepingGates = [] 188 | conesUnknown, left, right, tk, connected, start, earthToTrack = mapFile.readYaml(path) 189 | self.startPosition = start[0] 190 | self.startOrientation = start[1] 191 | self.originGeodeticCoordinates = earthToTrack[0] 192 | self.originENURotation = earthToTrack[1] 193 | for i in conesUnknown: 194 | self.cones.append([i[0], mapFile.stringToLandmarkType(i[1])]) 195 | for i in left: 196 | l = [i[0], mapFile.stringToLandmarkType(i[1])] 197 | self.cones.append(l) 198 | self.lanesConnectionLeft.append(l) 199 | for i in right: 200 | l = [i[0], mapFile.stringToLandmarkType(i[1])] 201 | self.cones.append(l) 202 | self.lanesConnectionRight.append(l) 203 | duringLine = False 204 | for i in tk: 205 | l = [i[0], mapFile.stringToLandmarkType(i[1])] 206 | self.cones.append(l) 207 | if(duringLine): 208 | self.timeKeepingGates[-1].append(l) 209 | duringLine = False 210 | else: 211 | self.timeKeepingGates.append([l]) 212 | duringLine = True 213 | if(len(self.lanesConnectionLeft) >= 2 and connected): 214 | self.lanesConnectionLeft.append(self.lanesConnectionLeft[0]) 215 | if(len(self.lanesConnectionRight) >= 2 and connected): 216 | self.lanesConnectionRight.append(self.lanesConnectionRight[0]) 217 | 218 | def drawCones(self): 219 | self.graphicsView.initFromLoad() 220 | # for c in self.cones: 221 | # self.graphicsView.addCone(c) 222 | def writeMapFile(self, path): 223 | conesUnknown = [] 224 | for i in self.cones: 225 | good = True 226 | for j in self.lanesConnectionLeft: 227 | good = good and (i is not j) 228 | for j in self.lanesConnectionRight: 229 | good = good and (i is not j) 230 | good = good and (i[1] != landmarkType.TIMEKEEPING) 231 | if(good): 232 | conesUnknown.append(i) 233 | timeKeeping = [] 234 | for i in self.timeKeepingGates: 235 | timeKeeping.append(i[0]) 236 | timeKeeping.append(i[1]) 237 | mapFile.writeYaml(path, conesUnknown, self.lanesConnectionLeft[:-1], self.lanesConnectionRight[:-1], timeKeeping, [self.startPosition, self.startOrientation], [self.originGeodeticCoordinates, self.originENURotation]) -------------------------------------------------------------------------------- /track_editor/icons/compass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/compass.png -------------------------------------------------------------------------------- /track_editor/icons/coneBigOrange.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneBigOrange.png -------------------------------------------------------------------------------- /track_editor/icons/coneBlue.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneBlue.png -------------------------------------------------------------------------------- /track_editor/icons/coneGreen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneGreen.png -------------------------------------------------------------------------------- /track_editor/icons/coneInvisible.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneInvisible.png -------------------------------------------------------------------------------- /track_editor/icons/coneOrange.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneOrange.png -------------------------------------------------------------------------------- /track_editor/icons/coneUnknown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneUnknown.png -------------------------------------------------------------------------------- /track_editor/icons/coneYellow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/coneYellow.png -------------------------------------------------------------------------------- /track_editor/icons/gnss.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/gnss.png -------------------------------------------------------------------------------- /track_editor/icons/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/icon.png -------------------------------------------------------------------------------- /track_editor/icons/laneConnectIcon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/laneConnectIcon.png -------------------------------------------------------------------------------- /track_editor/icons/rulesIcon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/rulesIcon.png -------------------------------------------------------------------------------- /track_editor/icons/start.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/start.png -------------------------------------------------------------------------------- /track_editor/icons/timeKeeping.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/track_editor/icons/timeKeeping.png -------------------------------------------------------------------------------- /track_editor/mapFile.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import YAML 2 | from ruamel.yaml.comments import CommentedMap as OrderedDict 3 | from pathlib import Path 4 | import numpy as np 5 | 6 | from guiLogic import landmarkType 7 | 8 | class My_Yaml_Dump: 9 | def __init__(self, f) -> None: 10 | self.f = f 11 | 12 | 13 | def write(self, s): 14 | self.f.write(self.__clean_yaml(s.decode('utf-8'))) 15 | 16 | 17 | def __clean_yaml(self, yaml_file: str) -> str: 18 | yaml_file = yaml_file.replace("'", "") 19 | return yaml_file 20 | 21 | def stringToLandmarkType(string): 22 | if(string == "blue"): 23 | return landmarkType.BLUE 24 | elif(string == "yellow"): 25 | return landmarkType.YELLOW 26 | elif(string == "small-orange"): 27 | return landmarkType.ORANGE 28 | elif(string == "big-orange"): 29 | return landmarkType.BIG_ORANGE 30 | elif(string == "timekeeping"): 31 | return landmarkType.TIMEKEEPING 32 | elif(string == "invisible"): 33 | return landmarkType.INVISIBLE 34 | 35 | return landmarkType.UNDEFINED 36 | 37 | def landmarkTypeToString(type): 38 | if(type == landmarkType.BLUE): 39 | return "blue" 40 | elif(type == landmarkType.YELLOW): 41 | return "yellow" 42 | elif(type == landmarkType.ORANGE): 43 | return "small-orange" 44 | elif(type == landmarkType.BIG_ORANGE): 45 | return "big-orange" 46 | elif(type == landmarkType.TIMEKEEPING): 47 | return "timekeeping" 48 | elif(type == landmarkType.INVISIBLE): 49 | return "invisible" 50 | return "unknown" 51 | 52 | def writeYaml(fileName, cones, leftLane, rightLane, timeKeeping, startPose, earthToTrack): 53 | path = Path(fileName) 54 | start_position = startPose[0] 55 | start_orientation = startPose[1] 56 | originGeodeticCoordinates = earthToTrack[0] 57 | originENURotation = earthToTrack[1] 58 | 59 | left = [] 60 | right = [] 61 | time_keeping = [] 62 | unknown = [] 63 | for c in cones: 64 | unknown.append({"position": f"[{c[0][0]}, {c[0][1]}, {c[0][2]}]", "class": landmarkTypeToString(c[1])}) 65 | for c in leftLane: 66 | left.append({"position": f"[{c[0][0]}, {c[0][1]}, {c[0][2]}]", "class": landmarkTypeToString(c[1])}) 67 | for c in rightLane: 68 | right.append({"position": f"[{c[0][0]}, {c[0][1]}, {c[0][2]}]", "class": landmarkTypeToString(c[1])}) 69 | for c in timeKeeping: 70 | time_keeping.append({"position": f"[{c[0][0]}, {c[0][1]}, {c[0][2]}]", "class": landmarkTypeToString(c[1])}) 71 | 72 | version_number = 1.0 73 | yaml_dict = OrderedDict({ 74 | "track": OrderedDict({ 75 | "version": str(version_number), 76 | "lanesFirstWithLastConnected" : True, 77 | "start": OrderedDict({ 78 | "position": f'[{start_position[0]}, {start_position[1]}, {start_position[2]}]', 79 | "orientation": f'[{start_orientation[0]}, {start_orientation[1]}, {start_orientation[2]}]'}), 80 | "earthToTrack": OrderedDict({ 81 | "position": f'[{originGeodeticCoordinates[0]}, {originGeodeticCoordinates[1]}, {originGeodeticCoordinates[2]}]', 82 | "orientation": f'[{originENURotation[0]}, {originENURotation[1]}, {originENURotation[2]}]'}), 83 | "left": left, 84 | "right": right, 85 | "time_keeping": time_keeping, 86 | "unknown": unknown 87 | }) 88 | }) 89 | with open(path, 'w+') as f: 90 | yaml_dumper = My_Yaml_Dump(f) 91 | yaml = YAML() 92 | yaml.dump(yaml_dict, yaml_dumper) 93 | return 94 | 95 | 96 | def readYaml(fileName): 97 | path = Path(fileName) 98 | 99 | yaml=YAML(typ='safe') # default, if not specfied, is 'rt' (round-trip) 100 | data = yaml.load(path) 101 | 102 | unkownCones = [] 103 | leftCones = [] 104 | rightCones = [] 105 | timekeeping = [] 106 | lanesFirstWithLastConnected = False 107 | startPose = [np.zeros(3), np.zeros(3)] 108 | earthToTrack = [np.zeros(3), np.zeros(3)] 109 | if('lanesFirstWithLastConnected') in data['track']: 110 | lanesFirstWithLastConnected = bool(data['track']['lanesFirstWithLastConnected']) 111 | if('start') in data['track']: 112 | startPose = [np.array(data['track']['start']['position']), np.array(data['track']['start']['orientation'])] 113 | if('earthToTrack') in data['track']: 114 | earthToTrack = [np.array(data['track']['earthToTrack']['position']), np.array(data['track']['earthToTrack']['orientation'])] 115 | for c in data['track']['left']: 116 | leftCones.append([np.array(c['position']), c['class']]) 117 | for c in data['track']['right']: 118 | rightCones.append([np.array(c['position']), c['class']]) 119 | for c in data['track']['time_keeping']: 120 | # overwrite class as timekeeping 121 | timekeeping.append([np.array(c['position']), landmarkTypeToString(landmarkType.TIMEKEEPING)]) 122 | for c in data['track']['unknown']: 123 | unkownCones.append([np.array(c['position']), c['class']]) 124 | return (unkownCones, leftCones, rightCones, timekeeping, lanesFirstWithLastConnected, startPose, earthToTrack) -------------------------------------------------------------------------------- /tracks/FSE23.yaml: -------------------------------------------------------------------------------- 1 | track: 2 | version: 1.0 3 | lanesFirstWithLastConnected: true 4 | start: 5 | position: [0.0, 0.0, 0.0] 6 | orientation: [0.0, 0.0, 0.0] 7 | earthToTrack: 8 | position: [0.0, 0.0, 0.0] 9 | orientation: [0.0, 0.0, 0.0] 10 | left: 11 | - position: [1.9610483157835772, 1.753462286312285, 0.0] 12 | class: blue 13 | - position: [5.03042808984456, 1.7474650661543938, 0.0] 14 | class: blue 15 | - position: [7.791714004070248, 1.4251214414027094, 0.0] 16 | class: blue 17 | - position: [9.777461737464256, 0.9400828816920224, 0.0] 18 | class: blue 19 | - position: [11.46396017103813, 0.20644971619447988, 0.0] 20 | class: blue 21 | - position: [14.228622789420754, -1.201110319044933, 0.0] 22 | class: blue 23 | - position: [16.87439957624675, -3.169680729405141, 0.0] 24 | class: blue 25 | - position: [18.995142190769045, -4.939564882351541, 0.0] 26 | class: blue 27 | - position: [20.99007041789345, -7.145358226752824, 0.0] 28 | class: blue 29 | - position: [23.83696004528599, -9.528512107624307, 0.0] 30 | class: blue 31 | - position: [25.114034313777733, -11.753260485513433, 0.0] 32 | class: blue 33 | - position: [26.545271218790678, -14.132020407319724, 0.0] 34 | class: blue 35 | - position: [27.106723521975642, -15.839372589470834, 0.0] 36 | class: blue 37 | - position: [27.402068372702317, -17.753571751289176, 0.0] 38 | class: blue 39 | - position: [27.857847995693735, -20.181398470478875, 0.0] 40 | class: blue 41 | - position: [27.40394171153798, -22.194402011068103, 0.0] 42 | class: blue 43 | - position: [25.91576928807018, -24.20791596804214, 0.0] 44 | class: blue 45 | - position: [24.34510911829934, -27.151981026406236, 0.0] 46 | class: blue 47 | - position: [23.430675718390084, -29.17353427826256, 0.0] 48 | class: blue 49 | - position: [23.615023051088336, -30.705749725047987, 0.0] 50 | class: blue 51 | - position: [24.3195145336805, -32.45111966200983, 0.0] 52 | class: blue 53 | - position: [24.980189348275868, -34.33343518946341, 0.0] 54 | class: blue 55 | - position: [24.59182488494988, -36.81136198651006, 0.0] 56 | class: blue 57 | - position: [24.181638139855224, -39.70499518601308, 0.0] 58 | class: blue 59 | - position: [22.924198734451174, -42.14937653142305, 0.0] 60 | class: blue 61 | - position: [21.17146566954623, -44.05140714509664, 0.0] 62 | class: blue 63 | - position: [19.33025558350435, -45.583332117357806, 0.0] 64 | class: blue 65 | - position: [17.172763342860033, -46.7153979872394, 0.0] 66 | class: blue 67 | - position: [15.052169975145139, -46.24057031937459, 0.0] 68 | class: blue 69 | - position: [13.18359686111935, -44.93438679581112, 0.0] 70 | class: blue 71 | - position: [11.44330715856305, -43.233526063097415, 0.0] 72 | class: blue 73 | - position: [9.988913491066631, -40.223441866486205, 0.0] 74 | class: blue 75 | - position: [9.500430938201603, -38.128853488618276, 0.0] 76 | class: blue 77 | - position: [9.207088056781192, -35.56000670047156, 0.0] 78 | class: blue 79 | - position: [9.910600238534347, -32.74047053389987, 0.0] 80 | class: blue 81 | - position: [9.797787778154765, -30.28725257024429, 0.0] 82 | class: blue 83 | - position: [8.943679940058551, -28.14618691717061, 0.0] 84 | class: blue 85 | - position: [7.6078160553979615, -26.671237211127995, 0.0] 86 | class: blue 87 | - position: [5.837201686749673, -26.07586512174623, 0.0] 88 | class: blue 89 | - position: [3.918170403358497, -25.381030873996792, 0.0] 90 | class: blue 91 | - position: [2.067662906635385, -24.877818736989035, 0.0] 92 | class: blue 93 | - position: [0.001993642409683933, -24.06273650739956, 0.0] 94 | class: blue 95 | - position: [-1.9857967327924306, -23.594883185307328, 0.0] 96 | class: blue 97 | - position: [-3.9120233027820466, -24.726064574465358, 0.0] 98 | class: blue 99 | - position: [-4.90014097183734, -26.91022477839099, 0.0] 100 | class: blue 101 | - position: [-5.002793269847473, -28.844884061000865, 0.0] 102 | class: blue 103 | - position: [-4.901503355228827, -31.054791941815612, 0.0] 104 | class: blue 105 | - position: [-4.577127164075985, -33.47938080477881, 0.0] 106 | class: blue 107 | - position: [-4.2460194318394615, -35.61168878000241, 0.0] 108 | class: blue 109 | - position: [-3.4946748740929676, -38.52504340379936, 0.0] 110 | class: blue 111 | - position: [-2.656194691840528, -41.14412737996933, 0.0] 112 | class: blue 113 | - position: [-1.2132309402638024, -43.78325315413566, 0.0] 114 | class: blue 115 | - position: [1.2754763981451611, -45.72841846106134, 0.0] 116 | class: blue 117 | - position: [2.9493096966776373, -47.11955063838757, 0.0] 118 | class: blue 119 | - position: [5.033954245865441, -48.83115775576157, 0.0] 120 | class: blue 121 | - position: [6.962020138575707, -50.606757104697515, 0.0] 122 | class: blue 123 | - position: [8.878372210711806, -52.39938444640465, 0.0] 124 | class: blue 125 | - position: [10.282918351443598, -54.28776043565119, 0.0] 126 | class: blue 127 | - position: [11.575842706741266, -56.417703116172966, 0.0] 128 | class: blue 129 | - position: [13.25579161027749, -58.824794085836864, 0.0] 130 | class: blue 131 | - position: [14.609376852850978, -60.81715019500757, 0.0] 132 | class: blue 133 | - position: [15.5367147835531, -63.53509161878633, 0.0] 134 | class: blue 135 | - position: [15.148865550599428, -66.32498923364984, 0.0] 136 | class: blue 137 | - position: [13.468881229668625, -68.73869433795036, 0.0] 138 | class: blue 139 | - position: [10.838876004892061, -70.39062409387432, 0.0] 140 | class: blue 141 | - position: [7.8410266689316055, -70.8318442922509, 0.0] 142 | class: blue 143 | - position: [5.300603942995569, -70.86264472320318, 0.0] 144 | class: blue 145 | - position: [3.3012656993919247, -69.85799755740152, 0.0] 146 | class: blue 147 | - position: [0.912453731429374, -68.42064324380884, 0.0] 148 | class: blue 149 | - position: [-1.507814403146287, -66.52866564365375, 0.0] 150 | class: blue 151 | - position: [-3.4335660243115322, -64.00977499945833, 0.0] 152 | class: blue 153 | - position: [-5.519498468503959, -60.95514720448636, 0.0] 154 | class: blue 155 | - position: [-6.734117998832944, -57.96580209245997, 0.0] 156 | class: blue 157 | - position: [-8.231808474381625, -54.99816522388418, 0.0] 158 | class: blue 159 | - position: [-9.778788023458349, -51.153333871212475, 0.0] 160 | class: blue 161 | - position: [-10.79606949109355, -47.44332889458991, 0.0] 162 | class: blue 163 | - position: [-12.041760953860493, -43.950126849536005, 0.0] 164 | class: blue 165 | - position: [-12.846781770815118, -41.379350240963305, 0.0] 166 | class: blue 167 | - position: [-13.677152601946819, -38.53184693785736, 0.0] 168 | class: blue 169 | - position: [-14.572035799236371, -35.50811953999034, 0.0] 170 | class: blue 171 | - position: [-14.82459152271503, -32.56087675725843, 0.0] 172 | class: blue 173 | - position: [-14.930925084184397, -29.48851580730295, 0.0] 174 | class: blue 175 | - position: [-14.489409074857344, -25.58000390672313, 0.0] 176 | class: blue 177 | - position: [-14.07358799168626, -22.357342597647374, 0.0] 178 | class: blue 179 | - position: [-13.743351311127611, -19.64548077897253, 0.0] 180 | class: blue 181 | - position: [-13.169985554569962, -16.586643860590712, 0.0] 182 | class: blue 183 | - position: [-12.4424857533384, -12.973797669657403, 0.0] 184 | class: blue 185 | - position: [-11.535111960592536, -9.577588711034538, 0.0] 186 | class: blue 187 | - position: [-9.950912080003368, -6.522225349407442, 0.0] 188 | class: blue 189 | - position: [-8.55187783190489, -3.8752749853979442, 0.0] 190 | class: blue 191 | - position: [-6.737104681414069, -1.3578608973297865, 0.0] 192 | class: blue 193 | - position: [-5.615193873106133, -0.14356096046024988, 0.0] 194 | class: blue 195 | - position: [-3.596536075691789, 0.7569016200221292, 0.0] 196 | class: blue 197 | - position: [-1.3018901281661337, 1.7857410176639754, 0.0] 198 | class: blue 199 | right: 200 | - position: [2.184410468444179, -1.6229612739277601, 0.0] 201 | class: yellow 202 | - position: [5.049054393773737, -1.7959728803950241, 0.0] 203 | class: yellow 204 | - position: [6.831708808902756, -2.2440261290276053, 0.0] 205 | class: yellow 206 | - position: [8.61940343445219, -2.7044464914287887, 0.0] 207 | class: yellow 208 | - position: [10.721547393754689, -3.2808502590007107, 0.0] 209 | class: yellow 210 | - position: [12.631145959950736, -4.562231119471902, 0.0] 211 | class: yellow 212 | - position: [15.252820757628726, -6.585066549434389, 0.0] 213 | class: yellow 214 | - position: [17.562538545383074, -8.655121370698007, 0.0] 215 | class: yellow 216 | - position: [20.198308316910854, -10.996026777794778, 0.0] 217 | class: yellow 218 | - position: [21.66069325812194, -13.05799431558735, 0.0] 219 | class: yellow 220 | - position: [22.7165942861715, -14.76487432853634, 0.0] 221 | class: yellow 222 | - position: [23.60772554311104, -16.4049606246555, 0.0] 223 | class: yellow 224 | - position: [24.237325866227195, -19.145790272719914, 0.0] 225 | class: yellow 226 | - position: [24.194605232825065, -20.78200846108214, 0.0] 227 | class: yellow 228 | - position: [23.005295289862403, -22.553468683560286, 0.0] 229 | class: yellow 230 | - position: [21.696612305115305, -24.212683735632712, 0.0] 231 | class: yellow 232 | - position: [20.37388614239442, -25.78627780747282, 0.0] 233 | class: yellow 234 | - position: [19.786910293589273, -28.00014026259458, 0.0] 235 | class: yellow 236 | - position: [19.644789281036857, -30.287158656458494, 0.0] 237 | class: yellow 238 | - position: [20.350061278499933, -32.640674115664545, 0.0] 239 | class: yellow 240 | - position: [20.896601646684374, -34.527728606603326, 0.0] 241 | class: yellow 242 | - position: [21.287641837220587, -36.28456091825568, 0.0] 243 | class: yellow 244 | - position: [21.384157167081003, -38.11261901913037, 0.0] 245 | class: yellow 246 | - position: [20.255138849940195, -40.37348125356171, 0.0] 247 | class: yellow 248 | - position: [18.289466615434584, -41.623524929015616, 0.0] 249 | class: yellow 250 | - position: [16.660102217452522, -42.35139921023516, 0.0] 251 | class: yellow 252 | - position: [15.262523163431258, -41.671532991777525, 0.0] 253 | class: yellow 254 | - position: [14.463401795811844, -39.69579864751497, 0.0] 255 | class: yellow 256 | - position: [13.433822064685522, -37.73851609955273, 0.0] 257 | class: yellow 258 | - position: [13.410722191790194, -35.382202725392965, 0.0] 259 | class: yellow 260 | - position: [13.553414948549102, -33.28569596328318, 0.0] 261 | class: yellow 262 | - position: [13.568506903404094, -30.88598193515851, 0.0] 263 | class: yellow 264 | - position: [13.406534925900313, -28.272135413846673, 0.0] 265 | class: yellow 266 | - position: [12.55549337591994, -26.14026661774122, 0.0] 267 | class: yellow 268 | - position: [10.865683204994152, -24.358690743179118, 0.0] 269 | class: yellow 270 | - position: [8.577667716751456, -22.95848706910739, 0.0] 271 | class: yellow 272 | - position: [6.2713104696584, -22.11726750040792, 0.0] 273 | class: yellow 274 | - position: [3.5917915890689986, -21.542125482547526, 0.0] 275 | class: yellow 276 | - position: [0.9785349498096743, -20.642309910515134, 0.0] 277 | class: yellow 278 | - position: [-1.3836050393008252, -20.105664846474454, 0.0] 279 | class: yellow 280 | - position: [-3.877445163408817, -20.410529807429697, 0.0] 281 | class: yellow 282 | - position: [-5.6943931121891636, -21.17962280389125, 0.0] 283 | class: yellow 284 | - position: [-6.8746810195633845, -21.904832562433086, 0.0] 285 | class: yellow 286 | - position: [-7.885376794345155, -23.057566747820992, 0.0] 287 | class: yellow 288 | - position: [-8.465016026136928, -24.555791294865017, 0.0] 289 | class: yellow 290 | - position: [-9.306195193967946, -26.491361695641125, 0.0] 291 | class: yellow 292 | - position: [-9.387313670612917, -28.302357908827638, 0.0] 293 | class: yellow 294 | - position: [-8.807547828115665, -29.770881093608494, 0.0] 295 | class: yellow 296 | - position: [-8.3642294370141, -31.998068725234454, 0.0] 297 | class: yellow 298 | - position: [-8.009760316795097, -33.91795259218898, 0.0] 299 | class: yellow 300 | - position: [-7.522555363626319, -36.84945956265123, 0.0] 301 | class: yellow 302 | - position: [-7.167504046924366, -40.091709697918134, 0.0] 303 | class: yellow 304 | - position: [-6.166185167864617, -42.933101149351515, 0.0] 305 | class: yellow 306 | - position: [-4.688182993448119, -45.322507559345105, 0.0] 307 | class: yellow 308 | - position: [-2.936006224550054, -47.44082080294637, 0.0] 309 | class: yellow 310 | - position: [-0.8209227202707524, -49.12741166393315, 0.0] 311 | class: yellow 312 | - position: [1.5023807528155293, -50.89489897504319, 0.0] 313 | class: yellow 314 | - position: [3.561859833519728, -52.198556108134554, 0.0] 315 | class: yellow 316 | - position: [5.100735582475625, -53.51997675180324, 0.0] 317 | class: yellow 318 | - position: [6.5345837826287125, -55.59023871251995, 0.0] 319 | class: yellow 320 | - position: [8.207891789837623, -57.88998750197898, 0.0] 321 | class: yellow 322 | - position: [9.913087294346033, -60.23577187224778, 0.0] 323 | class: yellow 324 | - position: [10.669593590392372, -62.28050276200009, 0.0] 325 | class: yellow 326 | - position: [11.082227596615317, -64.38809979929282, 0.0] 327 | class: yellow 328 | - position: [9.761478305498555, -65.66703161394851, 0.0] 329 | class: yellow 330 | - position: [7.170263156685366, -66.20942625308403, 0.0] 331 | class: yellow 332 | - position: [4.851235441396216, -66.06447554958784, 0.0] 333 | class: yellow 334 | - position: [3.458186706019677, -65.65383457071198, 0.0] 335 | class: yellow 336 | - position: [2.126119233739105, -64.79188308604944, 0.0] 337 | class: yellow 338 | - position: [0.7545248607929422, -63.32581572989945, 0.0] 339 | class: yellow 340 | - position: [-1.1064457178805611, -61.02872097275504, 0.0] 341 | class: yellow 342 | - position: [-2.3122887220417376, -59.17751035048029, 0.0] 343 | class: yellow 344 | - position: [-3.6684704408051525, -56.5655546228098, 0.0] 345 | class: yellow 346 | - position: [-5.020179418482198, -53.12004561518135, 0.0] 347 | class: yellow 348 | - position: [-6.532825009587925, -49.53562442678227, 0.0] 349 | class: yellow 350 | - position: [-7.766667893161621, -46.136579617688554, 0.0] 351 | class: yellow 352 | - position: [-8.775453780988022, -42.832403649198746, 0.0] 353 | class: yellow 354 | - position: [-9.682808366996952, -40.376752911934325, 0.0] 355 | class: yellow 356 | - position: [-10.336490033121487, -37.885554014790046, 0.0] 357 | class: yellow 358 | - position: [-11.096236719473394, -35.61938003303045, 0.0] 359 | class: yellow 360 | - position: [-11.49234945751806, -33.305413158611195, 0.0] 361 | class: yellow 362 | - position: [-11.489975066641145, -31.053848080533406, 0.0] 363 | class: yellow 364 | - position: [-11.519087798090766, -28.671492757351725, 0.0] 365 | class: yellow 366 | - position: [-11.272368601249713, -25.8464740146728, 0.0] 367 | class: yellow 368 | - position: [-10.693950966282072, -22.784402748323494, 0.0] 369 | class: yellow 370 | - position: [-10.182601938355258, -20.37893334935046, 0.0] 371 | class: yellow 372 | - position: [-9.63142970474839, -17.734476666095812, 0.0] 373 | class: yellow 374 | - position: [-8.966975973495048, -14.071698442759592, 0.0] 375 | class: yellow 376 | - position: [-8.212927760291953, -10.942095853456872, 0.0] 377 | class: yellow 378 | - position: [-6.638395581419781, -7.921898762285395, 0.0] 379 | class: yellow 380 | - position: [-5.466172580873669, -5.5986941559002075, 0.0] 381 | class: yellow 382 | - position: [-2.4749796438005927, -2.8330511150433244, 0.0] 383 | class: yellow 384 | - position: [-0.21710897378571714, -1.9881192164868446, 0.0] 385 | class: yellow 386 | time_keeping: 387 | - position: [10.170133113064752, 7.015054344337894, 0.0] 388 | class: timekeeping 389 | - position: [6.00111846509, -7.842651141276803, 0.0] 390 | class: timekeeping 391 | unknown: [] 392 | -------------------------------------------------------------------------------- /tracks/FSG23.yaml: -------------------------------------------------------------------------------- 1 | track: 2 | version: 1.0 3 | lanesFirstWithLastConnected: true 4 | start: 5 | position: [0.0, 0.0, 0.0] 6 | orientation: [0.0, 0.0, 0.0] 7 | earthToTrack: 8 | position: [0.0, 0.0, 0.0] 9 | orientation: [0.0, 0.0, 0.0] 10 | left: 11 | - position: [0.5082596448953368, 1.5486736081160606, 0.0] 12 | class: blue 13 | - position: [3.779953956604004, 1.5232335329055786, 0.0] 14 | class: blue 15 | - position: [7.222487449645996, 1.3082525730133057, 0.0] 16 | class: blue 17 | - position: [10.677363395690918, 1.1687562465667725, 0.0] 18 | class: blue 19 | - position: [14.327939987182617, 1.0138460397720337, 0.0] 20 | class: blue 21 | - position: [17.719303131103516, 0.9198496341705322, 0.0] 22 | class: blue 23 | - position: [21.169240951538086, 0.5079307556152344, 0.0] 24 | class: blue 25 | - position: [24.603490829467773, 0.05472254753112793, 0.0] 26 | class: blue 27 | - position: [27.98530387878418, -0.8069002628326416, 0.0] 28 | class: blue 29 | - position: [31.158859252929688, -1.889723777770996, 0.0] 30 | class: blue 31 | - position: [34.4276123046875, -3.1944663524627686, 0.0] 32 | class: blue 33 | - position: [37.23306655883789, -5.150421619415283, 0.0] 34 | class: blue 35 | - position: [39.226036071777344, -7.957284450531006, 0.0] 36 | class: blue 37 | - position: [40.22465133666992, -11.859920501708984, 0.0] 38 | class: blue 39 | - position: [40.59050369262695, -15.261568069458008, 0.0] 40 | class: blue 41 | - position: [39.628936767578125, -18.982858657836914, 0.0] 42 | class: blue 43 | - position: [37.180625915527344, -21.74787712097168, 0.0] 44 | class: blue 45 | - position: [34.399993896484375, -24.023616790771484, 0.0] 46 | class: blue 47 | - position: [31.811542510986328, -26.306177139282227, 0.0] 48 | class: blue 49 | - position: [28.903371810913086, -27.793670654296875, 0.0] 50 | class: blue 51 | - position: [25.55673599243164, -28.922487258911133, 0.0] 52 | class: blue 53 | - position: [22.007862091064453, -30.254629135131836, 0.0] 54 | class: blue 55 | - position: [18.580780029296875, -31.04642105102539, 0.0] 56 | class: blue 57 | - position: [14.980634689331055, -31.23284149169922, 0.0] 58 | class: blue 59 | - position: [11.363624572753906, -30.29991340637207, 0.0] 60 | class: blue 61 | - position: [7.736959934234619, -28.742589950561523, 0.0] 62 | class: blue 63 | - position: [4.45256233215332, -26.254575729370117, 0.0] 64 | class: blue 65 | - position: [1.8632380962371826, -22.8154296875, 0.0] 66 | class: blue 67 | - position: [-0.4406001567840576, -20.433135986328125, 0.0] 68 | class: blue 69 | - position: [-2.479065418243408, -18.485286712646484, 0.0] 70 | class: blue 71 | - position: [-4.914695739746094, -17.376089096069336, 0.0] 72 | class: blue 73 | - position: [-7.885650157928467, -17.09176254272461, 0.0] 74 | class: blue 75 | - position: [-10.243247032165527, -17.765798568725586, 0.0] 76 | class: blue 77 | - position: [-12.372162818908691, -19.01988983154297, 0.0] 78 | class: blue 79 | - position: [-14.106569290161133, -21.043447494506836, 0.0] 80 | class: blue 81 | - position: [-15.185487747192383, -23.673280715942383, 0.0] 82 | class: blue 83 | - position: [-15.157058715820312, -27.212068557739258, 0.0] 84 | class: blue 85 | - position: [-14.266414642333984, -30.794315338134766, 0.0] 86 | class: blue 87 | - position: [-15.632589340209961, -34.14773941040039, 0.0] 88 | class: blue 89 | - position: [-17.75958251953125, -36.45520782470703, 0.0] 90 | class: blue 91 | - position: [-18.876216888427734, -38.74409484863281, 0.0] 92 | class: blue 93 | - position: [-18.848867416381836, -40.76490020751953, 0.0] 94 | class: blue 95 | - position: [-18.25394058227539, -43.55034637451172, 0.0] 96 | class: blue 97 | - position: [-16.888845443725586, -45.89644241333008, 0.0] 98 | class: blue 99 | - position: [-15.344305038452148, -47.412105560302734, 0.0] 100 | class: blue 101 | - position: [-13.644372940063477, -48.664817810058594, 0.0] 102 | class: blue 103 | - position: [-10.90795612335205, -50.812068939208984, 0.0] 104 | class: blue 105 | - position: [-9.088140487670898, -53.94438171386719, 0.0] 106 | class: blue 107 | - position: [-8.358230590820312, -57.819740295410156, 0.0] 108 | class: blue 109 | - position: [-6.237361907958984, -60.79255676269531, 0.0] 110 | class: blue 111 | - position: [-3.292412042617798, -62.97040939331055, 0.0] 112 | class: blue 113 | - position: [-0.09450256824493408, -64.49473571777344, 0.0] 114 | class: blue 115 | - position: [2.909107208251953, -66.63183975219727, 0.0] 116 | class: blue 117 | - position: [5.0019636154174805, -69.27470397949219, 0.0] 118 | class: blue 119 | - position: [6.188177108764648, -72.87191772460938, 0.0] 120 | class: blue 121 | - position: [5.898281097412109, -76.27054595947266, 0.0] 122 | class: blue 123 | - position: [4.782037734985352, -79.67082214355469, 0.0] 124 | class: blue 125 | - position: [3.731675863265991, -81.4974594116211, 0.0] 126 | class: blue 127 | - position: [2.2604482173919678, -82.20571899414062, 0.0] 128 | class: blue 129 | - position: [0.8253939151763916, -81.93086242675781, 0.0] 130 | class: blue 131 | - position: [-1.2591516971588135, -81.11158752441406, 0.0] 132 | class: blue 133 | - position: [-4.885300636291504, -79.56458282470703, 0.0] 134 | class: blue 135 | - position: [-8.171649932861328, -78.18431091308594, 0.0] 136 | class: blue 137 | - position: [-11.54815673828125, -76.82373046875, 0.0] 138 | class: blue 139 | - position: [-14.884969711303711, -75.4636001586914, 0.0] 140 | class: blue 141 | - position: [-18.095428466796875, -74.0725326538086, 0.0] 142 | class: blue 143 | - position: [-21.293542861938477, -72.72721862792969, 0.0] 144 | class: blue 145 | - position: [-24.24964714050293, -70.84508514404297, 0.0] 146 | class: blue 147 | - position: [-26.30963706970215, -67.93836212158203, 0.0] 148 | class: blue 149 | - position: [-27.001171112060547, -64.35674285888672, 0.0] 150 | class: blue 151 | - position: [-26.13978385925293, -60.72052001953125, 0.0] 152 | class: blue 153 | - position: [-25.67031478881836, -57.11250686645508, 0.0] 154 | class: blue 155 | - position: [-26.128644943237305, -54.794132232666016, 0.0] 156 | class: blue 157 | - position: [-27.683691024780273, -52.44533157348633, 0.0] 158 | class: blue 159 | - position: [-29.94391441345215, -49.80458450317383, 0.0] 160 | class: blue 161 | - position: [-32.1567497253418, -47.23586654663086, 0.0] 162 | class: blue 163 | - position: [-34.23194885253906, -44.4019660949707, 0.0] 164 | class: blue 165 | - position: [-36.24302673339844, -41.6898078918457, 0.0] 166 | class: blue 167 | - position: [-38.020687103271484, -38.729270935058594, 0.0] 168 | class: blue 169 | - position: [-39.413291931152344, -35.7406120300293, 0.0] 170 | class: blue 171 | - position: [-41.01042175292969, -32.69443130493164, 0.0] 172 | class: blue 173 | - position: [-41.538272857666016, -28.80342674255371, 0.0] 174 | class: blue 175 | - position: [-41.45207977294922, -25.001962661743164, 0.0] 176 | class: blue 177 | - position: [-41.76758575439453, -21.425642013549805, 0.0] 178 | class: blue 179 | - position: [-42.14253234863281, -17.73838996887207, 0.0] 180 | class: blue 181 | - position: [-42.01902389526367, -14.200185775756836, 0.0] 182 | class: blue 183 | - position: [-40.756492614746094, -10.982475280761719, 0.0] 184 | class: blue 185 | - position: [-38.18800735473633, -9.414196014404297, 0.0] 186 | class: blue 187 | - position: [-34.36893081665039, -8.821358680725098, 0.0] 188 | class: blue 189 | - position: [-31.10109519958496, -7.7844390869140625, 0.0] 190 | class: blue 191 | - position: [-28.02092170715332, -6.371025085449219, 0.0] 192 | class: blue 193 | - position: [-25.11590576171875, -4.3730058670043945, 0.0] 194 | class: blue 195 | - position: [-22.56807518005371, -2.1307761669158936, 0.0] 196 | class: blue 197 | - position: [-19.895030975341797, 0.1059110164642334, 0.0] 198 | class: blue 199 | - position: [-16.632410049438477, 1.26746666431427, 0.0] 200 | class: blue 201 | - position: [-13.243362426757812, 1.5282526016235352, 0.0] 202 | class: blue 203 | - position: [-9.776121139526367, 1.5410120487213135, 0.0] 204 | class: blue 205 | - position: [-6.512820243835449, 1.5252960920333862, 0.0] 206 | class: blue 207 | - position: [-3.2676221920102795, 1.5018560238406589, 0.0] 208 | class: blue 209 | right: 210 | - position: [0.31971222162246704, -1.6262177228927612, 0.0] 211 | class: yellow 212 | - position: [3.6774771213531494, -1.734877586364746, 0.0] 213 | class: yellow 214 | - position: [7.220338821411133, -1.921045184135437, 0.0] 215 | class: yellow 216 | - position: [10.409687042236328, -2.186640739440918, 0.0] 217 | class: yellow 218 | - position: [14.057313919067383, -2.2234930992126465, 0.0] 219 | class: yellow 220 | - position: [17.37313461303711, -2.3453450202941895, 0.0] 221 | class: yellow 222 | - position: [20.9266300201416, -2.7789201736450195, 0.0] 223 | class: yellow 224 | - position: [24.07574462890625, -3.202238082885742, 0.0] 225 | class: yellow 226 | - position: [26.629405975341797, -3.8352978229522705, 0.0] 227 | class: yellow 228 | - position: [29.951948165893555, -4.936380386352539, 0.0] 229 | class: yellow 230 | - position: [32.70099639892578, -6.0220208168029785, 0.0] 231 | class: yellow 232 | - position: [34.5917854309082, -7.231252193450928, 0.0] 233 | class: yellow 234 | - position: [36.14643096923828, -9.55404281616211, 0.0] 235 | class: yellow 236 | - position: [37.08872985839844, -12.288402557373047, 0.0] 237 | class: yellow 238 | - position: [37.31753921508789, -14.82280445098877, 0.0] 239 | class: yellow 240 | - position: [36.81779861450195, -17.402334213256836, 0.0] 241 | class: yellow 242 | - position: [35.145103454589844, -19.2021427154541, 0.0] 243 | class: yellow 244 | - position: [32.55773162841797, -21.374494552612305, 0.0] 245 | class: yellow 246 | - position: [30.26765251159668, -23.440370559692383, 0.0] 247 | class: yellow 248 | - position: [27.52208709716797, -24.83510971069336, 0.0] 249 | class: yellow 250 | - position: [24.456762313842773, -25.892793655395508, 0.0] 251 | class: yellow 252 | - position: [21.026155471801758, -27.06560707092285, 0.0] 253 | class: yellow 254 | - position: [17.806028366088867, -27.798667907714844, 0.0] 255 | class: yellow 256 | - position: [15.116643905639648, -27.836936950683594, 0.0] 257 | class: yellow 258 | - position: [12.404610633850098, -27.110612869262695, 0.0] 259 | class: yellow 260 | - position: [9.191972732543945, -25.7758846282959, 0.0] 261 | class: yellow 262 | - position: [6.767352104187012, -23.744890213012695, 0.0] 263 | class: yellow 264 | - position: [4.34480619430542, -20.59305763244629, 0.0] 265 | class: yellow 266 | - position: [1.9860754013061523, -18.10840606689453, 0.0] 267 | class: yellow 268 | - position: [-0.6295260190963745, -15.639873504638672, 0.0] 269 | class: yellow 270 | - position: [-4.128792762756348, -14.048778533935547, 0.0] 271 | class: yellow 272 | - position: [-8.169021606445312, -13.778464317321777, 0.0] 273 | class: yellow 274 | - position: [-11.706680297851562, -14.684381484985352, 0.0] 275 | class: yellow 276 | - position: [-14.800485610961914, -16.68552589416504, 0.0] 277 | class: yellow 278 | - position: [-17.11359405517578, -19.486133575439453, 0.0] 279 | class: yellow 280 | - position: [-18.39120101928711, -22.8818359375, 0.0] 281 | class: yellow 282 | - position: [-18.696025848388672, -26.431020736694336, 0.0] 283 | class: yellow 284 | - position: [-18.559310913085938, -29.382362365722656, 0.0] 285 | class: yellow 286 | - position: [-18.950027465820312, -32.0696907043457, 0.0] 287 | class: yellow 288 | - position: [-20.34339714050293, -34.43321990966797, 0.0] 289 | class: yellow 290 | - position: [-22.019058227539062, -37.61231231689453, 0.0] 291 | class: yellow 292 | - position: [-22.088626861572266, -40.99843215942383, 0.0] 293 | class: yellow 294 | - position: [-21.455047607421875, -44.586856842041016, 0.0] 295 | class: yellow 296 | - position: [-19.47408676147461, -47.839927673339844, 0.0] 297 | class: yellow 298 | - position: [-17.28863525390625, -50.07672882080078, 0.0] 299 | class: yellow 300 | - position: [-14.576955795288086, -51.869606018066406, 0.0] 301 | class: yellow 302 | - position: [-13.059326171875, -53.30036544799805, 0.0] 303 | class: yellow 304 | - position: [-12.098737716674805, -55.38004684448242, 0.0] 305 | class: yellow 306 | - position: [-11.552937507629395, -59.02338790893555, 0.0] 307 | class: yellow 308 | - position: [-10.163169860839844, -61.697444915771484, 0.0] 309 | class: yellow 310 | - position: [-7.9956746101379395, -63.655574798583984, 0.0] 311 | class: yellow 312 | - position: [-4.800147533416748, -65.9620361328125, 0.0] 313 | class: yellow 314 | - position: [-1.497298002243042, -67.50564575195312, 0.0] 315 | class: yellow 316 | - position: [1.495, -70.4, 0.0] 317 | class: yellow 318 | - position: [2.9125945568084717, -73.07673645019531, 0.0] 319 | class: yellow 320 | - position: [2.7051756381988525, -75.51410675048828, 0.0] 321 | class: yellow 322 | - position: [1.9787471294403076, -77.7643814086914, 0.0] 323 | class: yellow 324 | - position: [-0.15898358821868896, -77.94950866699219, 0.0] 325 | class: yellow 326 | - position: [-3.4663240909576416, -76.58625793457031, 0.0] 327 | class: yellow 328 | - position: [-7.043816089630127, -75.12608337402344, 0.0] 329 | class: yellow 330 | - position: [-10.565814971923828, -73.6869125366211, 0.0] 331 | class: yellow 332 | - position: [-13.552702903747559, -72.44856262207031, 0.0] 333 | class: yellow 334 | - position: [-16.79291534423828, -71.0011978149414, 0.0] 335 | class: yellow 336 | - position: [-20.295738220214844, -69.63163757324219, 0.0] 337 | class: yellow 338 | - position: [-22.0855655670166, -68.4044189453125, 0.0] 339 | class: yellow 340 | - position: [-23.336034774780273, -66.72842407226562, 0.0] 341 | class: yellow 342 | - position: [-23.729175567626953, -64.31339263916016, 0.0] 343 | class: yellow 344 | - position: [-22.916730880737305, -61.07143783569336, 0.0] 345 | class: yellow 346 | - position: [-22.44605255126953, -57.2049674987793, 0.0] 347 | class: yellow 348 | - position: [-23.1043701171875, -53.721031188964844, 0.0] 349 | class: yellow 350 | - position: [-25.01572036743164, -50.315853118896484, 0.0] 351 | class: yellow 352 | - position: [-27.435476303100586, -47.787654876708984, 0.0] 353 | class: yellow 354 | - position: [-29.560487747192383, -45.15900802612305, 0.0] 355 | class: yellow 356 | - position: [-31.649559020996094, -42.468772888183594, 0.0] 357 | class: yellow 358 | - position: [-33.51590347290039, -39.883792877197266, 0.0] 359 | class: yellow 360 | - position: [-35.07546615600586, -37.15678405761719, 0.0] 361 | class: yellow 362 | - position: [-36.63813400268555, -34.057193756103516, 0.0] 363 | class: yellow 364 | - position: [-38.157657623291016, -31.31756591796875, 0.0] 365 | class: yellow 366 | - position: [-38.22207260131836, -28.503005981445312, 0.0] 367 | class: yellow 368 | - position: [-38.202213287353516, -24.828392028808594, 0.0] 369 | class: yellow 370 | - position: [-38.573394775390625, -20.722129821777344, 0.0] 371 | class: yellow 372 | - position: [-38.898555755615234, -17.29265594482422, 0.0] 373 | class: yellow 374 | - position: [-38.84148406982422, -14.723673820495605, 0.0] 375 | class: yellow 376 | - position: [-37.15932846069336, -12.620010375976562, 0.0] 377 | class: yellow 378 | - position: [-33.61030197143555, -12.04580020904541, 0.0] 379 | class: yellow 380 | - position: [-30.042089462280273, -10.833395004272461, 0.0] 381 | class: yellow 382 | - position: [-26.53583335876465, -9.302395820617676, 0.0] 383 | class: yellow 384 | - position: [-23.4637508392334, -7.1326904296875, 0.0] 385 | class: yellow 386 | - position: [-20.711103439331055, -4.837130069732666, 0.0] 387 | class: yellow 388 | - position: [-18.360551834106445, -2.789964437484741, 0.0] 389 | class: yellow 390 | - position: [-15.828241348266602, -1.9053760766983032, 0.0] 391 | class: yellow 392 | - position: [-13.174610137939453, -1.822721242904663, 0.0] 393 | class: yellow 394 | - position: [-9.732795715332031, -1.6356852054595947, 0.0] 395 | class: yellow 396 | - position: [-6.648148536682129, -1.7762550115585327, 0.0] 397 | class: yellow 398 | - position: [-3.2753477096557617, -1.7198089363813815, 0.0] 399 | class: yellow 400 | time_keeping: 401 | - position: [7.68, 4.0, 0.0] 402 | class: timekeeping 403 | - position: [7.68, -4.0, 0.0] 404 | class: timekeeping 405 | unknown: [] 406 | -------------------------------------------------------------------------------- /tracks/FSS22_V1.yaml: -------------------------------------------------------------------------------- 1 | track: 2 | version: 1.0 3 | lanesFirstWithLastConnected: true 4 | start: 5 | position: [0.0, 0.0, 0.0] 6 | orientation: [0.0, 0.0, 0.0] 7 | earthToTrack: 8 | position: [0.0, 0.0, 0.0] 9 | orientation: [0.0, 0.0, 0.0] 10 | left: [] 11 | right: [] 12 | time_keeping: 13 | - position: [7.90220550255071, -5.605370685200331, 0.0] 14 | class: timekeeping 15 | - position: [8.038921860726328, 4.210863831809029, 0.0] 16 | class: timekeeping 17 | unknown: 18 | - position: [7.552220644789528, -1.6354006500188585, 0.0] 19 | class: yellow 20 | - position: [7.665249387830143, 1.7371613596786928, 0.0] 21 | class: blue 22 | - position: [29.27377784043975, 1.7874630949487442, 0.0] 23 | class: blue 24 | - position: [21.47530106463788, -2.5375389180402363, 0.0] 25 | class: yellow 26 | - position: [12.838859358037727, -1.6718470784033046, 0.0] 27 | class: yellow 28 | - position: [10.462602706557417, 5.48108474649205, 0.0] 29 | class: blue 30 | - position: [12.79048701747968, 1.72390488898489, 0.0] 31 | class: blue 32 | - position: [17.721605404968166, 1.6137080814783313, 0.0] 33 | class: blue 34 | - position: [24.11042612194338, 0.3608379928346944, 0.0] 35 | class: blue 36 | - position: [21.661961490256783, 0.6159365182378186, 0.0] 37 | class: blue 38 | - position: [10.280773592849433, 9.058413775926205, 0.0] 39 | class: yellow 40 | - position: [26.339608602672282, 0.2279441811782933, 0.0] 41 | class: blue 42 | - position: [23.18320763174529, 22.962634792426584, 0.0] 43 | class: unknown 44 | - position: [20.284847721877814, 8.608638700292808, 0.0] 45 | class: yellow 46 | - position: [17.73425088387202, -1.8908358884555763, 0.0] 47 | class: yellow 48 | - position: [27.04204442594466, 4.518128726052922, 0.0] 49 | class: blue 50 | - position: [28.757136417036893, 4.01573248137712, 0.0] 51 | class: blue 52 | - position: [24.640569541246286, 4.656263337197634, 0.0] 53 | class: blue 54 | - position: [15.319172917137559, 8.64272949763001, 0.0] 55 | class: yellow 56 | - position: [20.207444785252612, 4.813144901027767, 0.0] 57 | class: blue 58 | - position: [26.542309620033404, -2.9098500980087008, 0.0] 59 | class: yellow 60 | - position: [15.343392234504304, 5.129727151907738, 0.0] 61 | class: blue 62 | - position: [33.61225844687579, 2.5079190078236624, 0.0] 63 | class: yellow 64 | - position: [24.050712222194075, -2.729341199161961, 0.0] 65 | class: yellow 66 | - position: [28.978093756310404, -2.524726050855797, 0.0] 67 | class: yellow 68 | - position: [27.388286850269772, 8.188138959484109, 0.0] 69 | class: yellow 70 | - position: [29.79875096703856, 8.013413709239579, 0.0] 71 | class: yellow 72 | - position: [31.96280777727214, 6.978551583494958, 0.0] 73 | class: yellow 74 | - position: [33.13329450406126, 5.150022234759962, 0.0] 75 | class: yellow 76 | - position: [24.862269395791586, 8.38629801175331, 0.0] 77 | class: yellow 78 | - position: [32.805330045986, 0.4132909833029524, 0.0] 79 | class: yellow 80 | - position: [31.678597974874272, -1.5274139279262395, 0.0] 81 | class: yellow 82 | - position: [5.549643673850138, 5.560643611743225, 0.0] 83 | class: blue 84 | - position: [3.334633001805431, -1.6566058590977555, 0.0] 85 | class: yellow 86 | - position: [0.6572491988939143, 5.5816618582166315, 0.0] 87 | class: blue 88 | - position: [3.344017432347119, 1.523493418189879, 0.0] 89 | class: blue 90 | - position: [5.299808524344298, 9.017223826940956, 0.0] 91 | class: yellow 92 | - position: [0.26234005381732, 8.935016004566377, 0.0] 93 | class: yellow 94 | - position: [-4.167303722222868, 5.5042345667374315, 0.0] 95 | class: blue 96 | - position: [-4.506720244451731, 8.853823784628338, 0.0] 97 | class: yellow 98 | - position: [-6.739767368529582, 1.340766131203411, 0.0] 99 | class: blue 100 | - position: [-9.030776777229015, 5.300412279622312, 0.0] 101 | class: blue 102 | - position: [-1.6412883908864784, 1.473784731008904, 0.0] 103 | class: blue 104 | - position: [-13.946428273217787, 5.057170611476806, 0.0] 105 | class: blue 106 | - position: [-14.60338622611035, 8.244437808574718, 0.0] 107 | class: yellow 108 | - position: [-9.579253491526904, 8.585685106035454, 0.0] 109 | class: yellow 110 | - position: [-11.365148385314093, 1.0441225678582133, 0.0] 111 | class: blue 112 | - position: [-18.78795571616848, 4.548010313017302, 0.0] 113 | class: blue 114 | - position: [-16.63779582423528, 0.6654335910872572, 0.0] 115 | class: blue 116 | - position: [-16.38856194008046, -2.770039568180989, 0.0] 117 | class: yellow 118 | - position: [-19.490231383225623, 7.839635320536948, 0.0] 119 | class: yellow 120 | - position: [-24.408662184011078, 7.169910716066285, 0.0] 121 | class: yellow 122 | - position: [-28.75437062239082, 3.149959563276633, 0.0] 123 | class: blue 124 | - position: [-21.71300754452379, 0.005950424873761581, 0.0] 125 | class: blue 126 | - position: [-23.571920680421197, 3.72844601478104, 0.0] 127 | class: blue 128 | - position: [-21.20393576545688, -3.2789919703479224, 0.0] 129 | class: yellow 130 | - position: [-29.41736875124476, 6.428777262715264, 0.0] 131 | class: yellow 132 | - position: [-26.505374393525432, -0.5656410988848639, 0.0] 133 | class: blue 134 | - position: [-33.50388479724538, 2.4234586856386633, 0.0] 135 | class: blue 136 | - position: [-31.82172020483318, -1.4558946525525251, 0.0] 137 | class: blue 138 | - position: [-25.950730323960403, -4.091189882966361, 0.0] 139 | class: yellow 140 | - position: [-36.49032188902615, -2.361475810619297, 0.0] 141 | class: blue 142 | - position: [-34.34309045484403, 5.844620429160196, 0.0] 143 | class: yellow 144 | - position: [-42.92381927892094, 0.5007491496971352, 0.0] 145 | class: blue 146 | - position: [-31.269500992107677, -4.784966164180651, 0.0] 147 | class: yellow 148 | - position: [-35.817710730544846, -5.6226468311606554, 0.0] 149 | class: yellow 150 | - position: [-38.35237779770629, 1.45110763762184, 0.0] 151 | class: blue 152 | - position: [-39.16105330772005, 4.843829342071292, 0.0] 153 | class: yellow 154 | - position: [-41.42005616895536, -3.2871590578762575, 0.0] 155 | class: blue 156 | - position: [-47.73052388460226, -0.4946272283816116, 0.0] 157 | class: blue 158 | - position: [-51.3169944945488, -5.654231869309464, 0.0] 159 | class: blue 160 | - position: [-46.472678578013664, -4.370488162263715, 0.0] 161 | class: blue 162 | - position: [-40.677402090107705, -6.711247359937322, 0.0] 163 | class: yellow 164 | - position: [-52.58882837077481, -1.7297083712159655, 0.0] 165 | class: blue 166 | - position: [-50.41897298256915, -8.867680596574711, 0.0] 167 | class: yellow 168 | - position: [-56.29294778703912, -6.964919624823834, 0.0] 169 | class: blue 170 | - position: [-43.98251631447368, 3.822817662626435, 0.0] 171 | class: yellow 172 | - position: [-57.39957069004944, -3.059098328231217, 0.0] 173 | class: blue 174 | - position: [-53.60602490246739, 1.5848794035610683, 0.0] 175 | class: yellow 176 | - position: [-48.729825958768984, 2.768751210700384, 0.0] 177 | class: yellow 178 | - position: [-62.253914866906584, -4.405548340227414, 0.0] 179 | class: blue 180 | - position: [-61.35936345738162, -8.305613453936274, 0.0] 181 | class: blue 182 | - position: [-66.75963428772751, -5.852101626458309, 0.0] 183 | class: blue 184 | - position: [-58.45961715656145, 0.24442936705087412, 0.0] 185 | class: yellow 186 | - position: [-63.495489202737915, -1.2045391942758887, 0.0] 187 | class: yellow 188 | - position: [-55.23043947561004, -10.240918015033207, 0.0] 189 | class: yellow 190 | - position: [-71.26639831329229, -7.413938318554699, 0.0] 191 | class: blue 192 | - position: [-65.9402321688091, -9.834522896442044, 0.0] 193 | class: blue 194 | - position: [-70.96253284942638, -11.861117787096722, 0.0] 195 | class: blue 196 | - position: [-68.59448149510652, -16.1919131235804, 0.0] 197 | class: unknown 198 | - position: [-64.80660696278508, -13.117478280128022, 0.0] 199 | class: yellow 200 | - position: [-60.00391307437816, -11.666520310011643, 0.0] 201 | class: yellow 202 | - position: [-68.05267136713617, -2.5775635100567174, 0.0] 203 | class: yellow 204 | - position: [-62.27843886900301, 0.3118131928223184, 0.0] 205 | class: unknown 206 | - position: [-73.97673579470906, -16.830563247864415, 0.0] 207 | class: yellow 208 | - position: [-76.00974673240033, -9.374614761703068, 0.0] 209 | class: blue 210 | - position: [-80.15095730809404, -11.133899040026625, 0.0] 211 | class: blue 212 | - position: [-72.94627412033448, -4.445308200333493, 0.0] 213 | class: yellow 214 | - position: [-84.7480549243168, -13.235451504672719, 0.0] 215 | class: blue 216 | - position: [-75.57803477835277, -13.744936751337201, 0.0] 217 | class: blue 218 | - position: [-77.48146274912706, -6.23201162950929, 0.0] 219 | class: yellow 220 | - position: [-88.74829223159387, -15.688638352449276, 0.0] 221 | class: blue 222 | - position: [-92.01254451571718, -19.571687139653314, 0.0] 223 | class: blue 224 | - position: [-79.90538450863977, -15.787773160011758, 0.0] 225 | class: blue 226 | - position: [-81.9952311607549, -8.318202526320908, 0.0] 227 | class: yellow 228 | - position: [-72.01284120789894, -2.5028724585075786, 0.0] 229 | class: unknown 230 | - position: [-91.67477465603748, -17.768875595451558, 0.0] 231 | class: blue 232 | - position: [-86.44917562182172, -10.411260937859389, 0.0] 233 | class: yellow 234 | - position: [-69.46930615281892, -14.829107703124215, 0.0] 235 | class: yellow 236 | - position: [-78.19877850342817, -19.125708173313978, 0.0] 237 | class: yellow 238 | - position: [-90.67622923355948, -12.876978714482423, 0.0] 239 | class: yellow 240 | - position: [-84.56792550962109, -18.301275665278443, 0.0] 241 | class: blue 242 | - position: [-96.07468047056304, -20.85717888521443, 0.0] 243 | class: yellow 244 | - position: [-94.21044972915543, -15.29861121676383, 0.0] 245 | class: yellow 246 | - position: [-77.90083808865019, -24.539536939716253, 0.0] 247 | class: unknown 248 | - position: [-95.17844465036862, -22.789470956671146, 0.0] 249 | class: yellow 250 | - position: [-96.06551802386186, -18.159759882616786, 0.0] 251 | class: yellow 252 | - position: [-93.08946360049731, -24.640586875254666, 0.0] 253 | class: yellow 254 | - position: [-82.09606252917636, -21.550732925464555, 0.0] 255 | class: yellow 256 | - position: [-88.643175257267, -20.582031185973495, 0.0] 257 | class: blue 258 | - position: [-85.01693121974849, -23.165009526170245, 0.0] 259 | class: yellow 260 | - position: [-116.48750347532733, -8.999242038593458, 0.0] 261 | class: unknown 262 | - position: [-90.79387170095754, -20.647023920329286, 0.0] 263 | class: blue 264 | - position: [-90.45496248466178, -25.376684973374044, 0.0] 265 | class: yellow 266 | - position: [-87.61923272719818, -24.57288964260573, 0.0] 267 | class: yellow 268 | - position: [5.117047546943121, -2.3634793602472715, 0.0] 269 | class: unknown 270 | - position: [-45.83773445053254, -7.63962240842209, 0.0] 271 | class: yellow 272 | - position: [-1.623508597420569, -1.6698945573468706, 0.0] 273 | class: yellow 274 | - position: [-6.679578229387483, -1.9482103169046827, 0.0] 275 | class: yellow 276 | - position: [-11.596489981575493, -2.2729120363887967, 0.0] 277 | class: yellow 278 | -------------------------------------------------------------------------------- /tracks/FSS22_V2.yaml: -------------------------------------------------------------------------------- 1 | track: 2 | version: 1.0 3 | lanesFirstWithLastConnected: true 4 | start: 5 | position: [0.0, 0.0, 0.0] 6 | orientation: [0.0, 0.0, 0.0] 7 | earthToTrack: 8 | position: [0.0, 0.0, 0.0] 9 | orientation: [0.0, 0.0, 0.0] 10 | left: 11 | - position: [5.401166801296542, 1.831752672814772, 0.0] 12 | class: blue 13 | - position: [7.776324093917727, 1.8643701502342438, 0.0] 14 | class: blue 15 | - position: [10.094630602805989, 1.723066724875636, 0.0] 16 | class: blue 17 | - position: [13.24280439887687, 1.6125611998674054, 0.0] 18 | class: blue 19 | - position: [16.367270976851827, 1.5347924012487284, 0.0] 20 | class: blue 21 | - position: [19.290484335811172, 1.4315202672367255, 0.0] 22 | class: blue 23 | - position: [22.12819234217677, 1.2827746607557453, 0.0] 24 | class: blue 25 | - position: [24.695220964198118, 1.4256113620865518, 0.0] 26 | class: blue 27 | - position: [27.102788563629332, 2.162588235850687, 0.0] 28 | class: blue 29 | - position: [28.647557078643885, 3.2227482218093604, 0.0] 30 | class: blue 31 | - position: [29.164842644724676, 4.857874647854416, 0.0] 32 | class: blue 33 | - position: [29.435008549294803, 6.775416074025698, 0.0] 34 | class: blue 35 | - position: [28.43001896959307, 8.478136869659963, 0.0] 36 | class: blue 37 | - position: [27.344612360928135, 9.43400099373857, 0.0] 38 | class: blue 39 | - position: [25.769655543757242, 10.330126112280212, 0.0] 40 | class: blue 41 | - position: [23.51350276338434, 10.90941794877742, 0.0] 42 | class: blue 43 | - position: [21.590655627275456, 11.099354269720404, 0.0] 44 | class: blue 45 | - position: [18.97311214477707, 10.418488813337042, 0.0] 46 | class: blue 47 | - position: [16.476752632936936, 8.895132962281348, 0.0] 48 | class: blue 49 | - position: [14.069628956930382, 8.237966932630373, 0.0] 50 | class: blue 51 | - position: [11.478188120088179, 7.863590495465774, 0.0] 52 | class: blue 53 | - position: [8.940554553693177, 7.888667230030492, 0.0] 54 | class: blue 55 | - position: [6.3658907620606815, 7.8352722487245465, 0.0] 56 | class: blue 57 | - position: [3.7385579433062435, 7.332883086209633, 0.0] 58 | class: blue 59 | - position: [0.6793649799856466, 6.943104859009487, 0.0] 60 | class: blue 61 | - position: [-2.797485902273183, 7.330910670736404, 0.0] 62 | class: blue 63 | - position: [-5.153396188624201, 7.77466472203129, 0.0] 64 | class: blue 65 | - position: [-7.817768801771586, 8.701498262140056, 0.0] 66 | class: blue 67 | - position: [-9.840780534075849, 9.063207338069926, 0.0] 68 | class: blue 69 | - position: [-11.959888832922795, 9.64463940108351, 0.0] 70 | class: blue 71 | - position: [-14.087900592032755, 9.281063967043442, 0.0] 72 | class: blue 73 | - position: [-15.593675076647688, 7.16568516959791, 0.0] 74 | class: blue 75 | - position: [-16.182902181734686, 4.3719113304766495, 0.0] 76 | class: blue 77 | - position: [-16.600925646135583, 2.225447478027955, 0.0] 78 | class: blue 79 | - position: [-16.628469994588674, -0.6655112651629251, 0.0] 80 | class: blue 81 | - position: [-16.697356119471088, -4.16363242995869, 0.0] 82 | class: blue 83 | - position: [-14.658877022544589, -6.491884631671322, 0.0] 84 | class: blue 85 | - position: [-12.634502010233732, -7.402784200291035, 0.0] 86 | class: blue 87 | - position: [-10.742173580914713, -7.521044547682066, 0.0] 88 | class: blue 89 | - position: [-9.172037138944933, -6.354964092926035, 0.0] 90 | class: blue 91 | - position: [-8.120024670466002, -4.308535996224065, 0.0] 92 | class: blue 93 | - position: [-7.093884440689835, -2.296387623457375, 0.0] 94 | class: blue 95 | - position: [-5.4254672873885115, 0.41941183559170536, 0.0] 96 | class: blue 97 | - position: [-3.511236517912966, 1.6351441987794348, 0.0] 98 | class: blue 99 | - position: [-0.6587064982864497, 1.9671797973417688, 0.0] 100 | class: blue 101 | - position: [2.5486453992390223, 1.7650739565819304, 0.0] 102 | class: blue 103 | right: 104 | - position: [5.251946390728667, -1.6585093865458949, 0.0] 105 | class: yellow 106 | - position: [7.663860776970562, -1.8813103292057722, 0.0] 107 | class: yellow 108 | - position: [10.075732449523363, -1.5775409238685743, 0.0] 109 | class: yellow 110 | - position: [13.261294533935137, -1.573231300605762, 0.0] 111 | class: yellow 112 | - position: [16.240462572618807, -1.6269296287090869, 0.0] 113 | class: yellow 114 | - position: [19.223877259253413, -1.64170852627233, 0.0] 115 | class: yellow 116 | - position: [21.817354350541898, -1.808004783659547, 0.0] 117 | class: yellow 118 | - position: [24.376338958711035, -1.8280427873894043, 0.0] 119 | class: yellow 120 | - position: [28.164517967041053, -1.5961805050770004, 0.0] 121 | class: yellow 122 | - position: [30.360616320902373, -0.30927912458303597, 0.0] 123 | class: yellow 124 | - position: [31.61612871563165, 0.6884584142432353, 0.0] 125 | class: yellow 126 | - position: [32.89281413010158, 2.4791886526861195, 0.0] 127 | class: yellow 128 | - position: [32.82784582805064, 4.869190526321729, 0.0] 129 | class: yellow 130 | - position: [32.4634621485464, 6.884584142432352, 0.0] 131 | class: yellow 132 | - position: [32.092753771646194, 8.685167687376198, 0.0] 133 | class: yellow 134 | - position: [31.2159698818679, 10.37941031472166, 0.0] 135 | class: yellow 136 | - position: [29.991316359518393, 11.800863596902598, 0.0] 137 | class: yellow 138 | - position: [28.601044859099343, 12.64857350951648, 0.0] 139 | class: yellow 140 | - position: [27.25308628163636, 13.691127308772144, 0.0] 141 | class: yellow 142 | - position: [25.208169629213845, 14.404668359550769, 0.0] 143 | class: yellow 144 | - position: [23.047491878600212, 14.450456429255677, 0.0] 145 | class: yellow 146 | - position: [19.864942828938275, 14.669147974805012, 0.0] 147 | class: yellow 148 | - position: [16.989266993508487, 13.705438907182474, 0.0] 149 | class: yellow 150 | - position: [14.086743590694553, 12.247579536832413, 0.0] 151 | class: yellow 152 | - position: [11.233065426526785, 11.24977538751286, 0.0] 153 | class: yellow 154 | - position: [8.868507080978766, 11.163065787358754, 0.0] 155 | class: yellow 156 | - position: [6.248987610520037, 11.26144260779063, 0.0] 157 | class: yellow 158 | - position: [2.691360384778314, 11.008637617772582, 0.0] 159 | class: yellow 160 | - position: [-0.630732187899841, 10.790167666818283, 0.0] 161 | class: yellow 162 | - position: [-3.5017202810710937, 11.17609576890842, 0.0] 163 | class: yellow 164 | - position: [-6.261574913063201, 11.898217290761478, 0.0] 165 | class: yellow 166 | - position: [-8.852076127837682, 12.592314338201877, 0.0] 167 | class: yellow 168 | - position: [-10.963682985401242, 13.539572889047596, 0.0] 169 | class: yellow 170 | - position: [-13.262800008407698, 13.407078841400434, 0.0] 171 | class: yellow 172 | - position: [-15.68328828500512, 12.783535436878402, 0.0] 173 | class: yellow 174 | - position: [-17.41429898539811, 11.489749216600446, 0.0] 175 | class: yellow 176 | - position: [-18.761160320342142, 9.879562450695067, 0.0] 177 | class: yellow 178 | - position: [-19.363674620994527, 8.004730884474576, 0.0] 179 | class: yellow 180 | - position: [-19.638593973383074, 6.1975880118825435, 0.0] 181 | class: yellow 182 | - position: [-20.063653550039053, 3.8892891794714988, 0.0] 183 | class: yellow 184 | - position: [-20.55604756231679, 1.4984924151558743, 0.0] 185 | class: yellow 186 | - position: [-20.843738815873383, -1.0467644941660985, 0.0] 187 | class: yellow 188 | - position: [-21.158086704701038, -4.71546677022808, 0.0] 189 | class: yellow 190 | - position: [-20.476198322006955, -6.753986751733846, 0.0] 191 | class: yellow 192 | - position: [-18.39568798276845, -8.566438863570568, 0.0] 193 | class: yellow 194 | - position: [-15.247005206021834, -10.307704396083093, 0.0] 195 | class: yellow 196 | - position: [-12.626131175642406, -11.063166865705204, 0.0] 197 | class: yellow 198 | - position: [-10.490171340182785, -10.927261812690402, 0.0] 199 | class: yellow 200 | - position: [-8.086173741390898, -9.397445158913746, 0.0] 201 | class: yellow 202 | - position: [-6.381520898611194, -6.993447560121857, 0.0] 203 | class: yellow 204 | - position: [-4.818033627982607, -4.578127190058266, 0.0] 205 | class: yellow 206 | - position: [-2.7589531638419222, -3.1622606623428107, 0.0] 207 | class: yellow 208 | - position: [-0.21052073133218205, -2.099499458000513, 0.0] 209 | class: yellow 210 | - position: [2.949585044674539, -1.6904733855079412, 0.0] 211 | class: yellow 212 | time_keeping: 213 | - position: [8.134246343923458, 4.236667164573754, 0.0] 214 | class: timekeeping 215 | - position: [8.036355679915891, -4.221875996238446, 0.0] 216 | class: timekeeping 217 | unknown: 218 | - position: [40.53416452805657, -6.405593193583404, 0.0] 219 | class: unknown 220 | - position: [43.39904517778767, -6.533330407945155, 0.0] 221 | class: unknown 222 | - position: [51.94396448667063, -2.098564707140014, 0.0] 223 | class: unknown 224 | - position: [21.949564195155272, 6.255589187748466, 0.0] 225 | class: unknown 226 | - position: [54.492364447277076, -1.3080094420075534, 0.0] 227 | class: unknown 228 | - position: [54.306685665952244, -6.734069667585001, 0.0] 229 | class: unknown 230 | - position: [-10.29434186180601, -16.757882494131003, 0.0] 231 | class: unknown 232 | - position: [4.323053713973186, 15.511341303392488, 0.0] 233 | class: unknown 234 | - position: [3.90746877337618, -18.5646435017631, 0.0] 235 | class: unknown 236 | - position: [-9.772683820527389, 3.0240876387485036, 0.0] 237 | class: unknown 238 | - position: [-29.721170840205794, 8.763781734599824, 0.0] 239 | class: unknown 240 | - position: [-27.727519607543314, 4.343409491101729, 0.0] 241 | class: unknown 242 | - position: [6.703464410154215, 15.77705187566441, 0.0] 243 | class: unknown 244 | - position: [51.02852125898048, -7.474961800010109, 0.0] 245 | class: unknown 246 | - position: [33.241281976319264, -18.93516171345064, 0.0] 247 | class: unknown 248 | - position: [64.89584162056528, -4.477116385158845, 0.0] 249 | class: unknown 250 | -------------------------------------------------------------------------------- /tracks/acceleration.yaml: -------------------------------------------------------------------------------- 1 | track: 2 | version: 1.0 3 | lanesFirstWithLastConnected: false 4 | start: 5 | position: [-1.98, 0.0, 0.0] 6 | orientation: [0.0, 0.0, 0.0] 7 | earthToTrack: 8 | position: [0.0, 0.0, 0.0] 9 | orientation: [0.0, 0.0, 0.0] 10 | left: 11 | - position: [-5.0, 1.9, 0.0] 12 | class: invisible 13 | - position: [0.0, 1.9, 0.0] 14 | class: invisible 15 | - position: [5.0, 1.9, 0.0] 16 | class: blue 17 | - position: [10.0, 1.9, 0.0] 18 | class: blue 19 | - position: [15.0, 1.9, 0.0] 20 | class: blue 21 | - position: [20.0, 1.9, 0.0] 22 | class: blue 23 | - position: [25.0, 1.9, 0.0] 24 | class: blue 25 | - position: [30.0, 1.9, 0.0] 26 | class: blue 27 | - position: [35.0, 1.9, 0.0] 28 | class: blue 29 | - position: [40.0, 1.9, 0.0] 30 | class: blue 31 | - position: [45.0, 1.9, 0.0] 32 | class: blue 33 | - position: [50.0, 1.9, 0.0] 34 | class: blue 35 | - position: [55.0, 1.9, 0.0] 36 | class: blue 37 | - position: [60.0, 1.9, 0.0] 38 | class: blue 39 | - position: [65.0, 1.9, 0.0] 40 | class: blue 41 | - position: [70.0, 1.9, 0.0] 42 | class: blue 43 | - position: [75.0, 1.9, 0.0] 44 | class: invisible 45 | - position: [80.0, 1.9, 0.0] 46 | class: small-orange 47 | - position: [85.0, 1.9, 0.0] 48 | class: small-orange 49 | - position: [90.0, 1.9, 0.0] 50 | class: small-orange 51 | - position: [95.0, 1.9, 0.0] 52 | class: small-orange 53 | - position: [100.0, 1.9, 0.0] 54 | class: small-orange 55 | - position: [105.0, 1.9, 0.0] 56 | class: small-orange 57 | - position: [110.0, 1.9, 0.0] 58 | class: small-orange 59 | - position: [115.0, 1.9, 0.0] 60 | class: small-orange 61 | - position: [120.0, 1.9, 0.0] 62 | class: small-orange 63 | - position: [125.0, 1.9, 0.0] 64 | class: small-orange 65 | - position: [130.0, 1.9, 0.0] 66 | class: small-orange 67 | - position: [135.0, 1.9, 0.0] 68 | class: small-orange 69 | - position: [140.0, 1.9, 0.0] 70 | class: small-orange 71 | - position: [145.0, 1.9, 0.0] 72 | class: small-orange 73 | - position: [150.0, 1.9, 0.0] 74 | class: small-orange 75 | right: 76 | - position: [-5.0, -1.9, 0.0] 77 | class: invisible 78 | - position: [0.0, -1.9, 0.0] 79 | class: invisible 80 | - position: [5.0, -1.9, 0.0] 81 | class: yellow 82 | - position: [10.0, -1.9, 0.0] 83 | class: yellow 84 | - position: [15.0, -1.9, 0.0] 85 | class: yellow 86 | - position: [20.0, -1.9, 0.0] 87 | class: yellow 88 | - position: [25.0, -1.9, 0.0] 89 | class: yellow 90 | - position: [30.0, -1.9, 0.0] 91 | class: yellow 92 | - position: [35.0, -1.9, 0.0] 93 | class: yellow 94 | - position: [40.0, -1.9, 0.0] 95 | class: yellow 96 | - position: [45.0, -1.9, 0.0] 97 | class: yellow 98 | - position: [50.0, -1.9, 0.0] 99 | class: yellow 100 | - position: [55.0, -1.9, 0.0] 101 | class: yellow 102 | - position: [60.0, -1.9, 0.0] 103 | class: yellow 104 | - position: [65.0, -1.9, 0.0] 105 | class: yellow 106 | - position: [70.0, -1.9, 0.0] 107 | class: yellow 108 | - position: [75.0, -1.9, 0.0] 109 | class: invisible 110 | - position: [80.0, -1.9, 0.0] 111 | class: small-orange 112 | - position: [85.0, -1.9, 0.0] 113 | class: small-orange 114 | - position: [90.0, -1.9, 0.0] 115 | class: small-orange 116 | - position: [95.0, -1.9, 0.0] 117 | class: small-orange 118 | - position: [100.0, -1.9, 0.0] 119 | class: small-orange 120 | - position: [105.0, -1.9, 0.0] 121 | class: small-orange 122 | - position: [110.0, -1.9, 0.0] 123 | class: small-orange 124 | - position: [115.0, -1.9, 0.0] 125 | class: small-orange 126 | - position: [120.0, -1.9, 0.0] 127 | class: small-orange 128 | - position: [125.0, -1.9, 0.0] 129 | class: small-orange 130 | - position: [130.0, -1.9, 0.0] 131 | class: small-orange 132 | - position: [135.0, -1.9, 0.0] 133 | class: small-orange 134 | - position: [140.0, -1.9, 0.0] 135 | class: small-orange 136 | - position: [145.0, -1.9, 0.0] 137 | class: small-orange 138 | - position: [150.0, -1.9, 0.0] 139 | class: small-orange 140 | time_keeping: 141 | - position: [0.0, 4.0, 0.0] 142 | class: timekeeping 143 | - position: [0.0, -4.0, 0.0] 144 | class: timekeeping 145 | - position: [75.0, 4.0, 0.0] 146 | class: timekeeping 147 | - position: [75.0, -4.0, 0.0] 148 | class: timekeeping 149 | unknown: 150 | - position: [-0.3, 1.9, 0.0] 151 | class: big-orange 152 | - position: [-0.3, -1.9, 0.0] 153 | class: big-orange 154 | - position: [0.3, 1.9, 0.0] 155 | class: big-orange 156 | - position: [0.3, -1.9, 0.0] 157 | class: big-orange 158 | - position: [74.7, 1.9, 0.0] 159 | class: big-orange 160 | - position: [74.7, -1.9, 0.0] 161 | class: big-orange 162 | - position: [75.3, 1.9, 0.0] 163 | class: big-orange 164 | - position: [75.3, -1.9, 0.0] 165 | class: big-orange 166 | - position: [150.0, 0.0, 0.0] 167 | class: small-orange 168 | -------------------------------------------------------------------------------- /tracks/gripMap.yaml: -------------------------------------------------------------------------------- 1 | grip_map: 2 | version: 1.0 3 | total_scale: 1.0 # every point is multiplied with that factor (when you want to easily change the grip for the whole track) 4 | # the current value is determined based on the closest point 5 | points: 6 | - position: [0.0, 0.0, 0.0] 7 | value: 1.0 -------------------------------------------------------------------------------- /tracks/skidpad.yaml: -------------------------------------------------------------------------------- 1 | track: 2 | version: 1.0 3 | lanesFirstWithLastConnected: false 4 | start: 5 | position: [-16.68, 0.0, 0.0] 6 | orientation: [0.0, 0.0, 0.0] 7 | earthToTrack: 8 | position: [0.0, 0.0, 0.0] 9 | orientation: [0.0, 0.0, 0.0] 10 | left: 11 | - position: [-20.0, 1.65, 0.0] 12 | class: invisible 13 | - position: [-15.0, 1.65, 0.0] 14 | class: small-orange 15 | - position: [-11.25, 1.65, 0.0] 16 | class: small-orange 17 | - position: [-7.760476789476275, 1.65, 0.0] 18 | class: yellow 19 | - position: [-9.954801962809116, 5.001586016266157, 0.0] 20 | class: yellow 21 | - position: [-10.775, 9.125, 0.0] 22 | class: yellow 23 | - position: [-9.954801962809116, 13.248413983733842, 0.0] 24 | class: yellow 25 | - position: [-7.6190755672850505, 16.744075567285048, 0.0] 26 | class: yellow 27 | - position: [-4.123413983733844, 19.079801962809114, 0.0] 28 | class: yellow 29 | - position: [-1.3195569260812732e-15, 19.9, 0.0] 30 | class: yellow 31 | - position: [4.123413983733841, 19.079801962809114, 0.0] 32 | class: yellow 33 | - position: [7.619075567285049, 16.74407556728505, 0.0] 34 | class: yellow 35 | - position: [9.954801962809112, 13.248413983733847, 0.0] 36 | class: yellow 37 | - position: [10.775, 9.125000000000002, 0.0] 38 | class: yellow 39 | - position: [9.954801962809114, 5.001586016266155, 0.0] 40 | class: yellow 41 | - position: [7.760476789476275, 1.65, 0.0] 42 | class: yellow 43 | - position: [11.0, 1.65, 0.0] 44 | class: small-orange 45 | - position: [16.0, 1.65, 0.0] 46 | class: small-orange 47 | - position: [21.0, 1.65, 0.0] 48 | class: small-orange 49 | right: 50 | - position: [-20.0, -1.65, 0.0] 51 | class: invisible 52 | - position: [-15.0, -1.65, 0.0] 53 | class: small-orange 54 | - position: [-11.25, -1.65, 0.0] 55 | class: small-orange 56 | - position: [-7.760476789476275, -1.65, 0.0] 57 | class: blue 58 | - position: [-9.954801962809114, -5.001586016266155, 0.0] 59 | class: blue 60 | - position: [-10.775, -9.125000000000002, 0.0] 61 | class: blue 62 | - position: [-9.954801962809112, -13.248413983733847, 0.0] 63 | class: blue 64 | - position: [-7.619075567285049, -16.74407556728505, 0.0] 65 | class: blue 66 | - position: [-4.123413983733841, -19.079801962809114, 0.0] 67 | class: blue 68 | - position: [1.3195569260812732e-15, -19.9, 0.0] 69 | class: blue 70 | - position: [4.123413983733844, -19.079801962809114, 0.0] 71 | class: blue 72 | - position: [7.6190755672850505, -16.744075567285048, 0.0] 73 | class: blue 74 | - position: [9.954801962809116, -13.248413983733842, 0.0] 75 | class: blue 76 | - position: [10.775, -9.125, 0.0] 77 | class: blue 78 | - position: [9.954801962809116, -5.001586016266157, 0.0] 79 | class: blue 80 | - position: [7.760476789476275, -1.65, 0.0] 81 | class: blue 82 | - position: [11.0, -1.65, 0.0] 83 | class: small-orange 84 | - position: [16.0, -1.65, 0.0] 85 | class: small-orange 86 | - position: [21.0, -1.65, 0.0] 87 | class: small-orange 88 | time_keeping: 89 | - position: [0.0, 3.0, 0.0] 90 | class: timekeeping 91 | - position: [0.0, -3.0, 0.0] 92 | class: timekeeping 93 | - position: [10.20, 3.0, 0.0] 94 | class: invisible 95 | - position: [10.20, -3.0, 0.0] 96 | class: invisible 97 | unknown: 98 | - position: [21.0, 0.0, 0.0] 99 | class: small-orange 100 | - position: [-1.3, 2.0, 0.0] 101 | class: big-orange 102 | - position: [-1.3, -2.0, 0.0] 103 | class: big-orange 104 | - position: [1.3, 2.0, 0.0] 105 | class: big-orange 106 | - position: [1.3, -2.0, 0.0] 107 | class: big-orange 108 | - position: [0.0, -1.6500000000000004, 0.0] 109 | class: yellow 110 | - position: [2.860558656929046, -2.2190004944781316, 0.0] 111 | class: yellow 112 | - position: [5.285623189369442, -3.839376810630557, 0.0] 113 | class: yellow 114 | - position: [6.905999505521868, -6.264441343070954, 0.0] 115 | class: yellow 116 | - position: [7.475, -9.125, 0.0] 117 | class: yellow 118 | - position: [6.905999505521868, -11.985558656929046, 0.0] 119 | class: yellow 120 | - position: [5.285623189369443, -14.410623189369442, 0.0] 121 | class: yellow 122 | - position: [2.860558656929047, -16.03099950552187, 0.0] 123 | class: yellow 124 | - position: [9.154234823626465e-16, -16.6, 0.0] 125 | class: yellow 126 | - position: [-2.860558656929045, -16.03099950552187, 0.0] 127 | class: yellow 128 | - position: [-5.285623189369442, -14.410623189369444, 0.0] 129 | class: yellow 130 | - position: [-6.905999505521867, -11.98555865692905, 0.0] 131 | class: yellow 132 | - position: [-7.475, -9.125000000000002, 0.0] 133 | class: yellow 134 | - position: [-6.9059995055218675, -6.264441343070953, 0.0] 135 | class: yellow 136 | - position: [-5.285623189369444, -3.839376810630559, 0.0] 137 | class: yellow 138 | - position: [-2.8605586569290504, -2.2190004944781334, 0.0] 139 | class: yellow 140 | - position: [-0.0, 1.6500000000000004, 0.0] 141 | class: blue 142 | - position: [-2.860558656929046, 2.2190004944781316, 0.0] 143 | class: blue 144 | - position: [-5.285623189369442, 3.839376810630557, 0.0] 145 | class: blue 146 | - position: [-6.905999505521868, 6.264441343070954, 0.0] 147 | class: blue 148 | - position: [-7.475, 9.125, 0.0] 149 | class: blue 150 | - position: [-6.905999505521868, 11.985558656929046, 0.0] 151 | class: blue 152 | - position: [-5.285623189369443, 14.410623189369442, 0.0] 153 | class: blue 154 | - position: [-2.860558656929047, 16.03099950552187, 0.0] 155 | class: blue 156 | - position: [-9.154234823626465e-16, 16.6, 0.0] 157 | class: blue 158 | - position: [2.860558656929045, 16.03099950552187, 0.0] 159 | class: blue 160 | - position: [5.285623189369442, 14.410623189369444, 0.0] 161 | class: blue 162 | - position: [6.905999505521867, 11.98555865692905, 0.0] 163 | class: blue 164 | - position: [7.475, 9.125000000000002, 0.0] 165 | class: blue 166 | - position: [6.9059995055218675, 6.264441343070953, 0.0] 167 | class: blue 168 | - position: [5.285623189369444, 3.839376810630559, 0.0] 169 | class: blue 170 | - position: [2.8605586569290504, 2.2190004944781334, 0.0] 171 | class: blue 172 | -------------------------------------------------------------------------------- /urdf/Model_without_Steering_Tires.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Model_without_Steering_Tires.stl -------------------------------------------------------------------------------- /urdf/Steering_Wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Steering_Wheel.stl -------------------------------------------------------------------------------- /urdf/Tires/FL_Inside.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Tires/FL_Inside.stl -------------------------------------------------------------------------------- /urdf/Tires/FL_Outside.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Tires/FL_Outside.stl -------------------------------------------------------------------------------- /urdf/Tires/FR_Inside.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Tires/FR_Inside.stl -------------------------------------------------------------------------------- /urdf/Tires/RL_Inside.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Tires/RL_Inside.stl -------------------------------------------------------------------------------- /urdf/Tires/RR_Inside.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PacSim/pacsim/995ca0c03371cfeeeacfb088ca4cfc996f22bdc8/urdf/Tires/RR_Inside.stl -------------------------------------------------------------------------------- /urdf/separate_model.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | --------------------------------------------------------------------------------