├── CITATION.CFF
├── Project.toml
├── README.md
├── parked_cars.png
└── src
    ├── VehicleSim.jl
    ├── assets
        ├── chevy.urdf
        ├── chevy_nomesh.urdf
        └── meshes
        │   ├── body_all.dae
        │   ├── bumpers.stl
        │   ├── front.dae
        │   ├── leaf.stl
        │   ├── leaf2.stl
        │   ├── rama_1.stl
        │   ├── rama_2.stl
        │   ├── rama_3.stl
        │   ├── shackles.stl
        │   ├── steer.stl
        │   ├── wheel.obj
        │   └── wheel.stl
    ├── client.jl
    ├── control.jl
    ├── example_project.jl
    ├── map.jl
    ├── measurements.jl
    ├── objects.jl
    ├── sim.jl
    ├── sink.jl
    └── view_car.jl


/CITATION.CFF:
--------------------------------------------------------------------------------
 1 | cff-version: 1.2.0
 2 | message: "If you use this software, please cite it as below."
 3 | authors:
 4 | - family-names: "Laine"
 5 |   given-names: "Forrest"
 6 |   orcid: "https://orcid.org/0000-0003-3982-3920"
 7 | title: "VehicleSim.jl"
 8 | version: 0.1.0
 9 | date-released: 2023-04-22
10 | url: "https://github.com/Vampir-lab/VehicleSim"
11 | 


--------------------------------------------------------------------------------
/Project.toml:
--------------------------------------------------------------------------------
 1 | name = "VehicleSim"
 2 | uuid = "e0769fa5-244c-4558-b677-4a3334c4b6a3"
 3 | authors = ["Forrest Laine <forrest.laine@vanderbilt.edu>"]
 4 | version = "0.1.0"
 5 | 
 6 | [deps]
 7 | ColorTypes = "3da002f7-5984-5a60-b8a6-cbb66c0b333f"
 8 | Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
 9 | GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326"
10 | Infiltrator = "5903a43b-9cc3-4c30-8d17-598619ec4e9b"
11 | JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
12 | LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
13 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11"
14 | MeshCatMechanisms = "6ad125db-dd91-5488-b820-c1df6aab299d"
15 | OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
16 | Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
17 | Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
18 | Revise = "295af30f-e4ad-537b-8983-00126c2a3abe"
19 | RigidBodyDynamics = "366cf18f-59d5-5db9-a4de-86a9f6786172"
20 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
21 | Serialization = "9e88b42a-f829-5b0c-bbe9-9e923198166b"
22 | Sockets = "6462fe0b-24de-5631-8697-dd941f90decc"
23 | SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
24 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
25 | Suppressor = "fd094767-a336-5f1f-9728-57cf17d0bbfb"
26 | 
27 | [compat]
28 | GeometryBasics = "0.4.5"
29 | julia = "~1.9.3"
30 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # VehicleSim
 2 | VehicleSim
 3 | 
 4 | <img src="https://github.com/forrestlaine/VehicleSim/blob/main/parked_cars.png" />
 5 | 
 6 | # Loading / instantiating code
 7 | 
 8 | It is recommended to start julia with multiple threads, since many concurrent tasks will be executing. Important: you must use julia 1.9.3 for all functionality to work properly. 
 9 | 
10 | ```
11 | julia +1.9.3 --project --threads=auto
12 | ```
13 | 
14 | ```julia
15 | (VehicleSim) pkg> instantiate
16 | (VehicleSim) pkg> add https://github.com/forrestlaine/MeshCat.jl
17 | (VehicleSim) pkg> add https://github.com/forrestlaine/RigidBodyDynamics.jl
18 | ```
19 | 
20 | You may need to restart Julia at this point before proceeding.
21 | 
22 | ```julia
23 | julia> using VehicleSim
24 | ```
25 | 
26 | # Running Simulation
27 | 
28 | ```julia
29 | julia> server();
30 | ┌ Info: MeshCat server started. You can open the visualizer by visiting the following URL in your browser:
31 | └ http://1.2.3.4:8700
32 | ┌ Info: ********************
33 | │       CONNECTING TO SERVER
34 | │       ********************
35 | │         -Connect a keyboard client by running (in a new REPL):
36 | │             using Vehicle Sim, Sockets; keyboard_client(ip"1.2.3.4")
37 | └         -Port for manual clients is 4444
38 | [ Info: Target for vehicle 1: 40
39 | ┌ Info: ***************
40 | │       VIEWER COMMANDS
41 | │       ***************
42 | │             -Make sure focus is on this terminal window. Then:
43 | │             -Press 'q' to shutdown server. 
44 | │             -Press '0' to switch to bird's-eye view and release controls to user.
45 | │             -Press a number '1'-'9' to view the follow-cam for the associated vehicle. Will default to '0' if vehicle doesn't exist.
46 | └             -Use the 'shift' modifier to follow-cam from top-down (e.g. '!' for vehicle 1).
47 | [ Info: Waiting for client
48 | ```
49 | 
50 | This will spin up the server / simulation engine. For now, the server will instantiate a single vehicle. 
51 | 
52 | # Connecting a keyboard client
53 | 
54 | In a separate REPL, you can connect to the server with a keyboard client, allowing you to manually drive a vehicle.
55 | 
56 | ```julia
57 | julia> using VehicleSim, Sockets # to allow ip strings
58 | julia> keyboard_client(ip"1.2.3.4") # ip address specified by @info statement when starting server
59 | [ Info: Client accepted.
60 | [ Info: Client follow-cam can be connected to at 1.2.3.4:8713
61 | [ Info: Press 'q' at any time to terminate vehicle.
62 | ```
63 | 


--------------------------------------------------------------------------------
/parked_cars.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/parked_cars.png


--------------------------------------------------------------------------------
/src/VehicleSim.jl:
--------------------------------------------------------------------------------
 1 | module VehicleSim
 2 | 
 3 | using ColorTypes
 4 | using Dates
 5 | using GeometryBasics
 6 | using MeshCat
 7 | using MeshCatMechanisms
 8 | using Random
 9 | using Rotations
10 | using RigidBodyDynamics
11 | using Infiltrator
12 | using LinearAlgebra
13 | using Printf
14 | using SparseArrays
15 | using Suppressor
16 | using Sockets
17 | using Serialization
18 | using StaticArrays
19 | using JLD2
20 | 
21 | include("view_car.jl")
22 | include("objects.jl")
23 | include("sim.jl")
24 | include("client.jl")
25 | include("control.jl")
26 | include("sink.jl")
27 | include("measurements.jl")
28 | include("map.jl")
29 | include("example_project.jl")
30 | 
31 | export server, shutdown!, keyboard_client, GroundTruthMeasurement, CameraMeasurement, IMUMeasurement, GPSMeasurement, VehicleCommand, training_map, load_mechanism, MeasurementMessage
32 | 
33 | end
34 | 


--------------------------------------------------------------------------------
/src/assets/chevy.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0"?>
  2 | 
  3 | <robot name="Chevy">
  4 | 
  5 |     <!--   LINKS   -->
  6 | 
  7 |     <!--link name="road">
  8 |         <visual>
  9 |             <geometry>
 10 |                 <mesh filename="package://assets/meshes/roads_kit.obj" scale="1 1 1"/>
 11 |             </geometry>
 12 |         </visual>
 13 |     </link-->
 14 |     
 15 |     <link name="body">
 16 |         <inertial>
 17 |             <origin xyz="0 0 0" rpy="0 0 0" />
 18 |             <mass value="1000" />
 19 |             <!-- TODO Calculate this correclty. And for lower COM"-->
 20 |             <inertia ixx="4166.67" ixy="0" ixz="0" iyy="10416.67" iyz="0" izz="10416.67" />
 21 |         </inertial>
 22 |         <visual>
 23 |             <origin xyz="7.25 -2.45 0" rpy="0 0 1.57" />
 24 |             <geometry>
 25 |                 <mesh filename="package://assets/meshes/body_all.dae"/>
 26 |             </geometry>
 27 |         </visual>
 28 |         <collision name="body_bounding_box">
 29 |             <origin xyz="0 0 0.05" rpy="0 0 0" />
 30 |             <geometry>
 31 |                 <box size="13.2 5.7 5.3" />
 32 |             </geometry>
 33 |         </collision>
 34 |     </link>
 35 | 
 36 |     <link name="grill">
 37 |         <visual>
 38 |             <origin xyz="-0.20 0.65 -0.55" rpy="0 0 1.57"/>
 39 |             <geometry>
 40 |                 <mesh filename="package://assets/meshes/front.dae"/>
 41 |             </geometry>
 42 |         </visual>
 43 |     </link>
 44 | 
 45 |     <link name="floor">
 46 |         <!-- TODO make most of vehicle mass be in here? -->
 47 |         <visual>
 48 |             <geometry>
 49 |                 <box size="12.5 4.7 1.10"/>
 50 |             </geometry>
 51 |             <material name="grey">
 52 |                 <color rgba="0.8 0.8 0.8 1"/>
 53 |             </material>
 54 |         </visual>
 55 |     </link>
 56 | 
 57 |     <link name="camera_1">
 58 |         <visual>
 59 |             <geometry>
 60 |                 <box size="0.4 0.25 0.25"/>
 61 |             </geometry>
 62 |             <material name="red">
 63 |                 <color rgba="1.0 0.2 0.2 1"/>
 64 |             </material>
 65 |         </visual>
 66 |     </link>
 67 |     
 68 |     <link name="camera_2">
 69 |         <visual>
 70 |             <geometry>
 71 |                 <box size="0.4 0.25 0.25"/>
 72 |             </geometry>
 73 |             <material name="red">
 74 |                 <color rgba="1.0 0.2 0.2 1"/>
 75 |             </material>
 76 |         </visual>
 77 |     </link>
 78 |     
 79 |     <link name="gps">
 80 |         <visual>
 81 |             <geometry>
 82 |                 <cylinder radius="0.1" length="0.5"/>
 83 |             </geometry>
 84 |             <material name="red">
 85 |                 <color rgba="1.0 0.2 0.2 1"/>
 86 |             </material>
 87 |         </visual>
 88 |     </link>
 89 |     
 90 |     <link name="imu">
 91 |         <visual>
 92 |             <geometry>
 93 |                 <box size="0.5 0.5 0.2"/>
 94 |             </geometry>
 95 |             <material name="red">
 96 |                 <color rgba="1.0 0.2 0.2 1"/>
 97 |             </material>
 98 |         </visual>
 99 |     </link>
100 | 
101 |     <link name="front_axle_mount"/>
102 | 
103 |     <link name="front_axle">
104 |         <visual>
105 |             <geometry>
106 |                 <cylinder radius="0.25" length="4.6"/>
107 |             </geometry>
108 |             <material name="grey">
109 |                 <color rgba="0.3 0.3 0.3 1"/>
110 |             </material>
111 |         </visual>
112 |         <inertial>
113 |             <origin xyz="0 0 0" rpy="0 0 0"/>
114 |             <mass value="8.0"/>
115 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
116 |             <inertia ixx="14.2316666667" iyy="14.2316666667" ixy="0.0" ixz="0.0" izz="0.25" iyz="0.0"/>
117 |         </inertial>
118 |     </link>
119 |     
120 |     <link name="rear_axle_mount"/>
121 | 
122 |     <link name="rear_axle">
123 |         <visual>
124 |             <geometry>
125 |                 <cylinder radius="0.25" length="5.0"/>
126 |             </geometry>
127 |             <material name="grey">
128 |                 <color rgba="0.3 0.3 0.3 1"/>
129 |             </material>
130 |         </visual>
131 |         <inertial>
132 |             <origin xyz="0 0 0" rpy="0 0 0"/>
133 |             <mass value="10.0"/>
134 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
135 |             <inertia ixx="20.9895833333" iyy="20.9895833333" ixy="0.0" ixz="0.0" izz="0.3125" iyz="0.0"/>
136 |         </inertial>
137 |     </link>
138 | 
139 |     <link name="front_left_steering_linkage">
140 |         <visual>
141 |             <geometry>
142 |                 <cylinder radius="0.25" length="0.2"/>
143 |             </geometry>
144 |             <material name="grey">
145 |                 <color rgba="0.3 0.3 0.3 1"/>
146 |             </material>
147 |         </visual>
148 |         <inertial>
149 |             <origin xyz="0 0 0" rpy="0 0 0"/>
150 |             <mass value="1.0"/>
151 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
152 |             <inertia ixx="0.01895833333" iyy="0.01895833333" ixy="0.0" ixz="0.0" izz="0.02" iyz="0.0"/>
153 |         </inertial>
154 |     </link>
155 |     <link name="front_right_steering_linkage">
156 |         <visual>
157 |             <geometry>
158 |                 <cylinder radius="0.25" length="0.2"/>
159 |             </geometry>
160 |             <material name="grey">
161 |                 <color rgba="0.3 0.3 0.3 1"/>
162 |             </material>
163 |         </visual>
164 |         <inertial>
165 |             <origin xyz="0 0 0" rpy="0 0 0"/>
166 |             <mass value="1.0"/>
167 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
168 |             <inertia ixx="0.01895833333" iyy="0.01895833333" ixy="0.0" ixz="0.0" izz="0.02" iyz="0.0"/>
169 |         </inertial>
170 |     </link>
171 | 
172 |     <link name="wheel_front_left">
173 |         <inertial>
174 |             <origin xyz="0 0 0" rpy="0 0 0" />
175 |             <mass value="30" />
176 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
177 |         </inertial>
178 |         <visual>
179 |             <origin xyz="0 0 0" rpy="1.57 0 0" />
180 |             <geometry>
181 |                 <mesh filename="package://assets/meshes/wheel.obj" scale="0.107 0.107 0.107"/>
182 |                 <!--<cylinder length="0.75" radius="1.1" />-->
183 |             </geometry>
184 |             <material name="black">
185 |                 <color rgba="0.2 0.2 0.21 1" />
186 |             </material>
187 |         </visual>
188 |     </link>
189 | 
190 |     <link name="wheel_front_right">
191 |         <inertial>
192 |             <origin xyz="0 0 0" rpy="0 0 0" />
193 |             <mass value="30" />
194 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
195 |         </inertial>
196 |         <visual>
197 |             <origin xyz="0 0 0" rpy="1.57 0 0" />
198 |             <geometry>
199 |                 <mesh filename="package://assets/meshes/wheel.obj" scale="0.107 0.107 0.107"/>
200 |                 <!--<cylinder length="0.75" radius="1.1" />-->
201 |             </geometry>
202 |             <material name="black">
203 |                 <color rgba="0.2 0.2 0.21 1" />
204 |             </material>
205 |         </visual>
206 |     </link>
207 | 
208 |     <link name="wheel_rear_left">
209 |         <inertial>
210 |             <origin xyz="0 0 0" rpy="0 0 0" />
211 |             <mass value="30" />
212 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
213 |         </inertial>
214 |         <visual>
215 |             <origin xyz="0 0 0" rpy="1.57 0 0" />
216 |             <geometry>
217 |                 <mesh filename="package://assets/meshes/wheel.obj" scale="0.107 0.107 0.107"/>
218 |                 <!--<cylinder length="0.75" radius="1.1" />-->
219 |             </geometry>
220 |             <material name="black">
221 |                 <color rgba="0.2 0.2 0.21 1" />
222 |             </material>
223 |         </visual>
224 |     </link>
225 | 
226 |     <link name="wheel_rear_right">
227 |         <inertial>
228 |             <origin xyz="0 0 0" rpy="0 0 0" />
229 |             <mass value="30" />
230 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
231 |         </inertial>
232 |         <visual>
233 |             <origin xyz="0 0 0" rpy="1.57 0 0" />
234 |             <geometry>
235 |                 <mesh filename="package://assets/meshes/wheel.obj" scale="0.107 0.107 0.107"/>
236 |                 <!--<cylinder length="0.75" radius="1.1" />-->
237 |             </geometry>
238 |             <material name="black">
239 |                 <color rgba="0.2 0.2 0.21 1" />
240 |             </material>
241 |         </visual>
242 |     </link>
243 | 
244 |     <!--   JOINTS  -->
245 | 
246 |     <!--joint name="world_to_road" type="fixed">
247 |         <parent link="world" />
248 |         <child link="road" />
249 |         <origin xyz="0 -150 0" rpy="1.57 0 0"/>
250 |     </joint-->
251 |     
252 |     <joint name="body_to_grill" type="fixed">
253 |         <parent link="body"/>
254 |         <child link="grill"/>
255 |         <origin xyz="7 0 0"/>
256 |     </joint>
257 |     
258 |     <joint name="body_to_floor" type="fixed">
259 |         <parent link="body"/>
260 |         <child link="floor"/>
261 |         <origin xyz="0 0 0.0" rpy="0 0.02 0"/>
262 |     </joint>
263 |     
264 |     <joint name="body_to_camera_1" type="fixed">
265 |         <parent link="body"/>
266 |         <child link="camera_1"/>
267 |         <origin xyz="1.35 1.7 2.4" rpy="0 0.02 0"/>
268 |     </joint>
269 |     
270 |     <joint name="body_to_camera_2" type="fixed">
271 |         <parent link="body"/>
272 |         <child link="camera_2"/>
273 |         <origin xyz="1.35 -1.7 2.4" rpy="0 0.02 0"/>
274 |     </joint>
275 |     
276 |     <joint name="body_to_gps" type="fixed">
277 |         <parent link="body"/>
278 |         <child link="gps"/>
279 |         <origin xyz="-3.0 1.0 2.6" rpy="0 0 0"/>
280 |     </joint>
281 |     
282 |     <joint name="body_to_imu" type="fixed">
283 |         <parent link="body"/>
284 |         <child link="imu"/>
285 |         <origin xyz="0 0 0.7" rpy="0 0.02 0"/>
286 |     </joint>
287 |     
288 |     <joint name="body_to_front_axle_mount" type="prismatic">
289 |         <parent link="body"/>
290 |         <child link="front_axle_mount"/>
291 |         <origin xyz="4.25 0 -2.25" rpy="0 0 0"/>
292 |         <axis xyz="0 0 1"/>
293 |     </joint>
294 | 
295 |     <joint name="body_to_rear_axle_mount" type="prismatic">
296 |         <parent link="body"/>
297 |         <child link="rear_axle_mount"/>
298 |         <origin xyz="-3.5 0 -2.35" rpy="0 0 0"/>
299 |         <axis xyz="0 0 1.0"/>
300 |     </joint>
301 | 
302 |     <joint name="mount_to_front_axle" type="revolute">
303 |         <parent link="front_axle_mount"/>
304 |         <child link="front_axle"/>
305 |         <origin xyz="0 0 0" rpy="1.57 0 0"/>
306 |         <axis xyz="1 0 0"/>
307 |         <limit lower="-0.4" upper="0.4"/>
308 |     </joint>
309 | 
310 |     <joint name="mount_to_rear_axle" type="revolute">
311 |         <parent link="rear_axle_mount"/>
312 |         <child link="rear_axle"/>
313 |         <origin xyz="0 0 0" rpy="1.57 0 0"/>
314 |         <axis xyz="1 0 0"/>
315 |         <limit lower="-0.4" upper="0.4"/>
316 |     </joint>
317 |     
318 |     <joint name="front_axle_to_left_linkage" type="revolute">
319 |         <parent link="front_axle"/>
320 |         <child link="front_left_steering_linkage"/>
321 |         <origin xyz="0 0 2.3" rpy="0 0 0"/>
322 |         <axis xyz="0 1 0"/>
323 |         <limit lower="-0.6" upper="0.6"/>
324 |     </joint>
325 | 
326 |     <joint name="front_axle_to_right_linkage" type="revolute">
327 |         <parent link="front_axle"/>
328 |         <child link="front_right_steering_linkage"/>
329 |         <origin xyz="0 0 -2.3" rpy="0 0 0"/>
330 |         <axis xyz="0 1 0"/>
331 |         <limit lower="-0.6" upper="0.6"/>
332 |     </joint>
333 | 
334 |     <joint name="front_linkage_to_left_wheel" type="continuous">
335 |         <parent link="front_left_steering_linkage" />
336 |         <child link="wheel_front_left" />
337 |         <origin xyz="0 0 0.2" rpy="0 0 0"/>
338 |         <axis xyz="0 0 1" />
339 |     </joint>
340 | 
341 |     <joint name="front_linkage_to_right_wheel" type="continuous">
342 |         <parent link="front_right_steering_linkage" />
343 |         <child link="wheel_front_right" />
344 |         <origin xyz="0 0 -0.2" rpy="0 0 0"/>
345 |         <axis xyz="0 0 1" />
346 |     </joint>
347 | 
348 |     <joint name="rear_axle_to_left_wheel" type="continuous">
349 |         <parent link="rear_axle" />
350 |         <child link="wheel_rear_left" />
351 |         <origin xyz="0 0 2.5" rpy="0 0 0"/>
352 |         <axis xyz="0 0 1" />
353 |     </joint>
354 | 
355 |     <joint name="rear_axle_to_right_wheel" type="continuous">
356 |         <parent link="rear_axle" />
357 |         <child link="wheel_rear_right" />
358 |         <origin xyz="0 0 -2.5" rpy="0 0 0"/>
359 |         <axis xyz="0 0 1" />
360 |     </joint>
361 | </robot>
362 | 


