Merge pull request #20 from lge-ros2/develop

Merge 'develop' branch into 'master'
lge-ros2 · Aug 7, 2020 · 3f7445b · 3f7445b
2 parents 5699941 + 2c28e2e
commit 3f7445b
Show file tree

Hide file tree

Showing 47 changed files with 940 additions and 2,301 deletions.
diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity
@@ -771,6 +771,8 @@ MonoBehaviour:
   pauseOnStart: 0
   clearAllOnStart: 1
   worldFileName: lg_seocho.world
+  modelRootDirectories: []
+  worldRootDirectories: []
 --- !u!114 &249819590
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -3800,10 +3802,13 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   blockControl: 0
-  mainSpeed: 10
-  shiftAdd: 20
-  maxShift: 50
-  camSens: 0.1
+  mainSpeed: 7
+  shiftAdd: 10
+  maxShift: 100
+  camSens: 0.25
+  edgeWidth: 30
+  edgeSens: 0.05
+  edgeSensMax: 1.7
 --- !u!20 &963194227
 Camera:
   m_ObjectHideFlags: 0
@@ -3924,12 +3929,8 @@ MonoBehaviour:
     m_Bits: 1
   SetMoveType: 116
   SetRotateType: 114
-  SetScaleType: 0
   SetAllTransformType: 121
   SetSpaceToggle: 120
-  SetPivotModeToggle: 0
-  SetCenterTypeToggle: 0
-  SetScaleTypeToggle: 0
   translationSnapping: 306
   AddSelection: 0
   RemoveSelection: 0

