Skip to content

Commit 958b718

Browse files
micahpearlmanmicahpearlman
committed
added more sensible naming to ROS <--> Unity conversion methods
1 parent d7d0153 commit 958b718

File tree

4 files changed

+71
-21
lines changed

4 files changed

+71
-21
lines changed

‎Runtime/Scripts/ROS/MessageTypes/Geometry/ZOROSGeometryMessages.cs‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ public Vector3Message() {
3333
/// z forward
3434
/// </summary>
3535
/// <param name="v">Unity Vector3</param>
36-
public void FromUnityVector3(UnityEngine.Vector3 v) {
36+
public void FromUnityVector3ToROS(UnityEngine.Vector3 v) {
3737
this.x = (double)v.z;
3838
this.y = -(double)v.x;
3939
this.z = (double)v.y;
@@ -62,7 +62,7 @@ public UnityEngine.Vector3 ToUnityVector3() {
6262
[Newtonsoft.Json.JsonIgnore]
6363
public UnityEngine.Vector3 UnityVector3 {
6464
get { return ToUnityVector3(); }
65-
set { FromUnityVector3(value); }
65+
set { FromUnityVector3ToROS(value); }
6666
}
6767
}
6868

@@ -122,7 +122,7 @@ public UnityEngine.Quaternion ToUnityQuaternion() {
122122
/// z forward
123123
/// </summary>
124124
/// <param name="q"></param>
125-
public void FromUnityQuaternion(UnityEngine.Quaternion q) {
125+
public void FromUnityQuaternionToROS(UnityEngine.Quaternion q) {
126126
this.x = (double)-q.x;
127127
this.y = (double)-q.z;
128128
this.z = (double)-q.y;
@@ -132,7 +132,7 @@ public void FromUnityQuaternion(UnityEngine.Quaternion q) {
132132
[Newtonsoft.Json.JsonIgnore]
133133
public UnityEngine.Quaternion UnityQuaternion {
134134
get { return ToUnityQuaternion(); }
135-
set { FromUnityQuaternion(value); }
135+
set { FromUnityQuaternionToROS(value); }
136136
}
137137

138138
[Newtonsoft.Json.JsonIgnore]
@@ -235,16 +235,16 @@ public TransformMessage() {
235235
/// </summary>
236236
/// <param name="transform"></param>
237237
public void FromLocalUnityTransform(UnityEngine.Transform transform) {
238-
this.translation.UnityVector3 = transform.localPosition;
239-
this.rotation.UnityQuaternion = transform.localRotation;
238+
this.translation.FromUnityVector3ToROS(transform.localPosition);
239+
this.rotation.FromUnityQuaternionToROS(transform.localRotation);
240240
}
241241

242242
/// <summary>
243243
/// Convert global Unity Transform to ROS Transform.
244244
/// Does Unity to ROS coordinate system transform.
245245
/// </summary>
246246
/// <param name="transform"></param>
247-
public void FromGlobalUnityTransform(UnityEngine.Transform transform) {
247+
public void FromGlobalUnityTransformToROS(UnityEngine.Transform transform) {
248248
translation.UnityVector3 = transform.position;
249249
rotation.UnityQuaternion = transform.rotation;
250250
}
@@ -256,7 +256,7 @@ public UnityEngine.Transform LocalUnityTransform {
256256

257257
[Newtonsoft.Json.JsonIgnore]
258258
public UnityEngine.Transform GlobalUnityTransform {
259-
set { FromGlobalUnityTransform(value); }
259+
set { FromGlobalUnityTransformToROS(value); }
260260
}
261261

262262
}
@@ -292,27 +292,27 @@ public TransformStampedMessage() {
292292
/// Does Unity to ROS coordinate system transform.
293293
/// </summary>
294294
/// <param name="transform"></param>
295-
public void FromLocalUnityTransform(UnityEngine.Transform transform) {
295+
public void FromLocalUnityTransformToROS(UnityEngine.Transform transform) {
296296
this.transform.LocalUnityTransform = transform;
297297
}
298298

299299
[Newtonsoft.Json.JsonIgnore]
300300
public UnityEngine.Transform UnityLocalTransform {
301-
set { FromLocalUnityTransform(value); }
301+
set { FromLocalUnityTransformToROS(value); }
302302
}
303303

304304
/// <summary>
305305
/// Convert global Unity Transform to ROS Transform.
306306
/// Does Unity to ROS coordinate system transform.
307307
/// </summary>
308308
/// <param name="transform"></param>
309-
public void FromGlobalUnityTransform(UnityEngine.Transform transform) {
309+
public void FromGlobalUnityTransformToROS(UnityEngine.Transform transform) {
310310
this.transform.GlobalUnityTransform = transform;
311311
}
312312

313313
[Newtonsoft.Json.JsonIgnore]
314314
public UnityEngine.Transform GlobalUnityTransform {
315-
set { FromGlobalUnityTransform(value); }
315+
set { FromGlobalUnityTransformToROS(value); }
316316
}
317317

318318
}

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSFakeOdometryPublisher.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -185,7 +185,7 @@ private void ApplyNoise(ref OdometryMessage currentOdomMessage, OdometryMessage
185185
theta2 += (rot1 + rot2);
186186

187187
Quaternion yawQuaternion = Quaternion.EulerAngles(0, (float)theta2, 0);
188-
currentOdomMessage.pose.pose.orientation.FromUnityQuaternion(yawQuaternion);
188+
currentOdomMessage.pose.pose.orientation.FromUnityQuaternionToROS(yawQuaternion);
189189

190190
}
191191

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSRGBDepthPublisher.cs‎

Lines changed: 57 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
using ZO.ROS.MessageTypes.Sensor;
66
using ZO.Sensors;
77
using ZO.ROS.Unity;
8-
using ZO.Document;
8+
using ZO.ROS.MessageTypes.Geometry;
99

1010
namespace ZO.ROS.Publisher {
1111

@@ -29,12 +29,15 @@ public class ZOROSRGBDepthPublisher : ZOROSUnityGameObjectBase {
2929
public string _rgbImageROSTopic = "rgb/image_rect_color";
3030
public string _depthROSTopic = "depth_registered/image_rect";
3131
public string _cameraInfoROSTopic = "rgb/camera_info";
32+
public string _depthCameraInfoROSTopic = "depth_registered/camera_info";
3233

3334
[Header("ROS Transforms")]
3435
public string _parentTransformName = "world";
3536
public string _depthTransformName = "depth_tf";
3637
public string _rgbTransformName = "rgb_tf";
3738

39+
public Vector3 _cameraRotationDegrees = new Vector3(270,270,0);
40+
3841
/// <summary>
3942
/// The depth camera sensor that we will publish.
4043
/// </summary>
@@ -46,6 +49,10 @@ public ZORGBDepthCamera RGBDepthCameraSensor {
4649
private ImageMessage _colorImageMessage = new ImageMessage();
4750
private ImageMessage _depthImageMessage = new ImageMessage();
4851
private CameraInfoMessage _cameraInfoMessage = new CameraInfoMessage();
52+
private CameraInfoMessage _depthInfoMessage = new CameraInfoMessage();
53+
54+
private TransformStampedMessage _depthTransformMessage = new TransformStampedMessage();
55+
private TransformStampedMessage _colorImageTransformMessage = new TransformStampedMessage();
4956

5057
protected override void ZOOnValidate() {
5158
base.ZOOnValidate();
@@ -58,10 +65,6 @@ protected override void ZOOnValidate() {
5865
}
5966
}
6067

61-
protected override void ZOReset() {
62-
base.ZOReset();
63-
64-
}
6568

6669
protected override void ZOStart() {
6770
base.ZOStart();
@@ -73,28 +76,69 @@ protected override void ZOStart() {
7376
_cameraInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height,
7477
(double)_rgbDepthCameraSensor.FocalLengthMM,
7578
(double)_rgbDepthCameraSensor.SensorSizeMM.x, (double)_rgbDepthCameraSensor.SensorSizeMM.y);
79+
_depthInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height,
80+
(double)_rgbDepthCameraSensor.FocalLengthMM,
81+
(double)_rgbDepthCameraSensor.SensorSizeMM.x, (double)_rgbDepthCameraSensor.SensorSizeMM.y);
82+
7683
} else {
7784
_cameraInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height, (double)_rgbDepthCameraSensor.FieldOfViewDegrees);
85+
_depthInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height, (double)_rgbDepthCameraSensor.FieldOfViewDegrees);
86+
7887
}
7988

89+
90+
8091
}
8192

8293
protected override void ZOOnDestroy() {
8394
base.ZOOnDestroy();
8495
Terminate();
8596
}
8697

98+
protected override void ZOUpdateHzSynchronized() {
99+
base.ZOUpdateHzSynchronized();
100+
101+
// publish TF
102+
_depthTransformMessage.header.Update();
103+
_depthTransformMessage.header.frame_id = _parentTransformName;
104+
_depthTransformMessage.child_frame_id = _depthTransformName;
105+
// _depthTransformMessage.transform.translation.FromUnityVector3ToROS(Quaternion.Euler(_cameraRotationDegrees) * transform.localPosition);
106+
_depthTransformMessage.transform.translation.FromUnityVector3ToROS(transform.localPosition);
107+
108+
_depthTransformMessage.transform.rotation.FromUnityQuaternionToROS(Quaternion.Euler(_cameraRotationDegrees) * transform.localRotation);
109+
110+
// _depthTransformMessage.FromLocalUnityTransformToROS(this.transform * );
111+
112+
ROSUnityManager.BroadcastTransform(_depthTransformMessage);
113+
114+
// publish TF
115+
_colorImageTransformMessage.header.Update();
116+
_colorImageTransformMessage.header.frame_id = _parentTransformName;
117+
_colorImageTransformMessage.child_frame_id = _rgbTransformName;
118+
// _colorImageTransformMessage.FromLocalUnityTransformToROS(this.transform);
119+
// _colorImageTransformMessage.transform.translation.FromUnityVector3ToROS(Quaternion.Euler(_cameraRotationDegrees) * transform.localPosition);
120+
_colorImageTransformMessage.transform.translation.FromUnityVector3ToROS(transform.localPosition);
121+
122+
_colorImageTransformMessage.transform.rotation.FromUnityQuaternionToROS(Quaternion.Euler(_cameraRotationDegrees) * transform.localRotation);
123+
124+
125+
ROSUnityManager.BroadcastTransform(_colorImageTransformMessage);
126+
127+
}
128+
87129
private void Initialize() {
88130
// advertise
89131
ROSBridgeConnection.Advertise(_rgbImageROSTopic, _colorImageMessage.MessageType);
90132
ROSBridgeConnection.Advertise(_depthROSTopic, _depthImageMessage.MessageType);
91133
ROSBridgeConnection.Advertise(_cameraInfoROSTopic, _cameraInfoMessage.MessageType);
134+
ROSBridgeConnection.Advertise(_depthCameraInfoROSTopic, _depthInfoMessage.MessageType);
92135

93136

94137
// setup the transforms
95138
_colorImageMessage.header.frame_id = _rgbTransformName;
96-
_depthImageMessage.header.frame_id = _rgbTransformName;
139+
_depthImageMessage.header.frame_id = _depthTransformName;
97140
_cameraInfoMessage.header.frame_id = _rgbTransformName;
141+
_depthInfoMessage.header.frame_id = _depthTransformName;
98142

99143
// hookup to the sensor update delegate
100144
_rgbDepthCameraSensor.OnPublishDelegate = OnPublishRGBDepthDelegate;
@@ -106,6 +150,7 @@ private void Terminate() {
106150
ROSBridgeConnection?.UnAdvertise(_rgbImageROSTopic);
107151
ROSBridgeConnection?.UnAdvertise(_depthROSTopic);
108152
ROSBridgeConnection?.UnAdvertise(_cameraInfoROSTopic);
153+
ROSBridgeConnection?.UnAdvertise(_depthCameraInfoROSTopic);
109154

110155
// remove delegate
111156
_rgbDepthCameraSensor.OnPublishDelegate = null;
@@ -137,7 +182,7 @@ private Task OnPublishRGBDepthDelegate(ZORGBDepthCamera rgbdCamera, string camer
137182

138183

139184
// publish depth image
140-
_depthImageMessage.header = _colorImageMessage.header;//.Update();
185+
_depthImageMessage.header = _colorImageMessage.header; // color image & depth image header need to match
141186
_depthImageMessage.height = (uint)height;
142187
_depthImageMessage.width = (uint)width;
143188
_depthImageMessage.encoding = "32FC1";
@@ -149,7 +194,12 @@ private Task OnPublishRGBDepthDelegate(ZORGBDepthCamera rgbdCamera, string camer
149194

150195
_cameraInfoMessage.Update();
151196
_cameraInfoMessage.header = _colorImageMessage.header;
197+
_depthInfoMessage.Update();
198+
_depthInfoMessage.header = _depthImageMessage.header;
152199
ROSBridgeConnection.Publish<CameraInfoMessage>(_cameraInfoMessage, _cameraInfoROSTopic, cameraId);
200+
ROSBridgeConnection.Publish<CameraInfoMessage>(_depthInfoMessage, _depthCameraInfoROSTopic, cameraId);
201+
202+
// publish TF
153203

154204

155205
return Task.CompletedTask;

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSTransformPublisher.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ protected override void ZOUpdateHzSynchronized() {
4545
_transformMessage.header.Update();
4646
_transformMessage.header.frame_id = FrameID;
4747
_transformMessage.child_frame_id = ChildFrameID;
48-
_transformMessage.UnityLocalTransform = this.transform;
48+
_transformMessage.FromLocalUnityTransformToROS(this.transform);
4949

5050
ROSUnityManager.BroadcastTransform(_transformMessage);
5151

0 commit comments

Comments
 (0)