Skip to content

Commit 092b68d

Browse files
micahpearlmanmicahpearlman
committed
publishing camera info
1 parent d368691 commit 092b68d

File tree

6 files changed

+159
-44
lines changed

6 files changed

+159
-44
lines changed

‎Editor/CustomEditors/ZOROSImagePublisherEditor.cs‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
using System.Linq;
2+
using System.Collections;
3+
using System.Collections.Generic;
4+
using UnityEngine;
5+
using UnityEditor;
6+
using ZO.ROS.Publisher;
7+
8+
namespace ZO.Editor {
9+
10+
[CustomEditor(typeof(ZO.ROS.Publisher.ZOROSImagePublisher))]
11+
public class ZOROSImagePublisherEditor : UnityEditor.Editor {
12+
13+
/// <summary>
14+
/// Hides unused ROSTopic. See: https://answers.unity.com/questions/316286/how-to-remove-script-field-in-inspector.html
15+
/// </summary>
16+
/// <value></value>
17+
private static readonly string[] _dontIncludeMe = new string[] { "_ROSTopic", "_name" };
18+
public override void OnInspectorGUI() {
19+
20+
DrawPropertiesExcluding(serializedObject, _dontIncludeMe);
21+
serializedObject.ApplyModifiedProperties();
22+
23+
}
24+
}
25+
26+
}

‎Editor/CustomEditors/ZOROSImagePublisherEditor.cs.meta‎

Lines changed: 11 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

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

Lines changed: 63 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,20 @@ public class ZOROSImagePublisher : ZOROSUnityGameObjectBase, ZOSerializationInte
1717

1818
public ZORGBCamera _rgbCameraSensor;
1919

20+
[Header("ROS Topics")]
21+
/// <summary>
22+
/// The ROS Image message topic to publish to.
23+
/// See: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html
24+
/// </summary>
25+
public string _imageROSTopic = "image/image_raw";
26+
27+
/// <summary>
28+
/// The CameraInfo message topic to publish to.
29+
/// See: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html
30+
/// </summary>
31+
public string _cameraInfoROSTopic = "image/camera_info";
32+
33+
2034
/// <summary>
2135
/// The camera sensor that we will publish.
2236
/// </summary>
@@ -25,7 +39,29 @@ public ZORGBCamera RGBCameraSensor {
2539
get => _rgbCameraSensor;
2640
set => _rgbCameraSensor = value;
2741
}
42+
2843
private ImageMessage _rosImageMessage = new ImageMessage();
44+
private CameraInfoMessage _rosCameraInfoMessage = new CameraInfoMessage();
45+
46+
/// <summary>
47+
/// The ROS Image message topic to publish to.
48+
/// See: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html
49+
/// </summary>
50+
/// <value></value>
51+
public string ImageROSTopic {
52+
get => _imageROSTopic;
53+
set => _imageROSTopic = value;
54+
}
55+
56+
/// <summary>
57+
/// The CameraInfo message topic to publish to.
58+
/// See: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html
59+
/// </summary>
60+
/// <value></value>
61+
public string CameraInfoROSTopic {
62+
get => _cameraInfoROSTopic;
63+
set => _cameraInfoROSTopic = value;
64+
}
2965

3066