--------------------------------------------------------------------------------
/src/assets/chevy_nomesh.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0"?>
  2 | 
  3 | <robot name="Chevy">
  4 | 
  5 |     <!--   LINKS   -->
  6 | 
  7 |     <link name="body">
  8 |         <inertial>
  9 |             <origin xyz="0 0 0" rpy="0 0 0" />
 10 |             <mass value="1000" />
 11 |             <!-- TODO Calculate this correclty. And for lower COM"-->
 12 |             <inertia ixx="4166.67" ixy="0" ixz="0" iyy="10416.67" iyz="0" izz="10416.67" />
 13 |         </inertial>
 14 |         <visual>
 15 |             <origin xyz="0.5 0 0.05" rpy="0 0 0" />
 16 |             <geometry>
 17 |                 <box size="11.0 5.0 4.0" />
 18 |             </geometry>
 19 |             <material name="blue">
 20 |                 <color rgba="0.2 0.2 1.0 1"/>
 21 |             </material>
 22 |         </visual>
 23 |         <collision name="body_bounding_box">
 24 |             <origin xyz="0 0 0.05" rpy="0 0 0" />
 25 |             <geometry>
 26 |                 <box size="13.2 5.7 5.3" />
 27 |             </geometry>
 28 |         </collision>
 29 |     </link>
 30 | 
 31 |     <link name="camera_1">
 32 |         <visual>
 33 |             <geometry>
 34 |                 <box size="0.4 0.25 0.25"/>
 35 |             </geometry>
 36 |             <material name="red">
 37 |                 <color rgba="1.0 0.2 0.2 1"/>
 38 |             </material>
 39 |         </visual>
 40 |     </link>
 41 |     
 42 |     <link name="camera_2">
 43 |         <visual>
 44 |             <geometry>
 45 |                 <box size="0.4 0.25 0.25"/>
 46 |             </geometry>
 47 |             <material name="red">
 48 |                 <color rgba="1.0 0.2 0.2 1"/>
 49 |             </material>
 50 |         </visual>
 51 |     </link>
 52 |     
 53 |     <link name="gps">
 54 |         <visual>
 55 |             <geometry>
 56 |                 <cylinder radius="0.1" length="0.5"/>
 57 |             </geometry>
 58 |             <material name="red">
 59 |                 <color rgba="1.0 0.2 0.2 1"/>
 60 |             </material>
 61 |         </visual>
 62 |     </link>
 63 |     
 64 |     <link name="imu">
 65 |         <visual>
 66 |             <geometry>
 67 |                 <box size="0.5 0.5 0.2"/>
 68 |             </geometry>
 69 |             <material name="red">
 70 |                 <color rgba="1.0 0.2 0.2 1"/>
 71 |             </material>
 72 |         </visual>
 73 |     </link>
 74 | 
 75 |     <link name="front_axle_mount"/>
 76 | 
 77 |     <link name="front_axle">
 78 |         <visual>
 79 |             <geometry>
 80 |                 <cylinder radius="0.25" length="4.6"/>
 81 |             </geometry>
 82 |             <material name="grey">
 83 |                 <color rgba="0.3 0.3 0.3 1"/>
 84 |             </material>
 85 |         </visual>
 86 |         <inertial>
 87 |             <origin xyz="0 0 0" rpy="0 0 0"/>
 88 |             <mass value="8.0"/>
 89 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
 90 |             <inertia ixx="14.2316666667" iyy="14.2316666667" ixy="0.0" ixz="0.0" izz="0.25" iyz="0.0"/>
 91 |         </inertial>
 92 |     </link>
 93 |     
 94 |     <link name="rear_axle_mount"/>
 95 | 
 96 |     <link name="rear_axle">
 97 |         <visual>
 98 |             <geometry>
 99 |                 <cylinder radius="0.25" length="5.0"/>
100 |             </geometry>
101 |             <material name="grey">
102 |                 <color rgba="0.3 0.3 0.3 1"/>
103 |             </material>
104 |         </visual>
105 |         <inertial>
106 |             <origin xyz="0 0 0" rpy="0 0 0"/>
107 |             <mass value="10.0"/>
108 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
109 |             <inertia ixx="20.9895833333" iyy="20.9895833333" ixy="0.0" ixz="0.0" izz="0.3125" iyz="0.0"/>
110 |         </inertial>
111 |     </link>
112 | 
113 |     <link name="front_left_steering_linkage">
114 |         <visual>
115 |             <geometry>
116 |                 <cylinder radius="0.25" length="0.2"/>
117 |             </geometry>
118 |             <material name="grey">
119 |                 <color rgba="0.3 0.3 0.3 1"/>
120 |             </material>
121 |         </visual>
122 |         <inertial>
123 |             <origin xyz="0 0 0" rpy="0 0 0"/>
124 |             <mass value="1.0"/>
125 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
126 |             <inertia ixx="0.01895833333" iyy="0.01895833333" ixy="0.0" ixz="0.0" izz="0.02" iyz="0.0"/>
127 |         </inertial>
128 |     </link>
129 |     <link name="front_right_steering_linkage">
130 |         <visual>
131 |             <geometry>
132 |                 <cylinder radius="0.25" length="0.2"/>
133 |             </geometry>
134 |             <material name="grey">
135 |                 <color rgba="0.3 0.3 0.3 1"/>
136 |             </material>
137 |         </visual>
138 |         <inertial>
139 |             <origin xyz="0 0 0" rpy="0 0 0"/>
140 |             <mass value="1.0"/>
141 |             <!--inertia ixx=iyy=1/12 * m * ( 3r^2 + h^2) izz=1/2*m*r^2     -->
142 |             <inertia ixx="0.01895833333" iyy="0.01895833333" ixy="0.0" ixz="0.0" izz="0.02" iyz="0.0"/>
143 |         </inertial>
144 |     </link>
145 | 
146 |     <link name="wheel_front_left">
147 |         <inertial>
148 |             <origin xyz="0 0 0" rpy="0 0 0" />
149 |             <mass value="30" />
150 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
151 |         </inertial>
152 |         <visual>
153 |             <origin xyz="0 0 0" rpy="0 0 0" />
154 |             <geometry>
155 |                 <cylinder length="0.75" radius="1.1" />
156 |             </geometry>
157 |             <material name="black">
158 |                 <color rgba="0.2 0.2 0.21 1" />
159 |             </material>
160 |         </visual>
161 |     </link>
162 | 
163 |     <link name="wheel_front_right">
164 |         <inertial>
165 |             <origin xyz="0 0 0" rpy="0 0 0" />
166 |             <mass value="30" />
167 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
168 |         </inertial>
169 |         <visual>
170 |             <origin xyz="0 0 0" rpy="0 0 0" />
171 |             <geometry>
172 |                 <cylinder length="0.75" radius="1.1" />
173 |             </geometry>
174 |             <material name="black">
175 |                 <color rgba="0.2 0.2 0.21 1" />
176 |             </material>
177 |         </visual>
178 |     </link>
179 | 
180 |     <link name="wheel_rear_left">
181 |         <inertial>
182 |             <origin xyz="0 0 0" rpy="0 0 0" />
183 |             <mass value="30" />
184 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
185 |         </inertial>
186 |         <visual>
187 |             <origin xyz="0 0 0" rpy="0 0 0" />
188 |             <geometry>
189 |                 <cylinder length="0.75" radius="1.1" />
190 |             </geometry>
191 |             <material name="black">
192 |                 <color rgba="0.2 0.2 0.21 1" />
193 |             </material>
194 |         </visual>
195 |     </link>
196 | 
197 |     <link name="wheel_rear_right">
198 |         <inertial>
199 |             <origin xyz="0 0 0" rpy="0 0 0" />
200 |             <mass value="30" />
201 |             <inertia ixx="10.4812" ixy="0" ixz="0" iyy="10.48125" iyz="0" izz="18.15" />
202 |         </inertial>
203 |         <visual>
204 |             <origin xyz="0 0 0" rpy="0 0 0" />
205 |             <geometry>
206 |                 <cylinder length="0.75" radius="1.1" />
207 |             </geometry>
208 |             <material name="black">
209 |                 <color rgba="0.2 0.2 0.21 1" />
210 |             </material>
211 |         </visual>
212 |     </link>
213 | 
214 |     <!--   JOINTS  -->
215 | 
216 |     <!--joint name="world_to_road" type="fixed">
217 |         <parent link="world" />
218 |         <child link="road" />
219 |         <origin xyz="0 -150 0" rpy="1.57 0 0"/>
220 |     </joint-->
221 |     
222 |     <joint name="body_to_camera_1" type="fixed">
223 |         <parent link="body"/>
224 |         <child link="camera_1"/>
225 |         <origin xyz="1.35 1.7 2.4" rpy="0 0.02 0"/>
226 |     </joint>
227 |     
228 |     <joint name="body_to_camera_2" type="fixed">
229 |         <parent link="body"/>
230 |         <child link="camera_2"/>
231 |         <origin xyz="1.35 -1.7 2.4" rpy="0 0.02 0"/>
232 |     </joint>
233 |     
234 |     <joint name="body_to_gps" type="fixed">
235 |         <parent link="body"/>
236 |         <child link="gps"/>
237 |         <origin xyz="-3.0 1.0 2.6" rpy="0 0 0"/>
238 |     </joint>
239 |     
240 |     <joint name="body_to_imu" type="fixed">
241 |         <parent link="body"/>
242 |         <child link="imu"/>
243 |         <origin xyz="0 0 0.7" rpy="0 0.02 0"/>
244 |     </joint>
245 |     
246 |     <joint name="body_to_front_axle_mount" type="prismatic">
247 |         <parent link="body"/>
248 |         <child link="front_axle_mount"/>
249 |         <origin xyz="4.25 0 -2.25" rpy="0 0 0"/>
250 |         <axis xyz="0 0 1"/>
251 |     </joint>
252 | 
253 |     <joint name="body_to_rear_axle_mount" type="prismatic">
254 |         <parent link="body"/>
255 |         <child link="rear_axle_mount"/>
256 |         <origin xyz="-3.5 0 -2.35" rpy="0 0 0"/>
257 |         <axis xyz="0 0 1.0"/>
258 |     </joint>
259 | 
260 |     <joint name="mount_to_front_axle" type="revolute">
261 |         <parent link="front_axle_mount"/>
262 |         <child link="front_axle"/>
263 |         <origin xyz="0 0 0" rpy="1.57 0 0"/>
264 |         <axis xyz="1 0 0"/>
265 |         <limit lower="-0.4" upper="0.4"/>
266 |     </joint>
267 | 
268 |     <joint name="mount_to_rear_axle" type="revolute">
269 |         <parent link="rear_axle_mount"/>
270 |         <child link="rear_axle"/>
271 |         <origin xyz="0 0 0" rpy="1.57 0 0"/>
272 |         <axis xyz="1 0 0"/>
273 |         <limit lower="-0.4" upper="0.4"/>
274 |     </joint>
275 |     
276 |     <joint name="front_axle_to_left_linkage" type="revolute">
277 |         <parent link="front_axle"/>
278 |         <child link="front_left_steering_linkage"/>
279 |         <origin xyz="0 0 2.3" rpy="0 0 0"/>
280 |         <axis xyz="0 1 0"/>
281 |         <limit lower="-0.6" upper="0.6"/>
282 |     </joint>
283 | 
284 |     <joint name="front_axle_to_right_linkage" type="revolute">
285 |         <parent link="front_axle"/>
286 |         <child link="front_right_steering_linkage"/>
287 |         <origin xyz="0 0 -2.3" rpy="0 0 0"/>
288 |         <axis xyz="0 1 0"/>
289 |         <limit lower="-0.6" upper="0.6"/>
290 |     </joint>
291 | 
292 |     <joint name="front_linkage_to_left_wheel" type="continuous">
293 |         <parent link="front_left_steering_linkage" />
294 |         <child link="wheel_front_left" />
295 |         <origin xyz="0 0 0.2" rpy="0 0 0"/>
296 |         <axis xyz="0 0 1" />
297 |     </joint>
298 | 
299 |     <joint name="front_linkage_to_right_wheel" type="continuous">
300 |         <parent link="front_right_steering_linkage" />
301 |         <child link="wheel_front_right" />
302 |         <origin xyz="0 0 -0.2" rpy="0 0 0"/>
303 |         <axis xyz="0 0 1" />
304 |     </joint>
305 | 
306 |     <joint name="rear_axle_to_left_wheel" type="continuous">
307 |         <parent link="rear_axle" />
308 |         <child link="wheel_rear_left" />
309 |         <origin xyz="0 0 2.5" rpy="0 0 0"/>
310 |         <axis xyz="0 0 1" />
311 |     </joint>
312 | 
313 |     <joint name="rear_axle_to_right_wheel" type="continuous">
314 |         <parent link="rear_axle" />
315 |         <child link="wheel_rear_right" />
316 |         <origin xyz="0 0 -2.5" rpy="0 0 0"/>
317 |         <axis xyz="0 0 1" />
318 |     </joint>
319 | </robot>
320 | 


--------------------------------------------------------------------------------
/src/assets/meshes/bumpers.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/bumpers.stl


--------------------------------------------------------------------------------
/src/assets/meshes/leaf.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/leaf.stl


--------------------------------------------------------------------------------
/src/assets/meshes/leaf2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/leaf2.stl


--------------------------------------------------------------------------------
/src/assets/meshes/rama_1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/rama_1.stl


--------------------------------------------------------------------------------
/src/assets/meshes/rama_2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/rama_2.stl


--------------------------------------------------------------------------------
/src/assets/meshes/rama_3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/rama_3.stl


--------------------------------------------------------------------------------
/src/assets/meshes/shackles.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/shackles.stl


--------------------------------------------------------------------------------
/src/assets/meshes/steer.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/steer.stl


--------------------------------------------------------------------------------
/src/assets/meshes/wheel.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/VAMPIR-Lab/VehicleSim/3131b4d595b687cdcce822fb96d000be5c2afb7e/src/assets/meshes/wheel.stl


--------------------------------------------------------------------------------
/src/client.jl:
--------------------------------------------------------------------------------
  1 | struct VehicleCommand
  2 |     steering_angle::Float64
  3 |     velocity::Float64
  4 |     controlled::Bool
  5 | end
  6 | 
  7 | function get_c()
  8 |     c = 'x'
  9 |     try
 10 |         ret = ccall(:jl_tty_set_mode, Int32, (Ptr{Cvoid},Int32), stdin.handle, true)
 11 |         ret == 0 || error("unable to switch to raw mode")
 12 |         c = read(stdin, Char)
 13 |         ccall(:jl_tty_set_mode, Int32, (Ptr{Cvoid},Int32), stdin.handle, false)
 14 |     catch e
 15 |     end
 16 |     c
 17 | end
 18 | 
 19 | function keyboard_client(host::IPAddr=IPv4(0), port=4444; v_step = 1.0, s_step = π/10, collect_buffer=false)
 20 |     socket = Sockets.connect(host, port)
 21 |     (peer_host, peer_port) = getpeername(socket)
 22 |     msg = deserialize(socket) # Visualization info
 23 |     @info msg
 24 | 
 25 |     errormonitor(@async begin
 26 |         msg_buf = []
 27 |         while isopen(socket)
 28 |             sleep(0.001)
 29 |             local state_msg
 30 |             try
 31 |                 state_msg = deserialize(socket)
 32 |             catch err
 33 |                 break
 34 |             end
 35 |             push!(msg_buf, state_msg)
 36 |             measurements = state_msg.measurements
 37 |             num_cam = 0
 38 |             num_imu = 0
 39 |             num_gps = 0
 40 |             num_gt = 0
 41 | 
 42 |             for meas in measurements
 43 |                 if meas isa GroundTruthMeasurement
 44 |                     num_gt += 1
 45 |                 elseif meas isa CameraMeasurement
 46 |                     num_cam += 1
 47 |                 elseif meas isa IMUMeasurement
 48 |                     num_imu += 1
 49 |                 elseif meas isa GPSMeasurement
 50 |                     num_gps += 1
 51 |                 end
 52 |             end
 53 |         end
 54 |         if collect_buffer 
 55 |             jldsave("message_buff.jld2"; msg_buf)
 56 |             @info "Message buffer saved to message_buff.jld2"
 57 |         end
 58 |     end)
 59 |     
 60 |     target_velocity = 0.0
 61 |     steering_angle = 0.0
 62 |     controlled = true
 63 |     
 64 |     client_info_string = 
 65 |         "********************
 66 |       Keyboard Control (manual mode)
 67 |       ********************
 68 |         -Press 'q' at any time to terminate vehicle.
 69 |         -Press 'i' to increase vehicle speed.
 70 |         -Press 'k' to decrease vehicle speed.
 71 |         -Press 'j' to increase steering angle (turn left).
 72 |         -Press 'l' to decrease steering angle (turn right)."
 73 |     @info client_info_string
 74 |     while controlled && isopen(socket)
 75 |         key = get_c()
 76 |         if key == 'q'
 77 |             # terminate vehicle
 78 |             controlled = false
 79 |             target_velocity = 0.0
 80 |             steering_angle = 0.0
 81 |             @info "Terminating Keyboard Client."
 82 |         elseif key == 'i'
 83 |             # increase target velocity
 84 |             target_velocity += v_step
 85 |             @info "Target velocity: $target_velocity"
 86 |         elseif key == 'k'
 87 |             # decrease forward force
 88 |             target_velocity -= v_step
 89 |             @info "Target velocity: $target_velocity"
 90 |         elseif key == 'j'
 91 |             # increase steering angle
 92 |             steering_angle += s_step
 93 |             @info "Target steering angle: $steering_angle"
 94 |         elseif key == 'l'
 95 |             # decrease steering angle
 96 |             steering_angle -= s_step
 97 |             @info "Target steering angle: $steering_angle"
 98 |         end
 99 |         cmd = (steering_angle, target_velocity, controlled)
100 |         serialize(socket, cmd)
101 |     end
102 | end
103 | 
104 | function example_client(host::IPAddr=IPv4(0), port=4444)
105 |     socket = Sockets.connect(host, port)
106 |     map_segments = training_map()
107 |     (; chevy_base) = load_mechanism()
108 | 
109 |     @async while isopen(socket)
110 |         state_msg = deserialize(socket)
111 |     end
112 |    
113 |     shutdown = false
114 |     persist = true
115 |     while isopen(socket)
116 |         position = state_msg.q[5:7]
117 |         @info position
118 |         if norm(position) >= 100
119 |             shutdown = true
120 |             persist = false
121 |         end
122 |         cmd = (0.0, 2.5, persist, shutdown)
123 |         serialize(socket, cmd) 
124 |     end
125 | 
126 | end
127 | 


