Skip to content

Commit

Permalink
[Joana] Fixed message body of type Pose, backendsubscriber added
Browse files Browse the repository at this point in the history
  • Loading branch information
@jpecholt
jpecholt committed Dec 12, 2017
1 parent 7263033 commit 088f6b3
Show file tree
Hide file tree
Showing 9 changed files with 286 additions and 212 deletions.
22 changes: 15 additions & 7 deletions Assets/ROSBridgeLib/ROSBridgeWebSocketConnection.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,7 @@ public void AddServiceResponse(Type serviceResponse)
public void RemoveServiceResponse(Type serviceResponse)
{
// Code here
throw new Exception("NOT IMPLEMENTED");
//-> somehow this method is called... No clue what it is supposed to do
}

/**
Expand All @@ -175,13 +175,14 @@ public void AddSubscriber(Type subscriber)
_subscribers.Add(subscriber);

string topic = GetMessageTopic(subscriber);
m_SubscribedTopics.Add(topic);
if (_running)
{
//only announce if not announced yet,
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] Adding Subscriber. Subscribing to " + topic);
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(subscriber), GetMessageType(subscriber)));
m_SubscribedTopics.Add(topic);
}
}
}
Expand All @@ -202,6 +203,7 @@ public void RemoveSubscriber(Type subscriber)
//since multiple subscribers can subscribe to same topic, only send unsubscribe if no more subscribers on this topic
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET]Not subcribing anymore to " + topic);
_ws.Send(ROSBridgeMsg.UnSubscribe(topic));
}
}
Expand All @@ -219,6 +221,7 @@ public void AddPublisher(Type publisher)
{
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] Adding publisher. Advertising " + topic);
_ws.Send(ROSBridgeMsg.Advertise(topic, GetMessageType(publisher)));
}
}
Expand All @@ -243,6 +246,7 @@ public void RemovePublisher(Type publisher)
//if we hvae no more publishers on this topic
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] not announcing anymore on: " + topic);
_ws.Send(ROSBridgeMsg.UnAdvertise(topic));
}
}
Expand All @@ -253,7 +257,7 @@ public void RemovePublisher(Type publisher)
*/
public void Connect()
{
Debug.Log("Connecting to ROS");
Debug.Log("[ROS WEBSOCKET] Connecting to ROS");
_myThread = new System.Threading.Thread(Run);
_myThread.Start();
}
Expand All @@ -263,7 +267,7 @@ public void Connect()
*/
public void Disconnect()
{
Debug.Log("Disconnecting from ROS");
Debug.Log("[ROS WEBSOCKET] Disconnecting from ROS");
_myThread.Abort();
foreach (Type p in _subscribers)
{
Expand All @@ -276,6 +280,7 @@ public void Disconnect()
//if no mo subscribers on this topic
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] not subscribing anymore to " + topic);
_ws.Send(ROSBridgeMsg.UnSubscribe(topic));
}
}
Expand All @@ -291,12 +296,13 @@ public void Disconnect()
//if no more advertiser on this topic
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] not publishing on topic: " + topic);
_ws.Send(ROSBridgeMsg.UnAdvertise(topic));
}
}
Debug.Log("[ROS WEBSOCKET] Closing websocket");
_ws.Close();
_running = false;

}

private void Run()
Expand All @@ -309,6 +315,7 @@ private void Run()

_running = true;
AnnouncePublishersAndSubscribers();
Debug.Log("Thread Run fct called");
}

/// <summary>
Expand All @@ -325,6 +332,7 @@ private void AnnouncePublishersAndSubscribers()
//only announce if not already known that we subscribed
if (!m_SubscribedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] Subscribing to " + topic);
_ws.Send(ROSBridgeMsg.Subscribe(topic, GetMessageType(p)));
}
//Debug.Log ("Sending " + ROSBridgeMsg.Subscribe (GetMessageTopic(p), GetMessageType (p)));
Expand All @@ -336,6 +344,7 @@ private void AnnouncePublishersAndSubscribers()
//only announce new publisher if we didn't already announce one for this topic
if (!m_AnnouncedTopics.Contains(topic))
{
Debug.Log("[ROS WEBSOCKET] Advertising " + topic);
_ws.Send(ROSBridgeMsg.Advertise(topic, GetMessageType(p)));
}
//Debug.Log ("Sending " + ROSBridgeMsg.Advertise (GetMessageTopic(p), GetMessageType(p)));
Expand All @@ -351,7 +360,7 @@ private void CheckConnection()
{
if (_ws.ReadyState == WebSocketState.Open)
{
Debug.Log("Connection to ROSBridge successful!");
Debug.Log("[ROS WEBSOCKET] Connection to ROSBridge successful!");
}
}

