Skip to content

Commit 5f0804a

Browse files
micahpearlmanmicahpearlman
committed
implemented IMU publisher
1 parent 368d06a commit 5f0804a

File tree

8 files changed

+3043
-2319
lines changed

8 files changed

+3043
-2319
lines changed

‎Runtime/Scripts/ROS/MessageTypes/Sensor/ZOROSSensorMessages.cs‎

Lines changed: 62 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
using System;
22

33
using ZO.ROS.MessageTypes.Std;
4+
using ZO.ROS.MessageTypes.Geometry;
45

56
namespace ZO.ROS.MessageTypes.Sensor {
67

@@ -197,6 +198,64 @@ public void Update() {
197198
}
198199
}
199200

201+
/// <summary>
202+
/// This is a message to hold data from an IMU (Inertial Measurement Unit)
203+
///
204+
/// Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
205+
///
206+
/// If the covariance of the measurement is known, it should be filled in (if all you know is the
207+
/// variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
208+
/// A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
209+
/// data a covariance will have to be assumed or gotten from some other source
210+
///
211+
/// If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation
212+
/// estimate), please set element 0 of the associated covariance matrix to -1
213+
/// If you are interpreting this message, please check for a value of -1 in the first element of each
214+
/// covariance matrix, and disregard the associated estimate.
215+
/// </summary>
216+
public class ImuMessage : ZOROSMessageInterface {
217+
[Newtonsoft.Json.JsonIgnore]
218+
public string MessageType { get { return ImuMessage.Type; } }
219+
220+
[Newtonsoft.Json.JsonIgnore]
221+
public static string Type { get { return "sensor_msgs/Imu"; } }
222+
223+
public HeaderMessage header { get; set; }
224+
225+
public QuaternionMessage orientation { get; set; }
226+
public double[] orientation_covariance { get; set; }
227+
public Vector3Message angular_velocity { get; set; }
228+
public double[] angular_velocity_covariance { get; set; }
229+
// Row major about x, y, z axes
230+
public Vector3Message linear_acceleration { get; set; }
231+
public double[] linear_acceleration_covariance { get; set; }
232+
// Row major x, y z
233+
234+
public ImuMessage() {
235+
this.header = new HeaderMessage();
236+
this.orientation = new QuaternionMessage();
237+
this.orientation_covariance = new double[9];
238+
this.angular_velocity = new Vector3Message();
239+
this.angular_velocity_covariance = new double[9];
240+
this.linear_acceleration = new Vector3Message();
241+
this.linear_acceleration_covariance = new double[9];
242+
}
243+
244+
public ImuMessage(HeaderMessage header, QuaternionMessage orientation, double[] orientation_covariance, Vector3Message angular_velocity, double[] angular_velocity_covariance, Vector3Message linear_acceleration, double[] linear_acceleration_covariance) {
245+
this.header = header;
246+
this.orientation = orientation;
247+
this.orientation_covariance = orientation_covariance;
248+
this.angular_velocity = angular_velocity;
249+
this.angular_velocity_covariance = angular_velocity_covariance;
250+
this.linear_acceleration = linear_acceleration;
251+
this.linear_acceleration_covariance = linear_acceleration_covariance;
252+
}
253+
254+
public void Update() {
255+
this.header.Update();
256+
}
257+
258+
}
200259

201260
/// <summary>
202261
/// This message is used to specify a region of interest within an image.
@@ -449,9 +508,9 @@ public void BuildCameraInfo(uint width, uint height, double fovRadians) {
449508
double cy = this.height / 2.0;
450509
// focal length pixels = focal length mm * pixel width / sensor size mm
451510
//double fx = 37.46 * 640.0 / 36.0;// //360;//37.46;//this.width / (2.0 * System.Math.Tan(fovRadians) * System.Math.PI / 360.0);
452-
double fx = this.width / System.Math.Tan(fovRadians/2.0);
511+
double fx = this.width / System.Math.Tan(fovRadians / 2.0);
453512
double fy = fx;
454-
513+
455514
this.K = new double[9] {fx, 0, cx,
456515
0, fy, cy,
457516
0, 0, 1};
@@ -487,7 +546,7 @@ public void BuildCameraInfo(uint width, uint height, double focalLengthMM, doubl
487546
//double fx = 37.46 * 640.0 / 36.0;// //360;//37.46;//this.width / (2.0 * System.Math.Tan(fovRadians) * System.Math.PI / 360.0);
488547
double fx = focalLengthMM * this.width / sensorSizeXMM;
489548
double fy = focalLengthMM * this.height / sensorSizeYMM;
490-
549+
491550
this.K = new double[9] {fx, 0, cx,
492551
0, fy, cy,
493552
0, 0, 1};

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

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
using System.Threading.Tasks;
2+
using UnityEngine;
3+
using Newtonsoft.Json.Linq;
4+
using ZO.ROS.MessageTypes.Sensor;
5+
using ZO.ROS.MessageTypes.Geometry;
6+
using ZO.ROS.Publisher;
7+
using ZO.Sensors;
8+
using ZO.ROS.Unity;
9+
using ZO.Document;
10+
11+
namespace ZO.ROS.Publisher {
12+
13+
/// <summary>
14+
/// Publish ROS Imu message using ZOIMU sensor.
15+
/// </summary>
16+
[RequireComponent(typeof(ZOROSTransformPublisher))]
17+
[RequireComponent(typeof(ZOIMU))]
18+
public class ZOROSIMUPublisher : ZOROSUnityGameObjectBase {
19+
20+
public ZOIMU _imuSensor;
21+
22+
/// <summary>
23+
/// The IMU sensor.
24+
/// </summary>
25+
/// <value></value>
26+
public ZOIMU IMUSensor {
27+
get => _imuSensor;
28+
set => _imuSensor = value;
29+
}
30+
31+
32+
private ZOROSTransformPublisher _transformPublisher = null;
33+
public ZOROSTransformPublisher TransformPublisher {
34+
get {
35+
if (_transformPublisher == null) {
36+
_transformPublisher = GetComponent<ZOROSTransformPublisher>();
37+
}
38+
39+
if (_transformPublisher == null) {
40+
Debug.LogError("ERROR: ZOROSLaserScanPublisher is missing corresponding ZOROSTransformPublisher");
41+
}
42+
return _transformPublisher;
43+
}
44+
}
45+
46+
private ImuMessage _imuMessage = new ImuMessage();
47+
48+
49+
protected override void ZOStart() {
50+
base.ZOStart();
51+
if (ZOROSBridgeConnection.Instance.IsConnected) {
52+
Initialize();
53+
}
54+
}
55+
56+
private void Initialize() {
57+
// advertise
58+
ROSBridgeConnection.Advertise(ROSTopic, _imuMessage.MessageType);
59+
60+
// hookup to the sensor update delegate
61+
IMUSensor.OnPublishDelegate = OnPublishImuDelegate;
62+
63+
}
64+
65+
66+
protected override void ZOOnDestroy() {
67+
ROSBridgeConnection?.UnAdvertise(ROSTopic);
68+
}
69+
70+
public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
71+
Debug.Log("INFO: ZOROSLaserScanPublisher::OnROSBridgeConnected");
72+
Initialize();
73+
}
74+
75+
public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
76+
Debug.Log("INFO: ZOROSLaserScanPublisher::OnROSBridgeDisconnected");
77+
ROSBridgeConnection.UnAdvertise(ROSTopic);
78+
}
79+
80+
private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel, Vector3 angularVelocity, Quaternion orientation) {
81+
_imuMessage.header.Update();
82+
_imuMessage.header.frame_id = TransformPublisher.ChildFrameID;
83+
_imuMessage.linear_acceleration.UnityVector3 = linearAccel;
84+
_imuMessage.angular_velocity.UnityVector3 = angularVelocity;
85+
_imuMessage.orientation.UnityQuaternion = orientation;
86+
87+
// if (_referenceFrame == ReferenceFrame.RightHanded_XBackward_YLeft_ZUp) {
88+
// // (x, y, z, w) -> (-x, -z, y, -w).
89+
// publishedOrientation = new Quaternion(-transform.rotation.z, -transform.rotation.x, transform.rotation.y, -transform.rotation.w);
90+
// // if (m_zReverse)
91+
// // rot = new Quaternion (-rot.x, rot.y, -rot.z, rot.w);
92+
// publishedAcceleration = new Vector3(-_acceleration.z, -_acceleration.x, _acceleration.y);
93+
// publishedAngularVelocity = new Vector3(-_angularVelocity.z, -_angularVelocity.x, _angularVelocity.y);
94+
// }
95+
96+
97+
ROSBridgeConnection.Publish(_imuMessage, ROSTopic, Name);
98+
99+
return Task.CompletedTask;
100+
}
101+
102+
#region ZOSerializationInterface
103+
public override string Type {
104+
get { return "ros.publisher.imu"; }
105+
}
106+
107+
108+
public override JObject Serialize(ZOSimDocumentRoot documentRoot, UnityEngine.Object parent = null) {
109+
JObject json = new JObject(
110+
new JProperty("name", Name),
111+
new JProperty("type", Type),
112+
new JProperty("ros_topic", ROSTopic),
113+
new JProperty("update_rate_hz", UpdateRateHz),
114+
new JProperty("imu_name", IMUSensor.Name)
115+
);
116+
JSON = json;
117+
return json;
118+
}
119+
120+
public override void Deserialize(ZOSimDocumentRoot documentRoot, JObject json) {
121+
_json = json;
122+
Name = json["name"].Value<string>();
123+
ROSTopic = json["ros_topic"].Value<string>();
124+
UpdateRateHz = json["update_rate_hz"].Value<float>();
125+
126+
// find connected imu. needs to be done post load hence the Lamda
127+
documentRoot.OnPostDeserializationNotification((docRoot) => {
128+
if (JSON.ContainsKey("imu_name")) {
129+
ZOIMU[] imus = docRoot.gameObject.GetComponentsInChildren<ZOIMU>();
130+
foreach (ZOIMU l in imus) {
131+
if (l.Name == JSON["imu_name"].Value<string>()) {
132+
IMUSensor = l;
133+
}
134+
}
135+
}
136+
});
137+
138+
}
139+
140+
#endregion // ZOSerializationInterface
141+
142+
}
143+
144+
}

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.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/ZOROSLaserScanPublisher.cs‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,8 @@
1111
namespace ZO.ROS.Publisher {
1212

1313
/// <summary>
14-
/// Publish /sensor/LaserScan message.
14+
/// Publish ROS /sensor/LaserScan message using the ZOLIDAR2D sensor.
15+
/// See: http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/LaserScan.html
1516
/// </summary>
1617
[RequireComponent(typeof(ZOROSTransformPublisher))]
1718
[RequireComponent(typeof(ZOLIDAR2D))]

‎Runtime/Scripts/Sensors/IMU/ZOIMU.cs‎

Lines changed: 64 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -3,24 +3,21 @@
33
using System.Collections.Generic;
44
using System.Threading.Tasks;
55
using UnityEngine;
6+
using Newtonsoft.Json.Linq;
67

7-
namespace ZO.Util {
8+
using ZO.Document;
9+
using ZO.Util;
10+
using ZO.Util.Extensions;
811

9-
[RequireComponent(typeof(Rigidbody))]
10-
public class ZOIMU : ZOGameObjectBase {
1112

12-
[Header("IMU Parametrs")]
13-
public String _imuId = "none";
13+
namespace ZO.Sensors {
1414

15-
// RightHanded_XBackward_YLeft_ZUp,
16-
// LeftHanded_XRight_YUp_ZForward // Unity Standard
15+
[RequireComponent(typeof(Rigidbody))]
16+
public class ZOIMU : ZOGameObjectBase, ZOSerializationInterface {
1717

18-
public enum ReferenceFrame {
19-
RightHanded_XBackward_YLeft_ZUp,
20-
LeftHanded_XRight_YUp_ZForward // Unity Standard
21-
};
18+
[Header("IMU Parameters")]
19+
public String _imuId = "none";
2220

23-
public ReferenceFrame _referenceFrame = ReferenceFrame.LeftHanded_XRight_YUp_ZForward;
2421

2522
[Header("Noise Models")]
2623
/// <summary>
@@ -90,8 +87,11 @@ public Quaternion Orientation {
9087
get { return _orientation; }
9188
}
9289

90+
91+
#region ZOGameObjectBase
92+
9393
// Start is called before the first frame update
94-
void Start() {
94+
protected override void ZOStart() {
9595
_rigidBody = GetComponent<Rigidbody>();
9696
}
9797

@@ -115,21 +115,15 @@ protected override async void ZOFixedUpdate() {
115115
Vector3 publishedAngularVelocity = _angularVelocity;
116116
Quaternion publishedOrientation = transform.rotation;
117117

118-
if (_referenceFrame == ReferenceFrame.RightHanded_XBackward_YLeft_ZUp) {
119-
// (x, y, z, w) -> (-x, -z, y, -w).
120-
publishedOrientation = new Quaternion(-transform.rotation.z, -transform.rotation.x, transform.rotation.y, -transform.rotation.w);
121-
// if (m_zReverse)
122-
// rot = new Quaternion (-rot.x, rot.y, -rot.z, rot.w);
123-
publishedAcceleration = new Vector3(-_acceleration.z, -_acceleration.x, _acceleration.y);
124-
publishedAngularVelocity = new Vector3(-_angularVelocity.z, -_angularVelocity.x, _angularVelocity.y);
125-
}
126118

127119
if (OnPublishDelegate != null) {
128120
await OnPublishDelegate(this, _imuId, publishedAcceleration, publishedAngularVelocity, publishedOrientation);
129121
}
130122

131123
}
132124

125+
#endregion // ZOGameObjectBase
126+
133127
/* # Notes:
134128
135129
* To calculate angle using complementary filter
@@ -138,6 +132,55 @@ protected override async void ZOFixedUpdate() {
138132
139133
*/
140134

135+
#region ZOSerializationInterface
136+
public string Type {
137+
get { return "sensor.imu"; }
138+
}
139+
140+
[SerializeField] public string _name;
141+
public string Name {
142+
get {
143+
return _name;
144+
}
145+
private set {
146+
_name = value;
147+
}
148+
}
149+
150+
private JObject _json;
151+
public JObject JSON {
152+
get => _json;
153+
}
154+
155+
156+
public JObject Serialize(ZOSimDocumentRoot documentRoot, UnityEngine.Object parent = null) {
157+
JObject json = new JObject(
158+
new JProperty("name", Name),
159+
new JProperty("type", Type),
160+
new JProperty("update_rate_hz", UpdateRateHz)
161+
);
162+
163+
164+
ZOSimOccurrence parent_occurrence = GetComponent<ZOSimOccurrence>();
165+
if (parent_occurrence) {
166+
json["parent_occurrence"] = parent_occurrence.Name;
167+
}
168+
169+
_json = json;
170+
171+
return json;
172+
}
173+
174+
175+
public void Deserialize(ZOSimDocumentRoot documentRoot, JObject json) {
176+
// Assert.Equals(json["type"].Value<string>() == Type);
177+
_json = json;
178+
Name = json.ValueOrDefault("name", Name);
179+
UpdateRateHz = json.ValueOrDefault("update_rate_hz", UpdateRateHz);
180+
181+
182+
}
183+
#endregion // ZOSerializationInterface
141184

142185

143186
}

0 commit comments

Comments
 (0)