--------------------------------------------------------------------------------
/src/control.jl:
--------------------------------------------------------------------------------
 1 | function wheel_control!(bodyid_to_wrench, chevy, t, state::MechanismState;
 2 |         reference_velocity=0.0, k₁=-1000.0)
 3 |     forward_velocity = state.v[4]
 4 |     drive_force = k₁ * (forward_velocity - reference_velocity)
 5 | 
 6 |     for i = 7:8
 7 |         bodyid = BodyID(i)
 8 |         wheel = bodies(chevy)[i]
 9 |         frame = wheel.frame_definitions[1].from
10 |         body_to_root = transform_to_root(state, bodyid, false)
11 |         wrench = Wrench{Float64}(frame, [0.0,0,0], [drive_force,0,0])
12 |         bodyid_to_wrench[BodyID(i)] = transform(wrench, body_to_root)
13 |     end
14 |     nothing
15 | end
16 | 
17 | function steering_control!(torques::AbstractVector, t, state::MechanismState;
18 |         reference_angle=0.0, k₁=-6500.0, k₂=-2500.0)
19 |     js = joints(state.mechanism)
20 |     linkage_left = js[6]
21 |     linkage_right = js[7]
22 | 
23 |     actual = configuration(state, linkage_left)
24 | 
25 |     torques[velocity_range(state, linkage_left)] .= k₁ * (configuration(state, linkage_left) .- reference_angle) + k₂ * velocity(state, linkage_left)
26 |     torques[velocity_range(state, linkage_right)] .= k₁ * (configuration(state, linkage_right) .- reference_angle) + k₂ * velocity(state, linkage_right)
27 | end
28 | 
29 | function suspension_control!(torques::AbstractVector, t, state::MechanismState; k₁=-6500.0, k₂=-2500.0, k₃ = -30000.0, k₄=-15000.0)
30 |     js = joints(state.mechanism)
31 |     front_axle_mount = js[2]
32 |     rear_axle_mount = js[3]
33 |     front_axle_roll = js[4]
34 |     rear_axle_roll = js[5]
35 | 
36 |     torques[velocity_range(state, front_axle_mount)] .= k₁ * configuration(state, front_axle_mount) + k₂ * velocity(state, front_axle_mount)
37 |     torques[velocity_range(state, rear_axle_mount)] .= k₁ * configuration(state, rear_axle_mount) + k₂ * velocity(state, rear_axle_mount)
38 |     torques[velocity_range(state, front_axle_roll)] .= k₃ * configuration(state, front_axle_roll) + k₄ * velocity(state, front_axle_roll)
39 |     torques[velocity_range(state, rear_axle_roll)] .= k₃ * configuration(state, rear_axle_roll) + k₄ * velocity(state, rear_axle_roll)
40 | end
41 | 


--------------------------------------------------------------------------------
/src/example_project.jl:
--------------------------------------------------------------------------------
  1 | struct MyLocalizationType
  2 |     field1::Int
  3 |     field2::Float64
  4 | end
  5 | 
  6 | struct MyPerceptionType
  7 |     field1::Int
  8 |     field2::Float64
  9 | end
 10 | 
 11 | function process_gt(
 12 |         gt_channel,
 13 |         shutdown_channel,
 14 |         localization_state_channel,
 15 |         perception_state_channel)
 16 | 
 17 |     while true
 18 |         fetch(shutdown_channel) && break
 19 | 
 20 |         fresh_gt_meas = []
 21 |         while isready(gt_channel)
 22 |             meas = take!(gt_channel)
 23 |             push!(fresh_gt_meas, meas)
 24 |         end
 25 | 
 26 |         # process the fresh gt_measurements to produce localization_state and
 27 |         # perception_state
 28 |         
 29 |         take!(localization_state_channel)
 30 |         put!(localization_state_channel, new_localization_state_from_gt)
 31 |         
 32 |         take!(perception_state_channel)
 33 |         put!(perception_state_channel, new_perception_state_from_gt)
 34 |     end
 35 | end
 36 | 
 37 | function localize(
 38 |         gps_channel, 
 39 |         imu_channel, 
 40 |         localization_state_channel, 
 41 |         shutdown_channel)
 42 |     # Set up algorithm / initialize variables
 43 |     while true
 44 | 
 45 |         fetch(shutdown_channel) && break
 46 | 
 47 |         fresh_gps_meas = []
 48 |         while isready(gps_channel)
 49 |             meas = take!(gps_channel)
 50 |             push!(fresh_gps_meas, meas)
 51 |         end
 52 |         fresh_imu_meas = []
 53 |         while isready(imu_channel)
 54 |             meas = take!(imu_channel)
 55 |             push!(fresh_imu_meas, meas)
 56 |         end
 57 |         
 58 |         # process measurements
 59 |         take!(localization_state_channel)
 60 |         put!(localization_state_channel, localization_state)
 61 |     end 
 62 | end
 63 | 
 64 | function perception(cam_meas_channel, localization_state_channel, perception_state_channel, shutdown_channel)
 65 |     # set up stuff
 66 |     while true
 67 |         
 68 |         fetch(shutdown_channel) && break
 69 | 
 70 |         fresh_cam_meas = []
 71 |         while isready(cam_meas_channel)
 72 |             meas = take!(cam_meas_channel)
 73 |             push!(fresh_cam_meas, meas)
 74 |         end
 75 | 
 76 |         latest_localization_state = fetch(localization_state_channel)
 77 |         
 78 |         # process bounding boxes / run ekf / do what you think is good
 79 | 
 80 |         perception_state = MyPerceptionType(0,0.0)
 81 |         if isready(perception_state_channel)
 82 |             take!(perception_state_channel)
 83 |         end
 84 |         put!(perception_state_channel, perception_state)
 85 |     end
 86 | end
 87 | 
 88 | function decision_making(localization_state_channel, 
 89 |         perception_state_channel, 
 90 |         target_segment_channel,
 91 |         shutdown_channel,
 92 |         map, 
 93 |         socket)
 94 |     # do some setup
 95 |     while true
 96 | 
 97 |         fetch(shutdown_channel) && break
 98 | 
 99 |         latest_localization_state = fetch(localization_state_channel)
100 |         latest_perception_state = fetch(perception_state_channel)
101 | 
102 |         # figure out what to do ... setup motion planning problem etc
103 |         steering_angle = 0.0
104 |         target_vel = 0.0
105 |         cmd = (steering_angle, target_vel, true)
106 |         serialize(socket, cmd)
107 |     end
108 | end
109 | 
110 | function isfull(ch::Channel)
111 |     length(ch.data) ≥ ch.sz_max
112 | end
113 | 
114 | 
115 | function my_client(host::IPAddr=IPv4(0), port=4444; use_gt=false)
116 |     socket = Sockets.connect(host, port)
117 |     map_segments = VehicleSim.city_map()
118 |     
119 |     msg = deserialize(socket) # Visualization info
120 |     @info msg
121 | 
122 |     gps_channel = Channel{GPSMeasurement}(32)
123 |     imu_channel = Channel{IMUMeasurement}(32)
124 |     cam_channel = Channel{CameraMeasurement}(32)
125 |     gt_channel = Channel{GroundTruthMeasurement}(32)
126 | 
127 |     localization_state_channel = Channel{MyLocalizationType}(1)
128 |     perception_state_channel = Channel{MyPerceptionType}(1)
129 |     target_segment_channel = Channel{Int}(1)
130 |     shutdown_channel = Channel{Bool}(1)
131 |     put!(shutdown_channel, false)
132 | 
133 |     target_map_segment = 0 # (not a valid segment, will be overwritten by message)
134 |     ego_vehicle_id = 0 # (not a valid id, will be overwritten by message. This is used for discerning ground-truth messages)
135 | 
136 |     put!(target_segment_channel, target_map_segment)
137 | 
138 |     errormonitor(@async while true
139 |         # This while loop reads to the end of the socket stream (makes sure you
140 |         # are looking at the latest messages)
141 |         sleep(0.001)
142 |         local measurement_msg
143 |         received = false
144 |         while true
145 |             @async eof(socket)
146 |             if bytesavailable(socket) > 0
147 |                 measurement_msg = deserialize(socket)
148 |                 received = true
149 |             else
150 |                 break
151 |             end
152 |         end
153 |         !received && continue
154 |         target_map_segment = measurement_msg.target_segment
155 |         old_target_segment = fetch(target_segment_channel)
156 |         if target_map_segment ≠ old_target_segment
157 |             take!(target_segment_channel)
158 |             put!(target_segment_channel, target_map_segment)
159 |         end
160 |         ego_vehicle_id = measurement_msg.vehicle_id
161 |         for meas in measurement_msg.measurements
162 |             if meas isa GPSMeasurement
163 |                 !isfull(gps_channel) && put!(gps_channel, meas)
164 |             elseif meas isa IMUMeasurement
165 |                 !isfull(imu_channel) && put!(imu_channel, meas)
166 |             elseif meas isa CameraMeasurement
167 |                 !isfull(cam_channel) && put!(cam_channel, meas)
168 |             elseif meas isa GroundTruthMeasurement
169 |                 !isfull(gt_channel) && put!(gt_channel, meas)
170 |             end
171 |         end
172 |     end)
173 | 
174 |     if use_gt
175 |         @async process_gt(gt_channel,
176 |                       shutdown_channel,
177 |                       localization_state_channel,
178 |                       perception_state_channel)
179 |     else
180 |         @async localize(gps_channel, 
181 |                     imu_channel, 
182 |                     localization_state_channel, 
183 |                     shutdown_channel)
184 | 
185 |         @async perception(cam_channel, 
186 |                       localization_state_channel, 
187 |                       perception_state_channel, 
188 |                       shutdown_channel)
189 |     end
190 | 
191 | 
192 | 
193 |     @async decision_making(localization_state_channel, 
194 |                            perception_state_channel, 
195 |                            target_segment_channel, 
196 |                            shutdown_channel,
197 |                            map, 
198 |                            socket)
199 | end
200 | 
201 | function shutdown_listener(shutdown_channel)
202 |     info_string = 
203 |         "***************
204 |       CLIENT COMMANDS
205 |       ***************
206 |             -Make sure focus is on this terminal window. Then:
207 |             -Press 'q' to shutdown threads. 
208 |     "
209 |     @info info_string
210 |     while true
211 |         sleep(0.1)
212 |         key = get_c()
213 | 
214 |         if key == 'q'
215 |             # terminate threads
216 |             take!(shutdown_channel)
217 |             put!(shutdown_channel, true)
218 |             break
219 |         end
220 |     end
221 | end
222 | 


--------------------------------------------------------------------------------
/src/map.jl:
--------------------------------------------------------------------------------
  1 | @enum LaneTypes begin
  2 |     standard
  3 |     loading_zone
  4 |     intersection
  5 |     stop_sign
  6 | end
  7 | 
  8 | @enum Direction begin
  9 |     north
 10 |     east
 11 |     south
 12 |     west
 13 | end
 14 | 
 15 | function opposite(direction::Direction)
 16 |     direction == north && return south
 17 |     direction == east && return west
 18 |     direction == south && return north
 19 |     direction == west && return east
 20 | end
 21 | 
 22 | 
 23 | 
 24 | """
 25 | Assume that pt_b is further along driving direction than pt_a
 26 | """
 27 | struct LaneBoundary
 28 |     pt_a::SVector{2,Float64}
 29 |     pt_b::SVector{2,Float64}
 30 |     curvature::Float64
 31 |     hard_boundary::Bool
 32 |     visualized::Bool
 33 | end
 34 | 
 35 | function lane_boundary(pt_a, pt_b, hard, vis, left=true)
 36 |     dx = abs(pt_a[1] - pt_b[1])
 37 |     dy = abs(pt_a[2] - pt_b[2])
 38 |     if isapprox(dx - dy, 0; atol=1e-6)
 39 |         sign = left ? 1.0 : -1.0
 40 |         LaneBoundary(pt_a, pt_b, sign / dx, hard, vis)
 41 |     else
 42 |         LaneBoundary(pt_a, pt_b, 0.0, hard, vis)
 43 |     end
 44 | end
 45 | 
 46 | """
 47 | lane_boundaries are in order from left to right
 48 | """
 49 | mutable struct RoadSegment
 50 |     id::Int
 51 |     lane_boundaries::Vector{LaneBoundary}
 52 |     lane_types::Vector{LaneTypes}
 53 |     speed_limit::Float64
 54 |     children::Vector{Int}
 55 | end
 56 | 
 57 | function reached_target(pos, vel, seg)
 58 |     @assert length(seg.lane_types) > 1 && seg.lane_types[2] == loading_zone
 59 |     A = seg.lane_boundaries[2].pt_a
 60 |     B = seg.lane_boundaries[2].pt_b
 61 |     C = seg.lane_boundaries[3].pt_a
 62 |     D = seg.lane_boundaries[3].pt_b
 63 |     min_x = min(A[1], B[1], C[1], D[1])
 64 |     max_x = max(A[1], B[1], C[1], D[1])
 65 |     min_y = min(A[2], B[2], C[2], D[2])
 66 |     max_y = max(A[2], B[2], C[2], D[2])
 67 |     return min_x <= pos[1] <= max_x && min_y <= pos[2] <= max_y && abs(vel) < 0.1
 68 | end
 69 | 
 70 | """
 71 | Assuming only 90° turns for now
 72 | """
 73 | function generate_laneline_mesh(lb::LaneBoundary; res=1.0, width=0.3)
 74 |     pt_a = lb.pt_a
 75 |     pt_b = lb.pt_b
 76 |     curvature = lb.curvature
 77 |     if lb.visualized
 78 |         if lb.hard_boundary
 79 |             color = RGBA{Float32}(1,1,0,1)
 80 |         else
 81 |             color = RGBA{Float32}(1,1,0,0.5)
 82 |         end
 83 |     else
 84 |         return nothing
 85 |     end
 86 |     curved = !isapprox(curvature, 0.0; atol=1e-6)
 87 |     delta = pt_b-pt_a
 88 |     if curved
 89 |         rad = 1.0 / abs(curvature)
 90 |         dist = π*rad/2.0
 91 |     else
 92 |         dist = norm(pt_b-pt_a) - 0.02
 93 |     end
 94 |     N = (dist / res) |> ceil |> Int
 95 |     Δ = dist / N
 96 |     t = 0:Δ:dist .+ 0.01
 97 |     if curved
 98 |         left = curvature > 0
 99 |         if left
