diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs index 3891059f..38ca46b8 100644 --- a/Assets/Scripts/Devices/Camera.cs +++ b/Assets/Scripts/Devices/Camera.cs @@ -104,7 +104,7 @@ protected virtual void SetupTexture() // Debug.Log("This is not a Depth Camera!"); targetRTname = "CameraColorTexture"; - var pixelFormat = CameraData.GetPixelFormat(camParameter.image_format); + var pixelFormat = CameraData.GetPixelFormat(camParameter.image.format); switch (pixelFormat) { case CameraData.PixelFormat.L_INT8: @@ -135,17 +135,17 @@ protected override void InitializeMessages() protected override void SetupMessages() { var image = imageStamped.Image; - var pixelFormat = CameraData.GetPixelFormat(camParameter.image_format); - image.Width = (uint)camParameter.image_width; - image.Height = (uint)camParameter.image_height; + var pixelFormat = CameraData.GetPixelFormat(camParameter.image.format); + image.Width = (uint)camParameter.image.width; + image.Height = (uint)camParameter.image.height; image.PixelFormat = (uint)pixelFormat; image.Step = image.Width * (uint)CameraData.GetImageDepth(pixelFormat); image.Data = new byte[image.Height * image.Step]; sensorInfo.HorizontalFov = camParameter.horizontal_fov; - sensorInfo.ImageSize.X = camParameter.image_width; - sensorInfo.ImageSize.Y = camParameter.image_height; - sensorInfo.ImageFormat = camParameter.image_format; + sensorInfo.ImageSize.X = camParameter.image.width; + sensorInfo.ImageSize.Y = camParameter.image.height; + sensorInfo.ImageFormat = camParameter.image.format; sensorInfo.NearClip = camParameter.clip.near; sensorInfo.FarClip = camParameter.clip.far; sensorInfo.SaveEnabled = camParameter.save_enabled; @@ -182,8 +182,8 @@ private void SetupCamera() RTHandles.SetHardwareDynamicResolutionState(true); _rtHandle = RTHandles.Alloc( - width: camParameter.image_width, - height: camParameter.image_height, + width: camParameter.image.width, + height: camParameter.image.height, slices: 1, depthBufferBits: DepthBits.None, colorFormat: targetColorFormat, @@ -214,6 +214,7 @@ private void SetupCamera() camSensor.projectionMatrix = projMatrix * invertMatrix; _universalCamData.enabled = false; + _universalCamData.stopNaN = true; _universalCamData.renderPostProcessing = false; _universalCamData.allowXRRendering = false; _universalCamData.volumeLayerMask = LayerMask.GetMask("Nothing"); @@ -227,7 +228,7 @@ private void SetupCamera() // camSensor.hideFlags |= HideFlags.NotEditable; - camImageData = new CameraData.ImageData(camParameter.image_width, camParameter.image_height, camParameter.image_format); + camImageData = new CameraData.ImageData(camParameter.image.width, camParameter.image.height, camParameter.image.format); } protected new void OnDestroy() diff --git a/Assets/Scripts/Devices/Clock.cs b/Assets/Scripts/Devices/Clock.cs index a09cbfed..04f085c2 100644 --- a/Assets/Scripts/Devices/Clock.cs +++ b/Assets/Scripts/Devices/Clock.cs @@ -88,8 +88,8 @@ protected override void InitializeMessages() void FixedUpdate() { - currentSimTime = Time.timeAsDouble - restartedSimTime; currentRealTime = Time.realtimeSinceStartupAsDouble - restartedRealTime; + currentSimTime = Time.timeAsDouble - restartedSimTime; } void LateUpdate() diff --git a/Assets/Scripts/Devices/DepthCamera.cs b/Assets/Scripts/Devices/DepthCamera.cs index 5c2b4580..688d452a 100644 --- a/Assets/Scripts/Devices/DepthCamera.cs +++ b/Assets/Scripts/Devices/DepthCamera.cs @@ -42,7 +42,6 @@ public void FlipXDepthData(in bool flip) // Debug.Log("OnDestroy(Depth Camera)"); Destroy(computeShader); computeShader = null; - Resources.UnloadAsset(ComputeShaderDepthBuffer); Resources.UnloadUnusedAssets(); @@ -75,7 +74,7 @@ protected override void SetupTexture() if (camParameter.depth_camera_output.Equals("points")) { Debug.Log("Enable Point Cloud data mode - NOT SUPPORT YET!"); - camParameter.image_format = "RGB_FLOAT32"; + camParameter.image.format = "RGB_FLOAT32"; } camSensor.backgroundColor = Color.white; @@ -91,7 +90,7 @@ protected override void SetupTexture() targetRTname = "CameraDepthTexture"; targetColorFormat = GraphicsFormat.R8G8B8A8_UNorm; - var pixelFormat = CameraData.GetPixelFormat(camParameter.image_format); + var pixelFormat = CameraData.GetPixelFormat(camParameter.image.format); switch (pixelFormat) { case CameraData.PixelFormat.L_INT16: @@ -104,18 +103,18 @@ protected override void SetupTexture() case CameraData.PixelFormat.R_FLOAT32: default: + Debug.Log("32bits depth format may cause application freezing."); readbackDstFormat = TextureFormat.RFloat; break; } var cb = new CommandBuffer(); + cb.name = "CommandBufferForDepthShading"; var tempTextureId = Shader.PropertyToID("_RenderImageCameraDepthTexture"); cb.GetTemporaryRT(tempTextureId, -1, -1); - cb.Blit(BuiltinRenderTextureType.CameraTarget, tempTextureId); cb.Blit(tempTextureId, BuiltinRenderTextureType.CameraTarget, depthMaterial); - camSensor.AddCommandBuffer(CameraEvent.AfterEverything, cb); - cb.ReleaseTemporaryRT(tempTextureId); + camSensor.AddCommandBuffer(CameraEvent.AfterEverything, cb); cb.Release(); } @@ -130,8 +129,8 @@ protected override void PostProcessing(ref byte[] buffer) computeShader.SetBuffer(kernelIndex, "_Buffer", computeBuffer); computeBuffer.SetData(buffer); - var threadGroupX = camParameter.image_width / threadGroupsX; - var threadGroupY = camParameter.image_height / threadGroupsY; + var threadGroupX = camParameter.image.width / threadGroupsX; + var threadGroupY = camParameter.image.height / threadGroupsY; computeShader.Dispatch(kernelIndex, threadGroupX, threadGroupY, 1); computeBuffer.GetData(buffer); computeBuffer.Release(); diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index 836555ff..767ffbc1 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -393,11 +393,6 @@ protected override void GenerateMessage() DeviceHelper.SetVector3d(laserScan.WorldPose.Position, lidarPosition); DeviceHelper.SetQuaternion(laserScan.WorldPose.Orientation, lidarRotation); - var srcBufferOffset = 0; - var dstBufferOffset = 0; - var copyLength = 0; - var doCopy = true; - const int BufferUnitSize = sizeof(double); var laserSamplesH = (int)horizontal.samples; var laserStartAngleH = (float)horizontal.angle.min; @@ -419,6 +414,11 @@ protected override void GenerateMessage() var dataEndAngleH = laserCamData.EndAngleH; var dividedDataTotalAngleH = 1f / laserCamData.TotalAngleH; + var srcBufferOffset = 0; + var dstBufferOffset = 0; + var copyLength = 0; + var doCopy = true; + if (srcBuffer != null) { if (laserStartAngleH < 0 && dataEndAngleH > DEG180) diff --git a/Assets/Scripts/Devices/Modules/Noise.cs b/Assets/Scripts/Devices/Modules/Noise.cs index 165e8dd1..4bbcdf27 100644 --- a/Assets/Scripts/Devices/Modules/Noise.cs +++ b/Assets/Scripts/Devices/Modules/Noise.cs @@ -63,6 +63,9 @@ public void SetNoiseModel(in string sensorType) { case "camera": case "depth": + case "depth_camera": + case "rgbd": + case "rgbd_camera": case "multicamera": case "wideanglecamera": noiseModel = new ImageGaussianNoiseModel(this.parameter); diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 92dea767..04218296 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -326,6 +326,7 @@ private IEnumerator LoadModel(string modelPath, string modelFileName) // Debug.Log("Parsed: " + item.Key + ", " + item.Value.Item1 + ", " + item.Value.Item2); model.Name = GetClonedModelName(model.Name); + yield return new WaitForEndOfFrame(); yield return StartCoroutine(sdfLoader.StartImport(model)); var targetObject = worldRoot.transform.Find(model.Name); diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs index 99b57b38..da5a6043 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs @@ -91,7 +91,7 @@ public static Device AddCamera(in SDF.Camera element, in GameObject targetObject if (element.noise != null) { - camera.noise = new SensorDevices.Noise(element.noise, "camera"); + camera.noise = new SensorDevices.Noise(element.noise, element.type); } return camera; @@ -105,34 +105,35 @@ public static Device AddDepthCamera(in SDF.Camera element, in GameObject targetO var depthCamera = newSensorObject.AddComponent(); depthCamera.DeviceName = GetFrameName(newSensorObject); - if (string.IsNullOrEmpty(element.image_format)) + switch (element.image.format) { - element.image_format = "R_FLOAT32"; - } - else - { - switch (element.image_format) - { - case "L16": - case "R_FLOAT16": - case "R_FLOAT32": - case "L_INT16": - case "L_UINT16": - // Debug.Log("Supporting data type for Depth camera"); - break; - - default: - Debug.LogWarningFormat("Not supporting data type({0}) for Depth camera", element.image_format); - element.image_format = "R_FLOAT32"; - break; - } + case "L16": + case "R_FLOAT16": + case "R_FLOAT32": + case "L_INT16": + case "L_UINT16": + // Debug.Log("Supporting data type for Depth camera"); + break; + + default: + if (element.image.format.Equals(string.Empty)) + { + Debug.LogWarning(element.name + " 'R_FLOAT16' will be set for Depth camera's image format"); + } + else + { + Debug.LogWarningFormat("Not supporting data type({0}) for Depth camera", element.image.format); + } + + element.image.format = "R_FLOAT16"; + break; } depthCamera.SetCamParameter(element); if (element.noise != null) { - depthCamera.noise = new SensorDevices.Noise(element.noise, "depthcamera"); + depthCamera.noise = new SensorDevices.Noise(element.noise, element.type); } return depthCamera; @@ -148,22 +149,11 @@ public static Device AddMultiCamera(in SDF.Cameras element, in GameObject target foreach (var camParam in element.cameras) { - var newCamObject = new GameObject(); - newCamObject.name = camParam.name; - - AttachSensor(newCamObject, newSensorObject, camParam.Pose); - - var newCam = newCamObject.AddComponent(); + var newCam = AddCamera(camParam, newSensorObject); newCam.Mode = Device.ModeType.NONE; - newCam.DeviceName = "MultiCamera::" + newCamObject.name; - newCam.SetCamParameter(camParam); + newCam.DeviceName = element.name + "::" + newCam.DeviceName; - if (camParam.noise != null) - { - newCam.noise = new SensorDevices.Noise(camParam.noise, "multicamera"); - } - - multicamera.AddCamera(newCam); + multicamera.AddCamera((SensorDevices.Camera)newCam); } return multicamera; @@ -192,40 +182,43 @@ public static Device AddImu(in SDF.IMU element, in GameObject targetObject) var imu = newSensorObject.AddComponent(); imu.DeviceName = GetFrameName(newSensorObject); - if (element.angular_velocity_x_noise != null) + if (element != null) { - imu.angular_velocity_noises["x"] = new SensorDevices.Noise(element.angular_velocity_x_noise, "imu"); - } + if (element.noise_angular_velocity.x != null) + { + imu.angular_velocity_noises["x"] = new SensorDevices.Noise(element.noise_angular_velocity.x, "imu"); + } - if (element.angular_velocity_y_noise != null) - { - imu.angular_velocity_noises["y"] = new SensorDevices.Noise(element.angular_velocity_y_noise, "imu"); - } + if (element.noise_angular_velocity.y != null) + { + imu.angular_velocity_noises["y"] = new SensorDevices.Noise(element.noise_angular_velocity.y, "imu"); + } - if (element.angular_velocity_z_noise != null) - { - imu.angular_velocity_noises["z"] = new SensorDevices.Noise(element.angular_velocity_z_noise, "imu"); - } + if (element.noise_angular_velocity.z != null) + { + imu.angular_velocity_noises["z"] = new SensorDevices.Noise(element.noise_angular_velocity.z, "imu"); + } - if (element.linear_acceleration_x_noise != null) - { - imu.linear_acceleration_noises["x"] = new SensorDevices.Noise(element.linear_acceleration_x_noise, "imu"); - } + if (element.noise_linear_acceleration.x != null) + { + imu.linear_acceleration_noises["x"] = new SensorDevices.Noise(element.noise_linear_acceleration.x, "imu"); + } - if (element.linear_acceleration_y_noise != null) - { - imu.linear_acceleration_noises["y"] = new SensorDevices.Noise(element.linear_acceleration_y_noise, "imu"); - } + if (element.noise_linear_acceleration.y != null) + { + imu.linear_acceleration_noises["y"] = new SensorDevices.Noise(element.noise_linear_acceleration.y, "imu"); + } - if (element.linear_acceleration_z_noise != null) - { - imu.linear_acceleration_noises["z"] = new SensorDevices.Noise(element.linear_acceleration_z_noise, "imu"); + if (element.noise_linear_acceleration.z != null) + { + imu.linear_acceleration_noises["z"] = new SensorDevices.Noise(element.noise_linear_acceleration.z, "imu"); + } } return imu; } - public static Device AddGps(in SDF.GPS element, in GameObject targetObject) + public static Device AddNavSat(in SDF.NavSat element, in GameObject targetObject) { var newSensorObject = new GameObject(); AttachSensor(newSensorObject, targetObject); @@ -233,24 +226,27 @@ public static Device AddGps(in SDF.GPS element, in GameObject targetObject) var gps = newSensorObject.AddComponent(); gps.DeviceName = GetFrameName(newSensorObject); - if (element.position_sensing.horizontal_noise != null) + if (element != null) { - gps.position_sensing_noises["horizontal"] = new SensorDevices.Noise(element.position_sensing.horizontal_noise, "gps"); - } + if (element.position_sensing.horizontal_noise != null) + { + gps.position_sensing_noises["horizontal"] = new SensorDevices.Noise(element.position_sensing.horizontal_noise, "gps"); + } - if (element.position_sensing.vertical_noise != null) - { - gps.position_sensing_noises["vertical"] = new SensorDevices.Noise(element.position_sensing.vertical_noise, "gps"); - } + if (element.position_sensing.vertical_noise != null) + { + gps.position_sensing_noises["vertical"] = new SensorDevices.Noise(element.position_sensing.vertical_noise, "gps"); + } - if (element.velocity_sensing.horizontal_noise != null) - { - gps.velocity_sensing_noises["horizontal"] = new SensorDevices.Noise(element.velocity_sensing.horizontal_noise, "gps"); - } + if (element.velocity_sensing.horizontal_noise != null) + { + gps.velocity_sensing_noises["horizontal"] = new SensorDevices.Noise(element.velocity_sensing.horizontal_noise, "gps"); + } - if (element.velocity_sensing.vertical_noise != null) - { - gps.velocity_sensing_noises["vertical"] = new SensorDevices.Noise(element.velocity_sensing.vertical_noise, "gps"); + if (element.velocity_sensing.vertical_noise != null) + { + gps.velocity_sensing_noises["vertical"] = new SensorDevices.Noise(element.velocity_sensing.vertical_noise, "gps"); + } } return gps; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs index ff611ab2..29220ff6 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs @@ -55,6 +55,8 @@ protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Ob device = Implement.Sensor.AddCamera(camera, targetObject); break; + case "rgbd_camera": + case "rgbd": case "multicamera": var cameras = sensor.GetSensor() as SDF.Cameras; device = Implement.Sensor.AddMultiCamera(cameras, targetObject); @@ -71,8 +73,12 @@ protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Ob break; case "gps": - var gps = sensor.GetSensor() as SDF.GPS; - device = Implement.Sensor.AddGps(gps, targetObject); + Debug.LogWarning("[SDF.Import] It is preferred to use 'navsat' since 'gps' will be deprecated."); + goto case "navsat"; + + case "navsat": + var navsat = sensor.GetSensor() as SDF.NavSat; + device = Implement.Sensor.AddNavSat(navsat, targetObject); break; case "contact": @@ -84,11 +90,10 @@ protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Ob case "altimeter": case "force_torque": case "logical_camera": + case "thermal_camera": case "magnetometer": case "rfid": case "rfidtag": - case "rgbd_camera": - case "thermal_camera": case "wireless_receiver": case "wireless_transmitter": Debug.LogWarningFormat("[Sensor] Not supported sensor name({0}) type({1})!!!!!", sensor.Name, sensorType); diff --git a/Assets/Scripts/Tools/SDF/Parser/Collision.cs b/Assets/Scripts/Tools/SDF/Parser/Collision.cs index ae30e4ba..9e1f5bc2 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Collision.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Collision.cs @@ -30,7 +30,26 @@ public class Bounce public class Friction { - // : TBD + // Description: Parameters for torsional friction + public class Torsional + { + // Description: Torsional friction coefficient, unitless maximum ratio of tangential stress to normal stress. + public double coefficient = 1; + + + // Description: If this flag is true, torsional friction is calculated using the "patch_radius" parameter. If this flag is set to false, "surface_radius" (R) and contact depth (d) are used to compute the patch radius as sqrt(R*d). + public bool use_patch_radius = true; + + // Description: Radius of contact patch surface. + public double patch_radius = 0; + + // Description: Surface radius on the point of contact. + public double surface_radius = 0; + + // Description: Torsional friction parameters for ODE + // Description: Force dependent slip for torsional friction, equivalent to inverse of viscous damping coefficient with units of rad/s/(Nm). A slip value of 0 is infinitely viscous. + public double ode_slip = 0; + } public class ODE { @@ -49,16 +68,103 @@ public class Bullet public double rolling_friction = 1; // Coefficient of rolling friction } + public Torsional torsional = null; public ODE ode = null; public Bullet bullet = null; } + public class Contact + { + public class ODE + { + // Description: Soft constraint force mixing. + public double soft_cfm = 0; + // Description: Soft error reduction parameter + public double soft_erp = 0.20000000000000001; + // Description: dynamically "stiffness"-equivalent coefficient for contact joints + public double kp = 1000000000000; + // Description: dynamically "damping"-equivalent coefficient for contact joints + public double kd = 1; + + // Description: maximum contact correction velocity truncation term. + public double max_vel = 0.01; + + // Description: minimum allowable depth before contact correction impulse is applied + public double min_depth = 0; + } + + public class Bullet + { + // Description: Soft constraint force mixing. + public double soft_cfm = 0; + + // Description: Soft error reduction parameter + public double soft_erp = 0.20000000000000001; + + // Description: dynamically "stiffness"-equivalent coefficient for contact joints + public double kp = 1000000000000; + + // Description: dynamically "damping"-equivalent coefficient for contact joints + public double kd = 1; + + // Description: Similar to ODE's max_vel implementation. See http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information. + public bool split_impulse = true; + + // Description: Similar to ODE's max_vel implementation. See http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information. + public double split_impulse_penetration_threshold = -0.01; + } + + // Description: Flag to disable contact force generation, while still allowing collision checks and contact visualization to occur. + public bool collide_without_contact = false; + + // Description: Bitmask for collision filtering when collide_without_contact is on + public uint collide_without_contact_bitmask = 1; + + // Description: Bitmask for collision filtering. This will override collide_without_contact. Parsed as 16-bit unsigned integer. + public uint collide_bitmask = 65535; + + // Description: Bitmask for category of collision filtering. Collision happens if ((category1 & collision2) | (category2 & collision1)) is not zero. If not specified, the category_bitmask should be interpreted as being the same as collide_bitmask. Parsed as 16-bit unsigned integer. + public uint category_bitmask = 65535; + + // Description: Poisson's ratio is the unitless ratio between transverse and axial strain. This value must lie between (-1, 0.5). Defaults to 0.3 for typical steel. Note typical silicone elastomers have Poisson's ratio near 0.49 ~ 0.50. For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio) for some of the typical materials are: Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41), Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50), Aluminum: (7e10 Pa, 0.32 ~ 0.35), Steel: (2e11 Pa, 0.26 ~ 0.31). + public double poissons_ratio = 0.29999999999999999; + + // Description: Young's Modulus in SI derived unit Pascal. Defaults to -1. If value is less or equal to zero, contact using elastic modulus (with Poisson's Ratio) is disabled. For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio) for some of the typical materials are: Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41), Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50), Aluminum: (7e10 Pa, 0.32 ~ 0.35), Steel: (2e11 Pa, 0.26 ~ 0.31). + public double elastic_modulus = -1; + + // Description: ODE contact parameters + public ODE ode = null; + // Description: Bullet contact parameters + public Bullet bullet = null; + } + + public class SoftContact + { + public class Dart + { + // Description: This is variable k_v in the soft contacts paper.Its unit is N/m. + public double bone_attachment = 100; + + // Description: This is variable k_e in the soft contacts paper. Its unit is N/m. + public double stiffness = 100; + + // Description: Viscous damping of point velocity in body frame. Its unit is N/m/s. + public double damping = 10; + + // Description: Fraction of mass to be distributed among deformable nodes. + public double flesh_mass_fraction = 0.050000000000000003; + } + + // Description: soft contact pamameters based on paper: http://www.cc.gatech.edu/graphics/projects/Sumit/homepage/papers/sigasia11/jain_softcontacts_siga11.pdf + public Dart dart = null; + } + public Bounce bounce = null; public Friction friction = null; - // : TBD - // : TBD + public Contact contact = null; + public SoftContact soft_contact = null; } /* @@ -67,9 +173,14 @@ public class Bullet */ public class Collision : Entity { + // Description: intensity value returned by laser sensor. private double laser_retro = 0.0; + + // Description: Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. private int max_contacts = 10; + private Geometry geometry = null; + private Surface surface = null; public Collision(XmlNode _node) diff --git a/Assets/Scripts/Tools/SDF/Parser/Entity.cs b/Assets/Scripts/Tools/SDF/Parser/Entity.cs index fc89f9ac..fb87ff01 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Entity.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Entity.cs @@ -46,7 +46,7 @@ protected void LoadData(XmlNode node) // Console.WriteLine("Num Of model nodes: " + nodeList.Count); foreach (var nodeItem in nodeList) { - //Console.WriteLine(" NAME: " + node.Attributes["name"].Value); + // Console.WriteLine(" NAME: " + node.Attributes["name"].Value); items.Add((T)Activator.CreateInstance(typeof(T), nodeItem)); } } diff --git a/Assets/Scripts/Tools/SDF/Parser/Geometry.cs b/Assets/Scripts/Tools/SDF/Parser/Geometry.cs index 6615ead1..039bbcdc 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Geometry.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Geometry.cs @@ -63,6 +63,7 @@ protected override void ParseElements() shape = new Sphere(); (shape as Sphere).radius = GetValue("sphere/radius"); } + else if (IsValidNode("cylinder")) { Type = "cylinder"; @@ -80,6 +81,7 @@ protected override void ParseElements() var size = GetValue("plane/size"); (shape as Plane).size.FromString(size); } +#region SDF 1.7 feature else if (IsValidNode("capsule")) { Type = "capsule"; @@ -87,6 +89,7 @@ protected override void ParseElements() (shape as Capsule).radius = GetValue("capsule/radius"); (shape as Capsule).length = GetValue("capsule/length"); } +#endregion else if (IsValidNode("ellipsoid")) { Type = "ellipsoid"; diff --git a/Assets/Scripts/Tools/SDF/Parser/Joint.cs b/Assets/Scripts/Tools/SDF/Parser/Joint.cs index d196082d..84218c75 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Joint.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Joint.cs @@ -57,10 +57,57 @@ public class Joint : Entity { public class Physics { + public class SimBody + { + // Description: Force cut in the multibody graph at this joint. + public bool must_be_loop_joint = false; + } + public class ODE { + // Description: If cfm damping is set to true, ODE will use CFM to simulate damping, allows for infinite damping, and one additional constraint row (previously used for joint limit) is always active. + public bool cfm_damping = false; + + // Description: If implicit_spring_damper is set to true, ODE will use CFM, ERP to simulate stiffness and damping, allows for infinite damping, and one additional constraint row (previously used for joint limit) is always active. This replaces cfm_damping parameter in SDFormat 1.4. + public bool implicit_spring_damper = false; + + // Description: Scale the excess for in a joint motor at joint limits. Should be between zero and one. + public double fudge_factor = 0; + + // Description: Constraint force mixing for constrained directions + public double cfm = 0; + // Description: Error reduction parameter for constrained directions + public double erp = 0.20000000000000001; + + // Description: Bounciness of the limits + public double bounce = 0; + + // Description: Maximum force or torque used to reach the desired velocity. public double max_force = 0; + + // Description: The desired velocity of the joint. Should only be set if you want the joint to move on load. + public double velocity = 0; + + // Description: Constraint force mixing parameter used by the joint stop + public double limit_cfm = 0; + + // Description: Error reduction parameter used by the joint stop + public double limit_erp = 0.20000000000000001; + + // Description: Suspension constraint force mixing parameter + public double suspension_cfm = 0; + // Description: Suspension error reduction parameter + public double suspension_erp = 0.20000000000000001; } + + // Description: Simbody specific parameters + public SimBody simbody = null; + + // Description: ODE specific parameters + public ODE ode = null; + + // Description: If provide feedback is set to true, physics engine will compute the constraint forces at this joint. + public bool provide_feedback = false; } private string parent = string.Empty; @@ -73,10 +120,9 @@ public class ODE private Axis axis = null; // for revolute/prismatic joints private Axis axis2 = null; // for revolute2(second axis)/universal joints - // : TBD - private Physics.ODE ode = null; + private Physics physics = new Physics(); - // : TBD, ??? + private Sensors sensors = null; public string ParentLinkName => parent; @@ -85,7 +131,7 @@ public class ODE public Axis Axis => axis; public Axis Axis2 => axis2; - public Physics.ODE PhysicsODE => ode; + public Physics.ODE PhysicsODE => physics.ode; public Joint(XmlNode _node) : base(_node) @@ -197,11 +243,11 @@ protected override void ParseElements() if (IsValidNode("physics/ode")) { - ode = new Physics.ODE(); + physics.ode = new Physics.ODE(); if (IsValidNode("physics/ode/max_force")) { - ode.max_force = GetValue("physics/ode/max_force"); + physics.ode.max_force = GetValue("physics/ode/max_force"); } } break; diff --git a/Assets/Scripts/Tools/SDF/Parser/Material.cs b/Assets/Scripts/Tools/SDF/Parser/Material.cs index 35f10a40..dc2b167a 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Material.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Material.cs @@ -56,17 +56,119 @@ public class Script public string name; } + public class Shader + { + // Description: vertex, pixel, normal_map_object_space, normal_map_tangent_space + public string type = "pixel"; + + // Description: filename of the normal map + public string normal_map = "__default__"; + } + + // Description: Physically Based Rendering (PBR) material. There are two PBR workflows: metal and specular. While both workflows and their parameters can be specified at the same time, typically only one of them will be used (depending on the underlying renderer capability). It is also recommended to use the same workflow for all materials in the world. + public class PBR + { + // Description: PBR using the Metallic/Roughness workflow. + public class Metal + { + // Description: Filename of the diffuse/albedo map. + public string albedo_map = string.Empty; + + // Description: Filename of the roughness map. + public string roughness_map = string.Empty; + + // Description: Material roughness in the range of [0,1], where 0 represents a smooth surface and 1 represents a rough surface. This is the inverse of a specular map in a PBR specular workflow. + public string roughness = "0.5"; + + // Description: Filename of the metalness map. + public string metalness_map = string.Empty; + + // Description: Material metalness in the range of [0,1], where 0 represents non-metal and 1 represents raw metal + public string metalness = "0.5"; + + // Description: Filename of the environment / reflection map, typically in the form of a cubemap + public string environment_map = string.Empty; + + // Description: Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. + public string ambient_occlusion_map = string.Empty; + + // Description: Filename of the normal map. The normals can be in the object space or tangent space as specified in the 'type' attribute + public string normal_map = string.Empty; + + // Description: The space that the normals are in. Values are: 'object' or 'tangent' + public string normal_map_type = "tangent"; + + // Description: Filename of the emissive map. + public string emissive_map = string.Empty; + + // Description: Filename of the light map. The light map is a prebaked light texture that is applied over the albedo map + public string light_map = string.Empty; + + // Description: Index of the texture coordinate set to use. + public uint light_map_uv_set = 0; + } + + // Description: PBR using the Specular/Glossiness workflow. + public class Specular + { + // Description: Filename of the diffuse/albedo map. + public string albedo_map = string.Empty; + + // Description: Filename of the specular map. + public string specular_map = string.Empty; + + // Description: Filename of the glossiness map. + public string glossiness_map = string.Empty; + + // Description: Material glossiness in the range of [0-1], where 0 represents a rough surface and 1 represents a smooth surface. This is the inverse of a roughness map in a PBR metal workflow. + public string glossiness = string.Empty; + + // Description: Filename of the environment / reflection map, typically in the form of a cubemap + public string environment_map = string.Empty; + + // Description: Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. + public string ambient_occlusion_map = string.Empty; + + // Description: Filename of the normal map. The normals can be in the object space or tangent space as specified in the 'type' attribute + public string normal_map = string.Empty; + + // Description: The space that the normals are in. Values are: 'object' or 'tangent' + public string normal_map_type = "tangent"; + + // Description: Filename of the emissive map. + public string emissive_map = string.Empty; + + // Description: Filename of the light map. The light map is a prebaked light texture that is applied over the albedo map + public string light_map = string.Empty; + + // Description: Index of the texture coordinate set to use. + public uint light_map_uv_set = 0; + } + + public Metal metal = null; + + public Specular specular = null; + } + public Script script = null; - // : TBD - // : TBD - // : TBD + public Shader shader = null; + + // Description: Set render order for coplanar polygons. The higher value will be rendered on top of the other coplanar polygons + public float render_order = 0; + + // Description: If false, dynamic lighting will be disabled + public bool lighting = true; + public Color ambient = null; public Color diffuse = null; public Color specular = null; public Color emissive = null; - // : TBD - // : TBD + + // Description: If true, the mesh that this material is applied to will be rendered as double sided + public bool double_sided = false; + + public PBR pbr = null; public Material(XmlNode _node) : base(_node) diff --git a/Assets/Scripts/Tools/SDF/Parser/Physics.cs b/Assets/Scripts/Tools/SDF/Parser/Physics.cs index 224b0732..679916e8 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Physics.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Physics.cs @@ -10,20 +10,20 @@ namespace SDF { public class Physics : Entity { + // Description: If true, this physics element is set as the default physics profile for the world. If multiple default physics elements exist, the first element marked as default is chosen. If no default physics element exists, the first physics element is chosen. + public bool _default = false; + // : attribute will be ignored. private double max_step_size = 0.001; private double real_time_factor = 1.0; private double real_time_update_rate = 1000.0; private int max_contacts = 20; - // // : TBD // : TBD // : TBD - // - solver - // - constraints + // : TBD // : TBD - I want to propose a new element in SDFormat specification - // public Physics(XmlNode _node) : base(_node) diff --git a/Assets/Scripts/Tools/SDF/Parser/Plugin.cs b/Assets/Scripts/Tools/SDF/Parser/Plugin.cs index 80078c80..73d82748 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Plugin.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Plugin.cs @@ -19,8 +19,6 @@ public class Plugin : Entity { private string filename = "__default__"; - public string FileName => filename; - public Plugin(XmlNode _node) : base(_node) { @@ -33,7 +31,7 @@ protected override void ParseElements() public string LibraryName() { - var pluginName = FileName; + var pluginName = filename; if (pluginName.StartsWith("lib")) { pluginName = pluginName.Substring(3); diff --git a/Assets/Scripts/Tools/SDF/Parser/Pose.cs b/Assets/Scripts/Tools/SDF/Parser/Pose.cs index c478bc2a..002f8796 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Pose.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Pose.cs @@ -5,14 +5,15 @@ */ using System.Text.RegularExpressions; +using System.Globalization; using System; namespace SDF { public class Vector2 { - private static string regex_num_pattern = "[^.0-9e-]"; - public static readonly Regex regex_num = new Regex(regex_num_pattern); + private static string regex_num_pattern = "[^.0-9Ee+-]"; + protected static readonly Regex regex_num = new Regex(regex_num_pattern); private T _x; private T _y; @@ -44,6 +45,7 @@ public void Set(T x, T y) { _x = x; _y = y; + // Console.WriteLine(typeof(T) + "::" + _x + ", " + _y); } public void Set(string x, string y) @@ -53,7 +55,10 @@ public void Set(string x, string y) { x = regex_num.Replace(x, string.Empty); y = regex_num.Replace(y, string.Empty); - Set((T)Convert.ChangeType(x, code), (T)Convert.ChangeType(y, code)); + var parsed_x = double.Parse(x, NumberStyles.Float); + var parsed_y = double.Parse(y, NumberStyles.Float); + // Console.WriteLine(typeof(T) + "::" + parsed_x + ", " + parsed_y); + Set((T)Convert.ChangeType(parsed_x, code), (T)Convert.ChangeType(parsed_y, code)); } } @@ -72,9 +77,8 @@ public void FromString(in string value) } } - public class Vector3 + public class Vector3 : Vector2 { - private Vector2 _xy = new Vector2(); private T _z; public Vector3() @@ -87,18 +91,6 @@ public Vector3(T x, T y, T z) Set(x, y, z); } - public T X - { - get => _xy.X; - set => _xy.X = value; - } - - public T Y - { - get => _xy.Y; - set => _xy.Y = value; - } - public T Z { get => _z; @@ -107,23 +99,26 @@ public T Z public void Set(T x, T y, T z) { - _xy.Set(x, y); + base.Set(x, y); _z = z; + // Console.WriteLine(typeof(T) + "::" + _z); } public void Set(string x, string y, string z) { + base.Set(x, y); + var code = Type.GetTypeCode(typeof(T)); - if (code != TypeCode.Empty) + if (!code.Equals(TypeCode.Empty)) { - x = Vector2.regex_num.Replace(x, string.Empty); - y = Vector2.regex_num.Replace(y, string.Empty); - z = Vector2.regex_num.Replace(z, string.Empty); - Set((T)Convert.ChangeType(x, code), (T)Convert.ChangeType(y, code), (T)Convert.ChangeType(z, code)); + z = regex_num.Replace(z, string.Empty); + var parsed_z = double.Parse(z, NumberStyles.Float); + // Console.WriteLine(typeof(T) + "::" + parsed_z); + _z = (T)Convert.ChangeType(parsed_z, code); } } - public void FromString(in string value) + new public void FromString(in string value) { if (string.IsNullOrEmpty(value)) { @@ -361,7 +356,18 @@ public class Pose private Vector3 _pos; private Quaternion _rot; - public string relative_to = string.Empty; // TBD : for SDF 1.7 + public string relative_to = string.Empty; // TBD : since SDF 1.7 + +#region SDF 1.9 feature + // Description: 'euler_rpy' by default. + // Supported rotation formats are 'euler_rpy', Euler angles representation in roll, pitch, yaw. + // The pose is expected to have 6 values. + // 'quat_xyzw', Quaternion representation in x, y, z, w. The pose is expected to have 7 values. + public string rotation_format = "euler_rpy"; + + // Description: Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default. + public bool degrees = false; +#endregion public Pose() : this(default(T), default(T), default(T)) diff --git a/Assets/Scripts/Tools/SDF/Parser/Root.cs b/Assets/Scripts/Tools/SDF/Parser/Root.cs index 223439c5..23b8404a 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Root.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Root.cs @@ -217,7 +217,7 @@ public void UpdateResourceModelTable() // Console.WriteLine(modelName + ", " + modelValue); if (resourceModelTable.ContainsKey(modelName)) { - failedModelTableList.AppendLine(""); + failedModelTableList.AppendLine(string.Empty); failedModelTableList.Append(String.Concat(modelName, " => Cannot register", modelValue)); numberOfFailedModelTable++; } @@ -317,28 +317,29 @@ private void replaceAllIncludedModel() private XmlNode GetIncludedModel(XmlNode included_node) { + var uri_node = included_node.SelectSingleNode("uri"); + if (uri_node == null) + { + Console.WriteLine("uri is empty."); + return null; + } + var nameNode = included_node.SelectSingleNode("name"); var name = (nameNode == null) ? null : nameNode.InnerText; + var staticNode = included_node.SelectSingleNode("static"); + var isStatic = (staticNode == null) ? null : staticNode.InnerText; + + var placementFrameNode = included_node.SelectSingleNode("placement_frame"); + var placementFrame = (placementFrameNode == null) ? null : placementFrameNode.InnerText; + var poseNode = included_node.SelectSingleNode("pose"); var pose = (poseNode == null) ? null : poseNode.InnerText; - // var placementFrameNode = included_node.SelectSingleNode("placement_frame"); - // var placementFrame = (placementFrameNode == null) ? null : placementFrameNode.InnerText; // var pluginNode = included_node.SelectSingleNode("plugin"); // var plugin = (pluginNode == null) ? null : pluginNode.InnerText; - var staticNode = included_node.SelectSingleNode("static"); - var isStatic = (staticNode == null) ? null : staticNode.InnerText; - - var uri_node = included_node.SelectSingleNode("uri"); - if (uri_node == null) - { - Console.WriteLine("uri is empty."); - return null; - } - var uri = uri_node.InnerText; // Console.WriteLineFormat("{0} | {1} | {2} | {3}", name, uri, pose, isStatic); diff --git a/Assets/Scripts/Tools/SDF/Parser/Scene.cs b/Assets/Scripts/Tools/SDF/Parser/Scene.cs index 86bd58e0..76cb92a5 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Scene.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Scene.cs @@ -8,21 +8,63 @@ namespace SDF { - public class Scene + public class Scene : Entity { - private XmlNode root = null; + public class Sky + { + public class Clouds + { + // Description: Speed of the clouds + public double speed = 0.59999999999999998; + + // Description: Direction of the cloud movement + public double direction = 0; + + // Description: Density of clouds + public double humidity = 0.5; + + // Description: Average size of the clouds + public double mean_size = 0.5; + + // Description: Ambient cloud color + public Color ambient = new Color(0.8, 0.8, 0.8, 1); + } + public double time = 10; + public double sunrise = 6; + public double sunset = 20; + + public Clouds clouds = null; + } + + public class Fog + { + // Description: Fog color + public Color color = new Color(1, 1, 1, 1); + + // Description: Fog type: constant, linear, quadratic + public string type = "none"; + + // Description: Distance to start of fog + public double start = 1; + + // Description: Distance to end of fog + public double end = 100; + + // Description: Density of fog + public double density = 1; + } - // : TBD - // : TBD - // : TBD - // : TBD - // : TBD - // : TBD - // : TBD + public Color ambient = new Color(0.4, 0.4, 0.4, 1); + public Color background = new Color(0.7, 0.7, 0.7, 1); + public Sky sky = null; + public bool shadows = true; + public Fog fog = null; + public bool grid = true; + public bool origin_visual = true; public Scene(XmlNode _node) + : base(_node) { - root = _node; } } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs b/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs index 6e8e71e3..2d47e228 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Sensor.Parse.cs @@ -79,9 +79,9 @@ private Camera ParseCamera(in int index = 1) camera.type = Type; camera.horizontal_fov = GetValue(cameraElement + "/horizontal_fov"); - camera.image_width = GetValue(cameraElement + "/image/width"); - camera.image_height = GetValue(cameraElement + "/image/height"); - camera.image_format = GetValue(cameraElement + "/image/format"); + camera.image.width = GetValue(cameraElement + "/image/width", camera.image.width); + camera.image.height = GetValue(cameraElement + "/image/height", camera.image.height); + camera.image.format = GetValue(cameraElement + "/image/format", string.Empty); if (IsValidNode(cameraElement + "/clip/near")) { @@ -195,27 +195,27 @@ private IMU ParseIMU() if (IsValidNode("imu/orientation_reference_frame")) { - Console.WriteLine("Not supported imue orientation_reference_frame!!!"); + Console.WriteLine("Not supported imu orientation_reference_frame!!!"); } if (IsValidNode("imu/angular_velocity")) { if (IsValidNode("imu/angular_velocity/x")) { - imu.angular_velocity_x_noise = new Noise(); - ParseNoise(ref imu.angular_velocity_x_noise, "imu/angular_velocity/x"); + imu.noise_angular_velocity.x = new Noise(); + ParseNoise(ref imu.noise_angular_velocity.x, "imu/angular_velocity/x"); } if (IsValidNode("imu/angular_velocity/y")) { - imu.angular_velocity_y_noise = new Noise(); - ParseNoise(ref imu.angular_velocity_y_noise, "imu/angular_velocity/y"); + imu.noise_angular_velocity.y = new Noise(); + ParseNoise(ref imu.noise_angular_velocity.y, "imu/angular_velocity/y"); } if (IsValidNode("imu/angular_velocity/z")) { - imu.angular_velocity_z_noise = new Noise(); - ParseNoise(ref imu.angular_velocity_z_noise, "imu/angular_velocity/z"); + imu.noise_angular_velocity.z = new Noise(); + ParseNoise(ref imu.noise_angular_velocity.z, "imu/angular_velocity/z"); } } @@ -223,65 +223,65 @@ private IMU ParseIMU() { if (IsValidNode("imu/linear_acceleration/x")) { - imu.linear_acceleration_x_noise = new Noise(); - ParseNoise(ref imu.linear_acceleration_x_noise, "imu/linear_acceleration/x"); + imu.noise_linear_acceleration.x = new Noise(); + ParseNoise(ref imu.noise_linear_acceleration.x, "imu/linear_acceleration/x"); } if (IsValidNode("imu/linear_acceleration/y")) { - imu.linear_acceleration_y_noise = new Noise(); - ParseNoise(ref imu.linear_acceleration_y_noise, "imu/linear_acceleration/y"); + imu.noise_linear_acceleration.y = new Noise(); + ParseNoise(ref imu.noise_linear_acceleration.y, "imu/linear_acceleration/y"); } if (IsValidNode("imu/linear_acceleration/z")) { - imu.linear_acceleration_z_noise = new Noise(); - ParseNoise(ref imu.linear_acceleration_z_noise, "imu/linear_acceleration/z"); + imu.noise_linear_acceleration.z = new Noise(); + ParseNoise(ref imu.noise_linear_acceleration.z, "imu/linear_acceleration/z"); } } return imu; } - private GPS ParseGPS() + private NavSat ParseNavSat(in string element_path = "navsat") { - var gps = new GPS(); + var navsat = new NavSat(); - if (IsValidNode("gps/position_sensing")) + if (IsValidNode(element_path + "/position_sensing")) { - gps.position_sensing = new GPS.SensingNoise(); + navsat.position_sensing = new NavSat.SensingNoise(); - if (IsValidNode("gps/position_sensing/horizontal/noise")) + if (IsValidNode(element_path + "/position_sensing/horizontal/noise")) { - gps.position_sensing.horizontal_noise = new Noise(); - ParseNoise(ref gps.position_sensing.horizontal_noise, "gps/position_sensing/horizontal"); + navsat.position_sensing.horizontal_noise = new Noise(); + ParseNoise(ref navsat.position_sensing.horizontal_noise, element_path + "/position_sensing/horizontal"); } - if (IsValidNode("gps/position_sensing/vertical/noise")) + if (IsValidNode(element_path + "/position_sensing/vertical/noise")) { - gps.position_sensing.vertical_noise = new Noise(); - ParseNoise(ref gps.position_sensing.vertical_noise, "gps/position_sensing/vertical"); + navsat.position_sensing.vertical_noise = new Noise(); + ParseNoise(ref navsat.position_sensing.vertical_noise, element_path + "/position_sensing/vertical"); } } - if (IsValidNode("gps/velocity_sensing")) + if (IsValidNode(element_path + "/velocity_sensing")) { - gps.velocity_sensing = new GPS.SensingNoise(); + navsat.velocity_sensing = new NavSat.SensingNoise(); - if (IsValidNode("gps/velocity_sensing/horizontal/noise")) + if (IsValidNode(element_path + "/velocity_sensing/horizontal/noise")) { - gps.velocity_sensing.horizontal_noise = new Noise(); - ParseNoise(ref gps.velocity_sensing.horizontal_noise, "gps/velocity_sensing/horizontal"); + navsat.velocity_sensing.horizontal_noise = new Noise(); + ParseNoise(ref navsat.velocity_sensing.horizontal_noise, element_path + "/velocity_sensing/horizontal"); } - if (IsValidNode("gps/velocity_sensing/vertical/noise")) + if (IsValidNode(element_path + "/velocity_sensing/vertical/noise")) { - gps.velocity_sensing.vertical_noise = new Noise(); - ParseNoise(ref gps.velocity_sensing.vertical_noise, "gps/velocity_sensing/vertical"); + navsat.velocity_sensing.vertical_noise = new Noise(); + ParseNoise(ref navsat.velocity_sensing.vertical_noise, element_path + "/velocity_sensing/vertical"); } } - return gps; + return navsat; } private Contact ParseContact() diff --git a/Assets/Scripts/Tools/SDF/Parser/Sensor.cs b/Assets/Scripts/Tools/SDF/Parser/Sensor.cs index 232fb1cc..50de96fb 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Sensor.cs @@ -19,11 +19,21 @@ public Sensors(XmlNode _node) : base(_node, TARGET_TAG) { } public partial class Sensor : Entity { + // Description: If true the sensor will always be updated according to the update rate. private bool always_on = false; + + // Description: The frequency at which the sensor data is generated. If left unspecified, the sensor will generate data every cycle. private double update_rate = 0.0; + + // Description: If true, the sensor is visualized in the GUI private bool visualize = false; + + // Description: Name of the topic on which data is published. This is necessary for visualization private string topic = "__default__"; + // Description: If true, the sensor will publish performance metrics + private bool enable_metrics = false; + private SensorType sensor = null; private Plugins plugins = null; @@ -77,19 +87,20 @@ protected override void ParseElements() } break; + case "rgbd_camera": + case "rgbd": case "multicamera": if (IsValidNode("camera")) { var cameras = new Cameras(); - cameras.name = "multiple_camera"; + cameras.name = Type; + cameras.type = Type; var nodes = GetNodes("camera"); // Console.WriteLine("totalCamera: " + nodes.Count); for (var index = 1; index <= nodes.Count; index++) - { cameras.Add(ParseCamera(index)); - } sensor = cameras; } @@ -112,37 +123,32 @@ protected override void ParseElements() break; case "imu": - if (IsValidNode("imu")) - { - sensor = ParseIMU(); - } + sensor = ParseIMU(); break; case "gps": - if (IsValidNode("gps")) - { - sensor = ParseGPS(); - } + case "navsat": + sensor = ParseNavSat(Type); break; case "contact": - if (IsValidNode("contact")) - { - sensor = ParseContact(); - } + sensor = ParseContact(); break; case "air_pressure": case "altimeter": case "force_torque": case "logical_camera": + case "boundingbox_camera": + case "segmentation_camera": case "magnetometer": case "rfid": case "rfidtag": - case "rgbd_camera": case "thermal_camera": + case "thermal": case "wireless_receiver": case "wireless_transmitter": + case "custom": Console.WriteLine("Not supported sensor type!!!!! => " + Type); break; @@ -160,7 +166,7 @@ protected override void ParseElements() } catch { - Console.WriteLine("sensor was not created!"); + Console.WriteLine("Sensor {0}::{1} was NOT created!", Name, Type); } plugins = new Plugins(root); diff --git a/Assets/Scripts/Tools/SDF/Parser/SensorTypes.cs b/Assets/Scripts/Tools/SDF/Parser/SensorTypes.cs index 7286c6d0..74da40ed 100644 --- a/Assets/Scripts/Tools/SDF/Parser/SensorTypes.cs +++ b/Assets/Scripts/Tools/SDF/Parser/SensorTypes.cs @@ -21,17 +21,50 @@ public Noise(in string type = "none") this.type = type; } + // Description: For type "gaussian*", the mean of the Gaussian distribution from which noise values are drawn. public string type; + + // Description: For type "gaussian*", the standard deviation of the Gaussian distribution from which noise values are drawn. public double mean = 0; + + // Description: For type "gaussian*", the mean of the Gaussian distribution from which bias values are drawn. public double stddev = 0; + + // Description: For type "gaussian*", the standard deviation of the Gaussian distribution from which bias values are drawn. public double bias_mean = 0; + + // Description: For type "gaussian*", the standard deviation of the noise used to drive a process to model slow variations in a sensor bias. public double bias_stddev = 0; + + // Description: The type of noise. Currently supported types are: "none" (no noise). "gaussian" (draw noise values independently for each measurement from a Gaussian distribution). "gaussian_quantized" ("gaussian" plus quantization of outputs (ie. rounding)) public double dynamic_bias_stddev = 0; + + // Description: For type "gaussian*", the correlation time in seconds of the noise used to drive a process to model slow variations in a sensor bias. A typical value, when used, would be on the order of 3600 seconds (1 hour). public double dynamic_bias_correlation_time = 0; + + // Description: For type "gaussian_quantized", the precision of output signals. A value of zero implies infinite precision / no quantization. public double precision = double.NaN; } - // : TBD + // Description: These elements are specific to an air pressure sensor. + public class AirPressure + { + // Description: The initial altitude in meters. This value can be used by a sensor implementation to augment the altitude of the sensor. For example, if you are using simulation instead of creating a 1000 m mountain model on which to place your sensor, you could instead set this value to 1000 and place your model on a ground plane with a Z height of zero. + public double reference_altitude = 0; + + // Description: Noise parameters for the pressure data. + public Noise pressure_noise = null; + } + + // Description: These elements are specific to an altimeter sensor. + public class Altimeter + { + // Description: Noise parameters for vertical position + public Noise vertical_position_noise = null; + + // Description: Noise parameters for vertical velocity + public Noise vertical_velocity_noise = null; + } public class Cameras : SensorType { @@ -45,8 +78,6 @@ public void Add(in Camera item) public class Camera : SensorType { - protected Pose pose = new Pose(); - public Pose Pose => pose; public void ParsePose(in string poseString, in string relativeTo = "") @@ -119,11 +150,20 @@ public class Intrinsics public Intrinsics intrinsics = new Intrinsics(); } + // Description: The image size in pixels and format. + public class Image + { + // Description: Width in pixels + public int width = 320; + // Description: Height in pixels + public int height = 240; + // Description: (L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + public string format = "R8G8B8"; + } + public double horizontal_fov = 1.047f; - public int image_width = 320; - public int image_height = 240; - public string image_format = "R8G8B8"; + public Image image = new Image(); public Clip clip = new Clip(); @@ -133,11 +173,24 @@ public class Intrinsics public string depth_camera_output = string.Empty; public Clip depth_camera_clip = new Clip(); + #region SDF 1.9 feature + // Description: The segmentation type of the segmentation camera. Valid options are: - semantic: Semantic segmentation, which provides 2 images: 1. A grayscale image, with the pixel values representing the label of an object 2. A colored image, with the pixel values being a unique color for each label - panoptic | instance: Panoptic segmentation, which provides an image where each pixel has 1 channel for label value of the object and 2 channels for the number of the instances of that label, and a colored image which its pixels have a unique color for each instance. + public string segmentation_type = "semantic"; + + // Description: The boundingbox type of the boundingbox camera. Valid options are: - 2d | visible_2d | visible_box_2d: a visible 2d box mode which provides axis aligned 2d boxes on the visible parts of the objects - full_2d | full_box_2d: a full 2d box mode which provides axis aligned 2d boxes that fills the object dimentions, even if it has an occluded part - 3d: a 3d mode which provides oriented 3d boxes + public string box_type = "2d"; + #endregion + public Noise noise = null; public Distortion distortion = null; public Lens lens = null; + + // Description: Visibility mask of a camera. When (camera's visibility_mask & visual's visibility_flags) evaluates to non-zero, the visual will be visible to the camera. + public uint visibility_mask = 4294967295; + + protected Pose pose = new Pose(); } public class Contact : SensorType @@ -146,7 +199,8 @@ public class Contact : SensorType public string topic; } - public class GPS : SensorType + // same as + public class NavSat : SensorType { public class SensingNoise { @@ -170,18 +224,35 @@ public class OrientationReferenceFrame public string grav_dir_x_parent_frame; } + public class NoiseDirection + { + public Noise x = null; + public Noise y = null; + public Noise z = null; + } + public OrientationReferenceFrame orientation_reference_frame = new OrientationReferenceFrame(); - public Noise angular_velocity_x_noise = null; - public Noise angular_velocity_y_noise = null; - public Noise angular_velocity_z_noise = null; - public Noise linear_acceleration_x_noise = null; - public Noise linear_acceleration_y_noise = null; - public Noise linear_acceleration_z_noise = null; + public NoiseDirection noise_angular_velocity = new NoiseDirection(); + public NoiseDirection noise_linear_acceleration = new NoiseDirection(); + + // Descripotion: Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation. + public bool enable_orientation = true; } - // : TBD + // Description: These elements are specific to logical camera sensors. A logical camera reports objects that fall within a frustum. Computation should be performed on the CPU. + public class LogicalCamera : SensorType + { + // Description: Near clipping distance of the view frustum + public double near = 0; + // Description: Far clipping distance of the view frustum + public double far = 1; + // Description: Aspect ratio of the near and far planes. This is the width divided by the height of the near or far planes. + public double aspect_ratio = 1; + // Description: Horizontal field of view of the frustum, in radians. This is the angle between the frustum's vertex and the edges of the near or far plane. + public double horizontal_fov = 1; + } public class Magnetometer : SensorType { @@ -190,6 +261,7 @@ public class Magnetometer : SensorType public Noise z = null; } + // same as public class Lidar : SensorType { public class Scan @@ -217,8 +289,13 @@ public class Range public Noise noise = null; } - // : TBD - // : TBD + public class RfidTag : SensorType + { + } + + public class Rfid: SensorType + { + } public class Sonar: SensorType { @@ -228,6 +305,39 @@ public class Sonar: SensorType public double radius = 0.5f; } - // : TBD - // : TBD + // Description: These elements are specific to a wireless transceiver. + public class transceiver : SensorType + { + + // Description: Service set identifier (network name) + public string essid = "wireless"; + + // Description: Specifies the frequency of transmission in MHz + public double frequency = 2442; + + // Description: Only a frequency range is filtered. Here we set the lower bound (MHz). + public double min_frequency = 2412; + + // Description: Only a frequency range is filtered. Here we set the upper bound (MHz). + public double max_frequency = 2484; + + // Description: Specifies the antenna gain in dBi + public double gain = 2.5; + + // Description: Specifies the transmission power in dBm + public double power = 14.5; + + // Description: Mininum received signal power in dBm + public double sensitivity = -90; + } + + // Description: These elements are specific to the force torque sensor. + public class ForceTorque + { + // Description: Frame in which to report the wrench values. Currently supported frames are: "parent" report the wrench expressed in the orientation of the parent link frame, "child" report the wrench expressed in the orientation of the child link frame, "sensor" report the wrench expressed in the orientation of the joint sensor frame. Note that for each option the point with respect to which the torque component of the wrench is expressed is the joint origin. + public string frame = "child"; + + // Description: Direction of the wrench measured by the sensor. The supported options are: "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. + public string measure_direction = "child_to_parent"; + } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Parser/ShapeTypes.cs b/Assets/Scripts/Tools/SDF/Parser/ShapeTypes.cs index 9744820b..3ec25ac0 100644 --- a/Assets/Scripts/Tools/SDF/Parser/ShapeTypes.cs +++ b/Assets/Scripts/Tools/SDF/Parser/ShapeTypes.cs @@ -19,7 +19,10 @@ public class Box : ShapeType public class Cylinder : ShapeType { + // Description: Radius of the cylinder public double radius = 1; + + // Description: Length of the cylinder along the z axis public double length = 1; } @@ -52,6 +55,52 @@ public class Ellipsoid : ShapeType public Vector3 radii = new Vector3(); } + // Description: A heightmap based on a 2d grayscale image. + public class HeightMap : ShapeType + { + // Description: The heightmap can contain multiple textures. The order of the texture matters. The first texture will appear at the lowest height, and the last texture at the highest height. Use blend to control the height thresholds and fade between textures. + public class Texture + { + + // Description: Size of the applied texture in meters. + public double size = 10; + + // Description: Diffuse texture image filename + public string diffuse = "__default__"; + + // Description: Normalmap texture image filename + public string normal = "__default__"; + } + + // Description: The blend tag controls how two adjacent textures are mixed. The number of blend elements should equal one less than the number of textures. + public class Blend + { + // Description: Min height of a blend layer + public double min_height = 0; + // Description: Distance over which the blend occurs + public double fade_dist = 0; + } + + // Description: URI to a grayscale image file + public string uri = "__default__"; + + // Description: The size of the heightmap in world units. When loading an image: "size" is used if present, otherwise defaults to 1x1x1. When loading a DEM: "size" is used if present, otherwise defaults to true size of DEM. + public Vector3 size = new Vector3(1, 1, 1); + + // Description: A position offset. + public Vector3 pos = new Vector3(0, 0, 0); + + public List texture = new List(); + + public List blend = new List(); + + // Description: Set if the rendering engine will use terrain paging + public bool use_terrain_paging = false; + + // Description: Samples per heightmap datum. For rasterized heightmaps, this indicates the number of samples to take per pixel. Using a higher value, e.g. 2, will generally improve the quality of the heightmap but lower performance. + public uint sampling = 1; + } + public class Image : ShapeType { public string uri = "__default__"; diff --git a/Assets/Scripts/Tools/SDF/Parser/State.cs b/Assets/Scripts/Tools/SDF/Parser/State.cs new file mode 100644 index 00000000..28178a6d --- /dev/null +++ b/Assets/Scripts/Tools/SDF/Parser/State.cs @@ -0,0 +1,146 @@ +/* + * Copyright (c) 2020 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ +using System.Collections.Generic; + +namespace SDF +{ + public class State + { + public class Time + { + public uint seconds = 0; + public uint nanoseconds = 0; + } + + public class Insertions + { + // Description: The model element defines a complete robot or any other physical object. + public Models models = null; + + // Description: The light element describes a light source. + public List lights = null; + } + + // Description: A list of names of deleted entities/ + public class Deletions + { + // Description: The name of a deleted entity. + // Default: __default__ + public List names = null; + } + + // Description: Model state + public class ModelState + { + // Description: Joint angle + public class JointState + { + // Description: Name of the joint + public string name = "__default__"; + + public class JointStateAngle + { + // Description: Angle of an axis + public double angle = 0; + + // Description: Index of the axis. + public uint axis = 0; + } + + public List angles = null; + } + + // Description: A frame of reference in which poses may be expressed. + public class Frame + { + // Description: Name of the frame. It must be unique whithin its scope (model/world), i.e., it must not match the name of another frame, link, joint, or model within the same scope. + public string name = string.Empty; + + // Description: If specified, this frame is attached to the specified frame. The specified frame must be within the same scope and may be defined implicitly, i.e., the name of any //frame, //model, //joint, or //link within the same scope may be used. If missing, this frame is attached to the containing scope's frame. Within a //world scope this is the implicit world frame, and within a //model scope this is the implicit model frame. A frame moves jointly with the frame it is @attached_to. This is different from //pose/@relative_to. @attached_to defines how the frame is attached to a //link, //model, or //world frame, while //pose/@relative_to defines how the frame's pose is represented numerically. As a result, following the chain of @attached_to attributes must always lead to a //link, //model, //world, or //joint (implicitly attached_to its child //link). + public string attached_to = string.Empty; + + public Pose pose = new Pose(); + } + + // Description: Link state + public class LinkState + { + // Description: Collision state + public class CollisionState + { + // Description: Name of the collision + string name = "__default__"; + } + + // Description: Name of the link + string name = "__default__"; + + // Description: Velocity of the link. The x, y, z components of the pose correspond to the linear velocity of the link, and the roll, pitch, yaw components correspond to the angular velocity of the link + public Pose velocity = null; + + // Description: Acceleration of the link. The x, y, z components of the pose correspond to the linear acceleration of the link, and the roll, pitch, yaw components correspond to the angular acceleration of the link + public Pose acceleration = null; + + // Description: Force and torque applied to the link. The x, y, z components of the pose correspond to the force applied to the link, and the roll, pitch, yaw components correspond to the torque applied to the link + public Pose wrench = null; + + public List collisions = null; + + public Pose pose = new Pose(); + } + + + // Description: Name of the model + public string name = "__default__"; + + public List joints = null; + + // Description: A nested model state element + public Models models = null; + + // Description: Scale for the 3 dimensions of the model. + public Vector3 scale = new Vector3(1, 1, 1); + + public List frames = null; + + public Pose pose = new Pose(); + + public List links = null; + } + + public class LightState + { + // Description: Name of the light + public string name = "__default__"; + public Pose pose = null; + } + + // Description: Name of the world this state applies to + public string world_name = "__default__"; + + // Description: Simulation time stamp of the state [seconds nanoseconds] + public Time sim_time = new Time(); + + + // Description: Wall time stamp of the state [seconds nanoseconds] + public Time wall_time = new Time(); + + // Description: Real time stamp of the state [seconds nanoseconds] + public Time real_time = new Time(); + + // Description: Number of simulation iterations. + public uint iterations = 0; + + // Description: A list containing the entire description of entities inserted. + public Insertions insertions = null; + + public Deletions deletions = null; + + public List models = null; + + public List lights = null; + } +} \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Parser/State.cs.meta b/Assets/Scripts/Tools/SDF/Parser/State.cs.meta new file mode 100644 index 00000000..c5a41bac --- /dev/null +++ b/Assets/Scripts/Tools/SDF/Parser/State.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 76d10df08d25bcaf09ed6e716ff6844f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Tools/SDF/Parser/World.cs b/Assets/Scripts/Tools/SDF/Parser/World.cs index 53ee3343..c2b0af9d 100644 --- a/Assets/Scripts/Tools/SDF/Parser/World.cs +++ b/Assets/Scripts/Tools/SDF/Parser/World.cs @@ -12,6 +12,36 @@ namespace SDF { public class World : Entity { + // Description: Global audio properties. + public class Audio + { + // Description: Device to use for audio playback. A value of "default" will use the system's default audio device. Otherwise, specify a an audio device file" + public string device = string.Empty; + } + + // Description: The wind tag specifies the type and properties of the wind. + public class Wind + { + // Description: Linear velocity of the wind. + public Vector3 linear_velocity = new Vector3(); + } + + // Description: The atmosphere tag specifies the type and properties of the atmosphere model. + public class Atmosphere + { + // Description: The type of the atmosphere engine. Current options are adiabatic. Defaults to adiabatic if left unspecified. + public string type = "adiabatic"; + + // Description: Temperature at sea level in kelvins. + public double temperature = 288.14999999999998; + + // Description: Pressure at sea level in pascals. + public double pressure = 101325; + + // Description: Temperature gradient with respect to increasing altitude at sea level in units of K/m. + public double temperature_gradient = -0.0064999999999999997; + } + public class Gui { public class Camera : Entity @@ -69,6 +99,23 @@ protected override void ParseElements() private Plugins plugins; } + public class Road + { + // Description: Name of the road + public string name = "__default__"; + + // Description: Width of the road + public double width = 1; + + // Required: + + // Description: A series of points that define the path of the road. + // Default: 0 0 0 + public Vector3 point = null; + + // Description: The material of the visual element. + public Material material = null; + } + public class SphericalCoordinates { // Description: Name of planetary surface model, used to determine the surface altitude at a given latitude and longitude. The default is an ellipsoid model of the earth based on the WGS-84 standard. It is used in Gazebo's GPS sensor implementation. @@ -90,19 +137,61 @@ public class SphericalCoordinates public double heading_deg = 0; } - //