diff --git a/Assets/Scenes/MainScene/ReflectionProbe-0.exr b/Assets/Scenes/MainScene/ReflectionProbe-0.exr
diff --git a/Assets/Scenes/MainScene/ReflectionProbe-0.exr.meta b/Assets/Scenes/MainScene/ReflectionProbe-0.exr.meta
diff --git a/Assets/Scripts/DevicePlugins/RobotControl.cs b/Assets/Scripts/DevicePlugins/RobotControl.cs
@@ -12,78 +12,20 @@ public class RobotControl : DevicePlugin
 {
 	private MicomInput micomInput = null;
 	private MicomSensor micomSensor = null;
-	private SensorDevices.IMU imuSensor = null;
 
-	private Motor motorLeft = null;
-	private Motor motorRight = null;
-
-	public float wheelRadius = 0.0f; // in mether
-	public float divideWheelRadius = 0.0f;
 
 	protected override void OnAwake()
 	{
 		micomInput = gameObject.AddComponent<MicomInput>();
 		micomSensor = gameObject.AddComponent<MicomSensor>();
-
-		imuSensor = gameObject.GetComponentInChildren<SensorDevices.IMU>();
+		micomSensor.SetPluginParameter(parameters);
 	}
 
 	protected override void OnStart()
 	{
-		const float MM2M = 0.001f;
-		var updateRate = parameters.GetValue<float>("update_rate", 20);
-		micomSensor.SetUpdateRate(updateRate);
-
-		var kp = parameters.GetValue<float>("PID/kp");
-		var ki = parameters.GetValue<float>("PID/ki");
-		var kd = parameters.GetValue<float>("PID/kd");
-
-		var pidControl = new PID(kp, ki, kd);
-
-		var wheelBase = parameters.GetValue<float>("wheel/base") * MM2M;
-		wheelRadius = parameters.GetValue<float>("wheel/radius") * MM2M;
-		divideWheelRadius = 1.0f/wheelRadius; // for performacne.
-
-		var wheelNameLeft = parameters.GetValue<string>("wheel/location[@type='left']");
-		var wheelNameRight = parameters.GetValue<string>("wheel/location[@type='right']");
-
 		var debugging = parameters.GetValue<bool>("debug", false);
 		micomInput.EnableDebugging = debugging;
 
-		var motorFriction = parameters.GetValue<float>("wheel/friction/motor", 0.1f); // Currently not used
-		var brakeFriction = parameters.GetValue<float>("wheel/friction/brake", 0.1f); // Currently not used
-
-		var modelList = GetComponentsInChildren<ModelPlugin>();
-		foreach (var model in modelList)
-		{
-			// Debug.Log(model.name);
-			if (model.name.Equals(wheelNameLeft))
-			{
-				var jointWheelLeft = model.GetComponentInChildren<HingeJoint>();
-				motorLeft = new Motor("Left", jointWheelLeft, pidControl);
-
-				var wheelLeftBody = jointWheelLeft.gameObject.GetComponent<Rigidbody>();
-
-				// Debug.Log("joint Wheel Left found : " + jointWheelLeft.name);
-				// Debug.Log("joint Wheel Left max angular velocity : " + jointWheelLeft.gameObject.GetComponent<Rigidbody>().maxAngularVelocity);
-			}
-			else if (model.name.Equals(wheelNameRight))
-			{
-				var jointWheelRight = model.GetComponentInChildren<HingeJoint>();
-				motorRight = new Motor("Right", jointWheelRight, pidControl);
-
-				var wheelRightBody = jointWheelRight.gameObject.GetComponent<Rigidbody>();
-
-				// Debug.Log("joint Wheel Right found : " + jointWheelRight.name);
-				// Debug.Log("joint Wheel Right max angular velocity : " + jointWheelRight.gameObject.GetComponent<Rigidbody>().maxAngularVelocity);
-			}
-
-			if (motorLeft != null && motorRight != null)
-			{
-				break;
-			}
-		}
-
 		var txHashKey = MakeHashKey("MICOM", "_SENSOR");
 		if (!RegisterTxDevice(txHashKey))
 		{
@@ -102,33 +44,26 @@ protected override void OnStart()
 
 	void FixedUpdate()
 	{
-		var localRotation = transform.rotation;
-		if (imuSensor != null)
-		{
-			micomSensor.SetIMU(imuSensor);
-			micomSensor.SetAccGyro(localRotation.eulerAngles);
-			// Debug.LogFormat("{0} {1}", localRotation, imuSensor.GetOrientation());
-		}
-
-		float linearVelocityLeft = 0;
-		float linearVelocityRight = 0;
-
-		if (motorLeft != null)
-		{
-			var targetWheelVelocityLeft = micomInput.GetWheelVelocityLeft() * divideWheelRadius;
-			motorLeft.SetVelocityTarget(targetWheelVelocityLeft);
-			linearVelocityLeft = motorLeft.GetCurrentVelocity() * wheelRadius;
-		}
-
-		if (motorRight != null)
+		if (micomInput != null && micomSensor != null)
 		{
-			var targetWheelVelocityRight = micomInput.GetWheelVelocityRight() * divideWheelRadius;
-			motorRight.SetVelocityTarget(targetWheelVelocityRight);
-			linearVelocityRight = motorRight.GetCurrentVelocity() * wheelRadius;
+			switch (micomInput.ControlType)
+			{
+				case MicomInput.VelocityType.LinearAndAngular:
+					var targetLinearVelocityLeft = micomInput.GetLinearVelocity();
+					var targetAngularVelocityRight = micomInput.GetAngularVelocity();
+					micomSensor.SetTwistDrive(targetLinearVelocityLeft, targetAngularVelocityRight);
+					break;
+
+				case MicomInput.VelocityType.LeftAndRight:
+					var targetWheelLeftVelocity = micomInput.GetWheelLeftVelocity();
+					var targetWheelRightVelocity = micomInput.GetWheelRightVelocity();
+					micomSensor.SetDifferentialDrive(targetWheelLeftVelocity, targetWheelRightVelocity);
+					break;
+
+				case MicomInput.VelocityType.Unknown:
+					break;
+			}
 		}
-
-		// Debug.LogFormat("{0} {1}, {2} {3} | {4}", motorLeft.GetCurrentVelocity(), linearVelocityLeft, motorRight.GetCurrentVelocity(), linearVelocityRight,  wheelRadius);
-		micomSensor.SetOdomData(linearVelocityLeft, linearVelocityRight);
 	}
 
 	private void Sender()

diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs
@@ -228,6 +228,7 @@ protected override void GenerateMessage()
 		{
 			var image = imageStamped.Image;
 			var imageData = camData.GetTextureData();
+
 			if (imageData != null && image.Data.Length == imageData.Length)
 			{
 				image.Data = imageData;