1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2019 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Asher Elmquist
13 // =============================================================================
14 //
15 // Chrono demonstration for testing the chrono sensor module
16 //
17 //
18 // =============================================================================
19
20 #include <cmath>
21 #include <cstdio>
22 #include <iomanip>
23
24 #include "chrono/assets/ChTriangleMeshShape.h"
25 #include "chrono/assets/ChVisualMaterial.h"
26 #include "chrono/assets/ChVisualization.h"
27 #include "chrono/geometry/ChTriangleMeshConnected.h"
28 #include "chrono/physics/ChBodyEasy.h"
29 #include "chrono/physics/ChSystemNSC.h"
30 #include "chrono/physics/ChInertiaUtils.h"
31 #include "chrono/utils/ChUtilsCreators.h"
32 #include "chrono_thirdparty/filesystem/path.h"
33
34 #include "chrono_sensor/sensors/ChCameraSensor.h"
35 #include "chrono_sensor/sensors/ChGPSSensor.h"
36 #include "chrono_sensor/sensors/ChIMUSensor.h"
37 #include "chrono_sensor/sensors/ChNoiseModel.h"
38 #include "chrono_sensor/sensors/ChLidarSensor.h"
39 #include "chrono_sensor/ChSensorManager.h"
40 #include "chrono_sensor/filters/ChFilterAccess.h"
41 #include "chrono_sensor/filters/ChFilterImageOps.h"
42 #include "chrono_sensor/filters/ChFilterLidarIntensityClip.h"
43 #include "chrono_sensor/filters/ChFilterCameraNoise.h"
44 #include "chrono_sensor/filters/ChFilterGrayscale.h"
45 #include "chrono_sensor/filters/ChFilterPCfromDepth.h"
46 #include "chrono_sensor/filters/ChFilterSave.h"
47 #include "chrono_sensor/filters/ChFilterSavePtCloud.h"
48 #include "chrono_sensor/filters/ChFilterVisualize.h"
49 #include "chrono_sensor/filters/ChFilterVisualizePointCloud.h"
50
51 using namespace chrono;
52 using namespace chrono::geometry;
53 using namespace chrono::sensor;
54
55 int num_cameras = 2 - 1;
56 int num_bodies = 10;
57 int num_groups = 1;
58
59 bool save_data = false;
60 bool display_data = true;
61
62 bool run_chrono = true;
63 float time_step = 0.002f;
64 float end_time = 30.0f;
65
main(int argc,char * argv[])66 int main(int argc, char* argv[]) {
67 GetLog() << "Copyright (c) 2019 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";
68
69 // -----------------
70 // Create the system
71 // -----------------
72 auto phys_mat = chrono_types::make_shared<ChMaterialSurfaceNSC>();
73 phys_mat->SetFriction(0.2f);
74
75 ChSystemNSC mphysicalSystem;
76 mphysicalSystem.Set_G_acc({0, 0, -9.81});
77
78 auto floor = chrono_types::make_shared<ChBodyEasyBox>(100, 100, 1, // x,y,z size
79 1000, // density
80 true, // collide enable?
81 true, phys_mat); // visualization?
82 floor->SetPos({0, 0, -1.0});
83 floor->SetRot(Q_from_AngZ(CH_C_PI / 2.0));
84 floor->SetBodyFixed(true);
85 mphysicalSystem.Add(floor);
86
87 // place objects to visually test rotations
88 auto scalebox = chrono_types::make_shared<ChBodyEasyBox>(1, .2, .4, // x,y,z size
89 1000, // density
90 true, // collide enable?
91 true, phys_mat); // visualization?
92 scalebox->SetPos({0, -1, 1});
93 scalebox->SetBodyFixed(true);
94 mphysicalSystem.Add(scalebox);
95
96 // test max reflections
97 auto top_mirror = chrono_types::make_shared<ChBodyEasyBox>(10, 10, .1, // x,y,z size
98 1000, // density
99 true, // collide enable?
100 true, phys_mat); // visualization?
101 top_mirror->SetPos({0, -1, 1.5});
102 top_mirror->SetBodyFixed(true);
103 mphysicalSystem.Add(top_mirror);
104 auto top_m_asset = top_mirror->GetAssets()[0];
105 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(top_m_asset)) {
106 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
107 // vis_mat->SetAmbientColor({0.f, 0.f, 0.f});
108 vis_mat->SetDiffuseColor({1, 1, 1});
109 // vis_mat->SetAmbientColor(.2 * vis_mat->GetDiffuseColor());
110 vis_mat->SetSpecularColor({1.f, 1.f, 1.f});
111 vis_mat->SetRoughness(0);
112 vis_mat->SetMetallic(.9);
113 visual_asset->material_list.push_back(vis_mat);
114 }
115
116 auto bottom_mirror = chrono_types::make_shared<ChBodyEasyBox>(10, 10, .1, // x,y,z size
117 1000, // density
118 true, // collide enable?
119 true, phys_mat); // visualization?
120 bottom_mirror->SetPos({0, -1, 0.5});
121 bottom_mirror->SetBodyFixed(true);
122 mphysicalSystem.Add(bottom_mirror);
123 auto bottom_m_asset = bottom_mirror->GetAssets()[0];
124 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(bottom_m_asset)) {
125 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
126 // vis_mat->SetAmbientColor({0.f, 0.f, 0.f});
127 vis_mat->SetDiffuseColor({1, 1, 1});
128 // vis_mat->SetAmbientColor(.2 * vis_mat->GetDiffuseColor());
129 vis_mat->SetSpecularColor({1.f, 1.f, 1.f});
130 vis_mat->SetRoughness(0);
131 vis_mat->SetMetallic(.9);
132 visual_asset->material_list.push_back(vis_mat);
133 }
134 // add a mesh
135 auto mmesh = chrono_types::make_shared<ChTriangleMeshConnected>();
136 mmesh->LoadWavefrontMesh(GetChronoDataFile("models/bulldozer/shoe_collision.obj"), false, true);
137 mmesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
138 mmesh->RepairDuplicateVertexes(1e-9);
139
140 double mmass;
141 ChVector<> mcog;
142 ChMatrix33<> minertia;
143 double mdensity = 1000;
144 mmesh->ComputeMassProperties(true, mmass, mcog, minertia);
145 ChMatrix33<> principal_inertia_rot;
146 ChVector<> principal_I;
147 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
148 auto trimesh_shape = chrono_types::make_shared<ChTriangleMeshShape>();
149 trimesh_shape->SetMesh(mmesh);
150 trimesh_shape->SetName("Mesh");
151 trimesh_shape->SetStatic(true);
152
153 std::shared_ptr<ChBody> imu_parent;
154 std::shared_ptr<ChBody> gps_parent;
155
156 // float size = 3;
157 // auto origin = chrono_types::make_shared<ChBodyEasyBox>(size, size, size, 1000, true, false);
158 // origin->SetPos({0, 0, 1});
159 // origin->SetBodyFixed(true);
160 //
161 // mphysicalSystem.Add(origin);
162 // auto origin_asset = origin->GetAssets()[0];
163 // if (std::shared_ptr<ChVisualization> v_asset = std::dynamic_pointer_cast<ChVisualization>(origin_asset)) {
164 // auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
165 // vis_mat->SetDiffuseColor({1, 0, 0});
166 // vis_mat->SetSpecularColor({.5f, .5f, .5f});
167 //
168 // v_asset->material_list.push_back(vis_mat);
169 // }
170
171 // walls to contain falling objects
172 auto wall1 = chrono_types::make_shared<ChBodyEasyBox>(40.0, .1, 10.0, 1000, true, true, phys_mat);
173 wall1->SetPos({0, -20, 4});
174 wall1->SetBodyFixed(true);
175 mphysicalSystem.Add(wall1);
176
177 auto wall2 = chrono_types::make_shared<ChBodyEasyBox>(40.0, .1, 10.0, 1000, true, true, phys_mat);
178 wall2->SetPos({0, 20, 4});
179 wall2->SetBodyFixed(true);
180 mphysicalSystem.Add(wall2);
181
182 auto wall3 = chrono_types::make_shared<ChBodyEasyBox>(.1, 40.0, 10.0, 1000, true, true, phys_mat);
183 wall3->SetPos({-20, 0, 4});
184 wall3->SetBodyFixed(true);
185 mphysicalSystem.Add(wall3);
186
187 auto wall4 = chrono_types::make_shared<ChBodyEasyBox>(.1, 40.0, 10.0, 1000, true, true, phys_mat);
188 wall4->SetPos({20, 0, 4});
189 wall4->SetBodyFixed(true);
190 mphysicalSystem.Add(wall4);
191
192 // add box, sphere, mesh with textures
193 // auto gator_mesh = chrono_types::make_shared<ChTriangleMeshConnected>();
194 // gator_mesh->LoadWavefrontMesh(GetChronoDataFile("vehicle/gator/gator_chassis.obj"), false, true);
195 // auto gator_meshshape = chrono_types::make_shared<ChTriangleMeshShape>();
196 // gator_meshshape->SetMesh(gator_mesh);
197 // gator_meshshape->SetName("gator");
198 // gator_meshshape->SetStatic(true);
199 // auto gator_mesh_body = chrono_types::make_shared<ChBody>();
200 // gator_mesh_body->SetPos({1, 2, 2});
201 // gator_mesh_body->AddAsset(gator_meshshape);
202 // gator_mesh_body->SetBodyFixed(true);
203 // mphysicalSystem.Add(gator_mesh_body);
204
205 auto texbox = chrono_types::make_shared<ChBodyEasyBox>(1, 1,
206 1, // x,y,z size
207 1000, // density
208 true, // vis enable?
209 false, phys_mat); //
210 texbox->SetPos({1, 0, 3});
211 texbox->SetBodyFixed(true);
212 mphysicalSystem.Add(texbox);
213
214 auto texbox_asset = texbox->GetAssets()[0];
215 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(texbox_asset)) {
216 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
217 vis_mat->SetSpecularColor({.2f, .2f, .2f});
218 vis_mat->SetKdTexture(GetChronoDataFile("textures/redwhite.png"));
219 vis_mat->SetNormalMapTexture(GetChronoDataFile("sensor/textures/FaceNormal.jpg"));
220 visual_asset->material_list.push_back(vis_mat);
221 }
222
223 auto texsphere = chrono_types::make_shared<ChBodyEasySphere>(.6, // size
224 1000, // density
225 true, // vis enable?
226 false, phys_mat); //
227 texsphere->SetPos({1, -2, 3});
228 texsphere->SetBodyFixed(true);
229 mphysicalSystem.Add(texsphere);
230
231 auto texsphere_asset = texsphere->GetAssets()[0];
232 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(texsphere_asset)) {
233 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
234 vis_mat->SetSpecularColor({.2f, .2f, .2f});
235 vis_mat->SetKdTexture(GetChronoDataFile("textures/redwhite.png"));
236 vis_mat->SetNormalMapTexture(GetChronoDataFile("sensor/textures/FaceNormal.jpg"));
237 visual_asset->material_list.push_back(vis_mat);
238 }
239
240 auto texcyl = chrono_types::make_shared<ChBodyEasyCylinder>(.5, 1, // size
241 1000, // density
242 true, // vis enable?
243 false, phys_mat); //
244 texcyl->SetPos({1, -4, 3});
245 texcyl->SetBodyFixed(true);
246 mphysicalSystem.Add(texcyl);
247
248 auto texcyl_asset = texcyl->GetAssets()[0];
249 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(texcyl_asset)) {
250 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
251 vis_mat->SetSpecularColor({.2f, .2f, .2f});
252 vis_mat->SetKdTexture(GetChronoDataFile("textures/redwhite.png"));
253 vis_mat->SetNormalMapTexture(GetChronoDataFile("sensor/textures/FaceNormal.jpg"));
254 visual_asset->material_list.push_back(vis_mat);
255 }
256
257 for (int i = 0; i < num_bodies; i++) {
258 // add a box
259 auto box =
260 chrono_types::make_shared<ChBodyEasyBox>((float)ChRandom() / 2.0 + 0.1, (float)ChRandom() / 2.0 + 0.1,
261 (float)ChRandom() / 2.0 + 0.1, // x,y,z size
262 1000, // density
263 true, // collide enable?
264 true, phys_mat); // visualization?
265 box->SetPos({(float)ChRandom(), (float)ChRandom(), 2.0 + i});
266 box->SetRot(Q_from_Euler123({(float)ChRandom(), (float)ChRandom(), (float)ChRandom()}));
267 mphysicalSystem.Add(box);
268
269 auto box_asset = box->GetAssets()[0];
270 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(box_asset)) {
271 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
272 // vis_mat->SetAmbientColor({0.f, 0.f, 0.f});
273 vis_mat->SetDiffuseColor({(float)ChRandom(), (float)ChRandom(), (float)ChRandom()});
274 // vis_mat->SetAmbientColor(.2 * vis_mat->GetDiffuseColor());
275 vis_mat->SetSpecularColor({.2f, .2f, .2f});
276
277 visual_asset->material_list.push_back(vis_mat);
278 }
279
280 if (!imu_parent) {
281 imu_parent = box;
282 }
283
284 auto cyl = chrono_types::make_shared<ChBodyEasyCylinder>((float)ChRandom() / 2.0 + 0.1, // radius
285 (float)ChRandom() / 2.0 + 0.1, // height
286 1000, // density
287 true, // collide enable?
288 true, phys_mat); // visualization?
289 cyl->SetPos({(float)ChRandom(), (float)ChRandom(), 2.0 + i});
290 cyl->SetRot(Q_from_Euler123({(float)ChRandom(), (float)ChRandom(), (float)ChRandom()}));
291 mphysicalSystem.Add(cyl);
292
293 auto cyl_asset = cyl->GetAssets()[0];
294 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(cyl_asset)) {
295 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
296 // vis_mat->SetAmbientColor({0.f, 0.f, 0.f});
297 vis_mat->SetDiffuseColor({(float)ChRandom(), (float)ChRandom(), (float)ChRandom()});
298 // vis_mat->SetAmbientColor(.2 * vis_mat->GetDiffuseColor());
299 vis_mat->SetSpecularColor({.2f, .2f, .2f});
300
301 visual_asset->material_list.push_back(vis_mat);
302 }
303
304 auto sphere = chrono_types::make_shared<ChBodyEasySphere>((float)ChRandom() / 2.0 + 0.1, // radius
305 1000, // density
306 true, // collide enable?
307 true, phys_mat); // visualization?
308 sphere->SetPos({(float)ChRandom(), (float)ChRandom(), 2.0 + i});
309 // sphere->SetRot(Q_from_Euler123({(float)ChRandom(), (float)ChRandom(), (float)rand() /
310 // RAND_MAX}));
311 mphysicalSystem.Add(sphere);
312 if (!gps_parent) {
313 gps_parent = sphere;
314 }
315
316 auto sphere_asset = sphere->GetAssets()[0];
317 if (std::shared_ptr<ChVisualization> visual_asset = std::dynamic_pointer_cast<ChVisualization>(sphere_asset)) {
318 auto vis_mat = chrono_types::make_shared<ChVisualMaterial>();
319 vis_mat->SetAmbientColor({0.f, 0.f, 0.f});
320 vis_mat->SetDiffuseColor({(float)ChRandom(), (float)ChRandom(), (float)ChRandom()});
321 vis_mat->SetSpecularColor({.2f, .2f, .2f});
322
323 visual_asset->material_list.push_back(vis_mat);
324 }
325
326 // auto mesh_body = chrono_types::make_shared<ChBody>();
327 // mesh_body->SetPos({0, 0, 0});
328 // mesh_body->AddAsset(trimesh_shape);
329 // mesh_body->SetBodyFixed(true);
330 // mesh_body->GetCollisionModel()->ClearModel();
331 // mesh_body->GetCollisionModel()->AddTriangleMesh(phys_mat, mmesh, true, false);
332 // mesh_body->GetCollisionModel()->BuildModel();
333 // mphysicalSystem.Add(mesh_body);
334
335 auto mesh_body = chrono_types::make_shared<ChBodyAuxRef>();
336 mesh_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
337 mesh_body->SetMass(mmass * mdensity);
338 mesh_body->SetInertiaXX(mdensity * principal_I);
339 mesh_body->SetFrame_REF_to_abs(ChFrame<>(ChVector<>((float)ChRandom(), (float)ChRandom(), 2.0 + i)));
340 mphysicalSystem.Add(mesh_body);
341
342 mesh_body->GetCollisionModel()->ClearModel();
343 mesh_body->GetCollisionModel()->AddTriangleMesh(phys_mat, mmesh, false, false, VNULL, ChMatrix33<>(1), 0.005);
344 mesh_body->GetCollisionModel()->BuildModel();
345 mesh_body->SetCollide(true);
346
347 mesh_body->AddAsset(trimesh_shape);
348 }
349
350 std::cout << "sensor manager being made\n";
351 auto manager = chrono_types::make_shared<ChSensorManager>(&mphysicalSystem);
352 manager->SetDeviceList({2, 1, 0});
353 manager->SetMaxEngines(num_groups); // THIS NEEDS MORE PERFORMANCE TESTING
354 manager->SetVerbose(false);
355
356 // make some changes/additions to the scene
357 PointLight p0 = {{10, 10, 10}, {1, 1, 1}, 1000};
358 PointLight p1 = {{10, 10, 10}, {0, 0, 1}, 1000};
359 manager->scene->AddPointLight(p0);
360 manager->scene->AddPointLight(p1);
361 manager->scene->AddPointLight({0, 0, 100}, {1, 1, 1}, 1000);
362
363 // set the background
364 Background b;
365 b.mode = BackgroundMode::GRADIENT;
366 b.color_zenith = {.5f, .6f, .7f};
367 b.color_horizon = {.9f, .8f, .7f};
368 // b.env_tex = GetChronoDataFile("sensor/textures/sky_2_4k.hdr");
369 manager->scene->SetBackground(b);
370
371 auto cam = chrono_types::make_shared<ChCameraSensor>(
372 floor, // body camera is attached to
373 60.0f, // update rate in Hz
374 chrono::ChFrame<double>({-10, 0, 1}, Q_from_AngAxis(0, {0, 0, 1})), // offset pose
375 1920, // image width
376 1080, // image height
377 (float)CH_C_PI / 4, // field of view
378 2, CameraLensModelType::FOV_LENS, false);
379
380 std::string color_data_path = "SENSOR_OUTPUT/cam_color/";
381 std::string gray_data_path = "SENSOR_OUTPUT/cam_gray/";
382 std::string lidar_data_path = "SENSOR_OUTPUT/lidar/";
383
384 cam->SetName("Camera Sensor 0");
385 // cam->SetLag();
386 // cam->SetCollectionWindow(imu_collection_time);
387
388 // cam->PushFilter(chrono_types::make_shared<ChFilterCameraNoiseConstNormal>(0.f, 0.01));
389 // cam->PushFilter(chrono_types::make_shared<ChFilterCameraNoisePixDep>(0.01f, 0.01f, 0.01f));
390
391 // we want to save the RGBA buffer to png
392 if (save_data)
393 cam->PushFilter(chrono_types::make_shared<ChFilterSave>(color_data_path));
394
395 // we want to visualize this sensor right after rendering, so add the visualize filter to the filter list.
396 // if (display_data)
397 cam->PushFilter(chrono_types::make_shared<ChFilterVisualize>(1280, 720, "Whitted, before Grayscale Filter"));
398
399 // we want to have access to this RGBA8 buffer on the host.
400 cam->PushFilter(chrono_types::make_shared<ChFilterRGBA8Access>());
401
402 cam->PushFilter(chrono_types::make_shared<ChFilterImageResize>(1280, 720, "resize filter"));
403
404 // // filter the sensor to grayscale
405 cam->PushFilter(chrono_types::make_shared<ChFilterGrayscale>());
406 //
407 // // we want to visualize this sensor after grayscale, so add the visualize filter to the filter list.
408 if (display_data)
409 cam->PushFilter(chrono_types::make_shared<ChFilterVisualize>(1280, 720, "Whitted, Final Visualization"));
410 //
411 // // we want to save the grayscale buffer to png
412 if (save_data)
413 cam->PushFilter(chrono_types::make_shared<ChFilterSave>(gray_data_path));
414 //
415 // // we also want to have access to this grayscale buffer on the host.
416 cam->PushFilter(chrono_types::make_shared<ChFilterR8Access>());
417
418 // add sensor to the manager
419 manager->AddSensor(cam);
420
421 // add a lidar to the floor facing the falling objects
422 auto lidar = chrono_types::make_shared<ChLidarSensor>(
423 floor, // body to which the IMU is attached
424 10.0f, // update rate
425 chrono::ChFrame<double>({-8, 0, 1}, Q_from_AngAxis(0, {1, 0, 0})), // offset pose from body
426 923, // horizontal samples
427 23, // vertical samples/channels
428 2.f * (float)CH_C_PI / 3.0f, // horizontal field of view
429 (float)CH_C_PI / 8.0f, -(float)CH_C_PI / 8.0f, 100.0f // vertical field of view
430 );
431 lidar->SetName("Lidar Sensor");
432 lidar->SetLag(.1f);
433 lidar->SetCollectionWindow(.1f);
434
435 // lidar->PushFilter(chrono_types::make_shared<ChFilterLidarIntensityClip>(.02f, 0.f, "Intensity Clip Filter"));
436
437 if (display_data)
438 lidar->PushFilter(chrono_types::make_shared<ChFilterVisualize>(923, 48, "Raw Lidar Data"));
439 lidar->PushFilter(chrono_types::make_shared<ChFilterDIAccess>());
440 lidar->PushFilter(chrono_types::make_shared<ChFilterPCfromDepth>());
441 if (display_data)
442 lidar->PushFilter(chrono_types::make_shared<ChFilterVisualizePointCloud>(640, 480, 2.0f, "Lidar Point Cloud"));
443 if (save_data)
444 lidar->PushFilter(chrono_types::make_shared<ChFilterSavePtCloud>(lidar_data_path));
445 lidar->PushFilter(chrono_types::make_shared<ChFilterXYZIAccess>());
446 // lidar->PushFilter(chrono_types::make_shared<ChFilterXYZIAccess>());
447 // manager->AddSensor(lidar);
448
449 // add a lidar to the floor facing the falling objects
450 auto lidar2 = chrono_types::make_shared<ChLidarSensor>(
451 floor, // body to which the IMU is attached
452 10.0f, // update rate
453 chrono::ChFrame<double>({-8, 0, 1}, Q_from_AngAxis(0, {1, 0, 0})), // offset pose from body
454 923, // horizontal samples
455 23, // vertical samples/channels
456 2.f * (float)CH_C_PI / 3.0f, // horizontal field of view
457 (float)CH_C_PI / 8.0f, // max vert angle
458 -(float)CH_C_PI / 8.0f, // min vert angle
459 100.0f, // max range
460 LidarBeamShape::RECTANGULAR, // beam shape
461 3, // beam sample radius
462 .003, // vert divergence angle
463 .003, // horizontal divergence angle
464 LidarReturnMode::STRONGEST_RETURN // return type
465 );
466 lidar2->SetName("Lidar Sensor 2");
467 lidar2->SetLag(.1f);
468 lidar2->SetCollectionWindow(.1f);
469
470 if (display_data)
471 lidar2->PushFilter(chrono_types::make_shared<ChFilterVisualize>(923, 48, "Raw Lidar Data"));
472 lidar2->PushFilter(chrono_types::make_shared<ChFilterDIAccess>());
473 lidar2->PushFilter(chrono_types::make_shared<ChFilterPCfromDepth>());
474 if (display_data)
475 lidar2->PushFilter(
476 chrono_types::make_shared<ChFilterVisualizePointCloud>(640, 480, 2.0f, "Lidar2 Point Cloud"));
477 lidar2->PushFilter(chrono_types::make_shared<ChFilterXYZIAccess>());
478 // manager->AddSensor(lidar2);
479
480 // add an IMU sensor to one of the boxes
481 auto imu_offset_pose = chrono::ChFrame<double>({0, 0, 0}, Q_from_AngAxis(0, {1, 0, 0}));
482 auto noise_none = chrono_types::make_shared<ChNoiseNone>();
483 auto acc = chrono_types::make_shared<ChAccelerometerSensor>(imu_parent, // body to which the IMU is
484 // attached
485 100, // update rate
486 imu_offset_pose, // offset pose from body
487 noise_none); // IMU noise model
488 acc->SetName("IMU - Accelerometer");
489 acc->PushFilter(chrono_types::make_shared<ChFilterAccelAccess>()); // Add a filter to access the imu data
490 manager->AddSensor(acc); // Add the IMU sensor to the sensor manager
491
492 auto gyro = chrono_types::make_shared<ChGyroscopeSensor>(imu_parent, // body to which the IMU is attached
493 100, // update rate
494 imu_offset_pose, // offset pose from body
495 noise_none); // IMU noise model
496 gyro->SetName("IMU - Accelerometer");
497 gyro->PushFilter(chrono_types::make_shared<ChFilterGyroAccess>()); // Add a filter to access the imu data
498 manager->AddSensor(gyro); // Add the IMU sensor to the sensor manager
499
500 auto mag = chrono_types::make_shared<ChMagnetometerSensor>(imu_parent, // body to which the IMU is attached
501 100, // update rate
502 imu_offset_pose, // offset pose from body
503 noise_none, // IMU noise model
504 ChVector<double>(43.300, -89.000, 260.0));
505 mag->SetName("IMU - Accelerometer");
506 mag->PushFilter(chrono_types::make_shared<ChFilterMagnetAccess>()); // Add a filter to access the imu data
507 manager->AddSensor(mag); // Add the IMU sensor to the sensor manager
508
509 // add an IMU sensor to one of the boxes
510 auto noise_model =
511 chrono_types::make_shared<ChNoiseNormal>(ChVector<float>(0.f, 0.f, 0.f), ChVector<float>(1.f, 1.f, 1.f));
512 auto gps = chrono_types::make_shared<ChGPSSensor>(
513 gps_parent, // body to which the GPS is attached
514 10, // update rate
515 chrono::ChFrame<double>({0, 0, 0}, Q_from_AngAxis(0, {1, 0, 0})), // offset pose from body
516 ChVector<double>(43.300, -89.000, 260.0), // reference GPS location (GPS coordinates of simulation origin)
517 noise_model // noise model to use for adding GPS noise (NOT THREAD SAFE)
518 );
519 gps->SetName("GPS");
520 gps->PushFilter(chrono_types::make_shared<ChFilterGPSAccess>());
521 manager->AddSensor(gps);
522
523 std::vector<std::shared_ptr<ChCameraSensor>> cams;
524
525 for (int i = 0; i < num_cameras; i++) {
526 auto cam1 = chrono_types::make_shared<ChCameraSensor>(
527 floor, // body camera is attached to
528 10.0f + 10.0f * (i % 4 + 1), // 30 + i, // update rate in Hz
529 chrono::ChFrame<double>({-3, 0, 2}, Q_from_AngAxis(0, {1, 0, 0})), // offset pose
530 1280, // image width
531 720, // image height
532 (float)CH_C_PI / 3);
533 cams.push_back(cam1);
534
535 std::stringstream nm;
536 nm << "Camera Sensor " << i + 1;
537 cam1->SetName(nm.str());
538 cam1->SetLag(.1f);
539 cam1->SetCollectionWindow(0);
540
541 // we want to visualize this sensor, so add the visualize filter to the filter list.
542 if (display_data)
543 cam1->PushFilter(chrono_types::make_shared<ChFilterVisualize>(1280, 720, "Before Grayscale Filter"));
544
545 // filter the sensor to grayscale
546 cam1->PushFilter(chrono_types::make_shared<ChFilterGrayscale>());
547
548 // we want to visualize this sensor after grayscale, so add the visualize filter to the filter list.
549 if (display_data)
550 cam1->PushFilter(chrono_types::make_shared<ChFilterVisualize>(1280, 720, "After Grayscale Filter"));
551
552 if (save_data)
553 cam1->PushFilter(chrono_types::make_shared<ChFilterSave>("SENSOR_OUTPUT/cam" + std::to_string(i) + "/"));
554
555 // add sensor to the manager
556 manager->AddSensor(cam1);
557 }
558
559 // manager->ReconstructScenes();
560 // ---------------
561 // Simulate system
562 // ---------------
563 float orbit_radius = 15.f;
564 float orbit_rate = .2f;
565 float ch_time = 0.f;
566
567 double render_time = 0;
568
569 std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
570
571 float light_change = -.002f;
572
573 std::cout << "Sensor Mangager has: " << manager->GetNumEngines() << " engines running.\n";
574 while (ch_time < end_time) {
575 std::chrono::high_resolution_clock::time_point r0 = std::chrono::high_resolution_clock::now();
576 cam->SetOffsetPose(chrono::ChFrame<double>(
577 {-orbit_radius * cos(ch_time * orbit_rate), -orbit_radius * sin(ch_time * orbit_rate), 3},
578 Q_from_AngAxis(ch_time * orbit_rate, {0, 0, 1})));
579
580 scalebox->SetRot(Q_from_AngY(ch_time * .3));
581
582 p0.pos = {-orbit_radius * cos(ch_time * orbit_rate * 2), -orbit_radius * sin(ch_time * orbit_rate * 2), 10};
583 manager->scene->ModifyPointLight(0, p0);
584
585 p1.color = make_float3(p1.color.x, p1.color.y, p1.color.z + light_change);
586 if (p1.color.z < 0) {
587 p1.color = make_float3(0, 0, 0);
588 light_change = -light_change;
589 }
590 if (p1.color.z > 1) {
591 p1.color = make_float3(0, 0, 1);
592 light_change = -light_change;
593 }
594 manager->scene->ModifyPointLight(1, p1);
595
596 // origin->SetRot(Q_from_AngAxis(ch_time * orbit_rate, {0, 0, 1}));
597 // origin->SetPos({0, 0, 3 * sin(ch_time * orbit_rate)});
598
599 manager->Update();
600 std::chrono::high_resolution_clock::time_point r1 = std::chrono::high_resolution_clock::now();
601 std::chrono::duration<double> t_render = std::chrono::duration_cast<std::chrono::duration<double>>(r1 - r0);
602 render_time += t_render.count();
603
604 mphysicalSystem.DoStepDynamics(time_step);
605
606 ch_time = (float)mphysicalSystem.GetChTime();
607 }
608 std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
609 std::chrono::duration<double> wall_time = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
610 std::cout << "Simulation time: " << ch_time << "s, wall time: " << wall_time.count() << "s.\n";
611 std::cout << "Time spent in Chrono: " << wall_time.count() - render_time
612 << ", extra time due to rendering: " << render_time << std::endl;
613
614 return 0;
615 }
616