100 |             if sign(delta[1]) == sign(delta[2])
101 |                 center = pt_a + [0, delta[2]]
102 |             else
103 |                 center = pt_a + [delta[1], 0]
104 |             end
105 |         else
106 |             if sign(delta[1]) == sign(delta[2])
107 |                 center = pt_a + [delta[1], 0]
108 |             else
109 |                 center = pt_a + [0, delta[2]]
110 |             end
111 |         end
112 |         pt_a_rel = pt_a - center
113 |         pt_b_rel = pt_b - center
114 |         θ0 = atan(pt_a_rel[2], pt_a_rel[1])
115 |         θT = atan(pt_b_rel[2], pt_b_rel[1]) 
116 |         Δθ = mod((θT - θ0) + π, 2π) - π
117 |         dθ = sign(Δθ) * π/2.0 / N
118 |         rad_1 = rad - width/2
119 |         rad_2 = rad + width/2
120 |         points = mapreduce(vcat, 0:N) do i
121 |             θ = θ0 + dθ * i
122 |             [GeometryBasics.Point{3,Float64}(center[1]+rad_1*cos(θ), center[2]+rad_1*sin(θ), 0),
123 |              GeometryBasics.Point{3,Float64}(center[1]+rad_2*cos(θ), center[2]+rad_2*sin(θ), 0)]
124 |         end
125 |     else
126 |         dir = delta / norm(delta)
127 |         right = [dir[2], -dir[1]]
128 |         points = mapreduce(vcat, t) do dist  
129 |             pt_1 = pt_a+dir*dist-width/2*right
130 |             pt_2 = pt_a+dir*dist+width/2*right
131 |             [GeometryBasics.Point{3, Float64}(pt_1[1], pt_1[2], 0),
132 |              GeometryBasics.Point{3, Float64}(pt_2[1], pt_2[2], 0)]
133 |         end
134 |     end
135 |     K = length(points)/2 |> Int
136 | 
137 |     faces = mapreduce(vcat, 1:K-1) do k
138 |         [GeometryBasics.TriangleFace((k-1)*2+1,k*2+1,k*2+2),
139 |          GeometryBasics.TriangleFace((k-1)*2+1,k*2,k*2+2)]
140 |     end
141 |     mesh = GeometryBasics.Mesh(points, faces)
142 |     MeshCat.Object(mesh, MeshPhongMaterial(color=color))
143 | end
144 | 
145 | function generate_lane_mesh(lb1, lb2, lane_type; width=0.3, res=1.0)
146 |     if lane_type == standard || lane_type == stop_sign || lane_type == intersection
147 |         color=RGBA{Float32}(.35,.35,.35,1)
148 |     elseif lane_type == loading_zone
149 |         color=RGBA{Float32}(.5,.5,.5,1)
150 |     end
151 |     pt_a = lb1.pt_a
152 |     pt_b = lb1.pt_b
153 |     curvature = lb1.curvature
154 |     curved = !isapprox(curvature, 0.0; atol=1e-6)
155 |     delta = pt_b-pt_a
156 |     if curved
157 |         rad = 1.0 / abs(curvature)
158 |         dist = π*rad/2.0
159 |     else
160 |         dist = norm(pt_b-pt_a)-0.002
161 |     end
162 |     N = (dist / res) |> ceil |> Int
163 |     if curved
164 |         left = curvature > 0
165 |         if left
166 |             if sign(delta[1]) == sign(delta[2])
167 |                 center = pt_a + [0, delta[2]]
168 |             else
169 |                 center = pt_a + [delta[1], 0]
170 |             end
171 |         else
172 |             if sign(delta[1]) == sign(delta[2])
173 |                 center = pt_a + [delta[1], 0]
174 |             else
175 |                 center = pt_a + [0, delta[2]]
176 |             end
177 |         end
178 |         pt_a_rel = pt_a - center
179 |         pt_b_rel = pt_b - center
180 |         θ0 = atan(pt_a_rel[2], pt_a_rel[1])
181 |         θT = atan(pt_b_rel[2], pt_b_rel[1]) 
182 |         Δθ = mod((θT - θ0) + π, 2π) - π
183 |         dθ = sign(Δθ) * π/2.0 / N
184 |         rad_1 = rad
185 |         rad_2 = abs(lb2.pt_b[1]-lb2.pt_a[1])
186 |         if rad_1 > rad_2
187 |             rad_1 -= width/2
188 |             rad_2 += width/2
189 |         else
190 |             rad_1 += width/2
191 |             rad_2 -= width/2
192 |         end
193 |         points = mapreduce(vcat, 0:N) do i
194 |             θ = θ0 + dθ * i
195 |             [GeometryBasics.Point{3,Float64}(center[1]+rad_1*cos(θ), center[2]+rad_1*sin(θ), 0),
196 |              GeometryBasics.Point{3,Float64}(center[1]+rad_2*cos(θ), center[2]+rad_2*sin(θ), 0)]
197 |         end
198 |     else
199 |         dir1 = delta / norm(delta)
200 |         right1 = [dir1[2], -dir1[1]]
201 |         delta2 = lb2.pt_b - lb2.pt_a
202 |         dir2 = delta2 / norm(delta2)
203 |         right2 = [dir2[2], -dir2[1]]
204 |         dist2 = norm(delta2)
205 | 
206 |         Δ = dist / N
207 |         t = 0:Δ:dist .+ 0.001
208 |         Δ2 = dist2 / N
209 |         t2 = 0:Δ2:dist2 .+ 0.001
210 | 
211 |         points = mapreduce(vcat, zip(t, t2)) do (dist1, dist2)
212 |             pt_1 = pt_a+dir1*dist1+width/2*right1
213 |             pt_2 = lb2.pt_a + dir2*dist2-width/2*right2
214 |             [GeometryBasics.Point{3,Float64}(pt_1[1], pt_1[2], 0),
215 |              GeometryBasics.Point{3,Float64}(pt_2[1], pt_2[2], 0)]
216 |         end
217 |     end
218 | 
219 |     if lane_type == stop_sign
220 |         points_end = points[end-3:end]
221 |         points = points[1:end-2]
222 |         faces_end = [GeometryBasics.TriangleFace(1,3,4),
223 |                      GeometryBasics.TriangleFace(1,2,4)]
224 |     else
225 |         faces_end = nothing
226 |     end
227 | 
228 |     K = length(points)/2 |> Int
229 | 
230 |     faces = mapreduce(vcat, 1:K-1) do k
231 |         [GeometryBasics.TriangleFace((k-1)*2+1,k*2+1,k*2+2),
232 |          GeometryBasics.TriangleFace((k-1)*2+1,k*2,k*2+2)]
233 |     end
234 |     mesh = GeometryBasics.Mesh(points, faces)
235 |     obj = MeshCat.Object(mesh, MeshPhongMaterial(color=color))
236 | 
237 |     if lane_type == stop_sign
238 |         mesh_end = GeometryBasics.Mesh(points_end, faces_end)
239 |         white = RGBA{Float32}(1,1,1,1)
240 |         obj_end = MeshCat.Object(mesh_end, MeshPhongMaterial(color=white))
241 |     else
242 |         obj_end = nothing
243 |     end
244 |     (; obj, obj_end)
245 | end
246 | 
247 | function ground_mesh()
248 |     green=RGBA{Float32}(0.34,0.49,0.27,1.0)
249 |     points = GeometryBasics.Point{3,Float64}[]
250 |     for x in [-200.0, 200.0]
251 |         for y in [-200.0, 200.0]
252 |             push!(points, GeometryBasics.Point{3,Float64}(x,y,-0.25))
253 |         end
254 |     end
255 |     faces = [GeometryBasics.TriangleFace(1,2,3), GeometryBasics.TriangleFace(2,4,3)]
256 |     mesh = GeometryBasics.Mesh(points, faces)
257 |     obj = MeshCat.Object(mesh, MeshPhongMaterial(color=green))
258 | end
259 | 
260 | 
261 | function view_map(vis, all_segs)
262 |     delete!(vis["Grid"])
263 |     gm = ground_mesh()
264 |     setobject!(vis["ground"], gm)
265 |     for (id, seg) in all_segs
266 |         meshes = generate_road_segment_mesh(seg)
267 |         for (mesh_id, m) in meshes
268 |             if isnothing(m)
269 |                 continue
270 |             end
271 |             setobject!(vis["map"]["$id"][mesh_id], m)
272 |         end
273 |     end
274 | end
275 | 
276 | function generate_road_segment_mesh(seg; lane_width=0.3, poly_res=1.0)
277 |     meshes = Dict{String, Any}()
278 |     for (e,lb) in enumerate(seg.lane_boundaries)
279 |         m = generate_laneline_mesh(lb; width=lane_width, res=poly_res)
280 |         meshes["line_$e"] = m
281 |         if e ≤ length(seg.lane_types)
282 |             ms = generate_lane_mesh(lb, seg.lane_boundaries[e+1], seg.lane_types[e]; res=poly_res)
283 |             meshes["lane_$e"] = ms.obj
284 |             if !isnothing(ms.obj_end)
285 |                 meshes["stop_$e"] = ms.obj_end
286 |             end
287 |         end
288 |     end
289 |     meshes
290 | end
291 | 
292 | function contains_lane_type(seg, types...)
293 |     return any(lane_type ∈ types for lane_type in seg.lane_types)
294 | end
295 | 
296 | function istaper(seg)
297 |     A = seg.lane_boundaries[2].pt_a
298 |     B = seg.lane_boundaries[2].pt_b
299 |     C = seg.lane_boundaries[3].pt_a
300 |     D = seg.lane_boundaries[3].pt_b
301 |     norm(A-C) < 0.1 || norm(B-D) < 0.1
302 | end
303 | 
304 | function identify_loading_segments(map)
305 |     target_segments = []
306 |     for (id, seg) in map
307 |         if contains_lane_type(seg, loading_zone) && !istaper(seg)
308 |             push!(target_segments, id)
309 |         end
310 |     end
311 |     target_segments
312 | end
313 | 
314 | function get_initialization_point(seg)
315 |     lb_1 = seg.lane_boundaries[1]
316 |     lb_2 = seg.lane_boundaries[2]
317 | 
318 |     pt_a = lb_1.pt_a
319 |     pt_b = lb_1.pt_b
320 |     pt_c = lb_2.pt_a
321 |     pt_d = lb_2.pt_b
322 | 
323 |     curvature = lb_1.curvature
324 |     curved = !isapprox(curvature, 0.0; atol=1e-6)
325 |     delta = pt_b-pt_a
326 |     if !curved
327 |         #pt = 0.25*(pt_a+pt_b+pt_c+pt_d)
328 |         pt = 0.9*0.5*(pt_a+pt_c) + 0.1*0.5*(pt_b+pt_d)
329 |         yaw = atan(delta[2], delta[1])
330 |     else
331 |         rad = 1.0 / abs(curvature)
332 |         dist = π*rad/2.0
333 |         left = curvature > 0
334 |         if left
335 |             if sign(delta[1]) == sign(delta[2])
336 |                 center = pt_a + [0, delta[2]]
337 |             else
338 |                 center = pt_a + [delta[1], 0]
339 |             end
340 |         else
341 |             if sign(delta[1]) == sign(delta[2])
342 |                 center = pt_a + [delta[1], 0]
343 |             else
344 |                 center = pt_a + [0, delta[2]]
345 |             end
346 |         end
347 |         pt_a_rel = pt_a - center
348 |         pt_b_rel = pt_b - center
349 | 
350 |         θ0 = atan(pt_a_rel[2], pt_a_rel[1])
351 |         θT = atan(pt_b_rel[2], pt_b_rel[1])
352 | 
353 |         θ = 0.5*(θ0 + θT)
354 | 
355 |         rad_1 = rad
356 |         rad_2 = abs(pt_d[1]-pt_c[1])
357 | 
358 |         rad = 0.5*(rad_1+rad_2)
359 | 
360 |         pt = center + rad*[cos(θ), sin(θ)]
361 |         yaw = left ? θ + π/2 : θ - π/2
362 |     end
363 |     CarConfig(SVector(pt[1], pt[2], 3.25), 0, 0, yaw, 0)
364 | end
365 | 
366 | function city_map(; lane_width = 10.0,
367 |                         speed_limit = 10.0,
368 |                         pullout_length = 40.0,
369 |                         pullout_taper = 10.0,
370 |                         block_length = 80.0,
371 |                         turn_curvature = 0.1,
372 |                         intersection_curvature = 0.15)
373 |     turn_r = 1.0/turn_curvature
374 |     int_r = 1.0/intersection_curvature
375 | 
376 |     shortened_block_length = block_length - 2*(turn_r-int_r)
377 |     single_shortened_block_length = block_length - (turn_r-int_r)
378 | 
379 |     all_segs = Dict{Int, RoadSegment}()
380 | 
381 |     segs_I = add_fourway_intersection!(all_segs, nothing, nothing; intersection_curvature, speed_limit, lane_width)
382 |     segs = add_straight_segments!(all_segs, segs_I, west; length=block_length, speed_limit, stop_outbound=true, stop_inbound=true)
383 |     segs_T = add_T_intersection!(all_segs, segs, west, east; intersection_curvature, lane_width, speed_limit)
384 |     segs_S = add_pullout_segments!(all_segs, segs_T, south; length=block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=false, pullout_outbound=true)
385 |     segs_S = add_curved_segments!(all_segs, segs_S, south, true; turn_curvature, speed_limit, lane_width)
386 |     segs_S = add_pullout_segments!(all_segs, segs_S, east; length=shortened_block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=false)
387 |     segs_S = add_curved_segments!(all_segs, segs_S, east, true; turn_curvature, speed_limit, lane_width)
388 |     segs_S = add_straight_segments!(all_segs, segs_S, north; length=block_length, speed_limit, stop_outbound=true, stop_inbound=false)
389 |     segs_I = add_segments!(all_segs, segs_S, north, segs_I)
390 | 
391 |     segs_N = add_pullout_segments!(all_segs, segs_T, north; length=single_shortened_block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=false)
392 |     segs_N = add_curved_segments!(all_segs, segs_N, north, false; turn_curvature, speed_limit, lane_width)
393 |     segs_N = add_straight_segments!(all_segs, segs_N, east; length=single_shortened_block_length, speed_limit, stop_outbound=true, stop_inbound=false)
394 |     segs_N = add_T_intersection!(all_segs, segs_N, east, south; intersection_curvature, lane_width, speed_limit)
395 |     segs_N2 = add_pullout_segments!(all_segs, segs_N, south; length=block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=true, stop_inbound=true, stop_outbound=true)
396 |     segs_I = add_segments!(all_segs, segs_N2, south, segs_I)
397 | 
398 |     segs_E = add_straight_segments!(all_segs, segs_N, east; length=block_length, speed_limit, lane_width, stop_inbound=true, stop_outbound=false)
399 |     segs_E = add_curved_segments!(all_segs, segs_E, east, false; turn_curvature, speed_limit, lane_width)
400 |     segs_E = add_pullout_segments!(all_segs, segs_E, south; length=shortened_block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=false)
401 |     segs_E = add_curved_segments!(all_segs, segs_E, south, false; turn_curvature, speed_limit, lane_width)
402 |     segs_E = add_straight_segments!(all_segs, segs_E, west; length=block_length, speed_limit, lane_width, stop_outbound=true, stop_inbound=false)
403 |     segs_I = add_segments!(all_segs, segs_E, west, segs_I)
404 | 
405 |     all_segs
406 | end
407 | 
408 | function training_map(; lane_width = 30.0,
409 |                         speed_limit = 10.0,
410 |                         pullout_length = 40.0,
411 |                         pullout_taper = 10.0,
412 |                         block_length = 200.0,
413 |                         turn_curvature = 0.1,
414 |                         intersection_curvature = 0.15)
415 |     turn_r = 1.0/turn_curvature
416 |     int_r = 1.0/intersection_curvature
417 | 
418 |     shortened_block_length = block_length - 2*(turn_r-int_r)
419 |     single_shortened_block_length = block_length - (turn_r-int_r)
420 | 
421 |     all_segs = Dict{Int, RoadSegment}()
422 |     segs = add_straight_segments!(all_segs, nothing, north; length=block_length, speed_limit, stop_outbound=false, lane_width)
423 | 
424 |     #segs_I = add_fourway_intersection!(all_segs, nothing, nothing; intersection_curvature, speed_limit, lane_width)
425 |     #segs = add_straight_segments!(all_segs, segs_I, west; length=block_length, speed_limit, stop_outbound=true, stop_inbound=true)
426 |     #segs_T = add_T_intersection!(all_segs, segs, west, east; intersection_curvature, lane_width, speed_limit)
427 |     #segs_S = add_pullout_segments!(all_segs, segs_T, south; length=block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=false, pullout_outbound=true)
428 |     #segs_S = add_curved_segments!(all_segs, segs_S, south, true; turn_curvature, speed_limit, lane_width)
429 |     #segs_S = add_pullout_segments!(all_segs, segs_S, east; length=shortened_block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=false)
430 |     #segs_S = add_curved_segments!(all_segs, segs_S, east, true; turn_curvature, speed_limit, lane_width)
431 |     #segs_S = add_straight_segments!(all_segs, segs_S, north; length=block_length, speed_limit, stop_outbound=true, stop_inbound=false)
432 |     #segs_I = add_segments!(all_segs, segs_S, north, segs_I)
433 | 
434 |     #segs_N = add_pullout_segments!(all_segs, segs_T, north; length=single_shortened_block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=false)
435 |     #segs_N = add_curved_segments!(all_segs, segs_N, north, false; turn_curvature, speed_limit, lane_width)
436 |     #segs_N = add_straight_segments!(all_segs, segs_N, east; length=single_shortened_block_length, speed_limit, stop_outbound=true, stop_inbound=false)
437 |     #segs_N = add_T_intersection!(all_segs, segs_N, east, south; intersection_curvature, lane_width, speed_limit)
438 |     #segs_N2 = add_pullout_segments!(all_segs, segs_N, south; length=block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=true, stop_inbound=true, stop_outbound=true)
439 |     #segs_I = add_segments!(all_segs, segs_N2, south, segs_I)
440 | 
441 |     #segs_E = add_straight_segments!(all_segs, segs_N, east; length=block_length, speed_limit, lane_width, stop_inbound=true, stop_outbound=false)
442 |     #segs_E = add_curved_segments!(all_segs, segs_E, east, false; turn_curvature, speed_limit, lane_width)
443 |     #segs_E = add_pullout_segments!(all_segs, segs_E, south; length=shortened_block_length, pullout_length, pullout_taper, lane_width, speed_limit, pullout_inbound=true, pullout_outbound=false)
444 |     #segs_E = add_curved_segments!(all_segs, segs_E, south, false; turn_curvature, speed_limit, lane_width)
445 |     #segs_E = add_straight_segments!(all_segs, segs_E, west; length=block_length, speed_limit, lane_width, stop_outbound=true, stop_inbound=false)
446 |     #segs_I = add_segments!(all_segs, segs_E, west, segs_I)
447 | 
448 |     all_segs
449 | end
450 | 
451 | function add_segments!(all_segs, base, direction, segs)
452 |     foreach(id->all_segs[id].children=segs.origins[opposite(direction)], base.sinks[direction])
453 |     foreach(id->all_segs[id].children=base.origins[direction], segs.sinks[opposite(direction)])
454 |     segs
455 | end
456 | 
457 | function add_curved_segments!(all_segs, base, direction, turn_left; turn_curvature=0.1, speed_limit=7.5, lane_width=5.0)
458 |     
459 |     if direction == west
460 |         lane_dir = SVector(-1.0, 0)
461 |     elseif direction == north
462 |         lane_dir = SVector(0.0, 1.0)
463 |     elseif direction == east
464 |         lane_dir = SVector(1.0, 0.0)
465 |     elseif direction == south
466 |         lane_dir = SVector(0.0, -1.0)
467 |     end
468 |     right_dir = SVector(lane_dir[2], -lane_dir[1])
469 | 
470 |     if isnothing(base)
471 |         pt_a = SVector(0.0, 0)
472 |         pt_b = pt_a + right_dir * lane_width
473 |         pt_c = pt_b + right_dir * lane_width
474 |     else
475 |         dir_sinks = base.sinks[direction]
476 |         dir_origins = base.origins[direction]
477 |         pt_a = (all_segs[first(dir_origins)].lane_boundaries[2]).pt_a
478 |         pt_b = (all_segs[first(dir_origins)].lane_boundaries[1]).pt_a
479 |         pt_c = (all_segs[first(dir_sinks)].lane_boundaries[2]).pt_b
480 |     end
481 |     inside_rad = 1.0 / turn_curvature
482 |     middle_rad = inside_rad + lane_width
483 |     outside_rad = middle_rad + lane_width
484 |     if turn_left
485 |         end_direction = mod(Int(direction) - 1, 4) |> Direction
486 |         pt_d = pt_a + lane_dir * inside_rad - right_dir * inside_rad
487 |         pt_e = pt_b + lane_dir * middle_rad - right_dir * middle_rad
488 |         pt_f = pt_c + lane_dir * outside_rad - right_dir * outside_rad
489 |     else
490 |         end_direction = mod(Int(direction) + 1, 4) |> Direction
491 |         pt_d = pt_a + lane_dir * outside_rad + right_dir * outside_rad
492 |         pt_e = pt_b + lane_dir * middle_rad + right_dir * middle_rad
493 |         pt_f = pt_c + lane_dir * inside_rad + right_dir * inside_rad
494 |     end
495 | 
496 |     seg_id = maximum(keys(all_segs); init=0)
497 | 
498 |     b1 = lane_boundary(pt_b, pt_e, true, true, turn_left)
499 |     b2 = lane_boundary(pt_c, pt_f, true, true, turn_left)
500 |     seg_1 = RoadSegment(seg_id+=1, [b1, b2], [standard,], speed_limit, [])
501 | 
502 |     b1 = lane_boundary(pt_e, pt_b, true, true, !turn_left)
503 |     b2 = lane_boundary(pt_d, pt_a, true, true, !turn_left)
504 |     seg_2 = RoadSegment(seg_id+=1, [b1, b2], [standard,], speed_limit, [])
505 | 
506 |     if !isnothing(base)
507 |         seg_2.children = base.origins[direction]
508 |         foreach(id->all_segs[id].children = [seg_1.id,], base.sinks[direction])
509 |     end
510 |     all_segs[seg_1.id] = seg_1
511 |     all_segs[seg_2.id] = seg_2
512 |     
513 |     sinks = Dict{Direction, Vector{Int}}()
514 |     origins = Dict{Direction, Vector{Int}}()
515 |     origins[end_direction] = [seg_2.id,]
516 |     sinks[end_direction] = [seg_1.id,]
517 | 
518 |     (; sinks, origins)
519 | end
520 | 
521 | function add_T_intersection!(all_segs, base, direction, T_direction; intersection_curvature=0.25, lane_width=5.0, speed_limit=7.5)
522 |     segs = add_fourway_intersection!(all_segs, base, direction; intersection_curvature, lane_width, speed_limit)
523 |     for id in segs.origins[opposite(T_direction)]
524 |         delete!(all_segs, id)
525 |         for (dir, seg_ids) in segs.sinks
526 |             filter!(sid->sid!=id, seg_ids)
527 |         end
528 |     end
529 |     for id in segs.sinks[opposite(T_direction)]
530 |         delete!(all_segs, id)
531 |         for (dir, seg_ids) in segs.origins
532 |             filter!(sid->sid!=id, seg_ids)
533 |         end
534 |     end
535 |     delete!(segs.origins, opposite(T_direction))
536 |     delete!(segs.sinks, opposite(T_direction))
537 |     segs
538 | end
539 | 
540 | function add_pullout_segments!(all_segs, base, direction; length=40.0, pullout_length=20.0, pullout_taper=5.0, lane_width = 5.0, speed_limit=7.5, pullout_inbound=false, pullout_outbound=false, stop_outbound=false, stop_inbound=false)
541 |     end_lengths = (length - pullout_length - 2*pullout_taper) / 2.0
542 |     base = add_straight_segments!(all_segs, base, direction; length=end_lengths, speed_limit, lane_width, stop_outbound=false, stop_inbound)
543 |     
544 |     base = add_double_segments!(all_segs, base, direction; taper=1, length=pullout_taper, speed_limit, lane_width, pullout_inbound, pullout_outbound)
545 |     base = add_double_segments!(all_segs, base, direction; taper=0, length=pullout_length, speed_limit, lane_width, pullout_inbound, pullout_outbound)
546 |     base = add_double_segments!(all_segs, base, direction; taper=-1, length=pullout_taper, speed_limit, lane_width, pullout_inbound, pullout_outbound)
547 |     
548 |     base = add_straight_segments!(all_segs, base, direction; length=end_lengths, speed_limit, lane_width, stop_outbound, stop_inbound=false)
549 | end
550 | 
551 | function add_double_segments!(all_segs, base, direction; taper=1, length=5.0, speed_limit=7.5, lane_width=5.0, pullout_inbound=false, pullout_outbound=false)
552 |     if isnothing(base)
553 |         @error "Unsupported"
554 |     end
555 | 
556 |     if direction == west
557 |         lane_dir = SVector(-1.0, 0)
558 |     elseif direction == north
559 |         lane_dir = SVector(0.0, 1.0)
560 |     elseif direction == east
561 |         lane_dir = SVector(1.0, 0.0)
562 |     elseif direction == south
563 |         lane_dir = SVector(0.0, -1.0)
564 |     end
565 |     right_dir = SVector(lane_dir[2], -lane_dir[1])
566 | 
567 |     if isnothing(base)
568 |         pt_a = SVector(0.0, 0)
569 |         pt_b = pt_a + right_dir * lane_width
570 |         pt_c = pt_b + right_dir * lane_width
571 |     else
572 |         dir_sinks = base.sinks[direction]
573 |         dir_origins = base.origins[direction]
574 |         pt_a = (all_segs[first(dir_origins)].lane_boundaries[2]).pt_a
575 |         pt_b = (all_segs[first(dir_origins)].lane_boundaries[1]).pt_a
576 |         pt_c = (all_segs[first(dir_sinks)].lane_boundaries[2]).pt_b
577 |     end
578 | 
579 |     pt_d = pt_a + lane_dir * length
580 |     pt_e = pt_b + lane_dir * length
581 |     pt_f = pt_c + lane_dir * length
582 |     
583 |     pt_a_l = pt_a - right_dir * lane_width
584 |     pt_c_r = pt_c + right_dir * lane_width
585 |     pt_d_l = pt_d - right_dir * lane_width
586 |     pt_f_r = pt_f + right_dir * lane_width
587 | 
588 |     seg_id = maximum(keys(all_segs); init=0)
589 | 
590 |     b1 = LaneBoundary(pt_b, pt_e, 0.0, true, true)
591 |     if pullout_outbound
592 |         b2 = LaneBoundary(pt_c, pt_f, 0.0, false, true)
593 |         if taper == 1
594 |             b3 = LaneBoundary(pt_c, pt_f_r, 0.0, true, true)
595 |         elseif taper == 0
596 |             b3 = LaneBoundary(pt_c_r, pt_f_r, 0.0, true, true)
597 |         elseif taper == -1
598 |             b3 = LaneBoundary(pt_c_r, pt_f, 0.0, true, true)
599 |         end
600 |         outbound_types = [standard, loading_zone]
601 |         seg_1 = RoadSegment(seg_id+=1, [b1,b2,b3], outbound_types, speed_limit, [])
602 |     else
603 |         b2 = LaneBoundary(pt_c, pt_f, 0.0, true, true)
604 |         seg_1 = RoadSegment(seg_id+=1, [b1, b2], [standard,], speed_limit, [])
605 |     end
606 | 
607 |     b1 = LaneBoundary(pt_e, pt_b, 0.0, true, true)
608 |     if pullout_inbound
609 |         b2 = LaneBoundary(pt_d, pt_a, 0.0, false, true)
610 |         if taper == 1
611 |             b3 = LaneBoundary(pt_d_l, pt_a, 0.0, true, true)
612 |         elseif taper == 0
613 |             b3 = LaneBoundary(pt_d_l, pt_a_l, 0.0, true, true)
614 |         elseif taper == -1
615 |             b3 = LaneBoundary(pt_d, pt_a_l, 0.0, true, true)
616 |         end
617 |         inbound_types = [standard, loading_zone]
618 |         seg_2 = RoadSegment(seg_id+=1, [b1, b2, b3], inbound_types, speed_limit, [])
619 |     else
620 |         b2 = LaneBoundary(pt_d, pt_a, 0.0, true, true)
621 |         seg_2 = RoadSegment(seg_id+=1, [b1, b2], [standard,], speed_limit, [])
622 |     end
623 |     
624 |     if !isnothing(base)
625 |         seg_2.children = base.origins[direction]
626 |         foreach(id->all_segs[id].children = [seg_1.id,], base.sinks[direction])
627 |     end
628 |     all_segs[seg_1.id] = seg_1
629 |     all_segs[seg_2.id] = seg_2
630 |     
631 |     sinks = Dict{Direction, Vector{Int}}()
632 |     origins = Dict{Direction, Vector{Int}}()
633 |     origins[direction] = [seg_2.id,]
634 |     sinks[direction] = [seg_1.id,]
635 | 
636 |     (; sinks, origins)
637 | end
638 | 
639 | function add_straight_segments!(all_segs, base, direction; length=40.0, speed_limit = 7.5, lane_width=5.0, stop_outbound=false, stop_inbound=false)
640 |     if direction == west
641 |         lane_dir = SVector(-1.0, 0)
642 |     elseif direction == north
643 |         lane_dir = SVector(0.0, 1.0)
644 |     elseif direction == east
645 |         lane_dir = SVector(1.0, 0.0)
646 |     elseif direction == south
647 |         lane_dir = SVector(0.0, -1.0)
648 |     end
649 |     right_dir = SVector(lane_dir[2], -lane_dir[1])
650 | 
651 |     if isnothing(base)
652 |         pt_a = SVector(0.0, 0)
653 |         pt_b = pt_a + right_dir * lane_width
654 |         pt_c = pt_b + right_dir * lane_width
655 |     else
656 |         dir_sinks = base.sinks[direction]
657 |         dir_origins = base.origins[direction]
658 |         pt_a = (all_segs[first(dir_origins)].lane_boundaries[2]).pt_a
659 |         pt_b = (all_segs[first(dir_origins)].lane_boundaries[1]).pt_a
660 |         pt_c = (all_segs[first(dir_sinks)].lane_boundaries[2]).pt_b
661 |     end
662 | 
663 |     pt_d = pt_a + lane_dir * length
664 |     pt_e = pt_b + lane_dir * length
665 |     pt_f = pt_c + lane_dir * length
666 |     
667 |     seg_id = maximum(keys(all_segs); init=0)
668 | 
669 |     b1 = LaneBoundary(pt_b, pt_e, 0.0, true, true)
670 |     b2 = LaneBoundary(pt_c, pt_f, 0.0, true, true)
671 |     outbound_type = stop_outbound ? stop_sign : standard
672 |     seg_1 = RoadSegment(seg_id+=1, [b1, b2], [outbound_type,], speed_limit, [])
673 | 
674 |     b1 = LaneBoundary(pt_e, pt_b, 0.0, true, true)
675 |     b2 = LaneBoundary(pt_d, pt_a, 0.0, true, true)
676 |     inbound_type = stop_inbound ? stop_sign : standard
677 |     seg_2 = RoadSegment(seg_id+=1, [b1, b2], [inbound_type], speed_limit, [])
678 | 
679 |     if !isnothing(base)
680 |         seg_2.children = base.origins[direction]
681 |         foreach(id->all_segs[id].children = [seg_1.id,], base.sinks[direction])
682 |     end
683 |     all_segs[seg_1.id] = seg_1
684 |     all_segs[seg_2.id] = seg_2
685 |     
686 |     sinks = Dict{Direction, Vector{Int}}()
687 |     origins = Dict{Direction, Vector{Int}}()
688 |     origins[direction] = [seg_2.id,]
689 |     sinks[direction] = [seg_1.id,]
690 |     (; sinks, origins)
691 | end
692 | 
693 | function add_fourway_intersection!(all_segs, base, direction; intersection_curvature = 0.25, lane_width = 5.0, speed_limit=7.5)
694 |     lw = lane_width
695 |     r = 1.0 / intersection_curvature
696 |     if isnothing(base)
697 |         offset = SVector(0.0, 0)
698 |     else
699 |         prev_sinks = base.sinks[direction]
700 |         prev_origins = base.origins[direction]
701 | 
702 |         if direction == north
703 |             offset = all_segs[first(prev_sinks)].lane_boundaries[1].pt_b + SVector(-r-lw, 0.0)
704 |         elseif direction == east
705 |             offset = all_segs[first(prev_sinks)].lane_boundaries[2].pt_b + SVector(0.0, -r)
706 |         elseif direction == south
707 |             offset = all_segs[first(prev_sinks)].lane_boundaries[2].pt_b + SVector(-r, -2*lw -2r)
708 |         elseif direction == west
709 |             offset = all_segs[first(prev_sinks)].lane_boundaries[1].pt_b + SVector(-2*lw - 2*r, -r-lw)
710 |         end
711 |     end
712 |     pt_a = offset+SVector(r, 0.0)
713 |     pt_b = offset+SVector(r+lw, 0.0)
714 |     pt_c = offset+SVector(r+2*lw, 0.0)
715 | 
716 |     pt_d = offset+SVector(2*lw+2r, r)
717 |     pt_e = offset+SVector(2*lw+2r, lw+r)
718 |     pt_f = offset+SVector(2*lw+2r, 2*lw+r)
719 | 
720 |     pt_g = offset+SVector(r+2*lw, 2*lw+2r)
721 |     pt_h = offset+SVector(r+lw, 2*lw+2r)
722 |     pt_i = offset+SVector(r, 2*lw+2r)
723 | 
724 |     pt_j = offset+SVector(0.0, 2*lw+r)
725 |     pt_k = offset+SVector(0.0, lw+r)
726 |     pt_l = offset+SVector(0.0, r)
727 | 
728 |     seg_id = maximum(keys(all_segs); init=0)
729 | 
730 |     # South origins
731 |     b1 = lane_boundary(pt_b, pt_k, true, false, true)
732 |     b2 = lane_boundary(pt_c, pt_j, true, false, true)
733 |     seg_1 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
734 | 
735 |     b1 = lane_boundary(pt_b, pt_h, true, false) 
736 |     b2 = lane_boundary(pt_c, pt_g, true, false) 
737 |     seg_2 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
738 | 
739 |     b1 = lane_boundary(pt_b, pt_e, true, false, false) 
740 |     b2 = lane_boundary(pt_c, pt_d, true, true, false) 
741 |     seg_3 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
742 |    
743 |     # East origins
744 |     b1 = lane_boundary(pt_e, pt_b, true, false, true)
745 |     b2 = lane_boundary(pt_f, pt_a, true, false, true)
746 |     seg_4 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
747 | 
748 |     b1 = lane_boundary(pt_e, pt_k, true, false) 
749 |     b2 = lane_boundary(pt_f, pt_j, true, false) 
750 |     seg_5 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
751 | 
752 |     b1 = lane_boundary(pt_e, pt_h, true, false, false) 
753 |     b2 = lane_boundary(pt_f, pt_g, true, true, false) 
754 |     seg_6 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
755 |     
756 |     # North origins
757 |     b1 = lane_boundary(pt_h, pt_e, true, false, true)
758 |     b2 = lane_boundary(pt_i, pt_d, true, false, true)
759 |     seg_7 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
760 | 
761 |     b1 = lane_boundary(pt_h, pt_b, true, false) 
762 |     b2 = lane_boundary(pt_i, pt_a, true, false) 
763 |     seg_8 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
764 | 
765 |     b1 = lane_boundary(pt_h, pt_k, true, false, false) 
766 |     b2 = lane_boundary(pt_i, pt_j, true, true, false) 
767 |     seg_9 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
768 |     
769 |     # West origins
770 |     b1 = lane_boundary(pt_k, pt_h, true, false, true)
771 |     b2 = lane_boundary(pt_l, pt_g, true, false, true)
772 |     seg_10 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
773 | 
774 |     b1 = lane_boundary(pt_k, pt_e, true, false) 
775 |     b2 = lane_boundary(pt_l, pt_d, true, false) 
776 |     seg_11 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
777 | 
778 |     b1 = lane_boundary(pt_k, pt_b, true, false, false) 
779 |     b2 = lane_boundary(pt_l, pt_a, true, true, false) 
780 |     seg_12 = RoadSegment(seg_id+=1, [b1, b2], [intersection,], speed_limit, [])
781 |     
782 |     foreach(seg->all_segs[seg.id]=seg, [seg_1, seg_2, seg_3, seg_4, seg_5, seg_6, seg_7, seg_8, seg_9, seg_10, seg_11, seg_12])
783 | 
784 |     sinks = Dict{Direction, Vector{Int}}()
785 |     origins = Dict{Direction, Vector{Int}}()
786 |     origins[south] = [seg_1.id, seg_2.id, seg_3.id]
787 |     sinks[south] = [seg_4.id, seg_8.id, seg_12.id]
788 |     origins[east] = [seg_4.id, seg_5.id, seg_6.id]
789 |     sinks[east] = [seg_3.id, seg_7.id, seg_11.id]
790 |     origins[north] = [seg_7.id, seg_8.id, seg_9.id]
791 |     sinks[north] = [seg_2.id, seg_6.id, seg_10.id]
792 |     origins[west] = [seg_10.id, seg_11.id, seg_12.id]
793 |     sinks[west] = [seg_1.id, seg_5.id, seg_9.id]
794 | 
795 | 
796 |     if !isnothing(base)
797 |         foreach(id->all_segs[id].children = origins[opposite(direction)], base.sinks[direction])
798 |         foreach(id->all_segs[id].children = base.origins[direction], sinks[opposite(direction)])
799 |     end
800 | 
801 |     (; sinks, origins)
802 | end
803 | 


