Bug fix in Micom Device

- fix rotation direction for ROS2 message there was bug for conversion between Left-handed and right-handed
lge-ros2 · Sep 18, 2020 · 6018e42 · 6018e42
1 parent 70eaf26
commit 6018e42
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs
@@ -342,8 +342,10 @@ public bool UpdateOdom()
 			var odom = micomSensorData.Odom;
 			if ((odom != null) && (motorLeft != null && motorRight != null))
 			{
-				var angularVelocityLeft = motorLeft.GetCurrentVelocity();
-				var angularVelocityRight = motorRight.GetCurrentVelocity();
+				// Set revsered value due to differnt direction
+				// Left-handed -> Right-handed direction of rotation
+				var angularVelocityLeft = -motorLeft.GetCurrentVelocity();
+				var angularVelocityRight = -motorRight.GetCurrentVelocity();
 
 				odom.AngularVelocityLeft = angularVelocityLeft * Mathf.Deg2Rad;
 				odom.AngularVelocityRight = angularVelocityRight * Mathf.Deg2Rad;
@@ -387,12 +389,13 @@ private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelo
 	{
 		if (motorLeft != null && motorRight != null)
 		{
-			motorLeft.SetVelocityTarget(angularVelocityLeft);
-			motorRight.SetVelocityTarget(angularVelocityRight);
+			// Set revsered value due to differnt direction
+			// Right-handed -> Left-handed direction of rotation
+			motorLeft.SetVelocityTarget(-angularVelocityLeft);
+			motorRight.SetVelocityTarget(-angularVelocityRight);
 		}
 	}
 
-
 	public Pose GetPartsPose(in string targetPartsName)
 	{
 		if (partsPoseMapTable.TryGetValue(targetPartsName, out var targetPartsPose))

diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs
@@ -72,9 +72,8 @@ public void SetVelocityTarget(in float targetAngularVelocity)
 		// 	name, currentVelocity, targetAngularVelocity, cmdForce, rigidBody.maxAngularVelocity);
 
 		// JointMotor.targetVelocity angular velocity in degrees per second.
-		// Set revsered value due to differnt direction between ROS2/SDF(Right-handed)
 		var motor = joint.motor;
-		motor.targetVelocity = -targetAngularVelocity;
+		motor.targetVelocity = targetAngularVelocity;
 		motor.force = Mathf.Round(cmdForce);
 
 		if (targetAngularVelocity == 0)