Expand Down Expand Up @@ -444,7 +453,6 @@ public void Render()
}
if (newTask != null)
Update(newTask.getSubscriber(), newTask.getMsg());

if (_serviceName != null)
{
ServiceResponse(_serviceResponse, _serviceName, _serviceValues);
Expand Down
4 changes: 4 additions & 0 deletions Assets/ROSBridgeLibExtension/custom_msgs/FloatArrayMsg.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,10 @@ public FloatArrayMsg(string identifier, List<float> values)
{
_identifier = identifier;
_values = values;
if (values == null)
{
_values = new List<float>();
}
}

public static string GetMessageType()
Expand Down
10 changes: 5 additions & 5 deletions Assets/ROSBridgeLibExtension/custom_msgs/RoboyPoseMsg.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,8 @@ public string Name

#region PRIVATE_MEMBER_VARIABLES

// TO DO MSG SO WE CAN PUBLISH IT
private std_msgs.StringMsg _roboyNameMsg;
// TODO this is not part of the officially defined msg
//private std_msgs.StringMsg _roboyNameMsg;
private StringArrayMsg _linkNames;
private FloatArrayMsg _xArray, _yArray, _zArray, _qxArray, _qyArray, _qzArray, _qwArray;

Expand Down Expand Up @@ -134,7 +134,7 @@ public RoboyPoseMsg(JSONNode msg)
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)
{
_roboyNameMsg = new std_msgs.StringMsg("roboyName", roboyName);
//_roboyNameMsg = new std_msgs.StringMsg("roboyName", roboyName);
_linkNames = new StringArrayMsg("name", linkNames);

List<float> xValues = CreateListWithLinkNames(linkNames, xDic);
Expand Down Expand Up @@ -173,7 +173,7 @@ public RoboyPoseMsg(JSONNode msg)
/// <param name="qw"></param>
public RoboyPoseMsg(string roboyName, string linkName, float x, float y, float z, float qx, float qy, float qz, float qw)
{
_roboyNameMsg = new std_msgs.StringMsg("roboyName", roboyName);
//_roboyNameMsg = new std_msgs.StringMsg("roboyName", roboyName);
List<string> linkNames = new List<string>();
linkNames.Add(linkName);
_linkNames = new StringArrayMsg("name", linkNames);
Expand Down Expand Up @@ -229,7 +229,7 @@ public override string ToString()

public override string ToYAMLString()
{
return "{" + _roboyNameMsg.ToYAMLString() + ", " + _linkNames.ToYAMLString() + ", " + _xArray.ToYAMLString() + ", " + _yArray.ToYAMLString() + ", " + _zArray.ToYAMLString() +
return "{" +/* _roboyNameMsg.ToYAMLString() + ", " + */ _linkNames.ToYAMLString() + ", " + _xArray.ToYAMLString() + ", " + _yArray.ToYAMLString() + ", " + _zArray.ToYAMLString() +
", " + _qxArray.ToYAMLString() + ", " + _qyArray.ToYAMLString() + ", " + _qzArray.ToYAMLString() + ", " + _qwArray.ToYAMLString() +
"}";
}
Expand Down
5 changes: 5 additions & 0 deletions Assets/ROSBridgeLibExtension/custom_msgs/StringArrayMsg.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ public class StringArrayMsg : ROSBridgeMsg
public StringArrayMsg(JSONNode msg)
{
JSONArray linkNames = msg[_identifier].AsArray;

foreach (var linkName in linkNames)
{
_linkNames.Add(linkName.ToString());
Expand All @@ -26,6 +27,10 @@ public StringArrayMsg(string identifier, List<string> linkNames)
{
_identifier = identifier;
_linkNames = linkNames;
if (linkNames == null)
{
_linkNames = new List<string>();
}
}

public static string GetMessageType()
Expand Down

0 comments on commit 088f6b3

Please sign in to comment.