moved spot control scripts into zerosim. publishing twist stamped message.

Author: micahpearlman

Runtime/Scripts/Controllers/Robots/Spot/ZOSpotCharacterController.cs

@@ -0,0 +1,105 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System;
+
+namespace ZO {
+    public class ZOSpotCharacterController : MonoBehaviour {
+
+        public Rigidbody _rigidBody;
+        public float _maxFloorAngle = 45;
+        public float _maxForwardVelocity = 1.0f;
+
+        public float MaxForwardVelocity {
+            get => _maxForwardVelocity;
+            set => _maxForwardVelocity = value;
+        }
+        public float _maxSideMoveVelocity = 0.5f;
+        public float MaxSideMoveVelocity {
+            get => _maxSideMoveVelocity;
+            set => _maxSideMoveVelocity = value;
+        }
+
+        public float _maxTurnVelocityDegreesSecond = 45.0f;
+        public float MaxTurnVelocityDegreesSecond {
+            get => _maxTurnVelocityDegreesSecond;
+            set => _maxTurnVelocityDegreesSecond = value;
+        }
+
+        private Vector2 _targetVelocity = Vector2.zero;
+        private float _targetTurnVelocityDegreesPerSecond = 0.0f;
+
+        public enum StateEnum {
+            Off,
+            Down,
+            UpAndReady,
+            Error
+        }
+
+        public ZOSpotCharacterController.StateEnum State {
+            get;
+            private set;
+        } = ZOSpotCharacterController.StateEnum.Off;
+
+        public enum ErrorEnum {
+            Ok,
+            GeneralError,
+            ErrorShutdown
+        }
+
+        public ZOSpotCharacterController.ErrorEnum Error {
+            get;
+            private set;
+        } = ZOSpotCharacterController.ErrorEnum.Ok;
+
+
+
+        public delegate void SpotControllerStatusDelegate(ZOSpotCharacterController thisClass, Vector2 linearVelocity, float turnVelocityDegreesPerSecond, float orientationDegrees, ZOSpotCharacterController.StateEnum state, ZOSpotCharacterController.ErrorEnum error);
+        private event SpotControllerStatusDelegate _spotContrllerStatusDelegate;
+        public event SpotControllerStatusDelegate SpotControllerStatus {
+            add {
+                _spotContrllerStatusDelegate += value;
+            }
+            remove {
+                _spotContrllerStatusDelegate -= value;
+            }
+        }
+
+
+
+        // Start is called before the first frame update
+        void Start() {
+
+        }
+
+        // Update is called once per frame
+        void Update() {
+
+        }
+
+        void FixedUpdate() {
+
+            _rigidBody.velocity = _rigidBody.transform.rotation * new Vector3(_targetVelocity.x, _rigidBody.velocity.y, _targetVelocity.y);
+
+            Vector3 angularVelocity = _rigidBody.angularVelocity;
+            angularVelocity.y = _targetTurnVelocityDegreesPerSecond * Mathf.Deg2Rad;
+            _rigidBody.angularVelocity = angularVelocity;
+
+            if (_spotContrllerStatusDelegate != null) {
+                _spotContrllerStatusDelegate.Invoke(this, new Vector2(_rigidBody.velocity.x, _rigidBody.velocity.z), _rigidBody.angularVelocity.y * Mathf.Rad2Deg, transform.rotation.eulerAngles.y, State, Error);
+
+            }
+        }
+
+
+        public void Move(Vector2 targetVelocity, float targetTurnVelocityDegreesPerSecond) {
+            _targetVelocity = targetVelocity;
+            _targetTurnVelocityDegreesPerSecond = targetTurnVelocityDegreesPerSecond;
+        }
+        // void Sit(Delegate OnSitFinishedOrError)
+        // void Stand(Delegate OnStandFinishedOrError)
+        // void TurnOn(Delegate OnTurnOnFinishedOrError)
+        // void TurnOff(Delegate OnTurnOffFinishedOrError)
+    }
+
+}

Runtime/Scripts/Controllers/Robots/Spot/ZOSpotCharacterController.cs.meta

@@ -10,16 +10,23 @@
 using ZO.ROS.Unity;
 using ZO.ROS.MessageTypes;
 using ZO.ROS.MessageTypes.Geometry;
