diff --git a/cfg/unreal_simulator.cfg b/cfg/unreal_simulator.cfg index 194a5bf..9a2ad9e 100755 --- a/cfg/unreal_simulator.cfg +++ b/cfg/unreal_simulator.cfg @@ -32,6 +32,20 @@ lidar_segmented = gen.add_group("Segmented LiDAR"); lidar_segmented.add("lidar_seg_enabled", bool_t, 0, "LiDAR segmented enabled", True) lidar_segmented.add("lidar_seg_rate", double_t, 0, "LiDAR segmented rate", 1.0, 1.0, 200.0) +lidar_intensity = gen.add_group("Intensity LiDAR"); + +lidar_intensity.add("lidar_int_enabled", bool_t, 0, "LiDAR intensity enabled", True) +lidar_intensity.add("lidar_int_rate", double_t, 0, "LiDAR intensity rate", 1.0, 1.0, 200.0) +lidar_intensity.add("lidar_int_noise_enabled", bool_t, 0, "LiDAR intensity noise enabled", True) +lidar_intensity.add("lidar_int_value_grass", double_t, 0, "LiDAR intensity value grass", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_value_road", double_t, 0, "LiDAR intensity value road", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_value_building", double_t, 0, "LiDAR intensity value building", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_value_tree", double_t, 0, "LiDAR intensity value tree", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_value_fence", double_t, 0, "LiDAR intensity value fence", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_value_dirt_road", double_t, 0, "LiDAR intensity value dirt road", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_value_other", double_t, 0, "LiDAR intensity value other", 0.0, 0.0, 255.0) +lidar_intensity.add("lidar_int_std_at_1m", double_t, 0, "LiDAR intensity STD at 1m", 0.0, 0.01, 1.0) +lidar_intensity.add("lidar_int_std_slope", double_t, 0, "LiDAR intensity STD slope", 0.0, 0.01, 1.0) rgb = gen.add_group("RGB"); rgb.add("rgb_enabled", bool_t, 0, "RGB enabled", False) diff --git a/config/unreal_simulator.yaml b/config/unreal_simulator.yaml index 25b308e..b7ebb1f 100644 --- a/config/unreal_simulator.yaml +++ b/config/unreal_simulator.yaml @@ -80,6 +80,25 @@ sensors: enabled: false rate: 10.0 # [Hz] + lidar_intensity: + enabled: false + rate: 10.0 # [Hz] + + values: + grass: 50 + road: 7 + tree: 90 + building: 70 + fence: 40 + dirt_road: 30 + other: 155 + + noise: + enabled: true + std_at_1m: 0.59 # [-] + std_slope: 0.81 # [-] # multiplies std_at_1m for each meter of measured distances + + rgb: enabled: false diff --git a/include/ueds_connector/data_types.h b/include/ueds_connector/data_types.h index 8158985..7114177 100644 --- a/include/ueds_connector/data_types.h +++ b/include/ueds_connector/data_types.h @@ -67,6 +67,21 @@ struct LidarSegData } }; +struct LidarIntData +{ + LidarIntData() = default; + + double distance; + double directionX; + double directionY; + double directionZ; + int intensity; + std::string toString() const { + return "(distance: " + std::to_string(distance) + ", directionX: " + std::to_string(directionX) + ", directionY: " + std::to_string(directionY) + + ", directionZ: " + std::to_string(directionZ) + ", intensity: " + std::to_string(intensity) + ")"; + } +}; + struct LidarConfig { LidarConfig() = default; diff --git a/include/ueds_connector/serialization/serializable_shared.h b/include/ueds_connector/serialization/serializable_shared.h index 779a661..f617772 100644 --- a/include/ueds_connector/serialization/serializable_shared.h +++ b/include/ueds_connector/serialization/serializable_shared.h @@ -104,7 +104,8 @@ enum MessageType : unsigned short get_lidar_seg = 19, set_location_and_rotation_async = 20, get_crash_state = 21, - get_rangefinder_data = 22 + get_lidar_int = 22, + get_rangefinder_data = 23, }; /* struct LidarConfig //{ */ @@ -561,7 +562,7 @@ namespace GetRangefinderData archive(cereal::base_class(this), range); } }; -} // namespace GetRangefinderData +} // namespace GetRangefinderData /* GetLidarData //{ */ @@ -657,6 +658,53 @@ struct Response : public Common::NetworkResponse //} +/* GetLidarIntData //{ */ + +namespace GetLidarIntData +{ +struct LidarIntData +{ + LidarIntData() = default; + + double distance; + double directionX; + double directionY; + double directionZ; + int intensity; + template + void serialize(Archive& archive) { + archive(distance, directionX, directionY, directionZ, intensity); + } +}; + +struct Request : public Common::NetworkRequest +{ + Request() : Common::NetworkRequest(static_cast(MessageType::get_lidar_int)) { + } +}; + +struct Response : public Common::NetworkResponse +{ + Response() : Common::NetworkResponse(static_cast(MessageType::get_lidar_int)) { + } + explicit Response(bool _status) : Common::NetworkResponse(MessageType::get_lidar_int, _status) { + } + + double startX; + double startY; + double startZ; + + std::vector lidarIntData; + + template + void serialize(Archive& archive) { + archive(cereal::base_class(this), startX, startY, startZ, lidarIntData); + } +}; +} // namespace GetLidarIntData + +//} + /* GetLidarConfig //{ */ namespace GetLidarConfig @@ -969,7 +1017,7 @@ struct Response : public Common::NetworkResponse Response() : Common::NetworkResponse(static_cast(MessageType::set_forest_hilly_level)){}; explicit Response(bool _status) : Common::NetworkResponse(MessageType::set_forest_hilly_level, _status){}; }; -}; // namespace SetForestHilly +}; // namespace SetForestHilly namespace SpawnDrone @@ -1010,12 +1058,12 @@ namespace SpawnDroneAtLocation archive(cereal::base_class(this), x, y, z); } }; - + struct Response : public Common::NetworkResponse { Response() : Common::NetworkResponse(static_cast(MessageType::spawn_drone_at_location)){}; explicit Response(bool _status) : Common::NetworkResponse(MessageType::spawn_drone, _status){}; - + int port; template @@ -1023,7 +1071,7 @@ namespace SpawnDroneAtLocation archive(cereal::base_class(this), port); } }; -}; // namespace SpawnDrone +}; // namespace SpawnDrone namespace RemoveDrone { @@ -1107,7 +1155,7 @@ enum GraphicsSettingsEnum : unsigned short EPIC = 3, CINEMATIC = 4 }; - + namespace SetGraphicsSettings { struct Request : public Common::NetworkRequest @@ -1127,7 +1175,7 @@ namespace SetGraphicsSettings Response() : Common::NetworkResponse(static_cast(MessageType::set_graphics_settings)){}; explicit Response(bool _status) : Common::NetworkResponse(MessageType::set_graphics_settings, _status){}; }; - + }// namespace SetGraphicsSettings enum WorldLevelEnum : unsigned short diff --git a/include/ueds_connector/ueds_connector.h b/include/ueds_connector/ueds_connector.h index bcfc953..4374ab4 100644 --- a/include/ueds_connector/ueds_connector.h +++ b/include/ueds_connector/ueds_connector.h @@ -46,6 +46,8 @@ class UedsConnector : public SocketClient { std::tuple, Coordinates> GetLidarData(); std::tuple, Coordinates> GetLidarSegData(); + + std::tuple, Coordinates> GetLidarIntData(); std::pair GetLidarConfig(); diff --git a/src/ueds_connector/ueds_connector.cpp b/src/ueds_connector/ueds_connector.cpp index 3708f66..d1c3c99 100644 --- a/src/ueds_connector/ueds_connector.cpp +++ b/src/ueds_connector/ueds_connector.cpp @@ -9,6 +9,7 @@ using kissnet::socket_status; using ueds_connector::Coordinates; using ueds_connector::LidarConfig; using ueds_connector::LidarData; +using ueds_connector::LidarIntData; using ueds_connector::LidarSegData; using ueds_connector::RgbCameraConfig; using ueds_connector::Rotation; @@ -262,6 +263,7 @@ std::tuple UedsConnector::SetLocationAndRotationAsync(const Coordinates& c std::tuple, Coordinates> UedsConnector::GetLidarData() { + /* std::cout << "GetLidarData()" << std::endl; */ Serializable::Drone::GetLidarData::Request request{}; Serializable::Drone::GetLidarData::Response response{}; @@ -338,6 +340,43 @@ std::tuple, Coordinates> UedsConnector::GetLidar } //} +/* getLidarIntData() //{ */ + +std::tuple, Coordinates> UedsConnector::GetLidarIntData() { + + /* std::cout << "GetLidarIntData()" << std::endl; */ + + Serializable::Drone::GetLidarIntData::Request request{}; + + Serializable::Drone::GetLidarIntData::Response response{}; + const auto status = Request(request, response); + const auto success = status && response.status; + std::vector lidarIntData; + Coordinates start{}; + + if (success) { + + const auto arrSize = response.lidarIntData.size(); + lidarIntData.resize(arrSize); + + for (size_t i = 0; i < arrSize; i++) { + lidarIntData[i] = LidarIntData{}; + lidarIntData[i].distance = response.lidarIntData[i].distance; + lidarIntData[i].directionX = response.lidarIntData[i].directionX; + lidarIntData[i].directionY = response.lidarIntData[i].directionY; + lidarIntData[i].directionZ = response.lidarIntData[i].directionZ; + lidarIntData[i].intensity = response.lidarIntData[i].intensity; + } + + start.x = response.startX; + start.y = response.startY; + start.z = response.startZ; + } + + return std::make_tuple(success, lidarIntData, start); +} +//} + /* getLidarConfig() //{ */ std::pair UedsConnector::GetLidarConfig() { diff --git a/src/unreal_simulator.cpp b/src/unreal_simulator.cpp index f4bb82f..2bc472c 100644 --- a/src/unreal_simulator.cpp +++ b/src/unreal_simulator.cpp @@ -52,9 +52,10 @@ //} -using PCLPoint = pcl::PointXYZ; -using PCLPointCloud = pcl::PointCloud; -using PCLPointCloudColor = pcl::PointCloud; +using PCLPoint = pcl::PointXYZ; +using PCLPointCloud = pcl::PointCloud; +using PCLPointCloudColor = pcl::PointCloud; +using PCLPointCloudIntensity = pcl::PointCloud; namespace mrs_uav_unreal_simulation { @@ -106,6 +107,9 @@ class UnrealSimulator : public nodelet::Nodelet { ros::Timer timer_seg_lidar_; void timerSegLidar(const ros::TimerEvent& event); + ros::Timer timer_int_lidar_; + void timerIntLidar(const ros::TimerEvent& event); + ros::Timer timer_rgb_; void timerRgb(const ros::TimerEvent& event); @@ -180,6 +184,16 @@ class UnrealSimulator : public nodelet::Nodelet { bool stereo_enable_temporal_aa_; bool stereo_enable_raytracing_; + double lidar_int_grass; + double lidar_int_road; + double lidar_int_tree; + double lidar_int_building; + double lidar_int_fence; + double lidar_int_dirt_road; + double lidar_int_other; + bool lidar_int_noise; + + // | ----------------------- publishers ----------------------- | mrs_lib::PublisherHandler ph_clock_; @@ -189,6 +203,7 @@ class UnrealSimulator : public nodelet::Nodelet { std::vector> ph_lidars_; std::vector> ph_seg_lidars_; + std::vector> ph_int_lidars_; std::vector imp_rgb_; std::vector imp_stereo_left_; @@ -320,6 +335,8 @@ void UnrealSimulator::onInit() { param_loader.loadParam("sensors/lidar/rate", drs_params_.lidar_rate); param_loader.loadParam("sensors/lidar/lidar_segmented/enabled", drs_params_.lidar_seg_enabled); param_loader.loadParam("sensors/lidar/lidar_segmented/rate", drs_params_.lidar_seg_rate); + param_loader.loadParam("sensors/lidar/lidar_intensity/enabled", drs_params_.lidar_int_enabled); + param_loader.loadParam("sensors/lidar/lidar_intensity/rate", drs_params_.lidar_int_rate); param_loader.loadParam("sensors/lidar/noise/enabled", drs_params_.lidar_noise_enabled); param_loader.loadParam("sensors/lidar/noise/std_at_1m", drs_params_.lidar_std_at_1m); param_loader.loadParam("sensors/lidar/noise/std_slope", drs_params_.lidar_std_slope); @@ -373,6 +390,17 @@ void UnrealSimulator::onInit() { param_loader.loadParam("sensors/stereo/enable_temporal_aa", stereo_enable_temporal_aa_); param_loader.loadParam("sensors/stereo/enable_raytracing", stereo_enable_raytracing_); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/grass", drs_params_.lidar_int_value_grass); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/road", drs_params_.lidar_int_value_road); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/tree", drs_params_.lidar_int_value_tree); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/building", drs_params_.lidar_int_value_building); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/fence", drs_params_.lidar_int_value_fence); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/dirt_road", drs_params_.lidar_int_value_dirt_road); + param_loader.loadParam("sensors/lidar/lidar_intensity/values/other", drs_params_.lidar_int_value_other); + param_loader.loadParam("sensors/lidar/lidar_intensity/noise/enabled", drs_params_.lidar_int_noise_enabled); + param_loader.loadParam("sensors/lidar/lidar_intensity/noise/std_at_1m", drs_params_.lidar_int_std_at_1m); + param_loader.loadParam("sensors/lidar/lidar_intensity/noise/std_slope", drs_params_.lidar_int_std_slope); + double clock_rate; param_loader.loadParam("clock_rate", clock_rate); @@ -503,9 +531,9 @@ void UnrealSimulator::onInit() { mrs_multirotor_simulator::MultirotorModel::State uav_state = uavs_[i]->getState(); ueds_connector::Coordinates pos = position2ue(uav_state.x, ueds_world_origin_); - + ROS_INFO("[UnrealSimulator]: %s spawning at [%.2lf, %.2lf, %.2lf] ...", uav_name.c_str(), uav_state.x.x(), uav_state.x.y(), uav_state.x.z()); - + auto [resSpawn, port] = ueds_game_controller_->SpawnDroneAtLocation(pos); //auto [resSpawn, port] = ueds_game_controller_->SpawnDrone(); @@ -528,13 +556,13 @@ void UnrealSimulator::onInit() { ROS_ERROR("[UnrealSimulator]: %s - Error connecting to drone controller, connect_result was %d", uav_name.c_str(), connect_result); ros::shutdown(); - } + } else { ROS_INFO("[UnrealSimulator]: %s - Connection succeed: %d", uav_name.c_str(), connect_result); - + // ROS_INFO("[UnrealSimulator]: wait until UAV fall on the ground ... && uptade their world origin"); - + // std::this_thread::sleep_for(std::chrono::seconds(3)); // const auto [res, location] = ueds_connector->GetLocation(); @@ -551,6 +579,7 @@ void UnrealSimulator::onInit() { ph_lidars_.push_back(mrs_lib::PublisherHandler(nh_, "/" + uav_name + "/lidar/points", 10)); ph_seg_lidars_.push_back(mrs_lib::PublisherHandler(nh_, "/" + uav_name + "/lidar_segmented/points", 10)); + ph_int_lidars_.push_back(mrs_lib::PublisherHandler(nh_, "/" + uav_name + "/lidar_intensity/points", 10)); imp_rgb_.push_back(it_->advertise("/" + uav_name + "/rgb/image_raw", 10)); imp_stereo_left_.push_back(it_->advertise("/" + uav_name + "/stereo/left/image_raw", 10)); @@ -687,6 +716,8 @@ void UnrealSimulator::onInit() { timer_seg_lidar_ = nh_.createTimer(ros::Duration(1.0 / drs_params_.lidar_rate), &UnrealSimulator::timerSegLidar, this); + timer_int_lidar_ = nh_.createTimer(ros::Duration(1.0 / drs_params_.lidar_rate), &UnrealSimulator::timerIntLidar, this); + timer_rgb_ = nh_.createTimer(ros::Duration(1.0 / drs_params_.rgb_rate), &UnrealSimulator::timerRgb, this); timer_stereo_ = nh_.createTimer(ros::Duration(1.0 / drs_params_.stereo_rate), &UnrealSimulator::timerStereo, this); @@ -822,6 +853,10 @@ void UnrealSimulator::timerStatus([[maybe_unused]] const ros::WallTimerEvent& ev highest_fps = drs_params.lidar_seg_rate; } + if (drs_params.lidar_int_rate > highest_fps) { + highest_fps = drs_params.lidar_int_rate; + } + const double ueds_rtf_ = ueds_fps_ / highest_fps; const double desired_rtf = (drs_params.dynamic_rtf && ueds_rtf_ < drs_params.realtime_factor) ? ueds_rtf_ : drs_params.realtime_factor; @@ -1178,6 +1213,118 @@ void UnrealSimulator::timerSegLidar([[maybe_unused]] const ros::TimerEvent& even //} +/* timerIntLidar() //{ */ + +void UnrealSimulator::timerIntLidar([[maybe_unused]] const ros::TimerEvent& event) { + + if (!is_initialized_) { + return; + } + + /* mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("timerLidar()"); */ + + auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_); + + if (!drs_params_.lidar_int_enabled) { + return; + } + + for (size_t i = 0; i < uavs_.size(); i++) { + + mrs_multirotor_simulator::MultirotorModel::State state = uavs_[i]->getState(); + + bool res; + std::vector lidarIntData; + ueds_connector::Coordinates start; + + { + std::scoped_lock lock(mutex_ueds_); + + std::tie(res, lidarIntData, start) = ueds_connectors_[i]->GetLidarIntData(); + } + + if (!res) { + ROS_ERROR("[UnrealSimulator]: [uav%d] - ERROR getLidarIntData", int(i)); + continue; + } + + PCLPointCloudIntensity pcl_cloud; + + for (const ueds_connector::LidarIntData& ray : lidarIntData) { + + pcl::PointXYZI point; + + tf::Vector3 dir = tf::Vector3(ray.directionX, ray.directionY, ray.directionZ); + + double ray_distance = ray.distance / 100.0; + + if (drs_params.lidar_noise_enabled && ray_distance > 0) { + + const double std = ray_distance * drs_params.lidar_std_slope * drs_params.lidar_std_at_1m; + + std::normal_distribution distribution(0, std); + + ray_distance += distribution(rng); + } + + dir = dir.normalized() * ray_distance; + + point.x = dir.x(); + point.y = -dir.y(); // convert left-hand to right-hand coordinates + point.z = dir.z(); + /* TODO: add the noise to the intensity */ + switch (ray.intensity) { + case 0: { + point.intensity = drs_params_.lidar_int_value_other; + break; + } + case 1: { + point.intensity = drs_params_.lidar_int_value_grass; + break; + } + case 2: { + point.intensity = drs_params_.lidar_int_value_road; + break; + } + case 3: { + point.intensity = drs_params_.lidar_int_value_tree; + break; + } + case 4: { + point.intensity = drs_params_.lidar_int_value_building; + break; + } + case 5: { + point.intensity = drs_params_.lidar_int_value_fence; + break; + } + case 6: { + point.intensity = drs_params_.lidar_int_value_dirt_road; + break; + } + } + if (drs_params.lidar_int_noise_enabled && ray_distance > 0) { + const double intensity_std = ray_distance * drs_params.lidar_int_std_slope * drs_params.lidar_int_std_at_1m; + std::normal_distribution intensity_distribution(0, intensity_std); + point.intensity += intensity_distribution(rng); + + // Clamp intensity to a reasonable range (e.g., 0-255) + point.intensity = std::clamp(point.intensity, 0.0f, 255.0f); + } + pcl_cloud.push_back(point); + } + + sensor_msgs::PointCloud2 pcl_msg; + pcl::toROSMsg(pcl_cloud, pcl_msg); + pcl_msg.header.stamp = ros::Time::now(); + pcl_msg.header.frame_id = "uav" + std::to_string(i + 1) + "/fcu"; + + ph_int_lidars_[i].publish(pcl_msg); + } +} // namespace mrs_uav_unreal_simulation + +//} + /* timerRgb() //{ */ void UnrealSimulator::timerRgb([[maybe_unused]] const ros::TimerEvent& event) { @@ -1443,6 +1590,7 @@ void UnrealSimulator::callbackDrs(mrs_uav_unreal_simulation::unreal_simulatorCon timer_rgb_segmented_.setPeriod(ros::Duration(1.0 / config.rgb_segmented_rate)); timer_lidar_.setPeriod(ros::Duration(1.0 / config.lidar_rate)); timer_seg_lidar_.setPeriod(ros::Duration(1.0 / config.lidar_rate)); + timer_int_lidar_.setPeriod(ros::Duration(1.0 / config.lidar_rate)); ROS_INFO("[UnrealSimulator]: DRS updated params"); } @@ -1515,7 +1663,7 @@ void UnrealSimulator::updateUnrealPoses(const bool teleport_without_collision) { ueds_connector::Coordinates UnrealSimulator::position2ue(const Eigen::Vector3d &pos, const ueds_connector::Coordinates &ueds_world_origin){ ueds_connector::Coordinates pos_ue; - + pos_ue.x = ueds_world_origin.x + pos.x() * 100.0; pos_ue.y = ueds_world_origin.y - pos.y() * 100.0; pos_ue.z = ueds_world_origin.z + pos.z() * 100.0; diff --git a/tmux/one_drone/config/simulator.yaml b/tmux/one_drone/config/simulator.yaml index a048489..2ebbe43 100644 --- a/tmux/one_drone/config/simulator.yaml +++ b/tmux/one_drone/config/simulator.yaml @@ -13,6 +13,26 @@ uav1: sensors: lidar: enabled: true + horizontal_rays: 128 # [-] + vertical_rays: 64 # [-] + lidar_intensity: + enabled: true + rate: 10.0 # [Hz] + + values: + grass: 50 + road: 7 + tree: 90 + building: 70 + fence: 40 + dirt_road: 30 + other: 155 + + noise: + enabled: true + std_at_1m: 0.59 # [-] + std_slope: 0.81 # [-] # multiplies std_at_1m for each meter of measured distances + rgb: enabled: true stereo: diff --git a/tmux/one_drone/rviz.rviz b/tmux/one_drone/rviz.rviz index d695d36..bae8c17 100644 --- a/tmux/one_drone/rviz.rviz +++ b/tmux/one_drone/rviz.rviz @@ -6,8 +6,10 @@ Panels: Expanded: - /TF1/Frames1 - /TF1/Tree1 + - /lidar_segmented1 + - /lidar_intensity1 Splitter Ratio: 0.6970587968826294 - Tree Height: 280 + Tree Height: 376 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -463,7 +465,7 @@ Visualization Manager: Unreliable: false Value: false - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /uav1/rgb/image_raw Max Value: 1 Median window: 5 @@ -473,9 +475,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /uav1/stereo/right/image_raw Max Value: 1 Median window: 5 @@ -485,9 +487,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /uav1/stereo/left/image_raw Max Value: 1 Median window: 5 @@ -497,7 +499,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -510,7 +512,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -525,12 +527,12 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 1.359739065170288 - Min Value: -1.1742526292800903 + Max Value: 7.1122894287109375 + Min Value: -1.2281334400177002 Value: true Axis: Z Channel Name: intensity @@ -538,7 +540,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -553,7 +555,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -566,7 +568,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -581,6 +583,34 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: lidar_intensity + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /uav1/lidar_intensity/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true Value: true - Class: rviz/MarkerArray Enabled: false @@ -591,13 +621,13 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /uav1/octomap_local_vis/occupied_cells_vis_array Name: local_map Namespaces: - map: true + {} Queue Size: 100 - Value: true + Value: false Enabled: true Global Options: Background Color: 255; 255; 255 @@ -662,10 +692,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1495 + Height: 521 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -674,9 +704,9 @@ Window Geometry: collapsed: true Views: collapsed: true - Width: 2548 - X: -32 - Y: -32 + Width: 1910 + X: 2 + Y: 22 rgb: collapsed: false rgb_segmented: diff --git a/tmux/one_drone/session.yml b/tmux/one_drone/session.yml index 64c7d7f..1fb607b 100644 --- a/tmux/one_drone/session.yml +++ b/tmux/one_drone/session.yml @@ -5,7 +5,7 @@ socket_name: mrs attach: false tmux_options: -f /etc/ctu-mrs/tmux.conf # you can modify these -pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500; source ~/ueds_workspace/devel/setup.zsh startup_window: goto windows: - roscore: