dynamic reconfigure changes

ctu-mrs · Mar 14, 2024 · 6486f40 · 6486f40
1 parent 50c0339
commit 6486f40
Show file tree

Hide file tree

Showing 3 changed files with 60 additions and 3 deletions.
diff --git a/cfg/unreal_simulator.cfg b/cfg/unreal_simulator.cfg
@@ -36,7 +36,7 @@ lidar_seg.add("lidar_seg_rate", double_t, 0, "LiDAR Segmentattion rate", 0.0, 0.
 
 rgb = gen.add_group("RGB");
 
-rgb.add("rgb_enabled", bool_t, 0, "RGB enabled", True)
+rgb.add("rgb_enabled", bool_t, 0, "RGB enabled", False)
 rgb.add("rgb_rate", double_t, 0, "RGB rate", 0.0, 0.01, 20.0)
 rgb.add("rgb_width", int_t, 0, "RGB width", 640, 1, 1920)
 rgb.add("rgb_height", int_t, 0, "RGB height", 480, 1, 1280)

diff --git a/config/unreal_simulator.yaml b/config/unreal_simulator.yaml
@@ -12,12 +12,33 @@ sensors:
   lidar:
     enabled: true
     rate: 10.0 # [Hz]
+    horizontal_fov: 360.0 # [deg]
+    vertical_fov: 45.0 # [deg]
+    horizontal_rays: 128
+    vertical_rays: 32
+    offset_x: 0.0 # [m]
+    offset_y: 0.0 # [m]
+    offset_z: 6.0 # [m]
+    rotation_pitch: 0.0 # [deg]
+    rotation_roll: 90.0 # [deg]
+    rotation_yaw: 0.0 # [deg]
+    beam_length: 1000.0 # [m]
   lidar_seg:
     enabled: false
     rate: 10.0 # [Hz]
   rgb:
     enabled: true
     rate: 10.0 # [Hz]
+    width: 640
+    height: 480
+    fov: 90
+    offset_x: 14.9
+    offset_y: 0.0
+    offset_z: 0.0
+    rotation_pitch: 0.0
+    rotation_yaw: 0.0
+    rotation_roll: 0.0
+
   depth:
     enabled: false
     rate: 10.0 # [Hz]

diff --git a/src/unreal_simulator.cpp b/src/unreal_simulator.cpp
@@ -180,12 +180,33 @@ void UnrealSimulator::onInit() {
 
   param_loader.loadParam("sensors/lidar/enabled", drs_params_.lidar_enabled);
   param_loader.loadParam("sensors/lidar/rate", drs_params_.lidar_rate);
+  param_loader.loadParam("sensors/lidar/horizontal_fov", drs_params_.lidar_horizontal_fov); 
+  param_loader.loadParam("sensors/lidar/vertical_fov", drs_params_.lidar_vertical_fov);
+  param_loader.loadParam("sensors/lidar/horizontal_rays", drs_params_.lidar_horizontal_rays);
+  param_loader.loadParam("sensors/lidar/vertical_rays", drs_params_.lidar_vertical_rays);
+  param_loader.loadParam("sensors/lidar/offset_x", drs_params_.lidar_offset_x);
+  param_loader.loadParam("sensors/lidar/offset_y", drs_params_.lidar_offset_y);
+  param_loader.loadParam("sensors/lidar/offset_z", drs_params_.lidar_offset_z);
+  param_loader.loadParam("sensors/lidar/rotation_pitch", drs_params_.lidar_rotation_pitch);
+  param_loader.loadParam("sensors/lidar/rotation_roll", drs_params_.lidar_rotation_roll);
+  param_loader.loadParam("sensors/lidar/rotation_yaw", drs_params_.lidar_rotation_yaw);
+  param_loader.loadParam("sensors/lidar/beam_length", drs_params_.lidar_beam_length);
+
 
   param_loader.loadParam("sensors/lidar_seg/enabled", drs_params_.lidar_seg_enabled);
   param_loader.loadParam("sensors/lidar_seg/rate", drs_params_.lidar_seg_rate);
 
   param_loader.loadParam("sensors/rgb/enabled", drs_params_.rgb_enabled);
   param_loader.loadParam("sensors/rgb/rate", drs_params_.rgb_rate);
+  param_loader.loadParam("sensors/rgb/width", drs_params_.rgb_width);
+  param_loader.loadParam("sensors/rgb/height", drs_params_.rgb_height);
+  param_loader.loadParam("sensors/rgb/fov", drs_params_.rgb_fov);
+  param_loader.loadParam("sensors/rgb/offset_x", drs_params_.rgb_offset_x);
+  param_loader.loadParam("sensors/rgb/offset_y", drs_params_.rgb_offset_y);
+  param_loader.loadParam("sensors/rgb/offset_z", drs_params_.rgb_offset_z);
+  param_loader.loadParam("sensors/rgb/rotation_pitch", drs_params_.rgb_rotation_pitch);
+  param_loader.loadParam("sensors/rgb/rotation_roll", drs_params_.rgb_rotation_roll);
+  param_loader.loadParam("sensors/rgb/rotation_yaw", drs_params_.rgb_rotation_yaw);
 
   param_loader.loadParam("sensors/depth/enabled", drs_params_.depth_enabled);
   param_loader.loadParam("sensors/depth/rate", drs_params_.depth_rate);
@@ -850,8 +871,21 @@ void UnrealSimulator::callbackDrs(mrs_uav_unreal_simulation::unreal_simulatorCon
 
     if (!old_params.paused && config.paused) {
       timer_main_.stop();
+      timer_rgb_.stop();
+      timer_seg_.stop();
+      timer_depth_.stop();
+      timer_lidar_.stop();
+      timer_seg_lidar_.stop();
+      timer_color_depth_.stop();
     } else if (old_params.paused && !config.paused) {
       timer_main_.start();
+      timer_rgb_.start();
+      timer_seg_.start();
+      timer_depth_.start();
+      timer_lidar_.start();
+      timer_seg_lidar_.start();
+      timer_color_depth_.start();
+
     }
   }
 
@@ -870,9 +904,11 @@ void UnrealSimulator::callbackDrs(mrs_uav_unreal_simulation::unreal_simulatorCon
   // | ------------------ set the camera rates ------------------ |
 
   timer_rgb_.setPeriod(ros::Duration(1.0 / config.rgb_rate));
-
+  timer_seg_.setPeriod(ros::Duration(1.0 / config.seg_rate));
+  timer_depth_.setPeriod(ros::Duration(1.0 / config.depth_rate));
+  timer_color_depth_.setPeriod(ros::Duration(1.0 / config.color_depth_rate));
   timer_lidar_.setPeriod(ros::Duration(1.0 / config.lidar_rate));
-
+  timer_seg_lidar_.setPeriod(ros::Duration(1.0 / config.lidar_rate)); 
 
   ueds_connector::CameraConfig cameraConfig{};
   cameraConfig.Width = config.rgb_width;