[Joana] Code clean-up, first refactoring steps

- messages partly rebuilt
- work in progress - not everything running as desired yet

Assets/ROSBridgeLib/ROSBridgeWebSocketConnection.cs

@@ -174,7 +174,7 @@ public void AddSubscriber(Type subscriber)
             IsValidSubscriber(subscriber);
             _subscribers.Add(subscriber);
             string topic = GetMessageTopic(subscriber);
-            
+
             if (_running)
             {
                 //only announce if not announced yet, 
@@ -472,7 +472,9 @@ public void Publish(string topic, ROSBridgeMsg msg)
                     _ws.Send(s);
                 }
                 else
-                {
+                {// if not announced that we're publishing on this topic 
+                 //-> go through list of publishers/ subscribers and see if one is not yet advertised, try again
+
                     AnnouncePublishersAndSubscribers();
                     //Try second time otherwise not working
                     if (m_AnnouncedTopics.Contains(topic))

Assets/ROSBridgeLib/geometry_msgs/PoseMsg.cs

@@ -1,7 +1,8 @@
-using System.Collections;
-using System.Text;
-using SimpleJSON;
+using SimpleJSON;
 
+/// <summary>
+/// This message is not used so far
+/// </summary>
 namespace ROSBridgeLib
 {
     namespace geometry_msgs

Assets/ROSBridgeLib/geometry_msgs/QuaternionMsg.cs

@@ -1,8 +1,8 @@
-using System.Collections;
-using System.Text;
-using SimpleJSON;
-using ROSBridgeLib.geometry_msgs;
+using SimpleJSON;
 
+/// <summary>
+/// This message is referenced in PoseMsg, but both are not used 
+/// </summary>
 namespace ROSBridgeLib
 {
     namespace geometry_msgs

Assets/ROSBridgeLibExtension/custom_msgs/ExternalForceMsg.cs

@@ -12,82 +12,108 @@ namespace custom_msgs
     {
         public class ExternalForceMsg : ROSBridgeMsg
         {
-            public custom_msgs.LinkMsg Linkname
-            {
-                get
-                {
-                    return _linkname;
-                }
-            }
+            /// <summary>
+            /// name of the affected link
+            /// </summary>
+            private string _linkname;
 
-            public custom_msgs.PositionCustomMsg Position
-            {
-                get
-                {
-                    return _position;
-                }
-            }
+            /// <summary>
+            /// position of the force in local space of link
+            /// </summary>
+            private Vector3 _position;
 
-            public custom_msgs.ForceMsg Force
-            {
-                get
-                {
-                    return _force;
-                }
-            }
-
-            public custom_msgs.DurationMsg Duration
-            {
-                get
-                {
-                    return _duration;
-                }
-            }
+            /// <summary>
+            /// direction of force in local space of link
+            /// </summary>
+            private Vector3 _force;
 
-            private custom_msgs.LinkMsg _linkname;
-            private custom_msgs.PositionCustomMsg _position;
-            private custom_msgs.ForceMsg _force;
-            private custom_msgs.DurationMsg _duration;
+            /// <summary>
+            /// duration in milliseconds
+            /// </summary>
+            private int _duration;
 
+            /// <summary>
+            /// If set to true, this message is outgoing -> values are in gazebo coord system
+            /// </summary>
+            private bool publish; 
 
 
+            /// <summary>
+            /// Creates message: normally called when receiving message on subscription topic
+            /// </summary>
+            /// <param name="msg"></param>
             public ExternalForceMsg(JSONNode msg)
             {
-                _linkname = new custom_msgs.LinkMsg(msg["linkname"]);
-                _position = new custom_msgs.PositionCustomMsg(msg["position"]);
-                _force = new custom_msgs.ForceMsg(msg["force"]);
-                _duration = new custom_msgs.DurationMsg(msg["duration"]);
+                _linkname = msg["linkname"];
+                _position.x = float.Parse(msg["x"]);
+                _position.y = float.Parse(msg["y"]);
+                _position.z = float.Parse(msg["z"]);
+
+                _force.x = float.Parse(msg["x"]);
+                _force.y = float.Parse(msg["y"]);
+                _force.z = float.Parse(msg["z"]);
+
+                _duration = int.Parse(msg["duration"]);
+                //incoming message -> parse from gazebo coordinate system to unity's
+                publish = false;
+                _position = GazeboUtility.GazeboPositionToUnity(_position);
+                _force = GazeboUtility.GazeboPositionToUnity(_force);
             }
 
-            public ExternalForceMsg(custom_msgs.LinkMsg linkname, custom_msgs.PositionCustomMsg position, custom_msgs.ForceMsg force, custom_msgs.DurationMsg duration)
+            /// <summary>
+            /// Creates message specifying given external force.
+            /// NOTICE: COORDINATE SYSTEM TRANSLATION HAPPENS HERE -> set publish to true
+            /// </summary>
+            /// <param name="linkname">Name of affected link</param>
+            /// <param name="position">position in UNity coordinate space</param>
+            /// <param name="force">force in Unity coordinate space</param>
+            /// <param name="duration"></param>
+            /// <param name="publish">If set to true, this message is outgoing -> will be parsed to gazebo coord system</param>
+            public ExternalForceMsg(string linkname, Vector3 position, Vector3 force, int duration, bool publish)
             {
                 _linkname = linkname;
                 _position = position;
                 _force = force;
                 _duration = duration;
+                //outgoing message-> parse to gazebo's coordinate system
+                if (publish)
+                {
+                    _position = GazeboUtility.UnityPositionToGazebo(_position);
+                    _force = GazeboUtility.UnityPositionToGazebo(_force);
+                }
             }
 
-            public ExternalForceMsg(string linkname, Vector3 position, Vector3 force, int duration)
+            /// <summary>
+            /// Returns whether this message's coordinates are defined in Unity's coordinate system or Gazebo's.
+            /// </summary>
+            /// <returns></returns>
+            public bool IsInUnityCoordinateSystem()
             {
-                _linkname = new LinkMsg(linkname);
-                _position = new PositionCustomMsg(position.x, position.y, position.z);
-                _force = new ForceMsg(force.x, force.y, force.z);
-                _duration = new DurationMsg(duration);
+                return !publish;
             }
 
             public static string GetMessageType()
             {
                 return "roboy_communication_simulation/ExternalForce";
             }
 
+            /// <summary>
+            /// TODO: never used before.... is this needed ? for now: not implemented exception!
+            /// </summary>
+            /// <returns></returns>
             public override string ToString()
             {
-                return "ExternalForce [name=" + _linkname + ", position=" + _position.ToString() + ",  force=" + _force.ToString() + ", duration=" + _duration.ToString() + "]";
+                throw new System.NotImplementedException();
+                //remainder from prev implementation below
+                //return "ExternalForce [name=" + _linkname + ", position=" + _position.ToString() + ",  force=" + _force.ToString() + ", duration=" + _duration.ToString() + "]";
             }
 
             public override string ToYAMLString()
             {
-                return "{" + _linkname.ToYAMLString() + ", " + _position.ToYAMLString() + ", " + _force.ToYAMLString() + ", " + _duration.ToYAMLString() + "}";
+                return "{" + "\"name\" : \"" + _linkname + "\", " + // name
+                    "\"x\" : " + _position.x + ", \"y\" : " + _position.y + ", \"z\" : " + _position.z + ", " + //pos
+                    "\"f_x\" : " + _position.x + ", \"f_y\" : " + _position.y + ", \"f_z\" : " + _position.z + ", " + //force 
+                    "\"duration\" : " + _duration + "}"; // duration
             }
         }
     }

Assets/ROSBridgeLibExtension/custom_msgs/ForceMsg.cs

@@ -1,5 +1,4 @@
-using System.Collections;
-using System.Collections.Generic;
+using UnityEngine;
 using SimpleJSON;
 
 namespace ROSBridgeLib
@@ -8,40 +7,48 @@ namespace custom_msgs
     {
         public class ForceMsg : ROSBridgeMsg
         {
-            private double _x;
-            private double _y;
-            private double _z;
+            private float _x;
+            private float _y;
+            private float _z;
 
             public ForceMsg(JSONNode msg)
             {
-                _x = double.Parse(msg["f_x"]);
-                _y = double.Parse(msg["f_y"]);
-                _z = double.Parse(msg["f_z"]);
+                Vector3 gazeboPos = new Vector3(float.Parse(msg["f_x"]), float.Parse(msg["f_y"]), float.Parse(msg["f_z"]));
+                Vector3 unityPos = GazeboUtility.GazeboPositionToUnity(gazeboPos);
+                _x = unityPos.x;
+                _y = unityPos.y;
+                _z = unityPos.z;
             }
 
-            public ForceMsg(double x, double y, double z)
+            /// <summary>
+            /// Constructor for force msg: Expects force in unity coordinate system and transforms it to Gazebo coordinate system
+            /// </summary>
+            /// <param name="force"></param>
+            public ForceMsg(Vector3 force)
             {
-                _x = x;
-                _y = y;
-                _z = z;
+                Vector3 gazeboForce = GazeboUtility.UnityPositionToGazebo(force);
+                _x = gazeboForce.x;
+                _y = gazeboForce.y;
+                _z = gazeboForce.z;
             }
 
+
             public static string GetMessageType()
             {
                 return "geometry_msgs/Vector3";
             }
 
-            public double GetX()
+            public float GetX()
             {
                 return _x;
             }
 
-            public double GetY()
+            public float GetY()
             {
                 return _y;
             }
 
-            public double GetZ()
+            public float GetZ()
             {
                 return _z;
             }

Assets/ROSBridgeLibExtension/custom_msgs/PositionCustomMsg.cs

@@ -1,47 +1,53 @@
-using System.Collections;
-using System.Collections.Generic;
-using SimpleJSON;
+using SimpleJSON;
+using UnityEngine;
 
 namespace ROSBridgeLib
 {
     namespace custom_msgs
     {
         public class PositionCustomMsg : ROSBridgeMsg
         {
-            private double _x;
-            private double _y;
-            private double _z;
+            private float _x;
+            private float _y;
+            private float _z;
 
             public PositionCustomMsg(JSONNode msg)
             {
-                _x = double.Parse(msg["x"]);
-                _y = double.Parse(msg["y"]);
-                _z = double.Parse(msg["z"]);
+                Vector3 gazeboPos = new Vector3(float.Parse(msg["x"]), float.Parse(msg["y"]), float.Parse(msg["z"]));
+                Vector3 unityPos = GazeboUtility.GazeboPositionToUnity(gazeboPos);
+                _x = unityPos.x;
+                _y = unityPos.y;
+                _z = unityPos.z;
             }
-
-            public PositionCustomMsg(double x, double y, double z)
+
+            /// <summary>
+            /// Constructor for position msg: Position in unity coord space
+            /// </summary>
+            /// <param name="position"></param>
+            public PositionCustomMsg(Vector3 position)
             {
-                _x = x;
-                _y = y;
-                _z = z;
+                Vector3 gazebopos = GazeboUtility.UnityPositionToGazebo(position);
+                _x = gazebopos.x;
+                _y = gazebopos.y;
+                _z = gazebopos.z;
             }
 
             public static string GetMessageType()
             {
                 return "geometry_msgs/Vector3";
             }
 
-            public double GetX()
+            public float GetX()
             {
                 return _x;
             }
 
-            public double GetY()
+            public float GetY()
             {
                 return _y;
             }
 
-            public double GetZ()
+            public float GetZ()
             {
                 return _z;
             }

Assets/ROSBridgeLibExtension/custom_msgs/RoboyPoseMsg.cs

@@ -130,7 +130,22 @@ public RoboyPoseMsg(JSONNode msg)
                 _qwDic = CreateDictionary(nameArray, qwArray);
             }
 
-            // TO DO: PARSE THE DICS SO THEY ARE IN FLOAT ARRAY MSGS IN THE RIGHT ORDER
+            /// <summary>
+            /// Create message based on given params. Attention: link names, positions and rotation need to correspond to same index (*obviously*)
+            /// TODO: NOT IMPLEMENTED YET
+            /// </summary>
+            /// <param name="roboyName"></param>
+            /// <param name="linkNames"></param>
+            /// <param name="positions"></param>
+            /// <param name="rotation"></param>
+            public RoboyPoseMsg(string roboyName, List<string> linkNames, List<Vector3> positions, List<Quaternion> rotation)
+            {
+                //GazeboUtility.UnityPositionToGazebo
+                /*TODO NO CONTENT YET*/
+                throw new System.NotImplementedException();
+            }
+
+
             public RoboyPoseMsg(string roboyName, List<string> linkNames, Dictionary<string, float> xDic, Dictionary<string, float> yDic, Dictionary<string, float> zDic, Dictionary<string, float> qxDic,
                 Dictionary<string, float> qyDic, Dictionary<string, float> qzDic, Dictionary<string, float> qwDic)
             {