+using ZO.ROS.MessageTypes.Nav;
 
 
 namespace ZO.Controllers {
 
     [RequireComponent(typeof(ZOROSJointStatesPublisher))]
+    [RequireComponent(typeof(ZOSpotCharacterController))]
     public class ZOSpotController : ZOROSUnityGameObjectBase {
 
 
         public string _TwistTopicSubscription = "/cmd_vel";
         private TwistMessage _twistMessage = new TwistMessage();
+        private TwistWithCovarianceStampedMessage _twistPublishMessage = new TwistWithCovarianceStampedMessage();
+        private ZOROSFakeOdometryPublisher _odomPublisher = null;
+
+
+        
 
 
         protected override void ZOOnValidate() {
@@ -30,6 +37,8 @@ protected override void ZOOnValidate() {
         protected override void ZOStart() {
             base.ZOStart();
 
+            _odomPublisher = GetComponentInChildren<ZOROSFakeOdometryPublisher>(true);
+                        
 
         }
 
@@ -49,6 +58,13 @@ protected override void ZOFixedUpdate() {
 
         protected override void ZOUpdateHzSynchronized() {
             base.ZOUpdateHzSynchronized();
+
+            if (_odomPublisher && ZOROSBridgeConnection.Instance.IsConnected == true) {
+                _twistPublishMessage.header = _odomPublisher.CurrentOdometryMessage.header;            
+                _twistPublishMessage.twist = _odomPublisher.CurrentOdometryMessage.twist;
+                ZOROSBridgeConnection.Instance.Publish<TwistWithCovarianceStampedMessage>(_twistPublishMessage, "/odometry/twist");
+                
+            }
         }
 
 
@@ -62,11 +78,14 @@ public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
 
             // subscribe to Twist Message
             ZOROSBridgeConnection.Instance.Subscribe<TwistMessage>(Name, _TwistTopicSubscription, _twistMessage.MessageType, OnROSTwistMessageReceived);
+            
+            // advertise twist
+            ZOROSBridgeConnection.Instance.Advertise("/odometry/twist", TwistWithCovarianceStampedMessage.Type);
 
         }
 
         public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
-            ZOROSBridgeConnection.Instance.UnAdvertise(_TwistTopicSubscription);
+            ZOROSBridgeConnection.Instance.UnAdvertise("/odometry/twist");
             Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeDisconnected");
         }
 

Runtime/Scripts/Controllers/Robots/Spot/ZOSpotController.cs

@@ -0,0 +1,27 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace ZO {
+    public class ZOSpotJoystickCmdVel : MonoBehaviour {
+
+        public ZOSpotCharacterController _spotCharacterController;
+
+        // Start is called before the first frame update
+        void Start() {
+
+        }
+
+        // Update is called once per frame
+        void Update() {
+
+            if (_spotCharacterController != null) {
+                Vector2 targetVelocity = new Vector2(Input.GetAxis("Roll") * _spotCharacterController.MaxSideMoveVelocity, Input.GetAxis("Pitch") * _spotCharacterController.MaxForwardVelocity);
+                _spotCharacterController.Move(targetVelocity, Input.GetAxis("Yaw") * _spotCharacterController._maxTurnVelocityDegreesSecond); // hack no turning yet
+
+            }
+
+        }
+    }
+
+}

Runtime/Scripts/Controllers/Robots/Spot/ZOSpotJoystickCmdVel.cs

@@ -212,6 +212,37 @@ public TwistWithCovarianceMessage(TwistMessage twist, double[] covariance) {
 
     }
 
+    /// <summary>
+    /// This represents an estimated twist with reference coordinate frame and timestamp.
+    /// See: https://docs.ros.org/en/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html
+    /// </summary>
+    public class TwistWithCovarianceStampedMessage : ZOROSMessageInterface {
+
+        [Newtonsoft.Json.JsonIgnore]
+        public string MessageType { get { return TwistWithCovarianceStampedMessage.Type; } }
+        [Newtonsoft.Json.JsonIgnore]
+        public static string Type = "geometry_msgs/TwistWithCovarianceStamped";
+
+        public HeaderMessage header { get; set; }
+
+        //  This expresses velocity in free space with uncertainty.
+        public TwistWithCovarianceMessage twist { get; set; }
+
+        public TwistWithCovarianceStampedMessage() {
+            this.twist = new TwistWithCovarianceMessage();
+        }
+
+        public TwistWithCovarianceStampedMessage(TwistWithCovarianceMessage twist) {
+            this.twist = twist;
+        }
+
+        public void Update() {
+             header.Update();
+        }
+
+
+    }
+
     /// <summary>
     /// This represents the transform between two coordinate frames in free space.
     /// See: http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Transform.html

Runtime/Scripts/Controllers/Robots/Spot/ZOSpotJoystickCmdVel.cs.meta

@@ -33,6 +33,12 @@ public Rigidbody Rigidbody {
             }
         }
 
+        public OdometryMessage CurrentOdometryMessage {
+            get {
+                return _currentOdometryMessage;
+            }
+        }
+
         private ZOGaussianNoiseModel _noiseModel = new ZOGaussianNoiseModel();
         private OdometryMessage _currentOdometryMessage = null;
         private OdometryMessage _previousOdometryMessage = null;