Merge from 'develop' into 'main' for CLOiSim-4.1.0 (#261)

.gitignore

@@ -114,3 +114,6 @@ tags
 
 # Persistent undo
 [._]*.un~
+
+# UI 
+/UIElementsSchema

Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs

@@ -5,9 +5,12 @@
  */
 
 using System.Collections.Generic;
+using Any = cloisim.msgs.Any;
+using messages = cloisim.msgs;
 
 public class JointControlPlugin : CLOiSimPlugin
 {
+	private List<TF> staticTfList = new List<TF>();
 	private List<TF> tfList = new List<TF>();
 	private SensorDevices.JointCommand jointCommand = null;
 	private SensorDevices.JointState jointState = null;
@@ -25,6 +28,11 @@ protected override void OnAwake()
 
 	protected override void OnStart()
 	{
+		if (RegisterServiceDevice(out var portService, "Info"))
+		{
+			AddThread(portService, ServiceThread);
+		}
+
 		if (RegisterRxDevice(out var portRx, "Rx"))
 		{
 			AddThread(portRx, ReceiverThread, jointCommand);
@@ -52,22 +60,82 @@ private void LoadJoints()
 		var updateRate = GetPluginParameters().GetValue<float>("update_rate", 20);
 		jointState.SetUpdateRate(updateRate);
 
-		if (GetPluginParameters().GetValues<string>("joints/link", out var links))
+		if (GetPluginParameters().GetValues<string>("joints/joint", out var joints))
 		{
-			foreach (var linkName in links)
+			foreach (var jointName in joints)
 			{
-				if (jointState != null)
+				// UnityEngine.Debug.Log("Joints loaded "+ jointName);
+				if (jointState.AddTargetJoint(jointName, out var targetLink, out var isStatic))
 				{
-					var parentFrameId = GetPluginParameters().GetAttributeInPath<string>("joints/link[text()='" + linkName + "']", "parent_frame_id", "base_link");
+					var tf = new TF(targetLink);
 
-					if (jointState.AddTarget(linkName, out var targetLink))
-					{
-						var tf = new TF(targetLink, linkName, parentFrameId);
+					if (isStatic)
+						staticTfList.Add(tf);
+					else
 						tfList.Add(tf);
-						// Debug.Log(link);
-					}
 				}
 			}
 		}
+		// UnityEngine.Debug.Log("Joints loaded");
+	}
+
+	protected override void HandleCustomRequestMessage(in string requestType, in Any requestValue, ref DeviceMessage response)
+	{
+		switch (requestType)
+		{
+			case "request_static_transforms":
+				SetStaticTransformsResponse(ref response);
+				break;
+
+			case "reset_odometry":
+				Reset();
+				SetEmptyResponse(ref response);
+				break;
+
+			default:
+				break;
+		}
+	}
+
+	private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info)
+	{
+		if (msRos2Info == null)
+		{
+			return;
+		}
+
+		var ros2CommonInfo = new messages.Param();
+		ros2CommonInfo.Name = "static_transforms";
+		ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };
+
+		foreach (var tf in staticTfList)
+		{
+			var ros2StaticTransformLink = new messages.Param();
+			ros2StaticTransformLink.Name = "parent_frame_id";
+			ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId };
+
+			{
+				var tfPose = tf.GetPose();
+
+				var poseMessage = new messages.Pose();
+				poseMessage.Position = new messages.Vector3d();
+				poseMessage.Orientation = new messages.Quaternion();
+
+				poseMessage.Name = tf.childFrameId;
+				DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position);
+				DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation);
+
+				var ros2StaticTransformElement = new messages.Param();
+				ros2StaticTransformElement.Name = "pose";
+				ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage };
+
+				ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement);
+				// Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation);
+			}
+
+			ros2CommonInfo.Childrens.Add(ros2StaticTransformLink);
+		}
+
+		msRos2Info.SetMessage<messages.Param>(ros2CommonInfo);
 	}
 }

Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs

@@ -15,7 +15,6 @@ enum Type { WORLD, GROUNDTRUTH, ELEVATOR, ACTOR, MICOM, JOINTCONTROL, GPS, IMU,
 	void Reset();
 }
 
-[DefaultExecutionOrder(560)]
 public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin
 {
 	public ICLOiSimPlugin.Type type { get; protected set; }

Assets/Scripts/CLOiSimPlugins/Modules/Base/TF.cs

@@ -21,11 +21,21 @@ public TF(in SDF.Helper.Link link, in string childFrameId, in string parentFrame
 		this.link = link;
 	}
 
+	public TF(in SDF.Helper.Link link)
+	{
+		var parentFrame = link.JointParentLinkName;
+		var childFrame = link.JointChildLinkName;
+		this.parentFrameId = parentFrame.Replace("::", "_");
+		this.childFrameId = childFrame.Replace("::", "_");
+		this.link = link;
+	}
+
 	public Pose GetPose()
 	{
 		var tfLink = this.link;
 		var tfPose = tfLink.GetPose(targetPoseFrame);
 
+		// Debug.Log(tfLink.Model.name + " <=>" + tfLink.RootModel.name);
 		if (!tfLink.Model.Equals(tfLink.RootModel))
 		{
 			var modelPose = tfLink.Model.GetPose(targetPoseFrame);
@@ -34,14 +44,17 @@ public Pose GetPose()
 			if (tfLink.JointAxis.Equals(Vector3.up) || tfLink.JointAxis.Equals(Vector3.down))
 			{
 				tfPose.rotation *= Quaternion.AngleAxis(180, Vector3.right);
+				// Debug.Log(parentFrameId + "::" + childFrameId + "(" + tfLink.JointAxis + ") = " + modelPose.position + ", " + tfPose.position);
 			}
 			else if (tfLink.JointAxis.Equals(Vector3.forward) || tfLink.JointAxis.Equals(Vector3.back))
 			{
 				// tfPose.rotation *= Quaternion.AngleAxis(180, Vector3.up);
+				// Debug.Log(parentFrameId + "::" + childFrameId + "(" + tfLink.JointAxis + ") = " + modelPose.position + ", " + tfPose.position);
 			}
 			else if (tfLink.JointAxis.Equals(Vector3.left) || tfLink.JointAxis.Equals(Vector3.right))
 			{
 				// tfPose.rotation *= Quaternion.AngleAxis(180, Vector3.forward);
+				// Debug.Log(parentFrameId + "::" + childFrameId + "(" + tfLink.JointAxis + ") = " + modelPose.position + ", " + tfPose.position);
 			}
 
 			tfPose.position += modelPose.position;

Assets/Scripts/Core/ObjectSpawning.cs

@@ -219,6 +219,7 @@ private IEnumerator DeleteTargetObject(List<Transform> targetObjectsTransform)
 			if (targetObjectTransform.CompareTag("Props") || targetObjectTransform.CompareTag("Model"))
 			{
 				Destroy(targetObjectTransform.gameObject);
+				yield return new WaitForEndOfFrame();
 			}
 		}
 

Assets/Scripts/Devices/Camera.cs

@@ -94,7 +94,7 @@ protected override void OnStart()
 
 		protected virtual void SetupTexture()
 		{
-			camSensor.clearFlags = CameraClearFlags.Nothing;
+			camSensor.clearFlags = CameraClearFlags.Skybox;
 			camSensor.allowHDR = true;
 			camSensor.depthTextureMode = DepthTextureMode.None;
 			_universalCamData.requiresColorOption = CameraOverrideOption.On;
@@ -174,7 +174,7 @@ private void SetupCamera()
 
 			camSensor.renderingPath = RenderingPath.Forward;
 			camSensor.allowMSAA = true;
-			camSensor.allowDynamicResolution = false;
+			camSensor.allowDynamicResolution = true;
 			camSensor.useOcclusionCulling = true;
 			camSensor.stereoTargetEye = StereoTargetEyeMask.None;
 			camSensor.orthographic = false;
@@ -192,7 +192,7 @@ private void SetupCamera()
 				filterMode: FilterMode.Trilinear,
 				wrapMode: TextureWrapMode.Clamp,
 				dimension: TextureDimension.Tex2D,
-				msaaSamples: MSAASamples.MSAA2x,
+				msaaSamples: MSAASamples.MSAA4x,
 				enableRandomWrite: false,
 				useMipMap: true,
 				autoGenerateMips: true,

Assets/Scripts/Devices/Clock.cs

@@ -13,18 +13,18 @@ public class Clock : Device
 {
 	private messages.WorldStatistics worldStat = null;
 
-#region Filter times
+	#region Filter times
 	private double prevSimTime = 0f;
 	private double prevRealTime = 0f;
-#endregion
+	#endregion
 
 	private double restartedSimTime = 0;
 	private double restartedRealTime = 0;
 
 	private double currentSimTime = 0;
 	private double currentRealTime = 0;
 
-#region time in hms format
+	#region time in hms format
 	public class HMS
 	{
 		private string _simTime = string.Empty;
@@ -50,7 +50,7 @@ public void SetDiffTime(in TimeSpan ts)
 
 		private void SetTimeString(ref string target, in TimeSpan ts)
 		{
-			_tempSB.AppendFormat("{0}d {1}:{2}:{3}.{4}", ts.Days.ToString(), ts.Hours.ToString(), ts.Minutes.ToString(), ts.Seconds.ToString(), ts.Milliseconds.ToString());
+			_tempSB.AppendFormat("{0}d {1:D2}:{2:D2}:{3:D2}.{4:D3}", ts.Days, ts.Hours, ts.Minutes, ts.Seconds, ts.Milliseconds);
 			target = _tempSB.ToString();
 			_tempSB.Clear();
 		}
@@ -63,7 +63,7 @@ private void SetTimeString(ref string target, in TimeSpan ts)
 	private HMS hms = new HMS();
 
 	private int hmsUpdateIndex = 0;
-#endregion
+	#endregion
 
 	public double SimTime => currentSimTime;
 

Assets/Scripts/Devices/JointCommand.cs

@@ -21,15 +21,18 @@ struct Command
 			public Command(in Articulation joint, in float targetPosition, in float targetVelocity)
 			{
 				this.joint = joint;
-				this.targetPosition = 0;
-				this.targetVelocity = 0;
+				this.targetPosition = float.NaN;
+				this.targetVelocity = float.NaN;
 				Set(targetPosition, targetVelocity);
 			}
 
 			public void Set(in float targetPosition, in float targetVelocity)
 			{
-				this.targetPosition = targetPosition * (this.joint.IsRevoluteType() ? Mathf.Rad2Deg : 1);
-				this.targetVelocity = targetVelocity * (this.joint.IsRevoluteType() ? Mathf.Rad2Deg : 1);
+				if (targetPosition != float.NaN)
+					this.targetPosition = targetPosition * (this.joint.IsRevoluteType() ? Mathf.Rad2Deg : 1);
+
+				if (targetVelocity != float.NaN)
+					this.targetVelocity = targetVelocity * (this.joint.IsRevoluteType() ? Mathf.Rad2Deg : 1);
 			}
 		}
 
@@ -54,13 +57,24 @@ protected override void ProcessDevice()
 		{
 			if (PopDeviceMessage<messages.JointCmd>(out var jointCommand))
 			{
-				var linkName = jointCommand.Name;
-				var articulation = jointState.GetArticulation(linkName);
-
+				var jointName = jointCommand.Name;
+				Debug.Log(jointName);
+				var articulation = jointState.GetArticulation(jointName);
 				if (articulation != null)
 				{
-					var targetPosition = (float)jointCommand.Position.Target;
-					var targetVelocity = (float)jointCommand.Velocity.Target;
+					var targetPosition = float.NaN;
+					if (jointCommand.Position != null)
+					{
+						targetPosition = (float)jointCommand.Position.Target;
+						Debug.Log("targetPosition=" + targetPosition);
+					}
+
+					var targetVelocity = float.NaN;
+					if (jointCommand.Velocity != null)
+					{
+						targetVelocity = (float)jointCommand.Velocity.Target;
+						Debug.Log("targetVelocity=" + targetVelocity);
+					}
 
 					var newCommand = new Command(articulation, targetPosition, targetVelocity);
 					jointCommandQueue.Enqueue(newCommand);
@@ -78,6 +92,7 @@ void FixedUpdate()
 			while (jointCommandQueue.Count > 0)
 			{
 				var command = jointCommandQueue.Dequeue();
+				// Debug.Log(command.targetVelocity + "," + command.targetPosition);
 				command.joint.Drive(command.targetVelocity, command.targetPosition);
 			}
 		}

Assets/Scripts/Devices/JointState.cs

@@ -44,10 +44,13 @@ protected override void GenerateMessage()
 			PushDeviceMessage<messages.JointStateV>(jointStateV);
 		}
 
-		public bool AddTarget(in string targetLinkName, out SDF.Helper.Link link)
+		public bool AddTargetJoint(in string targetJointName, out SDF.Helper.Link link, out bool isStatic)
 		{
 			var childArticulationBodies = gameObject.GetComponentsInChildren<ArticulationBody>();
 			var rootModelName = string.Empty;
+			link = null;
+			isStatic = false;
+
 			foreach (var childArticulationBody in childArticulationBodies)
 			{
 				// Debug.Log (childArticulationBody.name + " | " + childArticulationBody.transform.parent.name);
@@ -57,33 +60,42 @@ public bool AddTarget(in string targetLinkName, out SDF.Helper.Link link)
 					continue;
 				}
 
-				var parentModelName = childArticulationBody.transform.parent.name;
-				var linkName = ((rootModelName.CompareTo(parentModelName) == 0) ? "" : parentModelName + "::") + childArticulationBody.name;
-				// Debug.Log("!!!!!!! " + linkName);
-				if (linkName.Equals(targetLinkName))
+				var parentObject = childArticulationBody.transform.parent;
+				var parentModelName = parentObject.name;
+				// var linkName = ((!parentObject.CompareTag("Model") || rootModelName.CompareTo(parentModelName) == 0) ? "" : parentModelName + "::") + childArticulationBody.name;
+				var linkHelper = childArticulationBody.GetComponentInChildren<SDF.Helper.Link>();
+				// Debug.Log("linkHelper.JointName " + linkHelper.JointName);
+				if (linkHelper.JointName.Equals(targetJointName))
 				{
+					// Debug.Log("AddTargetJoint " + targetJointName);
+					link = linkHelper;
+					if (childArticulationBody.jointType == ArticulationJointType.FixedJoint)
+					{
+						Debug.LogWarning("Skip to AddTargetJoint due to fixed joint: " + targetJointName);
+						isStatic = true;
+						return true;
+					}
+
 					var articulation = new Articulation(childArticulationBody);
-					articulation.SetDriveType(ArticulationDriveType.Force);
 
 					var jointState = new messages.JointState();
-					jointState.Name = targetLinkName;
+					jointState.Name = targetJointName;
 
-					articulationTable.Add(targetLinkName, new Tuple<Articulation, messages.JointState>(articulation, jointState));
+					articulationTable.Add(targetJointName, new Tuple<Articulation, messages.JointState>(articulation, jointState));
 
 					jointStateV.JointStates.Add(jointState);
 
-					link = articulation.gameObject.GetComponentInChildren<SDF.Helper.Link>();
+					// link = articulation.gameObject.GetComponentInChildren<SDF.Helper.Link>();
 					return true;
 				}
 			}
 
-			link = null;
 			return false;
 		}
 
-		public Articulation GetArticulation(in string targetLinkName)
+		public Articulation GetArticulation(in string targetJointName)
 		{
-			return articulationTable.ContainsKey(targetLinkName) ? articulationTable[targetLinkName].Item1 : null;
+			return articulationTable.ContainsKey(targetJointName) ? articulationTable[targetJointName].Item1 : null;
 		}
 
 		void FixedUpdate()