--------------------------------------------------------------------------------
/src/measurements.jl:
--------------------------------------------------------------------------------
  1 | abstract type Measurement end
  2 | 
  3 | """
  4 | Records lat/long measurements (expressed as first and second coordinates of map frame)
  5 | of GPS sensor.
  6 | """
  7 | struct GPSMeasurement <: Measurement
  8 |     time::Float64
  9 |     lat::Float64
 10 |     long::Float64
 11 |     heading::Float64
 12 | end
 13 | 
 14 | """
 15 | Records linear and angular velocity experienced instantaneously by IMU sensor,
 16 | in local frame of IMU.
 17 | """
 18 | struct IMUMeasurement <: Measurement
 19 |     time::Float64
 20 |     linear_vel::SVector{3, Float64}
 21 |     angular_vel::SVector{3, Float64}
 22 | end
 23 | 
 24 | """
 25 | Records list of bounding boxes represented by top-left and bottom-right pixel coordinates.
 26 | Camera_id is 1 or 2, indicating which camera produced the bounding boxes.
 27 | 
 28 | Pixels start from (1,1) in top-left corner of image, and proceed right (first axis) and down (second axis)
 29 | to image_width and image_height pixels.
 30 | 
 31 | Each pixel corresponds to a pixel_len x pixel_len patch at focal_len away from a pinhole model.
 32 | """
 33 | struct CameraMeasurement <: Measurement
 34 |     time::Float64
 35 |     camera_id::Int
 36 |     focal_length::Float64
 37 |     pixel_length::Float64
 38 |     image_width::Int # pixels
 39 |     image_height::Int # pixels
 40 |     bounding_boxes::Vector{SVector{4, Int}}
 41 | end
 42 | 
 43 | """
 44 | Records position, orientation, linear and angular velocity, and size of 3D bounding box of vehicle
 45 | specified by vehicle ID
 46 | """
 47 | struct GroundTruthMeasurement <: Measurement
 48 |     time::Float64
 49 |     vehicle_id::Int
 50 |     position::SVector{3, Float64} # position of center of vehicle
 51 |     orientation::SVector{4, Float64} # represented as quaternion
 52 |     velocity::SVector{3, Float64}
 53 |     angular_velocity::SVector{3, Float64} # angular velocity around x,y,z axes
 54 |     size::SVector{3, Float64} # length, width, height of 3d bounding box centered at (position/orientation)
 55 | end
 56 | 
 57 | """
 58 | Vehicle ID is the id of the vehicle receiving this message.
 59 | target segment is the map segment that should be driven to.
 60 | mesaurements are the latest sensor measurements.
 61 | """
 62 | struct MeasurementMessage
 63 |     vehicle_id::Int
 64 |     target_segment::Int
 65 |     measurements::Vector{Measurement}
 66 | end
 67 | 
 68 | function Rot_from_quat(q)
 69 |     qw = q[1]
 70 |     qx = q[2]
 71 |     qy = q[3]
 72 |     qz = q[4]
 73 | 
 74 |     R = [qw^2+qx^2-qy^2-qz^2 2(qx*qy-qw*qz) 2(qw*qy+qx*qz);
 75 |          2(qx*qy+qw*qz) qw^2-qx^2+qy^2-qz^2 2(qy*qz-qw*qx);
 76 |          2(qx*qz-qw*qy) 2(qw*qx+qy*qz) qw^2-qx^2-qy^2+qz^2]
 77 | end
 78 | 
 79 | function J_R_q(q)
 80 |     qw = q[1]
 81 |     qx = q[2]
 82 |     qy = q[3]
 83 |     qz = q[4]
 84 | 
 85 |     dRdq1 = 2*[qw -qz qy;
 86 |              qz qw -qx;
 87 |              -qy qx qw]
 88 |     dRdq2 = 2*[qx qy qz;
 89 |                qy -qx -qw;
 90 |                qz qw -qx]
 91 |     dRdq3 = 2*[-qy qx qw;
 92 |                qx qy qz;
 93 |                -qw qz -qy]
 94 |     dRdq4 = 2*[-qz -qw qx;
 95 |                qw -qz qy;
 96 |                qx qy qz]
 97 |     (dRdq1, dRdq2, dRdq3, dRdq4)
 98 | end
 99 | 
