added camera config and lidar config to dynamic reconfigure[C

cfg/unreal_simulator.cfg

@@ -17,6 +17,17 @@ lidar = gen.add_group("LiDAR");
 
 lidar.add("lidar_enabled", bool_t, 0, "LiDAR enabled", True)
 lidar.add("lidar_rate", double_t, 0, "LiDAR rate", 0.0, 0.01, 20.0)
+lidar.add("lidar_horizontal_fov", double_t, 0, "LiDAR horizontal fov", 360.0, 0.0, 360.0)
+lidar.add("lidar_vertical_fov", double_t, 0, "LiDAR vertical fov", 45.0, 0.0, 180.0)
+lidar.add("lidar_horizontal_rays", int_t, 0, "LiDAR horizontal rays", 256, 1, 360)
+lidar.add("lidar_vertical_rays", int_t, 0, "LiDAR vertical rays", 32, 1, 64)
+lidar.add("lidar_offset_x", double_t, 0, "LiDAR offset x", 0.0, -100.0, 100.0)
+lidar.add("lidar_offset_y", double_t, 0, "LiDAR offset y", 0.0, -100.0, 100.0)
+lidar.add("lidar_offset_z", double_t, 0, "LiDAR offset z", 6.0, -100.0, 100.0)
+lidar.add("lidar_rotation_pitch", double_t, 0, "LiDAR rotation pitch", 0.0, 0.0, 360.0)
+lidar.add("lidar_rotation_roll", double_t, 0, "LiDAR rotation roll", 90.0, 0.0, 360.0)
+lidar.add("lidar_rotation_yaw", double_t, 0, "LiDAR rotation yaw", 0.0, 0.0, 360.0)
+lidar.add("lidar_beam_length", double_t, 0, "LiDAR beam length", 1000.0, 0.0, 2000.0)
 
 lidar_seg = gen.add_group("LiDAR Segmentation");
 
@@ -27,6 +38,15 @@ rgb = gen.add_group("RGB");
 
 rgb.add("rgb_enabled", bool_t, 0, "RGB enabled", True)
 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)
+rgb.add("rgb_fov", double_t, 0, "RGB fov", 90.0, 0.0, 180.0)
+rgb.add("rgb_offset_x", double_t, 0, "RGB offset x", 14.9, -100.0, 100.0)
+rgb.add("rgb_offset_y", double_t, 0, "RGB offset y", 0.0, -100.0, 100.0)
+rgb.add("rgb_offset_z", double_t, 0, "RGB offset z", 0.0, -100.0, 100.0)
+rgb.add("rgb_rotation_pitch", double_t, 0, "RGB rotation pitch", 0.0, 0.0, 360.0)
+rgb.add("rgb_rotation_roll", double_t, 0, "RGB rotation roll", 0.0, 0.0, 360.0)
+rgb.add("rgb_rotation_yaw", double_t, 0, "RGB rotation yaw", 0.0, 0.0, 360.0)
 
 depth = gen.add_group("Depth");
 

src/unreal_simulator.cpp

@@ -872,6 +872,36 @@ void UnrealSimulator::callbackDrs(mrs_uav_unreal_simulation::unreal_simulatorCon
   timer_rgb_.setPeriod(ros::Duration(1.0 / config.rgb_rate));
 
   timer_lidar_.setPeriod(ros::Duration(1.0 / config.lidar_rate));
+
+
+  ueds_connector::CameraConfig cameraConfig{};
+  cameraConfig.Width = config.rgb_width;
+  cameraConfig.Height = config.rgb_height;
+  cameraConfig.angleFOV = config.rgb_fov;
+  cameraConfig.offset = ueds_connector::Coordinates(config.rgb_offset_x, config.rgb_offset_y, config.rgb_offset_z);
+  cameraConfig.orientation = ueds_connector::Rotation(config.rgb_rotation_pitch, config.rgb_rotation_roll, config.rgb_rotation_yaw);
+  for (size_t i = 0; i < uavs_.size(); i++) {
+    const auto res = ueds_connectors_[i]->SetCameraConfig(cameraConfig);
+    if (!res) {
+      ROS_ERROR("[UnrealSimulator]: failed to set camera config for uav %d", i);
+    } 
+  }
+
+  ueds_connector::LidarConfig lidarConfig{};
+  lidarConfig.BeamHorRays = config.lidar_horizontal_rays;
+  lidarConfig.BeamVertRays = config.lidar_vertical_rays;
+  lidarConfig.FOVHor = config.lidar_horizontal_fov;
+  lidarConfig.FOVVert = config.lidar_vertical_fov;
+  lidarConfig.beamLength = config.lidar_beam_length;
+  lidarConfig.offset = ueds_connector::Coordinates(config.lidar_offset_x, config.lidar_offset_y, config.lidar_offset_z);
+  lidarConfig.orientation = ueds_connector::Rotation(config.lidar_rotation_pitch, config.lidar_rotation_roll, config.lidar_rotation_yaw);
+  for (size_t i = 0; i < uavs_.size(); i++) {
+    const auto res = ueds_connectors_[i]->SetLidarConfig(lidarConfig);
+    if (!res) {
+      ROS_ERROR("[UnrealSimulator]: failed to set lidar config for uav %d", i);
+    } 
+  } 
+
 
   ROS_INFO("[UnrealSimulator]: DRS updated params");
 }