3167
protected override void ZOStart() {
@@ -40,7 +76,8 @@ protected override void ZOOnDestroy() {
4076

4177
private void Initialize() {
4278
// advertise
43-
ROSBridgeConnection.Advertise(ROSTopic, _rosImageMessage.MessageType);
79+
ROSBridgeConnection.Advertise(ImageROSTopic, _rosImageMessage.MessageType);
80+
ROSBridgeConnection.Advertise(CameraInfoROSTopic, _rosCameraInfoMessage.MessageType);
4481

4582
// hookup to the sensor update delegate
4683
_rgbCameraSensor.OnPublishRGBImageDelegate = OnPublishRGBImageDelegate;
@@ -55,7 +92,8 @@ public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
5592

5693
public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
5794
Debug.Log("INFO: ZOImagePublisher::OnROSBridgeDisconnected");
58-
ROSBridgeConnection.UnAdvertise(ROSTopic);
95+
ROSBridgeConnection?.UnAdvertise(ImageROSTopic);
96+
ROSBridgeConnection?.UnAdvertise(CameraInfoROSTopic);
5997
}
6098

6199

@@ -70,21 +108,40 @@ public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager)
70108
/// <param name="rgbData">Raw RBG8 data </param>
71109
/// <returns></returns>
72110
private Task OnPublishRGBImageDelegate(ZORGBCamera rgbCamera, string cameraId, int width, int height, byte[] rgbData) {
111+
112+
// figure out the transforms
73113
ZOROSTransformPublisher transformPublisher = GetComponent<ZOROSTransformPublisher>();
74114
if (transformPublisher != null) {
75-
_rosImageMessage.header.frame_id = transformPublisher.ChildFrameID;
115+
_rosImageMessage.header.frame_id = transformPublisher.ChildFrameID;
116+
_rosCameraInfoMessage.header.frame_id = transformPublisher.ChildFrameID;
76117
} else {
77-
_rosImageMessage.header.frame_id = Name;
118+
_rosImageMessage.header.frame_id = Name;
119+
_rosCameraInfoMessage.header.frame_id = Name;
78120
}
79-
121+
122+
// setup and send Image message
80123
_rosImageMessage.header.Update();
81124
_rosImageMessage.height = (uint)height;
82125
_rosImageMessage.width = (uint)width;
83126
_rosImageMessage.encoding = "rgb8";
84127
_rosImageMessage.is_bigendian = 0;
85128
_rosImageMessage.step = 1 * 3 * (uint)width;
86129
_rosImageMessage.data = rgbData;
87-
ROSBridgeConnection.Publish<ImageMessage>(_rosImageMessage, ROSTopic, Name);
130+
ROSBridgeConnection.Publish<ImageMessage>(_rosImageMessage, _imageROSTopic, Name);
131+
132+
// setup and send CameraInfo message
133+
_rosCameraInfoMessage.Update();
134+
_rosCameraInfoMessage.header = _rosImageMessage.header;
135+
// initialize the camera info
136+
if (RGBCameraSensor.UnityCamera.usePhysicalProperties == true) {
137+
_rosCameraInfoMessage.BuildCameraInfo((uint)RGBCameraSensor.Width, (uint)RGBCameraSensor.Height,
138+
(double)RGBCameraSensor.FocalLengthMM,
139+
(double)RGBCameraSensor.SensorSizeMM.x, (double)RGBCameraSensor.SensorSizeMM.y);
140+
} else {
141+
_rosCameraInfoMessage.BuildCameraInfo((uint)RGBCameraSensor.Width, (uint)RGBCameraSensor.Height, (double)RGBCameraSensor.FieldOfViewDegrees);
142+
}
143+
144+
ROSBridgeConnection.Publish<CameraInfoMessage>(_rosCameraInfoMessage, _cameraInfoROSTopic, cameraId);
88145

89146
return Task.CompletedTask;
90147
}

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

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ public ZORGBDepthCamera RGBDepthCameraSensor {
4545
}
4646
private ImageMessage _colorImageMessage = new ImageMessage();
4747
private ImageMessage _depthImageMessage = new ImageMessage();
48-
private CameraInfoMessage _camerInfoMessage = new CameraInfoMessage();
48+
private CameraInfoMessage _cameraInfoMessage = new CameraInfoMessage();
4949

5050
protected override void ZOReset() {
5151
base.ZOReset();
@@ -59,11 +59,11 @@ protected override void ZOStart() {
5959
}
6060
// initialize the camera info
6161
if (_rgbDepthCameraSensor.UnityCamera.usePhysicalProperties == true) {
62-
_camerInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height,
62+
_cameraInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height,
6363
(double)_rgbDepthCameraSensor.FocalLengthMM,
6464
(double)_rgbDepthCameraSensor.SensorSizeMM.x, (double)_rgbDepthCameraSensor.SensorSizeMM.y);
6565
} else {
66-
_camerInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height, (double)_rgbDepthCameraSensor.FieldOfViewDegrees);
66+
_cameraInfoMessage.BuildCameraInfo((uint)_rgbDepthCameraSensor.Width, (uint)_rgbDepthCameraSensor.Height, (double)_rgbDepthCameraSensor.FieldOfViewDegrees);
6767
}
6868