100 | """
101 | Can be used as process model for EKF
102 | which estimates xₖ = [position; quaternion; velocity; angular_vel]
103 | 
104 | We haven't discussed quaternions in class much, but interfacing with GPS/IMU
105 | will be much easier using this representation, which is used internally by the simulator.
106 | """
107 | function rigid_body_dynamics(position, quaternion, velocity, angular_vel, Δt)
108 |     r = angular_vel
109 |     mag = norm(r)
110 | 
111 |     if mag < 1e-5
112 |         sᵣ = 1.0
113 |         vᵣ = zeros(3)
114 |     else
115 |         sᵣ = cos(mag*Δt / 2.0)
116 |         vᵣ = sin(mag*Δt / 2.0) * (r / mag)
117 |     end
118 | 
119 |     sₙ = quaternion[1]
120 |     vₙ = quaternion[2:4]
121 | 
122 |     s = sₙ*sᵣ - vₙ'*vᵣ
123 |     v = sₙ*vᵣ+sᵣ*vₙ+vₙ×vᵣ
124 | 
125 |     R = Rot_from_quat(quaternion)  
126 | 
127 |     new_position = position + Δt * R * velocity
128 |     new_quaternion = [s; v]
129 |     new_velocity = velocity
130 |     new_angular_vel = angular_vel
131 |     return [new_position; new_quaternion; new_velocity; new_angular_vel]
132 | end
133 | 
134 | function f(x, Δt)
135 |     rigid_body_dynamics(x[1:3], x[4:7], x[8:10], x[11:13], Δt)
136 | end
137 | 
138 | function Jac_x_f(x, Δt)
139 |     J = zeros(13, 13)
140 | 
141 |     r = x[11:13]
142 |     mag = norm(r)
143 |     if mag < 1e-5
144 |         sᵣ = 1.0
145 |         vᵣ = zeros(3)
146 |     else
147 |         sᵣ = cos(mag*Δt / 2.0)
148 |         vᵣ = sin(mag*Δt / 2.0) * (r / mag)
149 |     end
150 | 
151 |     sₙ = x[4]
152 |     vₙ = x[5:7]
153 | 
154 |     s = sₙ*sᵣ - vₙ'*vᵣ
155 |     v = sₙ*vᵣ+sᵣ*vₙ+vₙ×vᵣ
156 | 
157 |     R = Rot_from_quat([sₙ; vₙ])  
158 |     (J_R_q1, J_R_q2, J_R_q3, J_R_q4) = J_R_q([sₙ; vₙ])
159 |     
160 |     #new_position = position + Δt * R * velocity
161 |     #new_quaternion = [s; v]
162 |     #new_velocity = velocity
163 |     #new_angular_vel = angular_vel
164 |     
165 |     velocity = x[8:10]
166 | 
167 |     J[1:3, 1:3] = I(3)
168 |     J[1:3, 4] = Δt * J_R_q1*velocity
169 |     J[1:3, 5] = Δt * J_R_q2*velocity
170 |     J[1:3, 6] = Δt * J_R_q3*velocity
171 |     J[1:3, 7] = Δt * J_R_q4*velocity
172 |     J[1:3, 8:10] = Δt * R
173 |     J[4, 4] = sᵣ
174 |     J[4, 5:7] = -vᵣ'
175 |     J[5:7, 4] = vᵣ
176 |     J[5:7, 5:7] = [sᵣ vᵣ[3] -vᵣ[2];
177 |                    -vᵣ[3] sᵣ vᵣ[1];
178 |                    vᵣ[2] -vᵣ[1] sᵣ]
179 | 
180 |     if mag > 1e-5
181 |         Jsv_srvr = [sₙ -vₙ'
182 |                     vₙ [sₙ -vₙ[3] vₙ[2];
183 |                         vₙ[3] sₙ -vₙ[1];
184 |                         -vₙ[2] vₙ[1] sₙ]]
185 |         Jsrvr_mag = [-sin(mag*Δt / 2.0) * Δt / 2; sin(mag*Δt/2.0) * (-r / mag^2) + cos(mag*Δt/2)*Δt/2 * r/mag]
186 |         Jsrvr_r = [zeros(1,3); sin(mag*Δt / 2) / mag * I(3)]
187 |         Jmag_r = 1/mag * r'
188 | 
189 |         J[4:7, 11:13] = Jsv_srvr * (Jsrvr_mag*Jmag_r + Jsrvr_r)
190 |     end
191 |     J[8:10, 8:10] = I(3)
192 |     J[11:13, 11:13] = I(3)
193 |     J
194 | end
195 | 
196 | function get_rotated_camera_transform()
197 |     R = [0 0 1.;
198 |          -1 0 0;
199 |          0 -1 0]
200 |     t = zeros(3)
201 |     [R t]
202 | end
203 | 
204 | function get_cam_transform(camera_id)
205 |     # TODO load this from URDF
206 |     R_cam_to_body = RotY(0.02)
207 |     t_cam_to_body = [1.35, 1.7, 2.4]
208 |     if camera_id == 2
209 |         t_cam_to_body[2] = -1.7
210 |     end
211 | 
212 |     T = [R_cam_to_body t_cam_to_body]
213 | end
214 | 
215 | function get_gps_transform()
216 |     # TODO load this from URDF
217 |     R_gps_to_body = one(RotMatrix{3, Float64})
218 |     t_gps_to_body = [-3.0, 1, 2.6]
219 |     T = [R_gps_to_body t_gps_to_body]
220 | end
221 | 
222 | function get_imu_transform()
223 |     R_imu_to_body = RotY(0.02)
224 |     t_imu_to_body = [0, 0, 0.7]
225 | 
226 |     T = [R_imu_to_body t_imu_to_body]
227 | end
228 | 
229 | function get_body_transform(quat, loc)
230 |     R = Rot_from_quat(quat)
231 |     [R loc]
232 | end
233 | 
234 | function J_Tbody(x)
235 |     J_Tbody_xyz = (zeros(3,4), zeros(3,4), zeros(3,4))
236 |     for i = 1:3
237 |        J_Tbody_xyz[i][i,4] = 1.0
238 |     end
239 |     return (J_Tbody_xyz, [[dR zeros(3)] for dR in J_R_q(x[4:7])])
240 | end
241 | 
242 | function invert_transform(T)
243 |     R = T[1:3,1:3]
244 |     t = T[1:3,end]
245 |     R\[I(3) -t]
246 | end
247 | 
248 | """
249 | T = T1 * T2
250 | 
251 | i.e. apply T2 then T1
252 | """
253 | function multiply_transforms(T1, T2)
254 |     T1f = [T1; [0 0 0 1.]] 
255 |     T2f = [T2; [0 0 0 1.]]
256 | 
257 |     T = T1f * T2f
258 |     T = T[1:3, :]
259 | end
260 | 
261 | function h_gps(x)
262 |     T = get_gps_transform()
263 |     gps_loc_body = T*[zeros(3); 1.0]
264 |     xyz_body = x[1:3] # position
265 |     q_body = x[4:7] # quaternion
266 |     Tbody = get_body_transform(q_body, xyz_body)
267 |     xyz_gps = Tbody * [gps_loc_body; 1]
268 |     yaw = extract_yaw_from_quaternion(q_body)
269 |     meas = [xyz_gps[1:2]; yaw]
270 | end
271 | 
272 | function Jac_h_gps(x)
273 |     T = get_gps_transform()
274 |     gps_loc_body = T*[zeros(3); 1.0]
275 |     xyz_body = x[1:3] # position
276 |     q_body = x[4:7] # quaternion
277 |     Tbody = get_body_transform(q_body, xyz_body)
278 |     xyz_gps = Tbody * [gps_loc_body; 1]
279 |     yaw = extract_yaw_from_quaternion(q_body)
280 |     #meas = [xyz_gps[1:2]; yaw]
281 |     J = zeros(3, 13)
282 |     (J_Tbody_xyz, J_Tbody_q) = J_Tbody(x)
283 |     for i = 1:3
284 |         J[1:2,i] = (J_Tbody_xyz[i]*[gps_loc_body; 1])[1:2]
285 |     end
286 |     for i = 1:4
287 | 	J[1:2,3+i] = (J_Tbody_q[i]*[gps_loc_body; 1])[1:2]
288 |     end
289 |     w = q_body[1]
290 |     x = q_body[2]
291 |     y = q_body[3]
292 |     z = q_body[4]
293 |     J[3,4] = -(2 * z * (-1 + 2 * (y^2 + z^2)))/(4 * (x * y + w * z)^2 + (1 - 2 * (y^2 + z^2))^2)
294 |     J[3,5] = -(2 * y * (-1 + 2 * (y^2 + z^2)))/(4 * (x * y + w * z)^2 + (1 - 2 * (y^2 + z^2))^2)
295 |     J[3,6] = (2 * (x + 2 * x * y^2 + 4 * w * y * z - 2 * x * z^2))/(1 + 4 * y^4 + 8 * w * x * y * z + 4 * (-1 + w^2) * z^2 + 4 * z^4 + 4 * y^2 * (-1 + x^2 + 2 * z^2))
296 |     J[3,7] = (2 * (w - 2 * w * y^2 + 4 * x * y * z + 2 * w * z^2))/(1 + 4 * y^4 + 8 * w * x * y * z + 4 * (-1 + w^2) * z^2 + 4 * z^4 + 4 * y^2 * (-1 + x^2 + 2 * z^2))
297 |     J
298 | end
299 |  
300 | function gps(vehicle, state_channel, meas_channel; sqrt_meas_cov = Diagonal([1.0, 1.0, 0.1]), max_rate=10.0)
301 |     min_Δ = 1.0/max_rate
302 |     t = time()
303 |     while true
304 |         sleep(0.0001)
305 |         state = fetch(state_channel)
306 |         tnow = time()
307 |         if tnow - t > min_Δ
308 |             t = tnow
309 | 	    x = [state.q[5:7]; state.q[1:4]; state.v[4:6]; state.v[1:3]]
310 | 	    meas = h_gps(x) + sqrt_meas_cov*randn(3)
311 |             gps_meas = GPSMeasurement(t, meas[1], meas[2], meas[3])
312 |             put!(meas_channel, gps_meas)
313 |         end
314 |     end
315 | end
316 | 
317 | function imu(vehicle, state_channel, meas_channel; sqrt_meas_cov = Diagonal([0.001, 0.001, 0.001, 0.001, 0.001, 0.001]), max_rate=10.0) # Don't use 
318 |     min_Δ = 1.0/max_rate
319 |     t = time()
320 |     T_body_imu = get_imu_transform()
321 |     T_imu_body = invert_transform(T_body_imu)
322 |     R = T_imu_body[1:3,1:3]
323 |     p = T_imu_body[1:3,end]
324 | 
325 |     while true
326 |         sleep(0.0001)
327 |         state = fetch(state_channel)
328 |         tnow = time()
329 |         if tnow - t > min_Δ
330 |             t = tnow
331 |             v_body = state.v[4:6] # velocity
332 |             ω_body = state.v[1:3] # angular_vel
333 | 
334 |             ω_imu = R * ω_body
335 |             v_imu = R * v_body + p × ω_imu
336 | 
337 |             meas = [v_imu; ω_imu] + sqrt_meas_cov*randn(6)
338 | 
339 |             imu_meas = IMUMeasurement(t, meas[1:3], meas[4:6])
340 |             put!(meas_channel, imu_meas)
341 |         end
342 |     end
343 | end
344 | 
345 | function get_3d_bbox_corners(state, box_size)
346 |     quat = state.q[1:4] # quatnerion
347 |     xyz = state.q[5:7] # position
348 |     T = get_body_transform(quat, xyz)
349 |     corners = []
350 |     for dx in [-box_size[1]/2, box_size[1]/2]
351 |         for dy in [-box_size[2]/2, box_size[2]/2]
352 |             for dz in [-box_size[3]/2, box_size[3]/2]
353 |                 push!(corners, T*[dx, dy, dz, 1])
354 |             end
355 |         end
356 |     end
357 |     corners
358 | end
359 | 
360 | function convert_to_pixel(num_pixels, pixel_len, px)
361 |     min_val = -pixel_len*num_pixels/2
362 |     pix_id = cld(px - min_val, pixel_len)+1 |> Int
363 |     return pix_id
364 | end
365 | 
366 | function cameras(vehicles, state_channels, cam_channels; max_rate=10.0, focal_len = 0.64, pixel_len = 0.001, image_width = 640, image_height = 480)
367 |     min_Δ = 1.0/max_rate
368 |     t = time()
369 |     num_vehicles = length(vehicles)
370 |     vehicle_size = SVector(13.2, 5.7, 5.3)
371 | 
372 |     T_body_cam1 = get_cam_transform(1)
373 |     T_body_cam2 = get_cam_transform(2)
374 |     T_cam_camrot = get_rotated_camera_transform()
375 | 
376 |     T_body_camrot1 = multiply_transforms(T_body_cam1, T_cam_camrot)
377 |     T_body_camrot2 = multiply_transforms(T_body_cam2, T_cam_camrot)
378 |     
379 |     loop_count = 0
380 |     last_time = t
381 | 
382 |     while true
383 |         sleep(0.0001)
384 |         states = [fetch(state_channels[id]) for id in 1:num_vehicles]
385 |         corners_body = [get_3d_bbox_corners(state, vehicle_size) for state in states]
386 |         tnow = time()
387 |         if tnow - t > min_Δ
388 |             t = tnow
389 |             for i = 1:num_vehicles             
390 |                 ego_state = states[i]
391 |                 T_world_body = get_body_transform(ego_state.q[1:4], ego_state.q[5:7])
392 |                 T_world_camrot1 = multiply_transforms(T_world_body, T_body_camrot1)
393 |                 T_world_camrot2 = multiply_transforms(T_world_body, T_body_camrot2)
394 |                 T_camrot1_world = invert_transform(T_world_camrot1)
395 |                 T_camrot2_world = invert_transform(T_world_camrot2)
396 |                 for (camera_id, transform) in zip((1,2), (T_camrot1_world, T_camrot2_world))
397 |                     bboxes = []
398 |                     
399 |                     for j = 1:num_vehicles
400 |                         j == i && continue
401 |                         other_vehicle_corners = [transform * [pt;1] for pt in corners_body[j]]
402 |                         visible = false
403 | 
404 |                         left = image_width+1
405 |                         right = 0
406 |                         top = image_height+1
407 |                         bot = 0
408 | 
409 |                         for corner in other_vehicle_corners
410 |                             if corner[3] < focal_len
411 |                                 break
412 |                             end
413 |                             px = focal_len*corner[1]/corner[3]
414 |                             py = focal_len*corner[2]/corner[3]
415 |                             px = convert_to_pixel(image_height, pixel_len, px)
416 |                             py = convert_to_pixel(image_height, pixel_len, py)
417 |                             left = min(left, px)
418 |                             right = max(right, px)
419 |                             top = min(top, py)
420 |                             bot = max(bot, py)
421 |                         end
422 |                         if left>image_width || top>image_height || right < 1 || bot < 1
423 |                             # out of frame
424 |                             continue
425 |                         else 
426 |                             # project extents into frame
427 |                             top = max(top, 1)
428 |                             left = max(left, 1)
429 |                             bot = min(bot, image_height)
430 |                             right = min(right, image_width)
431 |                             push!(bboxes, SVector(top, left, bot, right))
432 |                         end
433 |                     end
434 |                 
435 |                     meas = CameraMeasurement(t, camera_id, focal_len, pixel_len, image_width, image_height, bboxes)
436 |                     put!(cam_channels[i], meas)
437 |                 end
438 |             end
439 |         end
440 |     end
441 | end
442 | 
443 | function ground_truth(vehicles, state_channels, gt_channels; max_rate=10.0) 
444 |     min_Δ = 1.0/max_rate
445 |     t = time()
446 |     num_vehicles = length(vehicles)
447 |     vehicle_size = SVector(13.2, 5.7, 5.3)
448 |     while true
449 |         sleep(0.0001)
450 |         states = [fetch(state_channels[i]) for i = 1:num_vehicles]
451 |         tnow = time()
452 |         if tnow - t > min_Δ
453 |             t = tnow
454 |             measurements = [GroundTruthMeasurement(t, i, 
455 |                                                    states[i].q[5:7], 
456 |                                                    states[i].q[1:4], 
457 |                                                    states[i].v[4:6], 
458 |                                                    states[i].v[1:3],
459 |                                                    vehicle_size) for i = 1:num_vehicles]
460 |             for i = 1:num_vehicles
461 |                 foreach(m->put!(gt_channels[i], m), measurements)
462 |             end
463 |         end
464 |     end
465 | end
466 | 
467 | function update_targets(target_channels, state_channels, target_segments, map)
468 |     scores = zeros(Int, length(target_channels))
469 |     while true
470 |         sleep(0.001)
471 |         for i = 1:length(target_channels)
472 |             current_target = fetch(target_channels[i])
473 |             current_target ≤ 0 && continue
474 |             state = fetch(state_channels[i])
475 |             pos = state.q[5:6]
476 |             vel = state.v[4]
477 |             if reached_target(pos, vel, map[current_target])
478 |                 scores[i] += 1
479 |                 new_target = rand(setdiff(target_segments, current_target))
480 | 		        @info "Vehicle $i reached target! New target is $new_target"
481 | 		        for i = 1:length(target_channels)
482 | 		            @info "Scores: team $i has $(scores[i]) successful pickup/dropoffs"
483 | 		        end
484 |                 take!(target_channels[i])
485 | 		        put!(target_channels[i], new_target)
486 |             end
487 |         end
488 |     end
489 | end
490 | 
491 | function totalshutdown(c)
492 |     while isready(c)
493 |         take!(c)
494 |     end
495 |     close(c)
496 | end
497 | 
498 | function measure_vehicles(map,
499 |         vehicles, 
500 |         state_channels, 
501 |         meas_channels, 
502 |         info_channel,
503 |         shutdown_channel;
504 |         measure_gps = true,
505 |         measure_imu = true,
506 |         measure_cam = true,
507 |         measure_gt = true,
508 |         rng=MersenneTwister(1)
509 |     )
510 |     
511 |     target_segments = identify_loading_segments(map)
512 | 
513 |  
514 |     num_vehicles = length(state_channels)
515 |     @assert num_vehicles == length(meas_channels)
516 | 
517 |     gps_channels = [Channel{GPSMeasurement}(32) for _ in 1:num_vehicles]
518 |     imu_channels = [Channel{IMUMeasurement}(32) for _ in 1:num_vehicles]
519 |     cam_channels = [Channel{CameraMeasurement}(32) for _ in 1:num_vehicles]
520 |     gt_channels = [Channel{GroundTruthMeasurement}(32) for _ in 1:num_vehicles]
521 |     target_channels = [Channel{Int}(1) for _ in 1:num_vehicles]
522 |     frequency_channel = Channel{Any}(32)
523 |     
524 |     shutdown = false
525 |     @async while true
526 |         if isready(shutdown_channel)
527 |             shutdown = fetch(shutdown_channel)
528 |             if shutdown
529 |                 foreach(c->totalshutdown(c), [gps_channels; imu_channels; cam_channels; gt_channels])
530 |                 foreach(c->totalshutdown(c), values(meas_channels))
531 |                 foreach(c->totalshutdown(c), values(state_channels))
532 |                 foreach(c->totalshutdown(c), target_channels)
533 |                 totalshutdown(frequency_channel)
534 |                 break
535 |             end
536 |         end
537 |         sleep(0.1)
538 |     end
539 |     
540 |     for id in 1:num_vehicles
541 |         target = isempty(target_segments) ? -1 : rand(target_segments)
542 | 	    target >= 0 && @info "Target for vehicle $id: $target"
543 |         put!(target_channels[id], target)
544 |     end
545 | 
546 |     @async update_targets(target_channels, state_channels, target_segments, map)
547 | 
548 |     # Centralized Measurements
549 |     measure_gt && @async ground_truth(vehicles, state_channels, gt_channels)
550 |     measure_cam && @async cameras(vehicles, state_channels, cam_channels)
551 | 
552 |     for id in 1:num_vehicles
553 |         # Intrinsic Measurements
554 |         measure_gps && @async gps(vehicles[id], state_channels[id], gps_channels[id])
555 |         measure_imu && @async imu(vehicles[id], state_channels[id], imu_channels[id])
556 |  
557 |         let id=id
558 |             @async begin
559 |                 gps_measurements = GPSMeasurement[]
560 |                 imu_measurements = IMUMeasurement[]
561 |                 cam_measurements = CameraMeasurement[]
562 |                 gt_measurements = GroundTruthMeasurement[]
563 |                 meas_lists = (gps_measurements, imu_measurements, cam_measurements, gt_measurements)
564 |                 channels = (gps_channels[id], imu_channels[id], cam_channels[id], gt_channels[id])
565 |                 ch_names = ("gps", "imu", "cam", "gt")
566 |                 t = time()
567 |                 loop_count = 0
568 |                 meas_counts = Dict(name=>0 for name in ch_names)
569 |                 send_count = 0
570 |                 while true
571 |                     sleep(0.0001)
572 |                     loop_count += 1
573 |                     for (channel, meas_list, ch_name) in zip(channels, meas_lists, ch_names)
574 |                         while isready(channel)
575 |                             msg = take!(channel)
576 |                             push!(meas_list, msg)
577 |                             meas_counts[ch_name] += 1
578 |                         end
579 |                     end
580 |                     
581 |                     if isempty(meas_channels[id]) && !all(isempty.(meas_lists))
582 |                         target = fetch(target_channels[id])
583 |                         msg = MeasurementMessage(id, target, vcat(meas_lists...))
584 |                         put!(meas_channels[id], msg)
585 |                         send_count += 1
586 |                         for meas_list in meas_lists
587 |                             deleteat!(meas_list, 1:length(meas_list))
588 |                         end
589 |                     end
590 |                     for meas_list in meas_lists
591 |                         L = length(meas_list)
592 |                         if L > 20
593 |                             deleteat!(meas_list, 1:L)
594 |                         end
595 |                     end 
596 |                     if loop_count == 300
597 |                         td = time() - t
598 |                         freq_info = (; msg_type = "freq",  id=id, gps = meas_counts["gps"] / td, imu = meas_counts["imu"] / td, cam = meas_counts["cam"] / (2*td), gt = meas_counts["gt"] / (num_vehicles*td), sent = send_count / td)
599 |                         put!(info_channel, freq_info)
600 |                         loop_count = 0
601 |                         t += td
602 |                         for k in keys(meas_counts)
603 |                             meas_counts[k] = 0
604 |                         end
605 |                         send_count = 0
606 |                     end
607 |                 end
608 |             end
609 |         end
610 |     end
611 | 
612 | end
613 | 


