forked from MathiasCiarlo/ROSBridgeLib
-
Notifications
You must be signed in to change notification settings - Fork 0
/
TestSubscriber.cs
37 lines (28 loc) · 1.4 KB
/
TestSubscriber.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
using SimpleJSON;
using UnityEngine;
public class TestSubscriber : ROSBridgeLib.ROSBridgeSubscriber {
public new static string GetMessageTopic() {
return "/test_object";
}
public new static string GetMessageType() {
return "geometry_msgs/TransformStamped";
}
public new static ROSBridgeMsg ParseMessage(JSONNode msg) {
return new ROSBridgeLib.geometry_msgs.TransformStampedMsg(msg);
}
// This function should fire on each received ros message
public new static void CallBack(ROSBridgeMsg msg) {
GameObject TestObject = GameObject.FindGameObjectWithTag("TestObject");
ROSBridgeLib.geometry_msgs.TransformStampedMsg incomingMessage = (ROSBridgeLib.geometry_msgs.TransformStampedMsg)msg;
float x = incomingMessage.GetTransform().GetTranslation().GetX();
float y = incomingMessage.GetTransform().GetTranslation().GetY();
float z = incomingMessage.GetTransform().GetTranslation().GetZ();
float qx = incomingMessage.GetTransform().GetRotation().GetX();
float qy = incomingMessage.GetTransform().GetRotation().GetY();
float qz = incomingMessage.GetTransform().GetRotation().GetZ();
float qw = incomingMessage.GetTransform().GetRotation().GetW();
Vector3 IncomingPosition = new Vector3(x, z, y);
Quaternion IncomingRotation = new Quaternion(qx, qy, qz, qw);
TestObject.transform.SetPositionAndRotation(IncomingPosition, IncomingRotation);
}
}