challenge wip, fix wheelchairphysics, fix motor based controller

Assets/AddOns/friction.physicMaterial

@@ -8,7 +8,7 @@ PhysicMaterial:
   m_PrefabAsset: {fileID: 0}
   m_Name: friction
   dynamicFriction: 1
-  staticFriction: 0
+  staticFriction: 0.5
   bounciness: 0
   frictionCombine: 3
   bounceCombine: 2

Assets/MotorWriter.cs

@@ -34,7 +34,7 @@ namespace RosSharp.RosBridgeClient
 
         public void Write(float value)
         {
-            targetVelocity = value * MaxVelocity;
+            targetVelocity = Mathf.Min(value, MaxVelocity);
             isMessageReceived = true;
         }
     }

Assets/Robots/RosSharp/Scripts/RosBridgeClient/RosCommuncation/TwistSubscriber.cs

@@ -17,14 +17,15 @@ limitations under the License.
 // © Siemens AG, 2018, Dr. Martin Bischoff (martin.bischoff@siemens.com)
 
 using UnityEngine;
+using RosSharp;
 
 namespace RosSharp.RosBridgeClient
 {
     public class TwistSubscriber : Subscriber<Messages.Geometry.Twist>
     {
+        public enum Mode {kinematic, force, motor};
         public Transform SubscribedTransform;
-
-        public bool is_forced_based = false;
+        public Mode mode = Mode.kinematic;
 
         private Rigidbody rigidbody;
         private float previousRealTime;
@@ -32,16 +33,28 @@ namespace RosSharp.RosBridgeClient
         private Vector3 angularVelocity;
         private bool isMessageReceived;
 
+        private DiffDriveController controller;
+
         protected override void Start()
         {
             base.Start();
             rigidbody = SubscribedTransform.GetComponent<Rigidbody>();
+
+            try
+            {
+                controller = SubscribedTransform.GetComponent<DiffDriveController>();
+            }
+            catch (System.Exception)
+            {
+                
+                throw;
+            }
         }
 
         protected override void ReceiveMessage(Messages.Geometry.Twist message)
         {
-            linearVelocity = ToVector3(message.linear);
-            angularVelocity = ToVector3(message.angular).Ros2Unity();
+            linearVelocity = ToVector3(message.linear).Ros2Unity();
+            angularVelocity = ToVector3(message.angular).Ros2Unity()*Mathf.Deg2Rad;
             isMessageReceived = true;
         }
 
@@ -59,34 +72,40 @@ namespace RosSharp.RosBridgeClient
         {
             // float deltaTime = Time.realtimeSinceStartup - previousRealTime;
             float deltaTime = ToolsTime.DeltaTime;
-                        
-            if(is_forced_based)
+
+            switch(mode)
             {
-                if(deltaTime > 0)
-                {
-                    if(linearVelocity.magnitude > 0)
+                case Mode.force:
+                    if(deltaTime > 0)
                     {
-                        Vector3 force = rigidbody.transform.rotation * (linearVelocity - rigidbody.velocity) * rigidbody.mass / deltaTime * 2;
-                        rigidbody.AddForce( new Vector3 (force.x, 0, force.z) );
-                    }
+                        if(linearVelocity.magnitude > 0)
+                        {
+                            Vector3 force = rigidbody.transform.rotation * (linearVelocity - rigidbody.velocity) * rigidbody.mass / deltaTime * 2;
+                            rigidbody.AddForce( new Vector3 (force.x, 0, force.z) );
+                        }
                     
-                    if (angularVelocity.magnitude > 0)
-                    {
-                        Vector3 torque = rigidbody.transform.rotation * (angularVelocity - rigidbody.angularVelocity) * rigidbody.mass / deltaTime * 2;
-                        rigidbody.AddTorque( new Vector3 (0, torque.y , 0)   );
-                    }
+                        if (angularVelocity.magnitude > 0)
+                        {
+                            Vector3 torque = rigidbody.transform.rotation * (angularVelocity - rigidbody.angularVelocity) * rigidbody.mass / deltaTime * 2;
+                            rigidbody.AddTorque( new Vector3 (0, torque.y , 0)   );
+                        }
 
-                }
-            }
-            else
-            {
-                SubscribedTransform.Translate(linearVelocity * deltaTime);
-                SubscribedTransform.Rotate(Vector3.forward, angularVelocity.x * deltaTime);
-                SubscribedTransform.Rotate(Vector3.up, angularVelocity.y * deltaTime);
-                SubscribedTransform.Rotate(Vector3.left, angularVelocity.z * deltaTime);
+                    }
+                break;
+        
+                case Mode.kinematic:
+                    SubscribedTransform.Translate(linearVelocity * deltaTime);
+                    SubscribedTransform.Rotate(Vector3.forward, angularVelocity.x * deltaTime);
+                    SubscribedTransform.Rotate(Vector3.up, angularVelocity.y * deltaTime);
+                    SubscribedTransform.Rotate(Vector3.left, angularVelocity.z * deltaTime);
+                break;
+        
+                case Mode.motor:
+                    if(controller == null) break;
+                    controller.ControlMotorsClosedLoop(deltaTime, linearVelocity.z, angularVelocity.y);
+                break;
             }
-
-
+            
             previousRealTime = Time.realtimeSinceStartup;
 
             isMessageReceived = false;

catkin_crowdbotsim @ 941aee0c

-Subproject commit e1f30f112fb9d3e923fc35eb1315269d9d17bf17
+Subproject commit 941aee0c7e1018c1846ca1f02043fa4d05b65961