diff --git a/components/testing/cu_sim_depthsense/Cargo.toml b/components/testing/cu_sim_depthsense/Cargo.toml new file mode 100644 index 000000000..0e67223df --- /dev/null +++ b/components/testing/cu_sim_depthsense/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "cu-sim-depthsense" +description = "A bevy based plugin to simulate depth sense sensor for Copper" +version.workspace = true +authors.workspace = true +edition.workspace = true +license.workspace = true +keywords.workspace = true +categories.workspace = true +homepage.workspace = true +repository.workspace = true + +[dependencies] +bevy = { version = "0.14.2", default-features = false, features = ["wayland", "bevy_asset", "bevy_render", "bevy_core_pipeline", "bevy_pbr", "bevy_gizmos", "bevy_winit", "x11", "tonemapping_luts"] } +bevy_mod_raycast = "0.18.0" +cu29 = { workspace = true } +cu-sensor-payloads = { path = "../../payloads/cu_sensor_payloads" } +uom = { workspace = true } \ No newline at end of file diff --git a/components/testing/cu_sim_depthsense/README.md b/components/testing/cu_sim_depthsense/README.md new file mode 100644 index 000000000..ca60cb96f --- /dev/null +++ b/components/testing/cu_sim_depthsense/README.md @@ -0,0 +1,3 @@ +This is as simple library to use in conjonction with Bevy to simulate a point cloud sensor in the scene. + +It is useful to test your code with the official PointCloud message in Copper. diff --git a/components/testing/cu_sim_depthsense/src/lib.rs b/components/testing/cu_sim_depthsense/src/lib.rs new file mode 100644 index 000000000..fda099d66 --- /dev/null +++ b/components/testing/cu_sim_depthsense/src/lib.rs @@ -0,0 +1,122 @@ +use bevy::prelude::*; +use bevy_mod_raycast::prelude::*; +use cu29::prelude::*; +use cu_sensor_payloads::{PointCloud, PointCloudSoa}; + +fn main() { + App::new() + .add_plugins(DefaultPlugins) + .add_systems(Startup, setup) + .add_systems(Update, raycast) + .run(); +} + +const RAY_DIST: Vec3 = Vec3::new(0.0, 0.0, -7.0); + +const HZ_PTS: usize = 180; +const BEAMS: usize = 32; +const TOTAL_PTS: usize = HZ_PTS * BEAMS; +const VFOV: f32 = 30.0; + +pub type VirtPointCloud = PointCloudSoa; + +pub fn compute_pointcloud( + sensor_pos: Vec3, + raycast: &mut Raycast, + time: CuTime, + pt: &mut VirtPointCloud, +) { + let horizontal_step_angle = 360.0 / HZ_PTS as f32; + let vertical_step_angle = VFOV / (BEAMS - 1) as f32; + let mut i = 0usize; + for h in 0..HZ_PTS { + let horizontal_angle = h as f32 * horizontal_step_angle; + let horizontal_rotation = Quat::from_rotation_y(horizontal_angle.to_radians()); + + for v in 0..BEAMS { + let vertical_angle = -VFOV / 2.0 + v as f32 * vertical_step_angle; + let vertical_rotation = Quat::from_rotation_z(vertical_angle.to_radians()); + + // Calculate the ray direction + let dir = horizontal_rotation * vertical_rotation * Vec3::X; + + // Perform the raycast and get the nearest intersection, if any + if let Some((_, intersection)) = raycast + .cast_ray(Ray3d::new(sensor_pos, dir.normalize()), &default()) + .first() + { + let lidar_pt = intersection.position(); + pt.push(PointCloud { + tov: time, + x: lidar_pt.x.into(), + y: lidar_pt.y.into(), + z: lidar_pt.z.into(), + i: 100.0.into(), + return_order: 0, + }); + } + } + } +} + +fn raycast(mut raycast: Raycast, mut gizmos: Gizmos, time: Res