--------------------------------------------------------------------------------
/src/objects.jl:
--------------------------------------------------------------------------------
  1 | mutable struct Quaternion{T}
  2 |     quaternion::SVector{4, T}
  3 | end
  4 | function Base.zero(::Type{Quaternion{T}}) where T
  5 |     Quaternion{T}([one(T), zero(T), zero(T), zero(T)])
  6 | end
  7 | 
  8 | function Base.getindex(q::Quaternion, i)
  9 |     q.quaternion[i]
 10 | end
 11 | function Base.setindex!(q::Quaternion{T}, v::T, i) where T
 12 |     q.quaternion[i] = v
 13 | end
 14 | function Base.firstindex(q::Quaternion)
 15 |     1
 16 | end
 17 | function Base.lastindex(q::Quaternion)
 18 |     4
 19 | end
 20 | 
 21 | 
 22 | function quaternion_to_rotation_matrix(q::Quaternion)
 23 |     w = q[1]
 24 |     x = q[2]
 25 |     y = q[3]
 26 |     z = q[4]
 27 |     @SMatrix [1-2y^2-2z^2 2x*y-2w*z 2x*z+2w*y;
 28 |               2x*y+2w*z 1-2x^2-2z^2 2y*z-2w*x
 29 |               2x*z-2w*y 2y*z+2w*x 1-2x^2-2y^2]
 30 | end
 31 | 
 32 | function quaternion_multiply!(q::Quaternion{T}, q1::Quaternion{T}, q2::Quaternion{T}) where T
 33 |     s1 = q1[1]
 34 |     s2 = q2[1]
 35 |     v1 = @SVector [q1.quaternion.x, q1.quaternion.y, q1.quaternion.z]
 36 |     v2 = @SVector [q2.quaternion.x, q2.quaternion.y, q2.quaternion.z]
 37 |     s = s1*s2 - v1'*v2
 38 |     v = s1*v2+s2*v1+v1×v2
 39 |     q[1] = s
 40 |     q[2:4] = v
 41 | end
 42 | 
 43 | function normalize!(q::Quaternion)
 44 |     qnorm = sqrt(q[1]^2 + q[2]^2 + q[3]^2 + q[4]^2)
 45 |     q[1:4] ./= qnorm
 46 |     nothing
 47 | end
 48 | 
 49 | 
 50 | function quaternion_multiply!(q, ω, q2::Quaternion{T}) where T
 51 |     s1 = zero(T) 
 52 |     s2 = q2[1]
 53 |     v1 = @SVector [ω[1], ω[2], ω[3]]
 54 |     v2 = @SVector [q2[2], q2[3], q2[4]]
 55 |     s = s1*s2 - v1'*v2
 56 |     v = s1*v2+s2*v1+v1×v2
 57 |     q[1] = s
 58 |     q[2:4] = v
 59 | end
 60 | 
 61 | struct RigidBodyState{T} <: AbstractVector{T}
 62 |     x::SVector{3, T}
 63 |     q::Quaternion{T}
 64 |     P::SVector{3, T}
 65 |     L::SVector{3, T}
 66 | end
 67 | 
 68 | function Base.zero(::Type{RigidBodyState{T}}) where T
 69 |     RigidBodyState{T}(zero(SVector{3,T}), zero(Quaternion{T}), zero(SVector{3,T}), zero(SVector{3,T}))
 70 | end
 71 | function Base.zero(::RigidBodyState{T}) where T
 72 |     RigidBodyState{T}(zero(SVector{3,T}), zero(Quaternion{T}), zero(SVector{3,T}), zero(SVector{3,T}))
 73 | end
 74 | function Base.getindex(rb::RigidBodyState{T}, i::Int) where T
 75 |     if i < 1 
 76 |         error("Out of bounds")
 77 |     elseif i < 4
 78 |         @inbounds rb.x[i]
 79 |     elseif i < 8
 80 |         @inbounds rb.q[i-3]
 81 |     elseif i < 11
 82 |         @inbounds rb.P[i-7]
 83 |     elseif i < 14
 84 |         @inbounds rb.L[i-10]
 85 |     else
 86 |         error("Out of bounds")
 87 |     end
 88 | end
 89 | function Base.setindex(rb::RigidBodyState{T}, v::T, i::Int) where T
 90 |     if i < 1 
 91 |         error("Out of bounds")
 92 |     elseif i < 4
 93 |         @inbounds rb.x[i] = v
 94 |     elseif i < 8
 95 |         @inbounds rb.q[i-3] = v
 96 |     elseif i < 11
 97 |         @inbounds rb.P[i-7] = v
 98 |     elseif i < 14
 99 |         @inbounds rb.L[i-10] = v
100 |     else
101 |         error("Out of bounds")
102 |     end
103 | end
104 | function Base.firstindex(rb::RigidBodyState{T}) where T
105 |     1
106 | end
107 | function Base.lastindex(rb::RigidBodyState{T}) where T
108 |     13
109 | end
110 | function Base.length(rb::RigidBodyState{T}) where T
111 |     13
112 | end
113 | function Base.eltype(::Type{RigidBodyState{T}}) where T
114 |     T
115 | end
116 | function Base.iterate(rb::RigidBodyState{T}) where T
117 |     rb[1], 2
118 | end
119 | function Base.iterate(rb::RigidBodyState{T}, state) where T
120 |     return state > 13 ? nothing : (rb[state], state+1)
121 | end
122 | 
123 | struct RigidBodyParameters
124 |     mass::Float64
125 |     I⁻¹::SMatrix{3,3,Float64}
126 | end
127 | 
128 | function rigid_body!(drbs, rbs, p, t)
129 |     (I⁻¹, mass, F, r, Fg) = p(t)
130 |     q = Quaternion{Float64}(rbs[4:7])
131 |     normalize!(q)
132 |     R = quaternion_to_rotation_matrix(q)
133 |     I⁻¹ = R' * I⁻¹ * R
134 |     ω = I⁻¹ * rbs[11:13]
135 |     τ = (R*r) × F - 100*ω
136 |     drbs[1:3] = rbs[8:10] ./ mass
137 |     quaternion_multiply!(@view(drbs[4:7]), ω, q)
138 |     drbs[8:10] = F + Fg
139 |     drbs[11:13] = τ
140 | end
141 | 
142 | 
143 | 
144 | function test_simulate(vis, urdf_filename)
145 |     urdf_path = joinpath(dirname(pathof(VehicleSim)), "assets", urdf_filename)
146 |     mechanism = parse_urdf(urdf_path)
147 |     mechanism_state = MechanismState(mechanism)
148 |     J = first(joints(mechanism))
149 | 
150 |     mvis = MechanismVisualizer(mechanism, URDFVisuals(urdf_path, package_path=[dirname(pathof(VehicleSim))]), vis)
151 |     
152 |     rbs = zero(RigidBodyState{Float64})
153 |     set_configuration!(mechanism_state, J, [rbs.q[1:4]; rbs.x])
154 |     set_configuration!(mvis, configuration(mechanism_state))
155 | 
156 |     inertia_obj = mechanism.graph.vertices[2] |> spatial_inertia
157 |     rb_params = RigidBodyParameters(inertia_obj.mass, inv(inertia_obj.moment))
158 |     F = @SVector [0, 0, 9.81*(rb_params.mass)]
159 |     Fg = @SVector [0, 0, -9.81*(rb_params.mass)]
160 |     r = @SVector [5, 2.5, 2.5]
161 | 
162 |     p = t -> (rb_params.I⁻¹, rb_params.mass, F, r, Fg)
163 | 
164 |     tspan = (0.0, 100.0)
165 |     rbs = zero(MVector{13, Float64})
166 |     rbsdot = zero(MVector{13, Float64})
167 |     rbs[4] = 1.0
168 |     prob = ODEProblem(rigid_body!, rbs, tspan, p)
169 |     sol = solve(prob)
170 | 
171 |     @infiltrate
172 |     for t = 1:1000000 
173 |         u = sol(t/10000.0)
174 |         set_configuration!(mechanism_state, J, [u[4:7]; u[1:3]])
175 |         set_configuration!(mvis, configuration(mechanism_state))
176 |     end
177 | end
178 | 


