diff --git a/.vscode/settings.json b/.vscode/settings.json index c9f71335..94a9211a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,7 +4,10 @@ "**/*.meta": true, "Library": true, "Temp": true, - "UserSettings": true + "UserSettings": true, + "obj": true, + "**/TextMesh Pro": true, + "**/VSCode": true }, "editor.detectIndentation": false, "editor.indentSize": "tabSize", diff --git a/Assets/Resources/Materials/Segmentation.mat b/Assets/Resources/Materials/Segmentation.mat new file mode 100644 index 00000000..e2373fd5 --- /dev/null +++ b/Assets/Resources/Materials/Segmentation.mat @@ -0,0 +1,46 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!21 &2100000 +Material: + serializedVersion: 8 + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_Name: Segmentation + m_Shader: {fileID: 4800000, guid: 419a697b1455619dea4cc9d729b3d0c8, type: 3} + m_Parent: {fileID: 0} + m_ModifiedSerializedProperties: 0 + m_ValidKeywords: [] + m_InvalidKeywords: [] + m_LightmapFlags: 4 + m_EnableInstancingVariants: 0 + m_DoubleSidedGI: 0 + m_CustomRenderQueue: 2000 + stringTagMap: {} + disabledShaderPasses: [] + m_LockedProperties: + m_SavedProperties: + serializedVersion: 3 + m_TexEnvs: [] + m_Ints: [] + m_Floats: + - _DisableColor: 0 + - _Hide: 0 + m_Colors: + - _SegmentationClassId: {r: 0, g: 0, b: 0, a: 1} + - _SegmentationColor: {r: 1, g: 1, b: 1, a: 1} + m_BuildTextureStacks: [] +--- !u!114 &6202412140821847777 +MonoBehaviour: + m_ObjectHideFlags: 11 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 0} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d0353a89b1f911e48b9e16bdc9f2e058, type: 3} + m_Name: + m_EditorClassIdentifier: + version: 7 diff --git a/Assets/Resources/Materials/Segmentation.mat.meta b/Assets/Resources/Materials/Segmentation.mat.meta new file mode 100644 index 00000000..73cce21c --- /dev/null +++ b/Assets/Resources/Materials/Segmentation.mat.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 748cb5dd9e7f8a9f7b5d0712426cd62c +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 2100000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Resources/PhysicsMaterials.meta b/Assets/Resources/PhysicsMaterials.meta new file mode 100644 index 00000000..a21f52f7 --- /dev/null +++ b/Assets/Resources/PhysicsMaterials.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: cac3813512007112193b6f905a198160 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Resources/Materials/DefaultZero.physicMaterial b/Assets/Resources/PhysicsMaterials/DefaultZero.physicMaterial similarity index 100% rename from Assets/Resources/Materials/DefaultZero.physicMaterial rename to Assets/Resources/PhysicsMaterials/DefaultZero.physicMaterial diff --git a/Assets/Resources/Materials/DefaultZero.physicMaterial.meta b/Assets/Resources/PhysicsMaterials/DefaultZero.physicMaterial.meta similarity index 100% rename from Assets/Resources/Materials/DefaultZero.physicMaterial.meta rename to Assets/Resources/PhysicsMaterials/DefaultZero.physicMaterial.meta diff --git a/Assets/Resources/Materials/Props.physicMaterial b/Assets/Resources/PhysicsMaterials/Props.physicMaterial similarity index 100% rename from Assets/Resources/Materials/Props.physicMaterial rename to Assets/Resources/PhysicsMaterials/Props.physicMaterial diff --git a/Assets/Resources/Materials/Props.physicMaterial.meta b/Assets/Resources/PhysicsMaterials/Props.physicMaterial.meta similarity index 100% rename from Assets/Resources/Materials/Props.physicMaterial.meta rename to Assets/Resources/PhysicsMaterials/Props.physicMaterial.meta diff --git a/Assets/Resources/Materials/temp_material.physicMaterial b/Assets/Resources/PhysicsMaterials/temp_material.physicMaterial similarity index 100% rename from Assets/Resources/Materials/temp_material.physicMaterial rename to Assets/Resources/PhysicsMaterials/temp_material.physicMaterial diff --git a/Assets/Resources/Materials/temp_material.physicMaterial.meta b/Assets/Resources/PhysicsMaterials/temp_material.physicMaterial.meta similarity index 100% rename from Assets/Resources/Materials/temp_material.physicMaterial.meta rename to Assets/Resources/PhysicsMaterials/temp_material.physicMaterial.meta diff --git a/Assets/Resources/RenderPipelines/SegmentationRenderer.asset b/Assets/Resources/RenderPipelines/SegmentationRenderer.asset new file mode 100644 index 00000000..2b8f77cf --- /dev/null +++ b/Assets/Resources/RenderPipelines/SegmentationRenderer.asset @@ -0,0 +1,104 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!114 &-5373475574186842855 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 0} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 6b3d386ba5cd94485973aee1479b272e, type: 3} + m_Name: RenderObjects + m_EditorClassIdentifier: + m_Active: 1 + settings: + passTag: RenderObjects + Event: 550 + filterSettings: + RenderQueueType: 0 + LayerMask: + serializedVersion: 2 + m_Bits: 9 + PassNames: [] + overrideMaterial: {fileID: 2100000, guid: 748cb5dd9e7f8a9f7b5d0712426cd62c, type: 2} + overrideMaterialPassIndex: 0 + overrideShader: {fileID: 4800000, guid: 419a697b1455619dea4cc9d729b3d0c8, type: 3} + overrideShaderPassIndex: 0 + overrideMode: 1 + overrideDepthState: 0 + depthCompareFunction: 2 + enableWrite: 0 + stencilSettings: + overrideStencilState: 0 + stencilReference: 5 + stencilCompareFunction: 5 + passOperation: 1 + failOperation: 1 + zFailOperation: 1 + cameraSettings: + overrideCamera: 0 + restoreCamera: 1 + offset: {x: 0, y: 0, z: 0, w: 0} + cameraFieldOfView: 25 +--- !u!114 &11400000 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 0} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: de640fe3d0db1804a85f9fc8f5cadab6, type: 3} + m_Name: SegmentationRenderer + m_EditorClassIdentifier: + debugShaders: + debugReplacementPS: {fileID: 4800000, guid: cf852408f2e174538bcd9b7fda1c5ae7, + type: 3} + hdrDebugViewPS: {fileID: 4800000, guid: 573620ae32aec764abd4d728906d2587, type: 3} + m_RendererFeatures: + - {fileID: -5373475574186842855} + m_RendererFeatureMap: 19e55a676f946db5 + m_UseNativeRenderPass: 0 + postProcessData: {fileID: 11400000, guid: 41439944d30ece34e96484bdb6645b55, type: 2} + shaders: + blitPS: {fileID: 4800000, guid: c17132b1f77d20942aa75f8429c0f8bc, type: 3} + copyDepthPS: {fileID: 4800000, guid: d6dae50ee9e1bfa4db75f19f99355220, type: 3} + screenSpaceShadowPS: {fileID: 0} + samplingPS: {fileID: 4800000, guid: 04c410c9937594faa893a11dceb85f7e, type: 3} + stencilDeferredPS: {fileID: 4800000, guid: e9155b26e1bc55942a41e518703fe304, type: 3} + fallbackErrorPS: {fileID: 4800000, guid: e6e9a19c3678ded42a3bc431ebef7dbd, type: 3} + fallbackLoadingPS: {fileID: 4800000, guid: 7f888aff2ac86494babad1c2c5daeee2, type: 3} + materialErrorPS: {fileID: 4800000, guid: 5fd9a8feb75a4b5894c241777f519d4e, type: 3} + coreBlitPS: {fileID: 4800000, guid: 93446b5c5339d4f00b85c159e1159b7c, type: 3} + coreBlitColorAndDepthPS: {fileID: 4800000, guid: d104b2fc1ca6445babb8e90b0758136b, + type: 3} + blitHDROverlay: {fileID: 4800000, guid: a89bee29cffa951418fc1e2da94d1959, type: 3} + cameraMotionVector: {fileID: 4800000, guid: c56b7e0d4c7cb484e959caeeedae9bbf, + type: 3} + objectMotionVector: {fileID: 4800000, guid: 7b3ede40266cd49a395def176e1bc486, + type: 3} + dataDrivenLensFlare: {fileID: 4800000, guid: 6cda457ac28612740adb23da5d39ea92, + type: 3} + m_AssetVersion: 2 + m_OpaqueLayerMask: + serializedVersion: 2 + m_Bits: 9 + m_TransparentLayerMask: + serializedVersion: 2 + m_Bits: 0 + m_DefaultStencilState: + overrideStencilState: 0 + stencilReference: 0 + stencilCompareFunction: 8 + passOperation: 2 + failOperation: 0 + zFailOperation: 0 + m_ShadowTransparentReceive: 0 + m_RenderingMode: 1 + m_DepthPrimingMode: 0 + m_CopyDepthMode: 0 + m_AccurateGbufferNormals: 0 + m_IntermediateTextureMode: 0 diff --git a/Assets/Resources/RenderPipelines/SegmentationRenderer.asset.meta b/Assets/Resources/RenderPipelines/SegmentationRenderer.asset.meta new file mode 100644 index 00000000..ff87f66a --- /dev/null +++ b/Assets/Resources/RenderPipelines/SegmentationRenderer.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e7e5daae73c962ecbbb5651a07d8ffe4 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset b/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset index ad6be46a..467a8ca0 100644 --- a/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset +++ b/Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset @@ -18,6 +18,7 @@ MonoBehaviour: m_RendererData: {fileID: 0} m_RendererDataList: - {fileID: 11400000, guid: a079301ff0f3b5c48ac29edc3797d4c7, type: 2} + - {fileID: 11400000, guid: e7e5daae73c962ecbbb5651a07d8ffe4, type: 2} m_DefaultRendererIndex: 0 m_RequireDepthTexture: 0 m_RequireOpaqueTexture: 0 @@ -83,11 +84,11 @@ MonoBehaviour: blueNoise64LTex: {fileID: 2800000, guid: e3d24661c1e055f45a7560c033dbb837, type: 3} bayerMatrixTex: {fileID: 2800000, guid: f9ee4ed84c1d10c49aabb9b210b0fc44, type: 3} m_PrefilteringModeMainLightShadows: 3 - m_PrefilteringModeAdditionalLight: 0 + m_PrefilteringModeAdditionalLight: 4 m_PrefilteringModeAdditionalLightShadows: 2 m_PrefilterXRKeywords: 1 - m_PrefilteringModeForwardPlus: 2 - m_PrefilteringModeDeferredRendering: 0 + m_PrefilteringModeForwardPlus: 1 + m_PrefilteringModeDeferredRendering: 1 m_PrefilteringModeScreenSpaceOcclusion: 0 m_PrefilterDebugKeywords: 1 m_PrefilterWriteRenderingLayers: 1 diff --git a/Assets/Resources/Shader/Segmentation.shader b/Assets/Resources/Shader/Segmentation.shader new file mode 100644 index 00000000..38dd6b17 --- /dev/null +++ b/Assets/Resources/Shader/Segmentation.shader @@ -0,0 +1,73 @@ +Shader "Sensor/Segmentation" +{ + Properties + { + _SegmentationColor ("Segmentation Color", Color) = (1, 1, 1, 1) + _SegmentationClassId ("Segmentation Class ID Value in 16bits", Color) = (0, 0, 0, 1) + _DisableColor ("Disable Color output", int) = 0 + _Hide ("Hide this label", int) = 0 + } + + SubShader + { + Tags { + "RenderType" = "Opaque" + } + + Pass + { + Name "Segmentation" + + + HLSLPROGRAM + #include "Packages/com.unity.render-pipelines.universal/ShaderLibrary/Core.hlsl" + + #pragma vertex vert + #pragma fragment frag + + CBUFFER_START(UnityPerMaterial) + half4 _SegmentationColor; + half4 _SegmentationClassId; + int _DisableColor; + int _Hide; + CBUFFER_END + + struct Attributes + { + float4 positionOS : POSITION; + }; + + struct Varyings + { + float4 positionCS : SV_POSITION; + half4 color : COLOR; + }; + + Varyings vert(Attributes input) + { + Varyings output = (Varyings)0; + VertexPositionInputs vertexInput = GetVertexPositionInputs(input.positionOS.xyz); + output.positionCS = vertexInput.positionCS; + + half4 output_color = half4(0, 0, 0, 1); + if (_Hide == 0) + { + output_color = (_DisableColor == 0)? _SegmentationColor : _SegmentationClassId; + } + output.color = output_color; + return output; + } + + half4 frag(Varyings i) : SV_Target + { + half4 output_color = half4(0, 0, 0, 1); + if (_Hide == 0) + { + output_color = (_DisableColor == 0)? _SegmentationColor : _SegmentationClassId; + } + return output_color; + } + ENDHLSL + } + } +} \ No newline at end of file diff --git a/Assets/Resources/Shader/Segmentation.shader.meta b/Assets/Resources/Shader/Segmentation.shader.meta new file mode 100644 index 00000000..43ac5826 --- /dev/null +++ b/Assets/Resources/Shader/Segmentation.shader.meta @@ -0,0 +1,9 @@ +fileFormatVersion: 2 +guid: 419a697b1455619dea4cc9d729b3d0c8 +ShaderImporter: + externalObjects: {} + defaultTextures: [] + nonModifiableTextures: [] + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Resources/Materials/icons.meta b/Assets/Resources/icons.meta similarity index 100% rename from Assets/Resources/Materials/icons.meta rename to Assets/Resources/icons.meta diff --git a/Assets/Resources/Materials/icons/android-chrome-192x192.png b/Assets/Resources/icons/android-chrome-192x192.png similarity index 100% rename from Assets/Resources/Materials/icons/android-chrome-192x192.png rename to Assets/Resources/icons/android-chrome-192x192.png diff --git a/Assets/Resources/Materials/icons/android-chrome-192x192.png.meta b/Assets/Resources/icons/android-chrome-192x192.png.meta similarity index 100% rename from Assets/Resources/Materials/icons/android-chrome-192x192.png.meta rename to Assets/Resources/icons/android-chrome-192x192.png.meta diff --git a/Assets/Resources/Materials/icons/android-chrome-512x512.png b/Assets/Resources/icons/android-chrome-512x512.png similarity index 100% rename from Assets/Resources/Materials/icons/android-chrome-512x512.png rename to Assets/Resources/icons/android-chrome-512x512.png diff --git a/Assets/Resources/Materials/icons/android-chrome-512x512.png.meta b/Assets/Resources/icons/android-chrome-512x512.png.meta similarity index 100% rename from Assets/Resources/Materials/icons/android-chrome-512x512.png.meta rename to Assets/Resources/icons/android-chrome-512x512.png.meta diff --git a/Assets/Resources/Materials/icons/apple-touch-icon.png b/Assets/Resources/icons/apple-touch-icon.png similarity index 100% rename from Assets/Resources/Materials/icons/apple-touch-icon.png rename to Assets/Resources/icons/apple-touch-icon.png diff --git a/Assets/Resources/Materials/icons/apple-touch-icon.png.meta b/Assets/Resources/icons/apple-touch-icon.png.meta similarity index 100% rename from Assets/Resources/Materials/icons/apple-touch-icon.png.meta rename to Assets/Resources/icons/apple-touch-icon.png.meta diff --git a/Assets/Resources/Materials/icons/cursor.png b/Assets/Resources/icons/cursor.png similarity index 100% rename from Assets/Resources/Materials/icons/cursor.png rename to Assets/Resources/icons/cursor.png diff --git a/Assets/Resources/Materials/icons/cursor.png.meta b/Assets/Resources/icons/cursor.png.meta similarity index 100% rename from Assets/Resources/Materials/icons/cursor.png.meta rename to Assets/Resources/icons/cursor.png.meta diff --git a/Assets/Resources/Materials/icons/favicon-16x16.png b/Assets/Resources/icons/favicon-16x16.png similarity index 100% rename from Assets/Resources/Materials/icons/favicon-16x16.png rename to Assets/Resources/icons/favicon-16x16.png diff --git a/Assets/Resources/Materials/icons/favicon-16x16.png.meta b/Assets/Resources/icons/favicon-16x16.png.meta similarity index 100% rename from Assets/Resources/Materials/icons/favicon-16x16.png.meta rename to Assets/Resources/icons/favicon-16x16.png.meta diff --git a/Assets/Resources/Materials/icons/favicon-32x32.png b/Assets/Resources/icons/favicon-32x32.png similarity index 100% rename from Assets/Resources/Materials/icons/favicon-32x32.png rename to Assets/Resources/icons/favicon-32x32.png diff --git a/Assets/Resources/Materials/icons/favicon-32x32.png.meta b/Assets/Resources/icons/favicon-32x32.png.meta similarity index 100% rename from Assets/Resources/Materials/icons/favicon-32x32.png.meta rename to Assets/Resources/icons/favicon-32x32.png.meta diff --git a/Assets/Resources/Materials/icons/favicon.ico b/Assets/Resources/icons/favicon.ico similarity index 100% rename from Assets/Resources/Materials/icons/favicon.ico rename to Assets/Resources/icons/favicon.ico diff --git a/Assets/Resources/Materials/icons/favicon.ico.meta b/Assets/Resources/icons/favicon.ico.meta similarity index 100% rename from Assets/Resources/Materials/icons/favicon.ico.meta rename to Assets/Resources/icons/favicon.ico.meta diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity index 797223aa..4b6e15c0 100644 --- a/Assets/Scenes/MainScene.unity +++ b/Assets/Scenes/MainScene.unity @@ -5207,7 +5207,7 @@ Camera: m_Enabled: 1 serializedVersion: 2 m_ClearFlags: 1 - m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0} + m_BackGroundColor: {r: 0, g: 0, b: 0, a: 1} m_projectionMatrixMode: 2 m_GateFitMode: 2 m_FOVAxisMode: 0 diff --git a/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs b/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs index ee0139e5..b7f72dfe 100644 --- a/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/CameraPlugin.cs @@ -8,34 +8,29 @@ public class CameraPlugin : CLOiSimPlugin { - private SensorDevices.Camera cam = null; - - public SensorDevices.Camera GetCamera() - { - return cam; - } - - public SensorDevices.DepthCamera GetDepthCamera() - { - return GetCamera() as SensorDevices.DepthCamera; - } + protected SensorDevices.Camera _cam = null; protected override void OnAwake() { - var depthcam = gameObject.GetComponent(); - if (depthcam is null) + var depthCam = gameObject.GetComponent(); + + var deviceName = string.Empty; + if (depthCam is not null) { - ChangePluginType(ICLOiSimPlugin.Type.CAMERA); - cam = gameObject.GetComponent(); - attachedDevices.Add("Camera", cam); + ChangePluginType(ICLOiSimPlugin.Type.DEPTHCAMERA); + deviceName = "DepthCamera"; + _cam = depthCam; } else { - ChangePluginType(ICLOiSimPlugin.Type.DEPTHCAMERA); - cam = depthcam; - attachedDevices.Add("DepthCamera", cam); + ChangePluginType(ICLOiSimPlugin.Type.CAMERA); + deviceName = "Camera"; + _cam = gameObject.GetComponent(); } + if (!string.IsNullOrEmpty(deviceName)) + attachedDevices.Add(deviceName, _cam); + partsName = DeviceHelper.GetPartName(gameObject); } @@ -48,7 +43,19 @@ protected override void OnStart() if (RegisterTxDevice(out var portTx, "Data")) { - AddThread(portTx, SenderThread, cam); + AddThread(portTx, SenderThread, _cam); + } + } + + protected override void OnPluginLoad() + { + if (GetPluginParameters() != null && type == ICLOiSimPlugin.Type.DEPTHCAMERA) + { + var depthScale = GetPluginParameters().GetValue("configuration/depth_scale", 1000); + if (_cam != null) + { + ((SensorDevices.DepthCamera)_cam).SetDepthScale(depthScale); + } } } @@ -57,13 +64,13 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any switch (requestType) { case "request_camera_info": - var cameraInfoMessage = cam.GetCameraInfo(); + var cameraInfoMessage = _cam.GetCameraInfo(); SetCameraInfoResponse(ref response, cameraInfoMessage); break; case "request_transform": - var devicePose = cam.GetPose(); - var deviceName = cam.DeviceName; + var devicePose = _cam.GetPose(); + var deviceName = _cam.DeviceName; SetTransformInfoResponse(ref response, deviceName, devicePose); break; diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh b/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh index 01857770..dc6eac22 100755 --- a/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh +++ b/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh @@ -16,7 +16,8 @@ PROTOGEN="protogen" ## 2. check and edit here ## set the location of protobuf messages in absolute path # PROTO_MSGS_PATH="../../../../../cloi3_ws/src/cloisim_ros/cloisim_ros_protobuf_msgs/msgs/" -PROTO_MSGS_PATH="../../../../../../../cloi3/src/cloisim_ros/cloisim_ros_protobuf_msgs/msgs/" +#PROTO_MSGS_PATH="../../../../../../../cloi3/src/cloisim_ros/cloisim_ros_protobuf_msgs/msgs/" +PROTO_MSGS_PATH="../../../../../../cloi_ws/src/simulator/cloisim_ros/cloisim_ros_protobuf_msgs/msgs/" ## 3. target protobuf message ## @@ -27,6 +28,7 @@ MSG="header any param param_v color empty " MSG+="time vector2d vector3d quaternion pose pose_v poses_stamped " MSG+="image images_stamped image_stamped camerasensor distortion camera_lens " MSG+="laserscan laserscan_stamped raysensor pointcloud " +MSG+="segmentation vision_class " MSG+="micom battery twist " MSG+="contact contacts contactsensor " MSG+="gps gps_sensor " @@ -51,4 +53,4 @@ done command="$PROTOGEN -I$PROTO_MSGS_PATH --csharp_out=$TARGET_PATH $TARGET_PROTO_MSGS" #echo $command -eval $command \ No newline at end of file +eval $command diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/segmentation.cs b/Assets/Scripts/CLOiSimPlugins/Messages/segmentation.cs new file mode 100644 index 00000000..75fb4bb1 --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/Messages/segmentation.cs @@ -0,0 +1,30 @@ +// +// This file was generated by a tool; you should avoid making direct changes. +// Consider using 'partial classes' to extend these types +// Input: segmentation.proto +// + +#region Designer generated code +#pragma warning disable CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 +namespace cloisim.msgs +{ + + [global::ProtoBuf.ProtoContract()] + public partial class Segmentation : global::ProtoBuf.IExtensible + { + private global::ProtoBuf.IExtension __pbn__extensionData; + global::ProtoBuf.IExtension global::ProtoBuf.IExtensible.GetExtensionObject(bool createIfMissing) + => global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing); + + [global::ProtoBuf.ProtoMember(1, Name = @"image_stamped", IsRequired = true)] + public ImageStamped ImageStamped { get; set; } + + [global::ProtoBuf.ProtoMember(2, Name = @"class_map")] + public global::System.Collections.Generic.List ClassMaps { get; } = new global::System.Collections.Generic.List(); + + } + +} + +#pragma warning restore CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 +#endregion diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/segmentation.cs.meta b/Assets/Scripts/CLOiSimPlugins/Messages/segmentation.cs.meta new file mode 100644 index 00000000..58e79243 --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/Messages/segmentation.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d561e9afd33d233209d5252c166b2fc5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/vision_class.cs b/Assets/Scripts/CLOiSimPlugins/Messages/vision_class.cs new file mode 100644 index 00000000..54a718ec --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/Messages/vision_class.cs @@ -0,0 +1,30 @@ +// +// This file was generated by a tool; you should avoid making direct changes. +// Consider using 'partial classes' to extend these types +// Input: vision_class.proto +// + +#region Designer generated code +#pragma warning disable CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 +namespace cloisim.msgs +{ + + [global::ProtoBuf.ProtoContract()] + public partial class VisionClass : global::ProtoBuf.IExtensible + { + private global::ProtoBuf.IExtension __pbn__extensionData; + global::ProtoBuf.IExtension global::ProtoBuf.IExtensible.GetExtensionObject(bool createIfMissing) + => global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing); + + [global::ProtoBuf.ProtoMember(1, Name = @"class_id", IsRequired = true)] + public uint ClassId { get; set; } + + [global::ProtoBuf.ProtoMember(2, Name = @"class_name", IsRequired = true)] + public string ClassName { get; set; } + + } + +} + +#pragma warning restore CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 +#endregion diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/vision_class.cs.meta b/Assets/Scripts/CLOiSimPlugins/Messages/vision_class.cs.meta new file mode 100644 index 00000000..374dacb4 --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/Messages/vision_class.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c5f64d65085fac3e4be8023bf8768aa7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index ae0bfc6a..fd5e32e8 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -16,7 +16,7 @@ protected void PublishTfThread(System.Object threadObject) { var paramsObject = threadObject as CLOiSimPluginThread.ParamObject; var publisher = GetTransport().Get(paramsObject.targetPort); - var tfList = paramsObject.paramObject as List; + var tfList = paramsObject.param as List; var tfMessage = new messages.TransformStamped(); tfMessage.Header = new messages.Header(); diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs index cf48df6c..36729854 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.Thread.cs @@ -21,8 +21,8 @@ protected bool AddThread(in ushort targetPortForThread, in ParameterizedThreadSt protected void SenderThread(System.Object threadObject) { var paramObject = threadObject as CLOiSimPluginThread.ParamObject; - var publisher = this.transport.Get(paramObject.targetPort); - var deviceParam = paramObject.paramObject as Device; + var publisher = GetTransport().Get(paramObject.targetPort); + var deviceParam = paramObject.param as Device; thread.Sender(publisher, deviceParam); } @@ -30,8 +30,8 @@ protected void SenderThread(System.Object threadObject) protected void ReceiverThread(System.Object threadObject) { var paramObject = threadObject as CLOiSimPluginThread.ParamObject; - var subscriber = this.transport.Get(paramObject.targetPort); - var deviceParam = paramObject.paramObject as Device; + var subscriber = GetTransport().Get(paramObject.targetPort); + var deviceParam = paramObject.param as Device; thread.Receiver(subscriber, deviceParam); } @@ -39,7 +39,7 @@ protected void ReceiverThread(System.Object threadObject) protected void ServiceThread(System.Object threadObject) { var paramObject = threadObject as CLOiSimPluginThread.ParamObject; - var responsor = this.transport.Get(paramObject.targetPort); + var responsor = GetTransport().Get(paramObject.targetPort); thread.Service(responsor); } diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs index b03cd770..5f4b860c 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs @@ -9,7 +9,7 @@ public interface ICLOiSimPlugin { - enum Type { WORLD, GROUNDTRUTH, ELEVATOR, ACTOR, MICOM, JOINTCONTROL, GPS, IMU, SONAR, LASER, CAMERA, DEPTHCAMERA, MULTICAMERA, REALSENSE }; + enum Type { WORLD, GROUNDTRUTH, ELEVATOR, ACTOR, MICOM, JOINTCONTROL, GPS, IMU, SONAR, LASER, CAMERA, DEPTHCAMERA, MULTICAMERA, REALSENSE, SEGMENTCAMERA}; void SetPluginParameters(in SDF.Plugin node); SDF.Plugin GetPluginParameters(); void Reset(); @@ -36,15 +36,16 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin protected abstract void OnAwake(); protected abstract void OnStart(); + protected virtual void OnPluginLoad() { } protected virtual void OnReset() { } protected void OnDestroy() { - DeregisterDevice(allocatedDevicePorts, allocatedDeviceHashKeys); - thread.Dispose(); transport.Dispose(); - Debug.Log($"({type.ToString()}){name}, CLOiSimPlugin destroyed."); + + DeregisterDevice(allocatedDevicePorts, allocatedDeviceHashKeys); + // Debug.Log($"({type.ToString()}){name}, CLOiSimPlugin destroyed."); } public void ChangePluginType(in ICLOiSimPlugin.Type targetType) @@ -89,6 +90,8 @@ void Start() partsName = pluginParameters.Name; } + OnPluginLoad(); + OnStart(); thread.Start(); diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs index 2271500c..a921241a 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPluginThread.cs @@ -17,12 +17,12 @@ public class CLOiSimPluginThread : IDisposable public class ParamObject { public ushort targetPort; - public System.Object paramObject; + public System.Object param; - public ParamObject(in ushort targetPort, in System.Object paramObject) + public ParamObject(in ushort targetPort, in System.Object parameter) { this.targetPort = targetPort; - this.paramObject = paramObject; + this.param = parameter; } } diff --git a/Assets/Scripts/CLOiSimPlugins/README.md b/Assets/Scripts/CLOiSimPlugins/README.md index 61805ba5..1b7162cb 100644 --- a/Assets/Scripts/CLOiSimPlugins/README.md +++ b/Assets/Scripts/CLOiSimPlugins/README.md @@ -6,7 +6,6 @@ Each class name is important to load plugin in SDF. Class name should be exact t For example, if it describes a name with 'RobotControl' in `` attributes, SDF Parser will start to find a filename in plugin element as 'RobotControl' in Unity project. - ## List of plugins Just take one of below plugins list and write it inside of `filename` attirbute. @@ -16,6 +15,7 @@ It is optional but recommend to append 'lib' and '.so' to the name as a prefix a - `LaserPlugin`: help to publish 2D or 3D lidar data - `CameraPlugin`: help to publish 2D color image data or depth image data +- `SegmentationCameraPlugin`: help to publish semantic segmentation image data and label info - `MultiCameraPlugin`: help to publish multiple color image data - `RealSensePlugin`: can handle ir1(left), ir2(right), depth, color - `MicomPlugin`: control micom(differential drive) input/output(sensor) @@ -82,3 +82,162 @@ It is optional but recommend to append 'lib' and '.so' to the name as a prefix a ``` + +## Realsense Camera + +Specify depth scale for depth in millimeter and which sensor to activate. + +```xml + + + ... + ... + + 0 0.032 0.0 0 0 0 + + 1.2113 + + 640 + 480 + R8G8B8 + + + 0.1 + 100 + + + + + + 0 0.032 0.0 0 0 0 + + 1.2113 + + 640 + 480 + L16 + + + 0.105 + 10 + + + + + + 0 0.017 0 0 0 0 + + 1.047 + + 640 + 480 + L8 + + + 0.1 + 100 + + + + + + 0 -0.032 0 0 0 0 + + 1.047 + + 640 + 480 + L8 + + + 0.1 + 100 + + + + + 0 -0.011 0 0 0 0 + + 1.2113 + + 640 + 480 + L16 + + + 0.105 + 10 + + + + + + + + 1000 + + + color + aligned_depth_to_color + + + + + +``` + +## Depth camera + +Specify depth scale for depth in millimeter and which sensor to activate. + +```xml + + + 0.98 + + 640 + 480 + L16 + + + 0.02 + 2.1 + + + + + + 1000 + + + depth + depthcam_link + + + +``` + +## Semantic Segmentation Camera + +Describe which object to label it for segmentation camera. + +```xml + + + semantic + + + + segmentation + camera_link + + + + + + + + + +``` diff --git a/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs b/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs index 38bb2968..190d625b 100644 --- a/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/RealSensePlugin.cs @@ -26,7 +26,6 @@ protected override void OnAwake() protected override void OnStart() { - var depthScale = GetPluginParameters().GetValue("configuration/depth_scale", 1000); var colorName = GetPluginParameters().GetValue("activate/module[@name='color']"); var leftImagerName = GetPluginParameters().GetValue("activate/module[@name='left_imager']"); var rightImagerName = GetPluginParameters().GetValue("activate/module[@name='right_imager']"); @@ -51,12 +50,12 @@ protected override void OnStart() if (!string.IsNullOrEmpty(depthName)) { - FindAndAddDepthCameraPlugin(depthName, depthScale); + FindAndAddCameraPlugin(depthName); } if (!string.IsNullOrEmpty(alignedDepthToColorName)) { - FindAndAddDepthCameraPlugin(alignedDepthToColorName, depthScale); + FindAndAddCameraPlugin(alignedDepthToColorName); } if (!string.IsNullOrEmpty(imuName)) @@ -85,24 +84,6 @@ private void AddImuPlugin(in string name) } } - private void FindAndAddDepthCameraPlugin(in string name, in uint depthScale) - { - var plugin = FindAndAddCameraPlugin(name); - if (plugin == null) - { - Debug.LogWarning(name + " plugin is not loaded."); - } - else - { - var depthCamera = plugin.GetDepthCamera(); - - if (depthCamera != null) - { - depthCamera.SetDepthScale(depthScale); - } - } - } - private CameraPlugin FindAndAddCameraPlugin(in string name) { foreach (var camera in cameras) diff --git a/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs b/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs new file mode 100644 index 00000000..db3d4e11 --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs @@ -0,0 +1,42 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using UnityEngine; + +public class SegmentationCameraPlugin : CameraPlugin +{ + protected SensorDevices.Camera cam = null; + + protected override void OnAwake() + { + var segCam = gameObject.GetComponent(); + + if (segCam is not null) + { + ChangePluginType(ICLOiSimPlugin.Type.SEGMENTCAMERA); + _cam = segCam; + attachedDevices.Add("SegmentationCamera", _cam); + } + else + { + Debug.LogError("SensorDevices.SegmentationCamera is missing."); + } + + partsName = DeviceHelper.GetPartName(gameObject); + } + + protected override void OnPluginLoad() + { + if (GetPluginParameters() != null && type == ICLOiSimPlugin.Type.SEGMENTCAMERA) + { + if (GetPluginParameters().GetValues("segmentation/label", out var labelList)) + { + Main.SegmentationManager.SetClassFilter(labelList); + } + Main.SegmentationManager.UpdateTags(); + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs.meta b/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs.meta new file mode 100644 index 00000000..5fae33b1 --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/SegmentationCameraPlugin.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 24072933f04993feead6512e0bbbdf6d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Core/Modules/ColorEncoding.cs b/Assets/Scripts/Core/Modules/ColorEncoding.cs new file mode 100644 index 00000000..5285d9ac --- /dev/null +++ b/Assets/Scripts/Core/Modules/ColorEncoding.cs @@ -0,0 +1,101 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using System; +using UnityEngine; + +public class ColorEncoding +{ + // public static byte ReverseBits(byte value) + // { + // return (byte)((value * 0x0202020202 & 0x010884422010) % 1023); + // } + + public static int SparsifyBits(byte value, in int sparse) + { + int retVal = 0; + for (int bits = 0; bits < 8; bits++, value >>= 1) + { + retVal |= (value & 1); + retVal <<= sparse; + } + return retVal >> sparse; + } + + public static Color EncodeIDAsColor(in int instanceId) + { + var uid = instanceId * 2; + if (uid < 0) + uid = -uid + 1; + + var sid = + (SparsifyBits((byte)(uid >> 16), 3) << 2) | + (SparsifyBits((byte)(uid >> 8), 3) << 1) | + SparsifyBits((byte)(uid), 3); + //Debug.Log(uid + " >>> " + System.Convert.ToString(sid, 2).PadLeft(24, '0')); + + var r = (byte)(sid >> 8); + var g = (byte)(sid >> 16); + var b = (byte)(sid); + // Debug.Log($"EncodeIDAsColor({instanceId}): {r} {g} {b}"); + return new Color32(r, g, b, 255); + } + + public static Color EncodeNameAsColor(in string name) + { + return EncodeTagAsColor(name); + } + + public static Color EncodeTagAsColor(in string tag) + { + var hash = tag.GetHashCode(); + var a = (byte)(hash >> 24); + var r = (byte)(hash >> 16); + var g = (byte)(hash >> 8); + var b = (byte)(hash); + // Debug.Log($"EncodeTagAsColor({tag}): {r} {g} {b}"); + return new Color32(r, g, b, 255); + } + + public static Color EncodeLayerAsColor(in int layer) + { + // Following value must be in the range (0.5 .. 1.0) + // in order to avoid color overlaps when using 'divider' in this func + var z = .7f; + + var uniqueColors = new Color[] { + new Color(1,1,1,1), new Color(z,z,z,1), // 0 + new Color(1,1,z,1), new Color(1,z,1,1), new Color(z,1,1,1), // + new Color(1,z,0,1), new Color(z,0,1,1), new Color(0,1,z,1), // 7 + + new Color(1,0,0,1), new Color(0,1,0,1), new Color(0,0,1,1), // 8 + new Color(1,1,0,1), new Color(1,0,1,1), new Color(0,1,1,1), // + new Color(1,z,z,1), new Color(z,1,z,1) // 15 + }; + + // Create as many colors as necessary by using base 16 color palette + // To create more than 16 - will simply adjust brightness with 'divider' + var color = uniqueColors[layer % uniqueColors.Length]; + var divider = 1.0f + Mathf.Floor(layer / uniqueColors.Length); + color /= divider; + + return color; + } + + public static Color Encode16BitsToRG(in UInt16 value) + { + var classIdR = (byte)((value >> 8) & 0xFF); + var classIdG = (byte)(value & 0xFF); + return new Color32(classIdR, classIdG, 0, 0); + } + + // for little endian order + public static Color Encode16BitsToGR(in UInt16 value) + { + var rgColor = Encode16BitsToRG(value); + return new Color(rgColor.g, rgColor.r, 0, 0); + } +} \ No newline at end of file diff --git a/Assets/Scripts/Core/Modules/ColorEncoding.cs.meta b/Assets/Scripts/Core/Modules/ColorEncoding.cs.meta new file mode 100644 index 00000000..6de3eaf2 --- /dev/null +++ b/Assets/Scripts/Core/Modules/ColorEncoding.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b27dae911b41a0538a9460e05376f313 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Core/Modules/SegmentationTag.cs b/Assets/Scripts/Core/Modules/SegmentationTag.cs new file mode 100644 index 00000000..969db970 --- /dev/null +++ b/Assets/Scripts/Core/Modules/SegmentationTag.cs @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using UnityEngine; +using System; + +public class SegmentationTag : MonoBehaviour +{ + [SerializeField] + private bool _hide = false; + + + [SerializeField] + private string _className = string.Empty; + + [SerializeField] + private UInt16 _classId = 0; + + [SerializeField] + private string _tagName = string.Empty; + + public string TagName + { + set => _tagName = value; + get => (string.IsNullOrEmpty(_tagName) ? this.name : _tagName); + } + + public int TagId + { + get => this.gameObject.GetInstanceID(); + } + + public int TagLayer + { + get => this.gameObject.layer; + } + + public UInt16 ClassId + { + get => _classId; + } + + public bool Hide + { + get => _hide; + set { + _hide = value; + HideLabelForMaterialPropertyBlock(_hide); + } + } + + void OnDestroy() { + // Debug.Log($"Destroy segmentation tag {this.name}"); + Main.SegmentationManager.RemoveClass(_className, this); + } + + public void Refresh() + { + var mpb = new MaterialPropertyBlock(); + + // Debug.Log(Main.SegmentationManager.Mode); + var color = new Color(); + switch (Main.SegmentationManager.Mode) + { + case SegmentationManager.ReplacementMode.ObjectId: + color = ColorEncoding.EncodeIDAsColor(TagId); + break; + + case SegmentationManager.ReplacementMode.ObjectName: + color = ColorEncoding.EncodeNameAsColor(TagName); + break; + + case SegmentationManager.ReplacementMode.LayerId: + color = ColorEncoding.EncodeLayerAsColor(TagId); + break; + + default: + color = Color.black; + break; + } + + var grayscale = color.grayscale; + + _classId = (UInt16)(grayscale * UInt16.MaxValue); + + var classValue = ColorEncoding.Encode16BitsToGR(_classId); + + mpb.SetColor("_SegmentationColor", color); + mpb.SetColor("_SegmentationClassId", classValue); + + // Debug.Log(TagName + ": mode=" + Main.SegmentationManager.Mode + + // " color=" + color + + // " calssId=" + classValue.r + " " + classValue.g); + // Debug.Log($"{TagName} : {grayscale} > {_classId}"); + + AllocateMaterialPropertyBlock(mpb); + + UpdateClass(); + } + + private void UpdateClass() + { + switch (Main.SegmentationManager.Mode) + { + case SegmentationManager.ReplacementMode.ObjectId: + _className = TagId.ToString(); + break; + + case SegmentationManager.ReplacementMode.ObjectName: + _className = TagName; + break; + + case SegmentationManager.ReplacementMode.LayerId: + _className = TagLayer.ToString(); + break; + + default: + return; + } + + Main.SegmentationManager.AddClass(_className, this); + } + + private void AllocateMaterialPropertyBlock(in MaterialPropertyBlock mpb) + { + var renderers = GetComponentsInChildren(); + // Debug.Log($"{this.name} {renderers.Length}"); + foreach (var renderer in renderers) + { + renderer.SetPropertyBlock(mpb); + } + + var terrains = GetComponentsInChildren(); + foreach (var terrain in terrains) + { + terrain.SetSplatMaterialPropertyBlock(mpb); + } + } + + /// + /// Hides the label in material property block. + /// + /// if set to true, hide this segmentation. + private void HideLabelForMaterialPropertyBlock(in bool value) + { + var mpb = new MaterialPropertyBlock(); + var renderers = GetComponentsInChildren(); + foreach (var renderer in renderers) + { + renderer.GetPropertyBlock(mpb); + mpb.SetInt("_Hide", value? 1 : 0); + renderer.SetPropertyBlock(mpb); + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/Core/Modules/SegmentationTag.cs.meta b/Assets/Scripts/Core/Modules/SegmentationTag.cs.meta new file mode 100644 index 00000000..bbe3fd5a --- /dev/null +++ b/Assets/Scripts/Core/Modules/SegmentationTag.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 16019d587cbee42b48eefcc6760f919a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Core/ObjectSpawning.cs b/Assets/Scripts/Core/ObjectSpawning.cs index 195dd461..10c05683 100644 --- a/Assets/Scripts/Core/ObjectSpawning.cs +++ b/Assets/Scripts/Core/ObjectSpawning.cs @@ -1,3 +1,9 @@ +/* + * Copyright (c) 2021 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + using System.Collections; using System.Collections.Generic; using UnityEngine.AI; @@ -19,7 +25,7 @@ public enum PropsType { BOX = 0, CYLINDER = 1, SPHERE = 2 }; private Dictionary props = new Dictionary(); public float maxRayDistance = 100; - private uint propsCount = 0; + private Dictionary propsCount = new Dictionary(); private string scaleFactorString = "0.5"; private int propType = 0; @@ -93,7 +99,11 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3 { GameObject spawnedObject = null; Mesh mesh = null; - var propsName = type.ToString() + "-" + propsCount++; + + if (!propsCount.ContainsKey(type)) + { + propsCount.Add(type, 0); + } if (!props.ContainsKey(type)) { @@ -114,13 +124,15 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3 if (mesh != null) { - var newTempPropsObject = CreateProps(propsName, mesh, scale); + var newTempPropsObject = CreateProps(type.ToString(), mesh, scale); props.Add(type, newTempPropsObject); newTempPropsObject.hideFlags = HideFlags.DontSaveInEditor | HideFlags.DontSave; newTempPropsObject.SetActive(false); } } + var propsName = type.ToString() + "-" + propsCount[type]++; + if (props.TryGetValue(type, out var targetObject)) { spawnedObject = Instantiate(targetObject); @@ -130,12 +142,18 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3 mesh = meshFilter.sharedMesh; var meshRender = spawnedObject.GetComponentInChildren(); - meshRender.material.color = Random.ColorHSV(0f, 1f, 0.6f, 1f, 0.5f, 1f); + meshRender.material.color = Random.ColorHSV(0f, 1f, 0.4f, 1f, 0.3f, 1f); var rigidBody = spawnedObject.GetComponentInChildren(); rigidBody.mass = CalculateMass(scale); rigidBody.ResetCenterOfMass(); rigidBody.ResetInertiaTensor(); + + // var propTypeName = (type.ToString() + scale.ToString()).Trim(); + // Debug.Log(propTypeName); + SegmentationManager.AttachTag(type.ToString(), spawnedObject); + + Main.SegmentationManager.UpdateTags(); } if (mesh != null) @@ -195,6 +213,8 @@ private GameObject CreateProps(in string name, in Mesh targetMesh, in Vector3 sc navMeshObstacle.carvingTimeToStationary = 0.2f; navMeshObstacle.carveOnlyStationary = true; + newObject.AddComponent(); + return newObject; } diff --git a/Assets/Scripts/Core/SegmentationManager.cs b/Assets/Scripts/Core/SegmentationManager.cs new file mode 100644 index 00000000..8ad9bfbe --- /dev/null +++ b/Assets/Scripts/Core/SegmentationManager.cs @@ -0,0 +1,136 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using System; +using UnityEngine; +using System.Collections.Generic; + +public class SegmentationManager : MonoBehaviour +{ + public enum ReplacementMode + { + ObjectId = 0, + ObjectName = 1, + LayerId = 2 + }; + + public ReplacementMode Mode => _ReplaceMode; + + private static readonly ReplacementMode _ReplaceMode = ReplacementMode.ObjectName; + + [SerializeField] + private static readonly bool _disableColor = true; + + private static Material _material = null; // linked to Render Objects + + [SerializeField] + private const int MAX_LABEL_INFO = 256; + + private Dictionary> _labelInfo = new Dictionary>(); + private List _labelClassFilters = new List(); + + public static void AttachTag(in string className, GameObject target) + { + AttachTag(className, target?.transform); + } + + public static void AttachTag(in string className, Transform target) + { + var segmentationTag = target?.gameObject.GetComponentInChildren(); + if (segmentationTag == null) + { + segmentationTag = target?.gameObject.AddComponent(); + } + + segmentationTag.TagName = className; + segmentationTag.Refresh(); + } + + void OnEnable() + { + if (_material == null) + { + _material = Resources.Load("Materials/Segmentation"); + } + + if (_material != null) + { + _material.SetInt("_DisableColor", _disableColor ? 1 : 0); + } + } + + void OnDisable() + { + if (_material != null) + { + _material.SetInt("_DisableColor", 0); + } + } + + public void SetClassFilter(in List items) + { + _labelClassFilters.Clear(); + _labelClassFilters.AddRange(items); + + // foreach (var item in items) + // { + // Debug.Log(item); + // } + } + + public void AddClass(in string className, in SegmentationTag tag) + { + if (_labelInfo.Count > MAX_LABEL_INFO) + { + Debug.LogWarning( + $"Cannot add className({className}) due to maximum count({MAX_LABEL_INFO}) reached"); + return; + } + + if (_labelInfo.ContainsKey(className)) + { + _labelInfo[className].Add(tag); + } + else + { + _labelInfo.TryAdd(className, new List { tag }); + } + // Debug.Log($"AddClass: {className}, {_labelInfo[className]}"); + } + + public void RemoveClass(in string className, in SegmentationTag tag) + { + if (_labelInfo.ContainsKey(className)) + { + _labelInfo[className].Remove(tag); + } + } + + public void UpdateTags() + { + foreach (var vk in _labelInfo) + { + var allowedTag = _labelClassFilters.Contains(vk.Key); + foreach (var tag in vk.Value) + { + tag.Hide = allowedTag ? false : true; + } + // Debug.Log(vk.Key + ", " + allowedTag); + } + } + + public Dictionary> GetLabelInfo() + { + // foreach (var item in _labelInfo) + // { + // if (item.Value.Count > 0) + // { + // Debug.Log($"{item.Key}, {item.Value[0].ClassId}"); + // } + // } + return _labelInfo; + } +} \ No newline at end of file diff --git a/Assets/Scripts/Core/SegmentationManager.cs.meta b/Assets/Scripts/Core/SegmentationManager.cs.meta new file mode 100644 index 00000000..b0f2f8de --- /dev/null +++ b/Assets/Scripts/Core/SegmentationManager.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 71b3c95390c30a9628935696c38169e9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs index 9ba74d9b..900641b9 100644 --- a/Assets/Scripts/Devices/Camera.cs +++ b/Assets/Scripts/Devices/Camera.cs @@ -19,7 +19,7 @@ public class Camera : Device { protected SDF.Camera camParameter = null; protected messages.CameraSensor sensorInfo = null; - protected messages.ImageStamped imageStamped = null; + protected messages.ImageStamped _imageStamped = null; // TODO : Need to be implemented!!! // TBD @@ -30,11 +30,11 @@ public class Camera : Device protected UnityEngine.Camera camSensor = null; protected UniversalAdditionalCameraData _universalCamData = null; - protected string targetRTname; - protected GraphicsFormat targetColorFormat; - protected GraphicsFormat readbackDstFormat; + protected string _targetRTname; + protected GraphicsFormat _targetColorFormat; + protected GraphicsFormat _readbackDstFormat; - private CameraData.Image camImageData; + protected CameraData.Image _camImageData; private List _readbackList = new List(); public Noise noise = null; protected bool _startCameraWork = false; @@ -87,6 +87,7 @@ protected override void OnStart() if (camSensor) { SetupTexture(); + SetupDefaultCamera(); SetupCamera(); _startCameraWork = true; } @@ -94,41 +95,32 @@ protected override void OnStart() protected virtual void SetupTexture() { - camSensor.clearFlags = CameraClearFlags.Skybox; - camSensor.allowHDR = true; - camSensor.depthTextureMode = DepthTextureMode.None; - _universalCamData.requiresColorOption = CameraOverrideOption.On; - _universalCamData.requiresDepthOption = CameraOverrideOption.Off; - _universalCamData.requiresColorTexture = true; - _universalCamData.requiresDepthTexture = false; - _universalCamData.renderShadows = true; - // Debug.Log("This is not a Depth Camera!"); - targetRTname = "CameraColorTexture"; + _targetRTname = "CameraColorTexture"; var pixelFormat = CameraData.GetPixelFormat(camParameter.image.format); switch (pixelFormat) { case CameraData.PixelFormat.L_INT8: - targetColorFormat = GraphicsFormat.R8G8B8A8_SRGB; - readbackDstFormat = GraphicsFormat.R8_SRGB; + _targetColorFormat = GraphicsFormat.R8G8B8A8_SRGB; + _readbackDstFormat = GraphicsFormat.R8_SRGB; break; case CameraData.PixelFormat.RGB_INT8: default: - targetColorFormat = GraphicsFormat.R8G8B8A8_SRGB; - readbackDstFormat = GraphicsFormat.R8G8B8_SRGB; + _targetColorFormat = GraphicsFormat.R8G8B8A8_SRGB; + _readbackDstFormat = GraphicsFormat.R8G8B8_SRGB; break; } - camImageData = new CameraData.Image(camParameter.image.width, camParameter.image.height, pixelFormat); + _camImageData = new CameraData.Image(camParameter.image.width, camParameter.image.height, pixelFormat); } protected override void InitializeMessages() { - imageStamped = new messages.ImageStamped(); - imageStamped.Time = new messages.Time(); - imageStamped.Image = new messages.Image(); + _imageStamped = new messages.ImageStamped(); + _imageStamped.Time = new messages.Time(); + _imageStamped.Image = new messages.Image(); sensorInfo = new messages.CameraSensor(); sensorInfo.ImageSize = new messages.Vector2d(); @@ -138,7 +130,7 @@ protected override void InitializeMessages() protected override void SetupMessages() { - var image = imageStamped.Image; + var image = _imageStamped.Image; var pixelFormat = CameraData.GetPixelFormat(camParameter.image.format); image.Width = (uint)camParameter.image.width; image.Height = (uint)camParameter.image.height; @@ -167,12 +159,28 @@ protected override void SetupMessages() } } - private void SetupCamera() + private void OnEnable() + { + RenderPipelineManager.beginCameraRendering += OnBeginCameraRendering; + RenderPipelineManager.endCameraRendering += OnEndCameraRendering; + } + + private void OnDisable() + { + RenderPipelineManager.beginCameraRendering -= OnBeginCameraRendering; + RenderPipelineManager.endCameraRendering -= OnEndCameraRendering; + } + + private void SetupDefaultCamera() { camSensor.ResetWorldToCameraMatrix(); camSensor.ResetProjectionMatrix(); + camSensor.backgroundColor = Color.black; + camSensor.clearFlags = CameraClearFlags.Skybox; + camSensor.depthTextureMode = DepthTextureMode.None; camSensor.renderingPath = RenderingPath.Forward; + camSensor.allowHDR = true; camSensor.allowMSAA = true; camSensor.allowDynamicResolution = true; camSensor.useOcclusionCulling = true; @@ -188,7 +196,7 @@ private void SetupCamera() height: camParameter.image.height, slices: 1, depthBufferBits: DepthBits.None, - colorFormat: targetColorFormat, + colorFormat: _targetColorFormat, filterMode: FilterMode.Bilinear, wrapMode: TextureWrapMode.Clamp, dimension: TextureDimension.Tex2D, @@ -202,7 +210,7 @@ private void SetupCamera() bindTextureMS: false, useDynamicScale: true, memoryless: RenderTextureMemoryless.None, - name: targetRTname); + name: _targetRTname); camSensor.targetTexture = _rtHandle.rt; @@ -215,6 +223,11 @@ private void SetupCamera() var invertMatrix = Matrix4x4.Scale(new Vector3(1, -1, 1)); camSensor.projectionMatrix = projMatrix * invertMatrix; + _universalCamData.requiresColorOption = CameraOverrideOption.On; + _universalCamData.requiresDepthOption = CameraOverrideOption.Off; + _universalCamData.requiresColorTexture = true; + _universalCamData.requiresDepthTexture = false; + _universalCamData.renderShadows = true; _universalCamData.enabled = false; _universalCamData.stopNaN = true; _universalCamData.dithering = true; @@ -223,24 +236,20 @@ private void SetupCamera() _universalCamData.volumeLayerMask = LayerMask.GetMask("Nothing"); _universalCamData.renderType = CameraRenderType.Base; _universalCamData.cameraStack.Clear(); - camSensor.enabled = false; - - RenderPipelineManager.beginCameraRendering += OnBeginCameraRendering; - RenderPipelineManager.endCameraRendering += OnEndCameraRendering; + camSensor.enabled = false; // camSensor.hideFlags |= HideFlags.NotEditable; } - protected new void OnDestroy() + protected virtual void SetupCamera() { - _startCameraWork = false; + } + protected new void OnDestroy() + { // Debug.Log("OnDestroy(Camera)"); - RenderPipelineManager.endCameraRendering -= OnEndCameraRendering; - RenderPipelineManager.beginCameraRendering -= OnBeginCameraRendering; - + _startCameraWork = false; _rtHandle?.Release(); - base.OnDestroy(); } @@ -263,7 +272,7 @@ private void CameraWorker() { camSensor.Render(); - var readbackRequest = AsyncGPUReadback.Request(camSensor.targetTexture, 0, readbackDstFormat, OnCompleteAsyncReadback); + var readbackRequest = AsyncGPUReadback.Request(camSensor.targetTexture, 0, _readbackDstFormat, OnCompleteAsyncReadback); lock (_readbackList) { @@ -301,23 +310,22 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request) protected override void GenerateMessage() { - DeviceHelper.SetCurrentTime(imageStamped.Time); - PushDeviceMessage(imageStamped); + PushDeviceMessage(_imageStamped); } protected virtual void ImageProcessing(ref NativeArray readbackData) { - var image = imageStamped.Image; - camImageData.SetTextureBufferData(readbackData); + var image = _imageStamped.Image; + _camImageData.SetTextureBufferData(readbackData); - var imageData = camImageData.GetImageData(image.Data.Length); + var imageData = _camImageData.GetImageData(image.Data.Length); if (imageData != null) { image.Data = imageData; if (camParameter.save_enabled && _startCameraWork) { var saveName = name + "_" + Time.time; - camImageData.SaveRawImageData(camParameter.save_path, saveName); + _camImageData.SaveRawImageData(camParameter.save_path, saveName); // Debug.LogFormat("{0}|{1} captured", camParameter.save_path, saveName); } } @@ -325,6 +333,8 @@ protected virtual void ImageProcessing(ref NativeArray readbackData) { Debug.LogWarningFormat("{0}: Failed to get image Data", name); } + + DeviceHelper.SetCurrentTime(_imageStamped.Time); } public messages.CameraSensor GetCameraInfo() @@ -334,7 +344,7 @@ public messages.CameraSensor GetCameraInfo() public messages.Image GetImageDataMessage() { - return (imageStamped == null || imageStamped.Image == null) ? null : imageStamped.Image; + return (_imageStamped == null || _imageStamped.Image == null) ? null : _imageStamped.Image; } } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/DepthCamera.cs b/Assets/Scripts/Devices/DepthCamera.cs index 1d834d2a..01e2cfb5 100644 --- a/Assets/Scripts/Devices/DepthCamera.cs +++ b/Assets/Scripts/Devices/DepthCamera.cs @@ -11,15 +11,17 @@ using Unity.Collections; using Unity.Jobs; using System.Threading.Tasks; +using System; namespace SensorDevices { + [RequireComponent(typeof(UnityEngine.Camera))] public class DepthCamera : Camera { #region "For Compute Shader" private static ComputeShader ComputeShaderDepthBuffer = null; - private ComputeShader computeShader = null; + private ComputeShader _computeShader = null; private int _kernelIndex = -1; private const int ThreadGroupsX = 32; private const int ThreadGroupsY = 32; @@ -28,7 +30,7 @@ public class DepthCamera : Camera #endregion - private Material depthMaterial = null; + private Material _depthMaterial = null; private uint depthScale = 1; private int _imageDepth; @@ -38,10 +40,8 @@ public class DepthCamera : Camera private byte[] _computedBufferOutput; private const uint OutputUnitSize = 4; private int _computedBufferOutputUnitLength; - private CameraData.Image _depthCamImage; private Texture2D _textureForCapture; - public static void LoadComputeShader() { if (ComputeShaderDepthBuffer == null) @@ -62,76 +62,39 @@ public static void UnloadComputeShader() public void ReverseDepthData(in bool reverse) { - if (depthMaterial != null) + if (_depthMaterial != null) { - depthMaterial.SetInt("_ReverseData", (reverse) ? 1 : 0); + _depthMaterial.SetInt("_ReverseData", (reverse) ? 1 : 0); } - else - Debug.Log("is null"); } public void FlipXDepthData(in bool flip) { - if (depthMaterial != null) + if (_depthMaterial != null) { - depthMaterial.SetInt("_FlipX", (flip) ? 1 : 0); + _depthMaterial.SetInt("_FlipX", (flip) ? 1 : 0); } } public void SetDepthScale(in uint value) { depthScale = value; - - if (computeShader != null) - { - computeShader.SetFloat("_DepthScale", (float)depthScale); - } } new void OnDestroy() { // Debug.Log("OnDestroy(Depth Camera)"); - Destroy(computeShader); - computeShader = null; + Destroy(_computeShader); + _computeShader = null; base.OnDestroy(); } protected override void SetupTexture() { - computeShader = Instantiate(ComputeShaderDepthBuffer); - _kernelIndex = computeShader.FindKernel("CSScaleDepthBuffer"); - - var depthShader = Shader.Find("Sensor/Depth"); - depthMaterial = new Material(depthShader); - - if (camParameter.depth_camera_output.Equals("points")) - { - Debug.Log("Enable Point Cloud data mode - NOT SUPPORT YET!"); - camParameter.image.format = "RGB_FLOAT32"; - } - - camSensor.clearFlags = CameraClearFlags.Depth; - camSensor.allowHDR = false; - camSensor.depthTextureMode = DepthTextureMode.Depth; - _universalCamData.requiresColorOption = CameraOverrideOption.Off; - _universalCamData.requiresDepthOption = CameraOverrideOption.On; - _universalCamData.requiresColorTexture = false; - _universalCamData.requiresDepthTexture = true; - _universalCamData.renderShadows = false; - - targetRTname = "CameraDepthTexture"; - targetColorFormat = GraphicsFormat.R8G8B8A8_UNorm; - readbackDstFormat = GraphicsFormat.R8G8B8A8_UNorm; - - var cb = new CommandBuffer(); - cb.name = "CommandBufferForDepthShading"; - var tempTextureId = Shader.PropertyToID("_RenderImageCameraDepthTexture"); - cb.GetTemporaryRT(tempTextureId, -1, -1); - cb.Blit(tempTextureId, BuiltinRenderTextureType.CameraTarget, depthMaterial); - cb.ReleaseTemporaryRT(tempTextureId); - camSensor.AddCommandBuffer(CameraEvent.AfterEverything, cb); - cb.Release(); + _targetRTname = "CameraDepthTexture"; + _targetColorFormat = GraphicsFormat.R8G8B8A8_UNorm; + _readbackDstFormat = GraphicsFormat.R8G8B8A8_UNorm; var width = camParameter.image.width; var height = camParameter.image.height; @@ -150,27 +113,63 @@ protected override void SetupTexture() graphicFormat = GraphicsFormat.R16_UNorm; break; } - _textureForCapture = new Texture2D(width, height, graphicFormat, 0, TextureCreationFlags.None); _depthCamBuffer = new DepthData.CamBuffer(width, height); _computedBufferOutputUnitLength = width * height; _computedBufferOutput = new byte[_computedBufferOutputUnitLength * OutputUnitSize]; - _depthCamImage = new CameraData.Image(width, height, format); + _threadGroupX = width / ThreadGroupsX; + _threadGroupY = height / ThreadGroupsY; + + _textureForCapture = new Texture2D(width, height, graphicFormat, 0, TextureCreationFlags.None); + _textureForCapture.filterMode = FilterMode.Point; - if (computeShader != null) + _computeShader = Instantiate(ComputeShaderDepthBuffer); + + if (_computeShader != null) { + _kernelIndex = _computeShader.FindKernel("CSScaleDepthBuffer"); _imageDepth = CameraData.GetImageDepth(format); - computeShader.SetFloat("_DepthMax", (float)camParameter.clip.far); - computeShader.SetInt("_Width", width); - computeShader.SetInt("_UnitSize", _imageDepth); + + _computeShader.SetFloat("_DepthMax", (float)camParameter.clip.far); + _computeShader.SetInt("_Width", width); + _computeShader.SetInt("_UnitSize", _imageDepth); + _computeShader.SetFloat("_DepthScale", (float)depthScale); + } + } + + protected override void SetupCamera() + { + var depthShader = Shader.Find("Sensor/Depth"); + _depthMaterial = new Material(depthShader); + + if (camParameter.depth_camera_output.Equals("points")) + { + Debug.Log("Enable Point Cloud data mode - NOT SUPPORT YET!"); + camParameter.image.format = "RGB_FLOAT32"; } + camSensor.clearFlags = CameraClearFlags.Depth; + camSensor.allowHDR = false; + camSensor.depthTextureMode = DepthTextureMode.Depth; + + _universalCamData.requiresColorOption = CameraOverrideOption.Off; + _universalCamData.requiresDepthOption = CameraOverrideOption.On; + _universalCamData.requiresColorTexture = false; + _universalCamData.requiresDepthTexture = true; + _universalCamData.renderShadows = false; + + var cb = new CommandBuffer(); + cb.name = "CommandBufferForDepthShading"; + var tempTextureId = Shader.PropertyToID("_RenderImageCameraDepthTexture"); + cb.GetTemporaryRT(tempTextureId, -1, -1); + cb.Blit(tempTextureId, BuiltinRenderTextureType.CameraTarget, _depthMaterial); + cb.ReleaseTemporaryRT(tempTextureId); + camSensor.AddCommandBuffer(CameraEvent.AfterEverything, cb); + cb.Release(); + ReverseDepthData(false); FlipXDepthData(false); - - _threadGroupX = width / ThreadGroupsX; - _threadGroupY = height / ThreadGroupsY; } protected override void ImageProcessing(ref NativeArray readbackData) @@ -178,18 +177,18 @@ protected override void ImageProcessing(ref NativeArray readbackData) _depthCamBuffer.Allocate(); _depthCamBuffer.raw = readbackData; - if (_depthCamBuffer.depth.IsCreated && computeShader != null) + if (_depthCamBuffer.depth.IsCreated && _computeShader != null) { var jobHandleDepthCamBuffer = _depthCamBuffer.Schedule(_depthCamBuffer.Length(), BatchSize); jobHandleDepthCamBuffer.Complete(); var computeBufferSrc = new ComputeBuffer(_depthCamBuffer.depth.Length, sizeof(float)); - computeShader.SetBuffer(_kernelIndex, "_Input", computeBufferSrc); + _computeShader.SetBuffer(_kernelIndex, "_Input", computeBufferSrc); computeBufferSrc.SetData(_depthCamBuffer.depth); var computeBufferDst = new ComputeBuffer(_computedBufferOutput.Length, sizeof(byte)); - computeShader.SetBuffer(_kernelIndex, "_Output", computeBufferDst); - computeShader.Dispatch(_kernelIndex, _threadGroupX, _threadGroupY, 1); + _computeShader.SetBuffer(_kernelIndex, "_Output", computeBufferDst); + _computeShader.Dispatch(_kernelIndex, _threadGroupX, _threadGroupY, 1); computeBufferDst.GetData(_computedBufferOutput); computeBufferSrc.Release(); @@ -197,28 +196,37 @@ protected override void ImageProcessing(ref NativeArray readbackData) Parallel.For(0, _computedBufferOutputUnitLength, (int i) => { - for (int j = 0; j < _imageDepth; j++) - imageStamped.Image.Data[i * _imageDepth + j] = _computedBufferOutput[i * OutputUnitSize + j]; + Buffer.BlockCopy( + _computedBufferOutput, i * (int)OutputUnitSize, + _imageStamped.Image.Data, i * _imageDepth, + _imageDepth); }); - // Debug.LogFormat("{0:X} {1:X} {2:X} {3:X} => {4}, {5}, {6}, {7}", image.Data[0], image.Data[1], image.Data[2], image.Data[3], System.BitConverter.ToInt16(image.Data, 0), System.BitConverter.ToUInt16(image.Data, 2), System.BitConverter.ToInt32(image.Data, 0), System.BitConverter.ToSingle(image.Data, 0)); + // Debug.LogFormat("{0:X} {1:X} {2:X} {3:X} => {4}, {5}, {6}, {7}", + // imageStamped.Image.Data[1000], imageStamped.Image.Data[2000], + // imageStamped.Image.Data[5000], imageStamped.Image.Data[8000], + // System.BitConverter.ToInt16(imageStamped.Image.Data, 0), + // System.BitConverter.ToUInt16(imageStamped.Image.Data, 2), + // System.BitConverter.ToInt32(imageStamped.Image.Data, 0), + // System.BitConverter.ToSingle(imageStamped.Image.Data, 0)); if (camParameter.save_enabled && _startCameraWork) { - SaveRawImageData(); + var saveName = name + "_" + Time.time; + SaveRawImageData(camParameter.save_path, saveName); } } _depthCamBuffer.Deallocate(); + + DeviceHelper.SetCurrentTime(_imageStamped.Time); } - private void SaveRawImageData() + private void SaveRawImageData(in string path, in string name) { - var saveName = name + "_" + Time.time; - _textureForCapture.SetPixelData(imageStamped.Image.Data, 0); - _textureForCapture.filterMode = FilterMode.Point; + _textureForCapture.SetPixelData(_imageStamped.Image.Data, 0); _textureForCapture.Apply(); var bytes = _textureForCapture.EncodeToJPG(); - var fileName = string.Format("{0}/{1}.jpg", camParameter.save_path, saveName); + var fileName = string.Format("{0}/{1}.jpg", path, name); System.IO.File.WriteAllBytes(fileName, bytes); // Debug.LogFormat("{0}|{1} captured", camParameter.save_path, saveName); } diff --git a/Assets/Scripts/Devices/Modules/Base/Device.cs b/Assets/Scripts/Devices/Modules/Base/Device.cs index 68987ba6..537e560e 100644 --- a/Assets/Scripts/Devices/Modules/Base/Device.cs +++ b/Assets/Scripts/Devices/Modules/Base/Device.cs @@ -221,17 +221,32 @@ private void DeviceThreadRx() public bool PushDeviceMessage(T instance) { - deviceMessage.SetMessage(instance); - deviceMessage.GetMessage(out var message); - return deviceMessageQueue.Push(message); + try + { + deviceMessage.SetMessage(instance); + deviceMessage.GetMessage(out var message); + return deviceMessageQueue.Push(message); + } + catch (Exception ex) + { + Debug.LogWarning("ERROR: PushDeviceMessage(): " + ex.Message); + return false; + } } public bool PushDeviceMessage(in byte[] data) { - if (deviceMessage.SetMessage(data)) + try { - deviceMessage.GetMessage(out var message); - return deviceMessageQueue.Push(message); + if (deviceMessage.SetMessage(data)) + { + deviceMessage.GetMessage(out var message); + return deviceMessageQueue.Push(message); + } + } + catch (Exception ex) + { + Debug.LogWarning("ERROR: PushDeviceMessage(): " + ex.Message); } return false; diff --git a/Assets/Scripts/Devices/Modules/DepthData.cs b/Assets/Scripts/Devices/Modules/DepthData.cs index 9a80959c..9bd0bbf6 100644 --- a/Assets/Scripts/Devices/Modules/DepthData.cs +++ b/Assets/Scripts/Devices/Modules/DepthData.cs @@ -55,19 +55,17 @@ public void Execute(int i) private float GetDecodedData(in int index) { var imageOffset = index * ColorFormatUnitSize; - if (raw != null && imageOffset < raw.Length) - { - var r = raw[imageOffset]; - var g = raw[imageOffset + 1]; - var b = raw[imageOffset + 2]; - var a = raw[imageOffset + 3]; - - return DecodeFloatRGBA(r, g, b, a); - } - else + if (raw == null || imageOffset >= raw.Length) { return 0; } + + var r = raw[imageOffset]; + var g = raw[imageOffset + 1]; + var b = raw[imageOffset + 2]; + var a = raw[imageOffset + 3]; + + return DecodeFloatRGBA(r, g, b, a); } private float DecodeFloatRGBA(in byte r, in byte g, in byte b, in byte a) diff --git a/Assets/Scripts/Devices/SegmentationCamera.cs b/Assets/Scripts/Devices/SegmentationCamera.cs new file mode 100644 index 00000000..48b997ef --- /dev/null +++ b/Assets/Scripts/Devices/SegmentationCamera.cs @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using UnityEngine; +using UnityEngine.Rendering.Universal; +using UnityEngine.Experimental.Rendering; +using messages = cloisim.msgs; +using Unity.Collections; + +namespace SensorDevices +{ + [RequireComponent(typeof(UnityEngine.Camera))] + public class SegmentationCamera : Camera + { + private messages.Segmentation _segmentation = null; + + protected override void SetupTexture() + { + _targetRTname = "SegmentationTexture"; + + var pixelFormat = CameraData.GetPixelFormat(camParameter.image.format); + if (pixelFormat != CameraData.PixelFormat.L_INT16) + { + Debug.Log("Only support INT16 format"); + } + + _targetColorFormat = GraphicsFormat.R8G8B8A8_SRGB; + _readbackDstFormat = GraphicsFormat.R8G8_UNorm; // for Unsigned 16-bit + + _camImageData = new CameraData.Image(camParameter.image.width, camParameter.image.height, pixelFormat); + } + + protected override void SetupCamera() + { + if (!camParameter.segmentation_type.Equals("semantic")) + { + Debug.Log("Only support semantic segmentation"); + } + + camSensor.backgroundColor = Color.black; + camSensor.clearFlags = CameraClearFlags.SolidColor; + camSensor.allowHDR = false; + camSensor.allowMSAA = false; + + // Refer to SegmentationRenderer (Universal Renderer Data) + _universalCamData.SetRenderer(1); + _universalCamData.renderPostProcessing = true; + _universalCamData.requiresColorOption = CameraOverrideOption.On; + _universalCamData.requiresDepthOption = CameraOverrideOption.Off; + _universalCamData.requiresColorTexture = true; + _universalCamData.requiresDepthTexture = false; + _universalCamData.renderShadows = false; + _universalCamData.dithering = false; + } + + protected override void InitializeMessages() + { + base.InitializeMessages(); + + _segmentation = new messages.Segmentation(); + _segmentation.ImageStamped = _imageStamped; + } + + protected override void GenerateMessage() + { + PushDeviceMessage(_segmentation); + } + + protected override void ImageProcessing(ref NativeArray readbackData) + { + var image = _imageStamped.Image; + _camImageData.SetTextureBufferData(readbackData); + + var imageData = _camImageData.GetImageData(image.Data.Length); + if (imageData != null) + { + image.Data = imageData; + + if (camParameter.save_enabled && _startCameraWork) + { + var saveName = name + "_" + Time.time; + _camImageData.SaveRawImageData(camParameter.save_path, saveName); + // Debug.LogFormat("{0}|{1} captured", camParameter.save_path, saveName); + } + } + else + { + Debug.LogWarning($"{name}: Failed to get image Data"); + } + + // update labels + var labelInfo = Main.SegmentationManager.GetLabelInfo(); + _segmentation.ClassMaps.Clear(); + foreach (var kv in labelInfo) + { + if (kv.Value.Count > 0 && !kv.Value[0].Hide) + { + var visionClass = new messages.VisionClass() + { + ClassName = kv.Key, + ClassId = kv.Value[0].ClassId + }; + _segmentation.ClassMaps.Add(visionClass); + } + } + + DeviceHelper.SetCurrentTime(_imageStamped.Time); + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/Devices/SegmentationCamera.cs.meta b/Assets/Scripts/Devices/SegmentationCamera.cs.meta new file mode 100644 index 00000000..148bce41 --- /dev/null +++ b/Assets/Scripts/Devices/SegmentationCamera.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b55d9fe82ae5d49d6913608370365457 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 522f56ee..e9272ba4 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -31,8 +31,8 @@ public class Main : MonoBehaviour private FollowingTargetList followingList = null; private static GameObject _core = null; - private static GameObject propsRoot = null; - private static GameObject worldRoot = null; + private static GameObject _propsRoot = null; + private static GameObject _worldRoot = null; private static GameObject lightsRoot = null; private static GameObject _roadsRoot = null; private static GameObject uiRoot = null; @@ -43,6 +43,7 @@ public class Main : MonoBehaviour private static WorldNavMeshBuilder worldNavMeshBuilder = null; private static RuntimeGizmos.TransformGizmo transformGizmo = null; private static CameraControl cameraControl = null; + private static SegmentationManager _segmentationManager = null; #region "Non-Component class" private static BridgeManager bridgeManager = null; @@ -52,8 +53,8 @@ public class Main : MonoBehaviour private static bool isResetting = false; private static bool resetTriggered = false; - public static GameObject PropsRoot => propsRoot; - public static GameObject WorldRoot => worldRoot; + public static GameObject PropsRoot => _propsRoot; + public static GameObject WorldRoot => _worldRoot; public static GameObject RoadsRoot => _roadsRoot; public static GameObject CoreObject => _core; public static GameObject UIObject => uiRoot; @@ -63,6 +64,7 @@ public class Main : MonoBehaviour public static InfoDisplay InfoDisplay => _infoDisplay; public static WorldNavMeshBuilder WorldNavMeshBuilder => worldNavMeshBuilder; public static BridgeManager BridgeManager => bridgeManager; + public static SegmentationManager SegmentationManager => _segmentationManager; public static CameraControl CameraControl => cameraControl; @@ -73,10 +75,10 @@ public class Main : MonoBehaviour private void CleanAllModels() { - foreach (var child in worldRoot.GetComponentsInChildren()) + foreach (var child in _worldRoot.GetComponentsInChildren()) { // skip root gameobject - if (child == null || child.gameObject == null || child.gameObject == worldRoot) + if (child == null || child.gameObject == null || child.gameObject == _worldRoot) { continue; } @@ -107,11 +109,11 @@ private void CleanAllResources() private void ResetRootModelsTransform() { - if (worldRoot != null) + if (_worldRoot != null) { - worldRoot.transform.localRotation = Quaternion.identity; - worldRoot.transform.localPosition = Vector3.zero; - worldRoot.transform.localScale = Vector3.one; + _worldRoot.transform.localRotation = Quaternion.identity; + _worldRoot.transform.localPosition = Vector3.zero; + _worldRoot.transform.localScale = Vector3.one; } } @@ -213,8 +215,8 @@ void Awake() Debug.LogError("Failed to Find 'Core'!!!!"); } - propsRoot = GameObject.Find("Props"); - worldRoot = GameObject.Find("World"); + _propsRoot = GameObject.Find("Props"); + _worldRoot = GameObject.Find("World"); lightsRoot = GameObject.Find("Lights"); _roadsRoot = GameObject.Find("Roads"); uiRoot = GameObject.Find("UI"); @@ -231,9 +233,9 @@ void Awake() cameraControl = mainCamera.GetComponent(); - worldNavMeshBuilder = worldRoot.GetComponent(); + worldNavMeshBuilder = _worldRoot.GetComponent(); - var simWorld = worldRoot.AddComponent(); + var simWorld = _worldRoot.AddComponent(); DeviceHelper.SetGlobalClock(simWorld.GetClock()); Main.bridgeManager = new BridgeManager(); @@ -245,6 +247,7 @@ void Awake() SensorDevices.DepthCamera.LoadComputeShader(); gameObject.AddComponent(); + _segmentationManager = gameObject.AddComponent(); } void Start() @@ -322,9 +325,9 @@ private void UpdateUIModelList() } } - private string GetClonedModelName(in string modelName) + private static string GetClonedModelName(in string modelName) { - var worldTrnasform = worldRoot.transform; + var worldTrnasform = _worldRoot.transform; var numbering = 0; var tmpModelName = modelName; for (var i = 0; i < worldTrnasform.childCount; i++) @@ -349,7 +352,7 @@ private IEnumerator LoadModel(string modelPath, string modelFileName) yield return StartCoroutine(_sdfLoader.StartImport(model)); - var targetObject = worldRoot.transform.Find(model.Name); + var targetObject = _worldRoot.transform.Find(model.Name); var addingModel = uiMainCanvasRoot.GetComponentInChildren(); addingModel.SetAddingModelForDeploy(targetObject); @@ -380,7 +383,7 @@ private IEnumerator LoadWorld() if (_sdfRoot.DoParse(out var world, worldFileName)) { _sdfLoader = new SDF.Import.Loader(); - _sdfLoader.SetRootModels(worldRoot); + _sdfLoader.SetRootModels(_worldRoot); _sdfLoader.SetRootLights(lightsRoot); _sdfLoader.SetRootRoads(_roadsRoot); @@ -441,12 +444,12 @@ public static bool TriggerResetService() void Reset() { - foreach (var helper in worldRoot.GetComponentsInChildren()) + foreach (var helper in _worldRoot.GetComponentsInChildren()) { helper.Reset(); } - foreach (var plugin in worldRoot.GetComponentsInChildren()) + foreach (var plugin in _worldRoot.GetComponentsInChildren()) { plugin.Reset(); } @@ -504,4 +507,4 @@ void OnDestroy() Assimp.Unmanaged.AssimpLibrary.Instance.FreeLibrary(); } } -} \ No newline at end of file +} diff --git a/Assets/Scripts/Tools/SDF/Helper/Base.cs b/Assets/Scripts/Tools/SDF/Helper/Base.cs index 4566788f..d14f5e0c 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Base.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Base.cs @@ -12,16 +12,16 @@ namespace Helper { public class Base : UE.MonoBehaviour { - private PoseControl poseControl = null; + private PoseControl _poseControl = null; - private bool isFirstChild = false; + private bool _isRootModel = false; - public bool IsFirstChild => isFirstChild; // root model + public bool IsFirstChild => _isRootModel; // root model protected void Awake() { - isFirstChild = SDF2Unity.IsRootModel(this.gameObject); - poseControl = new PoseControl(this.transform); + _isRootModel = SDF2Unity.IsRootModel(this.gameObject); + _poseControl = new PoseControl(this.transform); Reset(); } @@ -32,17 +32,17 @@ public void Reset() public void ClearPose() { - if (poseControl != null) + if (_poseControl != null) { - poseControl.Clear(); + _poseControl.Clear(); } } public void SetJointPoseTarget(in float targetAxis1, in float targetAxis2, in int targetFrame = 0) { - if (poseControl != null) + if (_poseControl != null) { - poseControl.SetJointTarget(targetAxis1, targetAxis2, targetFrame); + _poseControl.SetJointTarget(targetAxis1, targetAxis2, targetFrame); } } @@ -53,28 +53,28 @@ public void SetPose(in UE.Pose pose, in int targetFrame = 0) public void SetPose(in UE.Vector3 position, in UE.Quaternion rotation, in int targetFrame = 0) { - if (poseControl != null) + if (_poseControl != null) { - poseControl.Set(position, rotation, targetFrame); + _poseControl.Set(position, rotation, targetFrame); } } public void ResetPose() { - if (poseControl != null) + if (_poseControl != null) { - poseControl.Reset(); + _poseControl.Reset(); } } public UE.Pose GetPose(in int targetFrame = 0) { - return (poseControl != null) ? poseControl.Get(targetFrame) : UE.Pose.identity; + return (_poseControl != null) ? _poseControl.Get(targetFrame) : UE.Pose.identity; } public int GetPoseCount() { - return (poseControl != null) ? poseControl.Count : 0; + return (_poseControl != null) ? _poseControl.Count : 0; } } } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs index 04bd0739..44ea1da6 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs @@ -97,6 +97,44 @@ public static Device AddCamera(in SDF.Camera element, in GameObject targetObject return camera; } + public static Device AddSegmentaionCamera(in SDF.Camera element, in GameObject targetObject) + { + var newSensorObject = new GameObject(); + AttachSensor(newSensorObject, targetObject, element.Pose); + + var segmentationCamera = newSensorObject.AddComponent(); + segmentationCamera.DeviceName = GetFrameName(newSensorObject); + + switch (element.image.format) + { + case "L16": + case "L_UINT16": + // Debug.Log("Supporting data type for Depth camera"); + break; + + default: + if (element.image.format.Equals(string.Empty)) + { + Debug.LogWarningFormat("'L16' will be set for Depth camera({0})'s image format", element.name); + } + else + { + Debug.LogWarningFormat("Not supporting data type({0}) for Depth camera", element.image.format); + } + element.image.format = "L16"; + break; + } + + segmentationCamera.SetCamParameter(element); + + if (element.noise != null) + { + segmentationCamera.noise = new SensorDevices.Noise(element.noise, element.type); + } + + return segmentationCamera; + } + public static Device AddDepthCamera(in SDF.Camera element, in GameObject targetObject) { var newSensorObject = new GameObject(); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs index 8d5ca237..98c98b6b 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs @@ -74,15 +74,15 @@ protected virtual Object ImportModel(in Model model, in Object parentObject) return null; } - protected virtual System.Object ImportActor(in Actor actor) + protected virtual void AfterImportModel(in Model model, in Object targetObject) { - PrintNotImported(MethodBase.GetCurrentMethod().Name, actor.Name); - return null; + PrintNotImported(MethodBase.GetCurrentMethod().Name, model.Name); } - protected virtual void AfterImportModel(in Model model, in Object targetObject) + protected virtual System.Object ImportActor(in Actor actor) { - PrintNotImported(MethodBase.GetCurrentMethod().Name, model.Name); + PrintNotImported(MethodBase.GetCurrentMethod().Name, actor.Name); + return null; } protected virtual void ImportGeometry(in Geometry geometry, in Object parentObject) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs index 566cd74a..d8750ce6 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs @@ -24,7 +24,6 @@ private void CreateRootArticulationBody(in UE.GameObject targetObject) articulationBody = targetObject.AddComponent(); } - articulationBody.mass = 0; articulationBody.useGravity = false; articulationBody.immovable = false; @@ -109,20 +108,27 @@ protected override void AfterImportModel(in SDF.Model model, in System.Object ta var modelObject = (targetObject as UE.GameObject); var modelHelper = modelObject.GetComponent(); - if (modelHelper.IsFirstChild && !modelHelper.isStatic) + if (modelHelper.IsFirstChild) { - var childArticulationBodies = modelObject.GetComponentsInChildren(); - - if (childArticulationBodies.Length == 1 && childArticulationBodies[0].index == 0) + if (!modelHelper.isStatic) { - // remove root articulation body if there are no ariticulation body in childeren - UE.GameObject.Destroy(childArticulationBodies[0]); - modelHelper.hasRootArticulationBody = false; - } - else if (childArticulationBodies.Length > 1) - { - modelHelper.hasRootArticulationBody = true; + var childArticulationBodies = modelObject.GetComponentsInChildren(); + + if (childArticulationBodies.Length == 1 && childArticulationBodies[0].index == 0) + { + // remove root articulation body if there are no ariticulation body in childeren + UE.GameObject.Destroy(childArticulationBodies[0]); + modelHelper.hasRootArticulationBody = false; + } + else if (childArticulationBodies.Length > 1) + { + modelHelper.hasRootArticulationBody = true; + } } + + // UE.Debug.Log("AfterImportModel: " + model.OriginalName + ", " + modelObject.name); + SegmentationManager.AttachTag(model.OriginalName, modelObject); + Main.SegmentationManager.UpdateTags(); } } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Plugin.cs b/Assets/Scripts/Tools/SDF/Import/Import.Plugin.cs index c2878bcc..28bbdde3 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Plugin.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Plugin.cs @@ -16,44 +16,42 @@ public partial class Loader : Base { protected override void ImportPlugin(in SDF.Plugin plugin, in System.Object parentObject) { - var targetObject = (parentObject as UE.GameObject); - // filtering plugin name var pluginLibraryName = plugin.LibraryName(); - // Debug.Log("plugin name = " + pluginLibraryName); + // Debug.Log($"[Plugin] name={plugin.Name}"); var pluginType = Type.GetType(pluginLibraryName); - if (pluginType != null) + if (pluginType == null) + { + Debug.LogWarning($"[Plugin] No plugin({plugin.Name}) exist"); + return; + } + + var targetObject = (parentObject as UE.GameObject); + if (targetObject == null) { - if (targetObject == null) - { - Debug.LogError("[Plugin] targetObject is empty"); - return; - } + Debug.LogError("[Plugin] targetObject is empty"); + return; + } - var pluginComponent = targetObject.AddComponent(pluginType); + var pluginComponent = targetObject.AddComponent(pluginType); - var pluginObject = pluginComponent as CLOiSimPlugin; - var multiPluginObject = pluginComponent as CLOiSimMultiPlugin; + var pluginObject = pluginComponent as CLOiSimPlugin; + var multiPluginObject = pluginComponent as CLOiSimMultiPlugin; - if (pluginObject != null) - { - pluginObject.SetPluginParameters(plugin); - // Debug.Log("[Plugin] added : " + plugin.Name); - } - else if (multiPluginObject != null) - { - multiPluginObject.SetPluginParameters(plugin); - // Debug.Log("[Plugin] devices added : " + plugin.Name); - } - else - { - Debug.LogError("[Plugin] failed to add : " + plugin.Name); - } + if (multiPluginObject != null) + { + multiPluginObject.SetPluginParameters(plugin); + // Debug.Log("[Plugin] devices added : " + plugin.Name); + } + else if (pluginObject != null) + { + pluginObject.SetPluginParameters(plugin); + // Debug.Log("[Plugin] added : " + plugin.Name); } else { - Debug.LogWarningFormat("[Plugin] No plugin({0}) exist", plugin.Name); + Debug.LogError($"[Plugin] failed to add : {plugin.Name}"); } } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs index 29220ff6..0181b38a 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs @@ -55,6 +55,12 @@ protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Ob device = Implement.Sensor.AddCamera(camera, targetObject); break; + case "segmentation_camera": + case "segmentation": + var segmentationCamera = sensor.GetSensor() as SDF.Camera; + device = Implement.Sensor.AddSegmentaionCamera(segmentationCamera, targetObject); + break; + case "rgbd_camera": case "rgbd": case "multicamera": diff --git a/Assets/Scripts/Tools/SDF/Import/Import.World.Road.cs b/Assets/Scripts/Tools/SDF/Import/Import.World.Road.cs index e973ddd4..08356e3f 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.World.Road.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.World.Road.cs @@ -40,6 +40,81 @@ private string FindFile(in string currentDir, in string targetFileName) return string.Empty; } + private void ApplyOgreMaterial(in OgreMaterial.Material ogreMaterial, UE.Material material, in string uri) + { + foreach (var technique in ogreMaterial.techniques) + { + foreach (var pass in technique.passes) + { + // UE.Debug.Log($"Technique: {technique.passes.IndexOf(pass)}"); + + // foreach (var kvp in pass.properties) + // { + // UE.Debug.Log($" Pass: {kvp.Key}: {kvp.Value}"); + // } + + if (pass.properties.ContainsKey("diffuse")) + { + var diffuse = pass.properties["diffuse"]; + var diffuseColor = SDF2Unity.GetColor(diffuse); + material.SetColor("_BaseColor", diffuseColor); + + if (diffuseColor.a < 1) + { + SDF2Unity.SetMaterialTransparent(material); + } + else + { + SDF2Unity.SetMaterialOpaque(material); + } + } + else if (pass.properties.ContainsKey("emissive")) + { + var emissive = pass.properties["emissive"]; + var emissiveColor = SDF2Unity.GetColor(emissive); + material.SetColor("_EmissionColor", emissiveColor); + } + else if (pass.properties.ContainsKey("specular")) + { + var specular = pass.properties["specular"]; + var specularColor = SDF2Unity.GetColor(specular); + material.SetColor("_SpecColor", specularColor); + } + + foreach (var textureunit in pass.textureUnits) + { + // UE.Debug.Log($" TextureUnit: {pass.textureUnits.IndexOf(textureunit)}"); + + // foreach (var kvp in textureunit.properties) + // { + // UE.Debug.Log($" TextureUnit: {kvp.Key} -> {kvp.Value}"); + // } + + if (textureunit.properties.ContainsKey("texture")) + { + var textureFileName = textureunit.properties["texture"]; + var textureFilePath = FindFile(uri, textureFileName); + // UE.Debug.Log(textureFilePath); + if (!string.IsNullOrEmpty(textureFilePath)) + { + var texture = MeshLoader.GetTexture(textureFilePath); + if (texture != null) + { + var textureFiltering = textureunit.properties["filtering"]; + + // to make upper in First character + textureFiltering = textureFiltering.Remove(1).ToUpper() + textureFiltering.Substring(1); + texture.filterMode = (UE.FilterMode)Enum.Parse(typeof(UE.FilterMode), textureFiltering); + material.SetTexture("_BaseMap", texture); + } + } + } + break; + } + } + } + } + private void ImportRoad(in World.Road road) { var newRoadObject = new UE.GameObject(); @@ -58,7 +133,6 @@ private void ImportRoad(in World.Road road) } splineContainer.Spline.SetTangentMode(0, Splines.TangentMode.AutoSmooth); - var material = SDF2Unity.GetNewMaterial(road.Name + "_Material"); var targetMaterialName = road.material.script.name; @@ -67,81 +141,8 @@ private void ImportRoad(in World.Road road) var ogreMaterial = OgreMaterial.Parse(uri, targetMaterialName); if (ogreMaterial != null) { - if (ogreMaterial != null) - { - // UE.Debug.Log($"Found: {targetMaterialName} material"); - foreach (var technique in ogreMaterial.techniques) - { - foreach (var pass in technique.passes) - { - // UE.Debug.Log($"Technique: {technique.passes.IndexOf(pass)}"); - - // foreach (var kvp in pass.properties) - // { - // UE.Debug.Log($" Pass: {kvp.Key}: {kvp.Value}"); - // } - - if (pass.properties.ContainsKey("diffuse")) - { - var diffuse = pass.properties["diffuse"]; - var diffuseColor = SDF2Unity.GetColor(diffuse); - material.SetColor("_BaseColor", diffuseColor); - - if (diffuseColor.a < 1) - { - SDF2Unity.SetMaterialTransparent(material); - } - else - { - SDF2Unity.SetMaterialOpaque(material); - } - } - else if (pass.properties.ContainsKey("emissive")) - { - var emissive = pass.properties["emissive"]; - var emissiveColor = SDF2Unity.GetColor(emissive); - material.SetColor("_EmissionColor", emissiveColor); - } - else if (pass.properties.ContainsKey("specular")) - { - var specular = pass.properties["specular"]; - var specularColor = SDF2Unity.GetColor(specular); - material.SetColor("_SpecColor", specularColor); - } - - foreach (var textureunit in pass.textureUnits) - { - // UE.Debug.Log($" TextureUnit: {pass.textureUnits.IndexOf(textureunit)}"); - - // foreach (var kvp in textureunit.properties) - // { - // UE.Debug.Log($" TextureUnit: {kvp.Key} -> {kvp.Value}"); - // } - - if (textureunit.properties.ContainsKey("texture")) - { - var textureFileName = textureunit.properties["texture"]; - var textureFilePath = FindFile(uri, textureFileName); - // UE.Debug.Log(textureFilePath); - if (!string.IsNullOrEmpty(textureFilePath)) - { - var texture = MeshLoader.GetTexture(textureFilePath); - if (texture != null) - { - var textureFiltering = textureunit.properties["filtering"]; - - // to make upper in First character - textureFiltering = textureFiltering.Remove(1).ToUpper() + textureFiltering.Substring(1); - texture.filterMode = (UE.FilterMode)Enum.Parse(typeof(UE.FilterMode), textureFiltering); - material.SetTexture("_BaseMap", texture); - } - } - } - break; - } - } - } - } + // UE.Debug.Log($"Found: {targetMaterialName} material"); + ApplyOgreMaterial(ogreMaterial, material, uri); break; } } @@ -150,6 +151,10 @@ private void ImportRoad(in World.Road road) roadGenerator.Material = material; roadGenerator.LoftAllRoads(); roadGenerator.Widths.Add(new Splines.SplineData((float)road.width)); + + // UE.Debug.Log("AfterImportModel: " + model.OriginalName + ", " + modelObject.name); + SegmentationManager.AttachTag(newRoadObject.name, newRoadObject); + Main.SegmentationManager.UpdateTags(); } private void ImportRoads(IReadOnlyList items) diff --git a/Assets/Scripts/Tools/SDF/Parser/Geometry.cs b/Assets/Scripts/Tools/SDF/Parser/Geometry.cs index 5ae87db6..9c91eae6 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Geometry.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Geometry.cs @@ -144,7 +144,7 @@ protected override void ParseElements() { if (blendMinHeightList.Count > 0 && blendMinHeightList.Count == blendFadeDistList.Count) { - Console.WriteLine("Blend is not supported yet"); + Console.Write("Blend is not supported yet"); if (blendMinHeightList.Count + 1 > textureSizeList.Count) { diff --git a/Assets/Scripts/Tools/SDF/Parser/Model.cs b/Assets/Scripts/Tools/SDF/Parser/Model.cs index c1a1e776..4e2f4da5 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Model.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Model.cs @@ -6,6 +6,7 @@ using System.Collections.Generic; using System.Xml; +using System; namespace SDF { @@ -41,6 +42,11 @@ public class Model : Entity public bool IsWindEnabled => enableWind; + #region Segmentation Tag + private string originalName_ = string.Empty; + public string OriginalName => originalName_; + #endregion + public Model(XmlNode _node) : base(_node) { @@ -60,8 +66,13 @@ protected override void ParseElements() allowAutoDisable = GetValue("allow_auto_disable"); enableWind = GetValue("enable_wind"); - // Console.WriteLine("[{0}] {1} {2} {3} {4}", GetType().Name, + // Console.Write("[{0}] {1} {2} {3} {4}", GetType().Name, // isStatic, isSelfCollide, allowAutoDisable, enableWind); + + #region Segmentation Tag + originalName_ = GetAttribute("original_name"); + // Console.Write($"Model::ParseElements > [{originalName_}]"); + #endregion } public List GetModels() diff --git a/Assets/Scripts/Tools/SDF/Parser/Root.cs b/Assets/Scripts/Tools/SDF/Parser/Root.cs index 877600fa..08a66fde 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Root.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Root.cs @@ -33,6 +33,9 @@ public class Root private DebugLogWriter logger; private DebugLogWriter errLogger; + private static readonly string ProtocolModel = "model://"; + private static readonly string ProtocolFile = "file://"; + public Root() { logger = new DebugLogWriter(); @@ -95,9 +98,11 @@ public bool DoParse(out World world, in string worldFileName) public bool DoParse(out Model model, in string modelFullPath, in string modelFileName) { // Console.Write("Loading World File from SDF!!!!!"); - var modelFound = false; model = null; + var modelLocation = Path.Combine(modelFullPath, modelFileName); + var modelName = Path.GetFileName(modelFullPath); + // Console.Write(modelFullPath, modelName); try { doc.RemoveAll(); @@ -109,12 +114,15 @@ public bool DoParse(out Model model, in string modelFullPath, in string modelFil // Console.Write("Load World"); var modelNode = doc.SelectSingleNode("/sdf/model"); + + StoreOriginalModelName(doc, modelName, modelNode); + model = new Model(modelNode); - modelFound = true; - var infoMessage = model.Name + " Model(" + modelFileName + ") is loaded."; // logger.SetShowOnDisplayOnce(); - logger.Write(infoMessage); + // logger.Write($"Model({modelName}) is loaded. > {model.Name}"); + + return true; } catch (XmlException ex) { @@ -123,7 +131,7 @@ public bool DoParse(out Model model, in string modelFullPath, in string modelFil errLogger.Write(errorMessage); } - return modelFound; + return false; } #if false @@ -259,9 +267,9 @@ private void ConvertPathToAbsolutePath(in string targetElement) foreach (XmlNode node in nodeList) { var uri = node.InnerText; - if (uri.StartsWith("model://")) + if (uri.StartsWith(ProtocolModel)) { - var modelUri = uri.Replace("model://", string.Empty); + var modelUri = uri.Replace(ProtocolModel, string.Empty); var stringArray = modelUri.Split('/'); // Get Model name from Uri @@ -275,11 +283,11 @@ private void ConvertPathToAbsolutePath(in string targetElement) node.InnerText = value.Item2 + "/" + modelUri; } } - else if (uri.StartsWith("file://")) + else if (uri.StartsWith(ProtocolFile)) { foreach (var filePath in fileDefaultPaths) { - var fileUri = uri.Replace("file://", filePath + "/"); + var fileUri = uri.Replace(ProtocolFile, filePath + "/"); if (File.Exists(@fileUri)) { node.InnerText = fileUri; @@ -331,6 +339,16 @@ private void replaceAllIncludedModel() } while (nodes.Count != 0); } + #region Segmentation Tag + private void StoreOriginalModelName(XmlDocument doc, in string modelName, XmlNode targetNode) + { + // store original model's name for segmentation Tag + var newAttr = doc.CreateAttribute("original_name"); + newAttr.Value = modelName; + targetNode.Attributes.Append(newAttr); + } + #endregion + private XmlNode GetIncludedModel(XmlNode included_node) { var uri_node = included_node.SelectSingleNode("uri"); @@ -352,17 +370,16 @@ private XmlNode GetIncludedModel(XmlNode included_node) var poseNode = included_node.SelectSingleNode("pose"); var pose = (poseNode == null) ? null : poseNode.InnerText; - // var pluginNode = included_node.SelectSingleNode("plugin"); // var plugin = (pluginNode == null) ? null : pluginNode.InnerText; var uri = uri_node.InnerText; - // Console.WriteLineFormat("{0} | {1} | {2} | {3}", name, uri, pose, isStatic); + var modelName = uri.Replace(ProtocolModel, string.Empty); - var modelName = uri.Replace("model://", string.Empty); if (resourceModelTable.TryGetValue(modelName, out var value)) { uri = value.Item2 + "/" + value.Item3; + // Console.WriteLine($"{name} | {uri} | {modelName} | {pose} | {isStatic}"); } else { @@ -394,8 +411,11 @@ private XmlNode GetIncludedModel(XmlNode included_node) if (attributes.GetNamedItem("version") != null) { var modelSdfDocVersion = attributes.GetNamedItem("version").Value; + // TODO: Version check } + StoreOriginalModelName(modelSdfDoc, modelName, sdfNode); + // Edit custom parameter if (nameNode != null) { diff --git a/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs b/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs index 6fa09802..dfeb2071 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs @@ -114,6 +114,11 @@ private Camera ParseCamera(in int index = 1) } } + if (IsValidNode(cameraElement + "/segmentation_type")) + { + camera.segmentation_type = GetValue(cameraElement + "/segmentation_type", "semantic"); + } + if (IsValidNode(cameraElement + "/noise")) { camera.noise = new Noise("gaussian"); diff --git a/Assets/Scripts/Tools/SDF/Parser/Sensor.cs b/Assets/Scripts/Tools/SDF/Parser/Sensor.cs index af9c2f2c..9cab77b2 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Sensor.cs @@ -109,6 +109,8 @@ protected override void ParseElements() case "camera": case "depth_camera": case "depth": + case "segmentation_camera": + case "segmentation": if (IsValidNode("camera")) { sensor = ParseCamera(); @@ -140,7 +142,6 @@ protected override void ParseElements() case "force_torque": case "logical_camera": case "boundingbox_camera": - case "segmentation_camera": case "magnetometer": case "rfid": case "rfidtag": diff --git a/Assets/Scripts/Tools/SDF/Parser/World.cs b/Assets/Scripts/Tools/SDF/Parser/World.cs index 5d254411..4e7f0196 100644 --- a/Assets/Scripts/Tools/SDF/Parser/World.cs +++ b/Assets/Scripts/Tools/SDF/Parser/World.cs @@ -222,7 +222,7 @@ public class Distribution private Models models = null; private Actors actors = null; - private List roads_ = new List(); + private List _roads = new List(); public SphericalCoordinates spherical_coordinates = null; @@ -302,7 +302,7 @@ protected override void ParseElements() foreach (XmlNode roadNode in GetNodes("road")) { var road = new Road(roadNode); - roads_.Add(road); + _roads.Add(road); } if (IsValidNode("spherical_coordinates")) @@ -373,7 +373,7 @@ public List GetLights() public List GetRoads() { - return roads_; + return _roads; } } } \ No newline at end of file diff --git a/Packages/manifest.json b/Packages/manifest.json index 2c790555..2dd2261a 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -9,7 +9,7 @@ "com.unity.searcher": "4.9.2", "com.unity.splines": "2.5.2", "com.unity.terrain-tools": "5.0.4", - "com.unity.textmeshpro": "3.0.7", + "com.unity.textmeshpro": "3.0.8", "com.unity.toolchain.linux-x86_64": "2.0.6", "com.unity.ugui": "1.0.0", "com.unity.modules.ai": "1.0.0", diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index ec0e0e35..62f47a10 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -139,7 +139,7 @@ "url": "https://packages.unity.com" }, "com.unity.textmeshpro": { - "version": "3.0.7", + "version": "3.0.8", "depth": 0, "source": "registry", "dependencies": { diff --git a/ProjectSettings/GraphicsSettings.asset b/ProjectSettings/GraphicsSettings.asset index 1ffd1b13..957d2600 100644 --- a/ProjectSettings/GraphicsSettings.asset +++ b/ProjectSettings/GraphicsSettings.asset @@ -29,6 +29,7 @@ GraphicsSettings: m_AlwaysIncludedShaders: - {fileID: 10770, guid: 0000000000000000f000000000000000, type: 0} - {fileID: 4800000, guid: 580493fad5661e266b759ad2bd7f2c54, type: 3} + - {fileID: 4800000, guid: 419a697b1455619dea4cc9d729b3d0c8, type: 3} m_PreloadedShaders: - {fileID: 20000000, guid: 444c3b2060d68fa04a081d66c72c2066, type: 2} m_PreloadShadersBatchTimeLimit: -1 diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 1360aa96..cee3c1bb 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -139,7 +139,7 @@ PlayerSettings: loadStoreDebugModeEnabled: 0 visionOSBundleVersion: 1.0 tvOSBundleVersion: 1.0 - bundleVersion: 4.3.0 + bundleVersion: 4.4.0 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0 diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index 307ccd35..c281c052 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.19f1 -m_EditorVersionWithRevision: 2022.3.19f1 (244b723c30a6) +m_EditorVersion: 2022.3.21f1 +m_EditorVersionWithRevision: 2022.3.21f1 (bf09ca542b87) diff --git a/README.md b/README.md index 435e1d05..d62d292d 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,7 @@ Here are the list of items that is implemented(marked) or planned to be implemen - [X] Depth Camera - [X] Multi-camera - [X] RealSense (RGB + IR1 + IR2 + Depth) + - [X] Semantic Segmentation Camera - [X] GPS sensor - [ ] Sensor noise models - [X] Gaussian @@ -91,12 +92,13 @@ For example, ``` -more details in [here](https://github.com/lge-ros2/cloisim/tree/main/Assets/Scripts/CLOiSimPlugins)). +More details and usages for configuration/parameters in [here](https://github.com/lge-ros2/cloisim/tree/main/Assets/Scripts/CLOiSimPlugins). #### Model Specific - `LaserPlugin`: help to publish 2D or 3D lidar data - `CameraPlugin`: help to publish 2D color image data or depth image data +- `SegmentationCameraPlugin`: help to publish semantic segmentation image data and label info - `MultiCameraPlugin`: help to publish multiple color image data - `RealSensePlugin`: can handle ir1(left), ir2(right), depth, color - `MicomPlugin`: control micom(differential drive) input/output(sensor) @@ -143,7 +145,7 @@ if `` element of `