6969
}
@@ -77,13 +77,13 @@ private void Initialize() {
7777
// advertise
7878
ROSBridgeConnection.Advertise(_rgbImageROSTopic, _colorImageMessage.MessageType);
7979
ROSBridgeConnection.Advertise(_depthROSTopic, _depthImageMessage.MessageType);
80-
ROSBridgeConnection.Advertise(_cameraInfoROSTopic, _camerInfoMessage.MessageType);
80+
ROSBridgeConnection.Advertise(_cameraInfoROSTopic, _cameraInfoMessage.MessageType);
8181

8282

8383
// setup the transforms
8484
_colorImageMessage.header.frame_id = _rgbTransformName;
8585
_depthImageMessage.header.frame_id = _rgbTransformName;
86-
_camerInfoMessage.header.frame_id = _rgbTransformName;
86+
_cameraInfoMessage.header.frame_id = _rgbTransformName;
8787

8888
// hookup to the sensor update delegate
8989
_rgbDepthCameraSensor.OnPublishDelegate = OnPublishRGBDepthDelegate;
@@ -136,9 +136,9 @@ private Task OnPublishRGBDepthDelegate(ZORGBDepthCamera rgbdCamera, string camer
136136
System.Buffer.BlockCopy(depthData, 0, _depthImageMessage.data, 0, 4 * width * height);
137137
ROSBridgeConnection.Publish<ImageMessage>(_depthImageMessage, _depthROSTopic, cameraId);
138138

139-
_camerInfoMessage.Update();
140-
_camerInfoMessage.header = _colorImageMessage.header;
141-
ROSBridgeConnection.Publish<CameraInfoMessage>(_camerInfoMessage, _cameraInfoROSTopic, cameraId);
139+
_cameraInfoMessage.Update();
140+
_cameraInfoMessage.header = _colorImageMessage.header;
141+
ROSBridgeConnection.Publish<CameraInfoMessage>(_cameraInfoMessage, _cameraInfoROSTopic, cameraId);
142142

143143

144144
return Task.CompletedTask;

‎Runtime/Scripts/Sensors/RGBCamera/ZORGBCamera.cs‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,60 @@ public class ZORGBCamera : ZOGameObjectBase, ZOSerializationInterface {
1717
[Header("Camera Parameters")]
1818
// string _cameraId = "none";
1919
public Camera _camera;
20+
21+
/// <summary>
22+
/// The Unity Camera associated with this depth camera. (readonly)
23+
/// </summary>
24+
/// <value></value>
2025
public Camera UnityCamera {
2126
get => _camera;
2227
private set => _camera = value;
2328
}
2429
public int _width = 1280;
30+
31+
/// <summary>
32+
/// Camera frame width in pixels. (readonly)
33+
/// </summary>
34+
/// <value></value>
35+
public int Width {
36+
get => _width;
37+
}
38+
2539
public int _height = 720;
40+
/// <summary>
41+
/// Camera frame height in pixels. (readonly)
42+
/// </summary>
43+
/// <value></value>
44+
public int Height {
45+
get => _height;
46+
}
47+
48+
49+
/// <summary>
50+
/// The camera focal length in millimeters.
51+
/// </summary>
52+
/// <value></value>
53+
public float FocalLengthMM {
54+
get { return UnityCamera.focalLength; }
55+
}
56+
57+
/// <summary>
58+
/// The cameras field of view in degrees.
59+
/// </summary>
60+
/// <value></value>
61+
public float FieldOfViewDegrees {
62+
get { return UnityCamera.fieldOfView; }
63+
}
64+
65+
/// <summary>
66+
/// Sensor width and height in millimeters.
67+
/// </summary>
68+
/// <value></value>
69+
public Vector2 SensorSizeMM {
70+
get { return UnityCamera.sensorSize; }
71+
}
72+
73+
2674

2775
[Header("Render Parameters")]
2876
[SerializeField]
@@ -84,6 +132,7 @@ protected void Initialize() {
84132
} else {
85133
Debug.LogWarning("WARNING: NO support for native AsyncGPUReadback. Using 3rd party.");
86134
}
135+
87136

88137
}
89138

‎Samples~/ZeroSimSamples/Scenes/ROSImagePublish_test.unity‎

Lines changed: 2 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -384,36 +384,6 @@ Transform:
384384
m_Father: {fileID: 0}
385385
m_RootOrder: 1
386386
m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0}
387-
--- !u!1 &523318794
388-
GameObject:
389-
m_ObjectHideFlags: 0
390-
m_CorrespondingSourceObject: {fileID: 0}
391-
m_PrefabInstance: {fileID: 0}
392-
m_PrefabAsset: {fileID: 0}
393-
serializedVersion: 6
394-
m_Component:
395-
- component: {fileID: 523318796}
396-
m_Layer: 0
397-
m_Name: ROSImagePublisher
398-
m_TagString: Untagged
399-
m_Icon: {fileID: 0}
400-
m_NavMeshLayer: 0
401-
m_StaticEditorFlags: 0
402-
m_IsActive: 1
403-
--- !u!4 &523318796
404-
Transform:
405-
m_ObjectHideFlags: 0
406-
m_CorrespondingSourceObject: {fileID: 0}
407-
m_PrefabInstance: {fileID: 0}
408-
m_PrefabAsset: {fileID: 0}
409-
m_GameObject: {fileID: 523318794}
410-
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
411-
m_LocalPosition: {x: -2.4141068, y: 6.1704483, z: 1.466458}
412-
m_LocalScale: {x: 1, y: 1, z: 1}
413-
m_Children: []
414-
m_Father: {fileID: 0}
415-
m_RootOrder: 5
416-
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
417387
--- !u!1 &602654387 stripped
418388
GameObject:
419389
m_CorrespondingSourceObject: {fileID: 367586929377166707, guid: bf48e06576e6281ca91670f71ef1f6b4,
@@ -459,6 +429,8 @@ MonoBehaviour:
459429
_ROSTopic: unity_image/image
460430
_name:
461431
_rgbCameraSensor: {fileID: 1852383705}
432+
_imageROSTopic: image/image_raw
433+
_cameraInfoROSTopic: image/camera_info
462434
--- !u!1 &975845539
463435
GameObject:
464436
m_ObjectHideFlags: 0

0 commit comments

Comments
 (0)