--------------------------------------------------------------------------------
/src/sim.jl:
--------------------------------------------------------------------------------
  1 | struct EndOfSimException <: Exception end
  2 | struct EndOfVehicleException <: Exception end
  3 | 
  4 | function vehicle_simulate(state::MechanismState{X}, mviss, vehicle_id, final_time, internal_control!, external_control!, publisher!; Δt=1e-3, stabilization_gains=RigidBodyDynamics.default_constraint_stabilization_gains(X), max_realtime_rate=Inf) where X
  5 |     T = RigidBodyDynamics.cache_eltype(state)
  6 |     result = DynamicsResult{T}(state.mechanism)
  7 |     control_torques = similar(velocity(state))
  8 |     external_wrenches = Dict{RigidBodyDynamics.BodyID, RigidBodyDynamics.Wrench{T}}()
  9 |     closed_loop_dynamics! = let result=result, control_torques=control_torques, stabilization_gains=stabilization_gains 
 10 |         function (v̇::AbstractArray, ṡ::AbstractArray, t, state)
 11 |             internal_control!(control_torques, t, state)
 12 |             external_control!(external_wrenches, t, state)
 13 |             publisher!(t, state)
 14 |             dynamics!(result, state, control_torques, external_wrenches; stabilization_gains=stabilization_gains)
 15 |             copyto!(v̇, result.v̇)
 16 |             copyto!(ṡ, result.ṡ)
 17 |             nothing
 18 |         end
 19 |     end
 20 |     tableau = RigidBodyDynamics.runge_kutta_4(T)
 21 |     sink = FollowCamSink(mviss, vehicle_id)
 22 |     integrator = RigidBodyDynamics.MuntheKaasIntegrator(state, closed_loop_dynamics!, tableau, sink)
 23 |     RigidBodyDynamics.integrate(integrator, final_time, Δt; max_realtime_rate)
 24 | end
 25 | 
 26 | function load_mechanism(; no_mesh=false)
 27 |     if no_mesh
 28 |         urdf_path = joinpath(dirname(pathof(VehicleSim)), "assets", "chevy_nomesh.urdf")
 29 |     else
 30 |         urdf_path = joinpath(dirname(pathof(VehicleSim)), "assets", "chevy.urdf")
 31 |     end
 32 |     chevy_base = parse_urdf(urdf_path, floating=true)
 33 |     chevy_joints = joints(chevy_base)
 34 |     (; urdf_path, chevy_base, chevy_joints)
 35 | end
 36 | 
 37 | function logger(info_channel, shutdown_channel, num_vehicles)
 38 |     sleep(1.0)
 39 |     frequencies = Dict(id => Dict("gps"=>0.0, "imu" => 0.0, "cam" => 0.0, "gt"=>0.0, "sent" => 0.0) for id in 1:num_vehicles)
 40 |     foreach(i->println(), 1:num_vehicles+3)
 41 | 
 42 |     sim_view_str = ""
 43 |     sim_info_str = ""
 44 | 
 45 |     vehicles_connected = [false for i in 1:num_vehicles]
 46 |     vehicle_connection_str = "Vehicles connected: "
 47 |     for i in 1:num_vehicles
 48 |         vehicle_connection_str *= "$i : $(vehicles_connected[i]). "
 49 |     end
 50 |     
 51 |     while true
 52 |         if isready(shutdown_channel)
 53 |             break
 54 |         end
 55 |         sleep(0.001)
 56 |         while isready(info_channel)
 57 |             meas = take!(info_channel)
 58 |             if meas.msg_type == "freq"
 59 |                 frequencies[meas.id]["gps"] = meas.gps
 60 |                 frequencies[meas.id]["imu"] = meas.imu
 61 |                 frequencies[meas.id]["cam"] = meas.cam
 62 |                 frequencies[meas.id]["gt"] = meas.gt
 63 |                 frequencies[meas.id]["sent"] = meas.sent
 64 |             elseif meas.msg_type == "sim_status"
 65 |                 sim_info_str = meas.str
 66 |             elseif meas.msg_type == "connection_status"
 67 |                 vehicles_connected[meas.id] = meas.status
 68 |                 vehicle_connection_str = "Vehicles connected: "
 69 |                 for i in 1:num_vehicles
 70 |                     vehicle_connection_str *= "$i : $(vehicles_connected[i]). "
 71 |                 end
 72 |             elseif meas.msg_type == "view_status"
 73 |                 sim_view_str = meas.str
 74 |             end
 75 |         end
 76 |         str = repeat("\033[F", num_vehicles+3)
 77 |         print(str)
 78 |         for i in 1:num_vehicles
 79 |             gps = frequencies[i]["gps"]
 80 |             imu = frequencies[i]["imu"]
 81 |             cam = frequencies[i]["cam"]
 82 |             gt = frequencies[i]["gt"]
 83 |             sent = frequencies[i]["sent"]
 84 |             @printf("Vehicle %i: gps freq: %6.3f, imu freq: %6.3f, cam freq: %6.3f, gt freq: %6.3f, send freq: %6.3f \u1b[0K \n", i, gps, imu, cam, gt, sent)
 85 |         end
 86 |         @printf("%s \u1b[0K \n", sim_info_str)
 87 |         @printf("%s \u1b[0K \n", vehicle_connection_str)
 88 |         @printf("%s \u1b[0K \n", sim_view_str)
 89 |         flush(stdout)
 90 |     end
 91 | end
 92 | 
 93 | function server(max_vehicles=1, 
 94 |         port=4444; 
 95 |         full_state=true,
 96 |         open_vis=true,
 97 |         no_mesh=false,
 98 |         map_type=:city,
 99 |         rng=MersenneTwister(1), 
100 |         measure_gps=false, 
101 |         measure_imu=false, 
102 |         measure_cam=false, 
103 |         measure_gt=false,
104 |         measure_all=false,
105 |         monitor_rates=true)
106 | 
107 |     host = getipaddr()
108 |     if map_type == :city
109 |         map = city_map()
110 |     else
111 |         @assert max_vehicles==1
112 |         map = training_map()
113 |     end
114 |     server_visualizer = get_vis(map, open_vis, host)
115 |     server_info_string = 
116 |         "********************
117 |       CONNECTING TO SERVER. V3
118 |       ********************
119 |         -Connect a keyboard client by running (in a new REPL):
120 |             using VehicleSim, Sockets
121 |             keyboard_client(ip\"$host\");
122 |         -Port for manual clients is $port"
123 |     @info server_info_string
124 |     #@info inform_hostport(server_visualizer, "Server visualizer")
125 |     #client_visualizers = [get_vis(map, false, host) for _ in 1:max_vehicles]
126 |     #all_visualizers = [client_visualizers; server_visualizer]
127 |     all_visualizers = [server_visualizer,]
128 | 
129 |     (; urdf_path, chevy_base, chevy_joints) = load_mechanism(; no_mesh)
130 |     chevy_visuals = URDFVisuals(urdf_path, package_path=[dirname(pathof(VehicleSim))])
131 | 
132 |     viable_segments = Set(keys(map))
133 |     spawn_points = Dict()
134 |     vehicles = Dict()
135 | 
136 |     state_channels = Dict(id=>Channel{MechanismState}(1) for id in 1:max_vehicles)
137 |     cmd_channels = Dict(id=>Channel{VehicleCommand}(1) for id in 1:max_vehicles)
138 |     meas_channels = Dict(id=>Channel{MeasurementMessage}(1) for id in 1:max_vehicles)
139 |     watch_channel = Channel{Int}(1)
140 |     shutdown_channel = Channel{Bool}(1)
141 |     info_channel = Channel{Any}(32)
142 |     put!(watch_channel, 0)
143 | 
144 |     for vehicle_id in 1:max_vehicles
145 |         local seg
146 |         while true
147 |             seg_id = rand(rng, viable_segments)
148 |             if map_type ≠ :city
149 |                 seg_id = 1
150 |                 seg = map[seg_id]
151 |                 break
152 |             end
153 |             delete!(viable_segments, seg_id)
154 |             if contains_lane_type(map[seg_id], intersection, stop_sign)
155 |                 continue
156 |             else
157 |                 seg = map[seg_id]
158 |                 break
159 |             end
160 |         end
161 |         spawn_points[vehicle_id] = seg
162 |         vehicle = spawn_car_on_map(all_visualizers, seg, chevy_base, chevy_visuals, chevy_joints, vehicle_id)
163 |         @async sim_car(cmd_channels[vehicle_id], state_channels[vehicle_id], shutdown_channel, vehicle, vehicle_id)
164 |         vehicles[vehicle_id] = vehicle
165 |     end
166 | 
167 |     @async visualize_vehicles(vehicles, state_channels, shutdown_channel, watch_channel)
168 | 
169 |     measure_gps |= measure_all
170 |     measure_imu |= measure_all
171 |     measure_cam |= measure_all
172 |     measure_gt |= measure_all
173 | 
174 |     measure_vehicles(map,
175 |                      vehicles,
176 |                      state_channels,
177 |                      meas_channels,
178 |                      info_channel,
179 |                      shutdown_channel;
180 |                      rng,
181 |                      measure_gps,
182 |                      measure_imu,
183 |                      measure_cam,
184 |                      measure_gt)
185 | 
186 |     print_line_offset = monitor_rates ? max_vehicles : 0
187 | 
188 |     client_connections = [false for _ in 1:max_vehicles]
189 | 
190 |     client_count = 0
191 |     sim_task = errormonitor(@async begin
192 |         server = listen(host, port)
193 |         @async begin
194 |             while true
195 |                 if isready(shutdown_channel)
196 |                     shutdown = fetch(shutdown_channel)
197 |                     if shutdown
198 |                         put!(info_channel, (; msg_type="sim_status", str="Shutting down TCP server."))
199 |                         #@info "Shutting down TCP server."
200 |                         close(server)
201 |                         break
202 |                     end
203 |                 end
204 |                 sleep(0.1)
205 |             end
206 |         end
207 |         while true
208 |             try
209 |                 put!(info_channel, (; msg_type="sim_status", str="Waiting for client."))
210 |                 sock = accept(server)
211 |                 #put!(info_channel, (; msg_type="sim_status", str="Client accepted."))
212 |                 client_count = mod1(client_count+1, max_vehicles)
213 |                 put!(info_channel, (; msg_type="connection_status", id=client_count, status=true))
214 |                 if client_connections[client_count]
215 |                     put!(info_channel, (; msg_type="sim_status", str="ERROR! Requested vehicle already in use!"))
216 |                     close(sock)
217 |                     break
218 |                 end
219 |                 serialize(sock, inform_hostport(all_visualizers[1], "Visualizer"))
220 |                 let vehicle_id=client_count
221 |                     @async begin
222 |                         try
223 |                             while isopen(sock)
224 |                                 sleep(0.001)
225 |                                 if isready(shutdown_channel) && fetch(shutdown_channel)
226 |                                     close(sock)
227 |                                     break
228 |                                 end
229 |                                 local car_cmd
230 |                                 received = false
231 |                                 while true
232 |                                     t = @async eof(sock)
233 |                                     if bytesavailable(sock) > 0
234 |                                         car_cmd = deserialize(sock)
235 |                                         received = true
236 |                                     else
237 |                                         #!istaskdone(t) && schedule(t, InterruptException(); error=true)
238 |                                         break
239 |                                     end
240 |                                 end
241 | 
242 |                                 !received && continue
243 |                                 car_cmd =  VehicleCommand(car_cmd...)
244 |                                 put!(cmd_channels[vehicle_id], car_cmd)
245 |                                 if !car_cmd.controlled
246 |                                     put!(info_channel, (; msg_type="connection_status", id=vehicle_id, status=false))
247 |                                     close(sock)
248 |                                end
249 |                             end
250 |                         catch e
251 |                             put!(info_channel, (; msg_type="sim_status", str="WARNING! Error receiving command for vehicle $vehicle_id. Client may have failed. Closing socket connection to client."))
252 |                             close(sock)
253 |                         end
254 |                     end
255 |                     @async begin
256 |                         try
257 |                             while isopen(sock)
258 |                                 sleep(0.001)
259 |                                 if isready(shutdown_channel) && fetch(shutdown_channel)
260 |                                     break
261 |                                 end
262 |                                 if isready(meas_channels[vehicle_id])
263 |                                     msg = take!(meas_channels[vehicle_id])
264 |                                     serialize(sock, msg)
265 |                                 end
266 |                             end
267 |                         catch e
268 |                             put!(info_channel, (; msg_type="sim_status", str="WARNING! Error sending measurements to vehicle $vehicle_id. Did client fail?."))
269 |                         end
270 |                     end
271 |                 end
272 |             catch e
273 |                 break
274 |             end
275 |         end
276 |     end)
277 |     @async logger(info_channel, shutdown_channel, max_vehicles)
278 |     update_viewer(watch_channel, shutdown_channel, max_vehicles, info_channel)
279 | end
280 | 
281 | function update_viewer(watch_channel, shutdown_channel, max_vehicles, info_channel)
282 |     info_string = 
283 |         "***************
284 |       VIEWER COMMANDS
285 |       ***************
286 |             -Make sure focus is on this terminal window. Then:
287 |             -Press 'q' to shutdown server. 
288 |             -Press '0' to switch to bird's-eye view and release controls to user.
289 |             -Press a number '1'-'9' to view the follow-cam for the associated vehicle. Will default to '0' if vehicle doesn't exist.
290 |             -Use the 'shift' modifier to follow-cam from top-down (e.g. '!' for vehicle 1)."
291 |     @info info_string
292 |     while true
293 |         sleep(0.1)
294 |         key = get_c()
295 | 
296 |         if key == '!'
297 |             intkey = 11
298 |         elseif key == '@'
299 |             intkey = 12
300 |         elseif key == '#'
301 |             intkey = 13
302 |         elseif key == '$'
303 |             intkey = 14
304 |         elseif key == '%'
305 |             intkey = 15
306 |         elseif key == '^'
307 |             intkey = 16
308 |         elseif key == '&'
309 |             intkey = 17
310 |         elseif key == '*'
311 |             intkey = 18
312 |         elseif key == '('
313 |             intkey = 19
314 |         else
315 |             intkey = try
316 |                 parse(Int, key)
317 |             catch err
318 |                 nothing
319 |             end
320 |         end
321 |         if key == 'q'
322 |             # terminate vehicle 
323 |             put!(info_channel, (; msg_type="view_status", str="Shutting down server."))
324 |             shutdown!(shutdown_channel)
325 |             break
326 |         elseif key == '0' || (!isnothing(intkey) && mod(intkey,10) > max_vehicles)
327 |             put!(info_channel, (; msg_type="view_status", str="Switching to server view."))
328 |             take!(watch_channel)
329 |             put!(watch_channel, 0)
330 |         elseif key ∈ ['!','@','#','$','%','^','&','*','(']
331 |             put!(info_channel, (; msg_type="view_status", str="Switching view to vehicle $(mod(intkey, 10)) (top-down)"))
332 |             take!(watch_channel)
333 |             put!(watch_channel, intkey)
334 |         elseif key ∈ ['1','2','3','4','5','6','7','8','9']
335 |             put!(info_channel, (; msg_type="view_status", str="Switching view to vehicle $intkey (follow-cam)"))
336 |             take!(watch_channel)
337 |             put!(watch_channel, intkey)
338 |         else
339 |             put!(info_channel, (; msg_type="view_status", str="Unrecognized command, try again."))
340 |         end
341 |     end
342 | end
343 | 
344 | function shutdown!(shutdown_channel)
345 |     put!(shutdown_channel, true)
346 |     nothing
347 | end
348 | 
349 | function reset_vehicle!(vehicle, seg)
350 |     mviss = vehicle.mviss
351 |     chevy = vehicle.chevy
352 |     state = vehicle.state
353 | 
354 |     config = get_initialization_point(seg)
355 |     foreach(mvis->configure_car!(mvis, state, joints(chevy), config), mviss)
356 | end
357 |     
358 | function spawn_car_on_map(visualizers, seg, chevy_base, chevy_visuals, chevy_joints, vehicle_id)
359 |     chevy = deepcopy(chevy_base)
360 |     chevy.graph.vertices[2].name="chevy_$vehicle_id"
361 |     configure_contact_points!(chevy)
362 |     state = MechanismState(chevy)
363 | 
364 |     mviss = map(visualizers) do vis
365 |         MechanismVisualizer(chevy, chevy_visuals, vis)
366 |     end
367 | 
368 |     vehicle = (; chevy, state, mviss)
369 |     reset_vehicle!(vehicle, seg)
370 |     vehicle
371 | end
372 | 
373 | function sim_car(cmd_channel, state_channel, shutdown_channel, vehicle, vehicle_id)
374 | 
375 |     chevy = vehicle.chevy
376 |     state = vehicle.state
377 |     mviss = vehicle.mviss
378 | 
379 |     v = 0.0
380 |     δ = 0.0
381 |     controlled = false
382 | 
383 |     set_reference! = (cmd) -> begin
384 |         v = cmd.velocity # rename
385 |         δ = cmd.steering_angle
386 |         controlled = cmd.controlled
387 |     end
388 | 
389 |     control! = (torques, t, state) -> begin
390 |             torques .= 0.0
391 |             steering_control!(torques, t, state; reference_angle=δ)
392 |             suspension_control!(torques, t, state)
393 |     end
394 | 
395 |     wrenches! = (bodyid_to_wrench, t, state) -> begin
396 |         RigidBodyDynamics.update_transforms!(state)
397 |         wheel_control!(bodyid_to_wrench, chevy, t, state; reference_velocity=v)
398 |     end
399 | 
400 |     publisher! = (t, state) -> begin
401 |         if isready(shutdown_channel)
402 |             fetch(shutdown_channel) && throw(error("Shutdown!"))
403 |         end
404 |         if isready(state_channel)
405 |             stale = take!(state_channel)
406 |         end
407 |         put!(state_channel, state)
408 |     end
409 | 
410 |     @async while true
411 |         sleep(0.001)
412 |         if isready(shutdown_channel)
413 |             fetch(shutdown_channel) && break
414 |         end
415 |         if isready(cmd_channel)
416 |             car_cmd = take!(cmd_channel)
417 |             set_reference!(car_cmd)
418 |         end
419 |     end
420 | 
421 |     try 
422 |         vehicle_simulate(state, 
423 |                          mviss,
424 |                          vehicle_id,
425 |                          Inf, 
426 |                          control!, 
427 |                          wrenches!,
428 |                          publisher!;
429 |                          max_realtime_rate=1.0)
430 |     catch e
431 |         @warn "Terminating simulation for vehicle $vehicle_id."
432 |         foreach(mvis->delete_vehicle!(mvis), mviss)
433 |     end
434 | end
435 | 
436 | 


--------------------------------------------------------------------------------
/src/sink.jl:
--------------------------------------------------------------------------------
 1 | mutable struct FollowCamSink <: RigidBodyDynamics.OdeIntegrators.OdeResultsSink
 2 |     mviss::Vector{MechanismVisualizer}
 3 |     follow_cam_id::Int
 4 |     min_wall_Δt::Float64
 5 |     last_update_wall_time::Float64
 6 |     follow_dist::Float64
 7 |     follow_height::Float64
 8 |     follow_offset::Float64
 9 | 
10 |     function FollowCamSink(mviss, follow_cam_id; 
11 |             max_fps::Float64 = 60., 
12 |             follow_dist=35.0, 
13 |             follow_height=6.0,
14 |             follow_offset=6.0)
15 |         new(mviss, follow_cam_id, 1 / max_fps, -Inf, follow_dist, follow_height, follow_offset)
16 |     end
17 | end
18 | 
19 | function RigidBodyDynamics.OdeIntegrators.initialize(sink::FollowCamSink, t, state)
20 |     sink.last_update_wall_time = -Inf
21 |     RigidBodyDynamics.OdeIntegrators.process(sink, t, state)
22 | end
23 | 
24 | function RigidBodyDynamics.OdeIntegrators.process(sink::FollowCamSink, t, state)
25 |     wall_Δt = time() - sink.last_update_wall_time
26 |     if wall_Δt > sink.min_wall_Δt
27 |         sink.last_update_wall_time = time()
28 |     end
29 |     nothing
30 | end
31 | 


--------------------------------------------------------------------------------
/src/view_car.jl:
--------------------------------------------------------------------------------
  1 | function get_vis(map=nothing, open_vis=true, host::IPAddr = ip"127.0.0.1", default_port=8700)
  2 |     #vis = @suppress Visualizer(MeshCat.CoreVisualizer(host, default_port), ["meshcat"])
  3 |     vis = Visualizer(MeshCat.CoreVisualizer(host, default_port), ["meshcat"])
  4 |     !isnothing(map) && view_map(vis, map)
  5 |     open_vis && open(vis)
  6 |     remove_grid!(vis)
  7 |     return vis
  8 | end
  9 | 
 10 | function inform_hostport(vis, name=nothing)
 11 |     if isnothing(name) 
 12 |         name = "Visualizer"
 13 |     end
 14 |     "$name can be connected to at $(vis.core.host):$(vis.core.port)"
 15 | end
 16 | 
 17 | function extract_yaw_from_quaternion(q)
 18 |     atan(2(q[1]*q[4]+q[2]*q[3]), 1-2*(q[3]^2+q[4]^2))
 19 | end
 20 | 
 21 | function remove_grid!(vis)
 22 |     delete!(vis["/Grid"])
 23 |     delete!(vis["/Axes"])
 24 |     vis
 25 | end
 26 | 
 27 | function configure_car!(mvis, state, joints, config)
 28 |     wb = joints[1]
 29 |     lsl = joints[6]
 30 |     rsl = joints[7]
 31 |     
 32 |     axis_roll = [1, 0, 0]
 33 |     axis_pitch = [0, 1, 0]
 34 |     axis_yaw = [0, 0, 1]
 35 |     
 36 |     q1 = [cos(config.roll_angle/2); sin(config.roll_angle/2)*axis_roll]
 37 |     q2 = [cos(config.pitch_angle/2); sin(config.pitch_angle/2)*axis_pitch]
 38 |     q3 = [cos(config.yaw_angle/2); sin(config.yaw_angle/2)*axis_yaw]
 39 |     s1 = q1[1]
 40 |     s2 = q2[1]
 41 |     s3 = q3[1]
 42 |     v1 = q1[2:4]
 43 |     v2 = q2[2:4]
 44 |     v3 = q3[2:4]
 45 |     s = s1*s2 - v1'*v2
 46 |     v = s1*v2+s2*v1+v1×v2
 47 |     q = [s; v]
 48 |     s12 = q[1]
 49 |     v12 = q[2:4]
 50 |     s = s12*s3 - v12'*v3
 51 |     v = s12*v3+s3*v12+v12×v3
 52 |     q = [s; v]
 53 |     
 54 |     for joint in joints
 55 |         set_velocity!(state, joint, zero(velocity(state, joint)))
 56 |         set_configuration!(state, joint, zero(configuration(state, joint)))
 57 |     end
 58 | 
 59 |     set_configuration!(state, wb, [q; config.position])
 60 |     set_configuration!(state, lsl, config.steering_angle)
 61 |     set_configuration!(state, rsl, config.steering_angle)
 62 |     
 63 |     if !isnothing(mvis)
 64 |         set_configuration!(mvis, configuration(state))
 65 |     end
 66 | end
 67 | 
 68 | function delete_vehicle!(mvis)
 69 |     vehicle = mvis.state.mechanism
 70 |     vis = mvis.visualizer
 71 |     delete_vehicle!(vis, vehicle)
 72 | end
 73 | 
 74 | function delete_vehicle!(vis, vehicle)
 75 |     name = vehicle.graph.vertices[2].name
 76 |     path = "/meshcat/world/$name"
 77 |     delete!(vis[path])
 78 |     setcameratarget!(vis, [0,0,0])
 79 |     setcameraposition!(vis, [0, -3, 1])
 80 |     nothing
 81 | end
 82 | 
 83 | function configure_contact_points!(chevy)
 84 |     origin = RigidBodyDynamics.Point3D(bodies(chevy)[1].frame_definitions[1].from, [0.0,0,0])
 85 |     one_up = RigidBodyDynamics.Point3D(bodies(chevy)[1].frame_definitions[1].from, [0.0,0,1.0])
 86 |     dir_up = RigidBodyDynamics.FreeVector3D(one_up)
 87 | 
 88 |     hs = RigidBodyDynamics.HalfSpace3D(origin, dir_up)
 89 |     RigidBodyDynamics.add_environment_primitive!(chevy, hs)
 90 | 
 91 |     # define contact point on wheel origins (will need to change
 92 |     # this for sloped surfaces   
 93 |     friction_model = RigidBodyDynamics.ViscoelasticCoulombModel{Float64}(1000.0,0.0,1000.0)
 94 |     normal_contact_model = RigidBodyDynamics.hunt_crossley_hertz() # investigate parameters if penetrating
 95 |     soft_contact_model = RigidBodyDynamics.SoftContactModel(normal_contact_model, friction_model)
 96 | 
 97 |     for (link_id, frame_id) in zip((6,6,7,8), (2,3,2,2))
 98 |         link = bodies(chevy)[link_id]
 99 |         frame = link.frame_definitions[frame_id].from
100 |         pt = RigidBodyDynamics.Point3D(frame, SVector(0.0,-1.25,0))
101 |         fl_dir = RigidBodyDynamics.Point3D(frame, SVector(1.0, 0, 0)) |> RigidBodyDynamics.FreeVector3D
102 |         contact_point = RigidBodyDynamics.ContactPoint(pt, fl_dir, soft_contact_model)
103 |         RigidBodyDynamics.add_contact_point!(link, contact_point)
104 |     end
105 | end
106 | 
107 | function visualize_vehicles(vehicles, state_channels, shutdown_channel, watch_channel;
108 |                             follow_dist=35.0,
109 |                             follow_height=6.0,
110 |                             follow_offset=6.0)
111 |     released = false
112 |     while true
113 |         sleep(0.001)
114 |         if isready(shutdown_channel)
115 |             fetch(shutdown_channel) && break
116 |         end
117 |         watch_key = fetch(watch_channel)
118 |         vehicle_to_watch = mod(watch_key, 10)
119 |         style_to_watch = div(watch_key, 10, RoundDown)
120 | 
121 |         if vehicle_to_watch == 0 || vehicle_to_watch ∉ keys(state_channels)
122 |             state_to_watch = nothing
123 |         else
124 |             state_to_watch = fetch(state_channels[vehicle_to_watch])
125 |         end
126 |         all_vis = [mvis.visualizer for mvis in vehicles[1].mviss]
127 |         for i in keys(vehicles)
128 |             mviss = vehicles[i].mviss
129 |             if isready(state_channels[i])
130 |                 state = fetch(state_channels[i])
131 |                 config = configuration(state)
132 |                 foreach(mvis->set_configuration!(mvis, config), mviss)
133 |             end
134 |         end
135 |         for vis in all_vis
136 |             if vehicle_to_watch > 0
137 |                 config = configuration(state_to_watch)
138 | 
139 |                 quat = config[1:4]
140 |                 pose = config[5:7]
141 |                 yaw = extract_yaw_from_quaternion(quat) 
142 | 
143 |                 if style_to_watch == 0
144 |                     offset = [follow_dist * [cos(yaw), sin(yaw)]; -follow_height] +
145 |                              follow_offset * [sin(yaw), -cos(yaw), 0]
146 |                 elseif style_to_watch == 1
147 |                     offset = [0.0, 0.0, -50.0]
148 |                 end
149 |                 setcameratarget!(vis, pose)
150 |                 setcameraposition!(vis, pose-offset)
151 |                 path = "/Cameras/default/rotated/<object>"
152 |                 setprop!(vis[path], "zoom", 1.0)
153 |                 released = false
154 |             elseif vehicle_to_watch == 0 && !released
155 |                 setcameratarget!(vis, [0.0,0,0])
156 |                 setcameraposition!(vis, [0,0,95.0])
157 |                 path = "/Cameras/default/rotated/<object>"
158 |                 setprop!(vis[path], "zoom", 0.5)
159 |                 released = true
160 |             end
161 |         end
162 |     end
163 | end
164 | 
165 | function view_car(vis; max_realtime_rate=1.0)
166 |     delete!(vis)
167 |     urdf_path = joinpath(dirname(pathof(VehicleSim)), "assets", "chevy_nomesh.urdf")
168 |     chevy = parse_urdf(urdf_path, floating=true)
169 |     
170 |     configure_contact_points!(chevy)
171 | 
172 |     state = MechanismState(chevy)
173 | 
174 |     wheel_angular_vel = 3.0
175 |     drive_force = 1000.0
176 |     steering_angle = 0.15
177 | 
178 |     for jid in 8:11
179 |         wheel_joint = joints(chevy)[jid]
180 |         set_velocity!(state, wheel_joint, -wheel_angular_vel)
181 |     end
182 |     
183 |     chevy_visuals = URDFVisuals(urdf_path, package_path=[dirname(pathof(VehicleSim))])
184 |     
185 |     mvis = MechanismVisualizer(chevy, chevy_visuals, vis)
186 | 
187 |     all_joints = joints(chevy)
188 |     config = CarConfig(SVector(0.0,0,4.0), 0.0, 0.0, 0.0, 0.0)
189 |     configure_car!(mvis, state, all_joints, config)
190 | 
191 |     control! = (torques, t, state) -> begin
192 |         torques .= 0.0
193 |         steering_control!(torques, t, state; reference_angle=steering_angle)
194 |         suspension_control!(torques, t, state)
195 |     end
196 | 
197 |     wrenches! = (bodyid_to_wrench, t, state) -> begin
198 |         RigidBodyDynamics.update_transforms!(state)
199 |         df = t > 0.1 ? drive_force : 0.0
200 |         for i = 7:8
201 |             bodyid = BodyID(i)
202 |             wheel = bodies(chevy)[i]
203 |             frame = wheel.frame_definitions[1].from
204 |             body_to_root = transform_to_root(state, bodyid, false)
205 |             wrench = Wrench{Float64}(frame, [0.0,0,0], [df,0,0])
206 |             bodyid_to_wrench[BodyID(i)] = transform(wrench, body_to_root)
207 |         end
208 |         nothing
209 |     end
210 |     
211 |     vehicle_simulate(state, mvis, 10.0, control!, wrenches!; max_realtime_rate)
212 |     #ts, qs, vs = RigidBodyDynamics.simulate(state, 5.0, control!)
213 |     #MeshCatMechanisms.animate(mvis, ts, qs; realtimerate=1.0)
214 |     #display(qs[end])
215 | end
216 | 
217 | struct CarConfig
218 |     position::SVector{3, Float64}
219 |     roll_angle::Float64
220 |     pitch_angle::Float64
221 |     yaw_angle::Float64
222 |     steering_angle::Float64
223 | end
224 | 
225 | 
226 | 
227 | 


--------------------------------------------------------------------------------