diff --git a/cfg/unreal_simulator.cfg b/cfg/unreal_simulator.cfg index 7486caf..8549416 100755 --- a/cfg/unreal_simulator.cfg +++ b/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"); diff --git a/src/unreal_simulator.cpp b/src/unreal_simulator.cpp index b1b378b..da97a86 100644 --- a/src/unreal_simulator.cpp +++ b/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"); }