Skip to content

Commit 5550017

Browse files
micahpearlmanmicahpearlman
committed
implemented fake odometry publisher
1 parent 664af39 commit 5550017

File tree

7 files changed

+351
-23
lines changed

7 files changed

+351
-23
lines changed

‎Runtime/Scripts/Physics/ZOFixedJoint.cs‎

Lines changed: 38 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
using Newtonsoft.Json.Linq;
77
using ZO.Util.Extensions;
88
using ZO.Document;
9+
using ZO.Util;
910

1011

1112
namespace ZO.Physics {
@@ -21,9 +22,12 @@ namespace ZO.Physics {
2122
/// constrains the joint angle.
2223
/// </summary>
2324
[ExecuteAlways]
24-
public class ZOFixedJoint : MonoBehaviour, ZOJointInterface {
25+
[RequireComponent(typeof(Rigidbody))]
26+
[RequireComponent(typeof(HingeJoint))]
27+
public class ZOFixedJoint : ZOGameObjectBase, ZOJointInterface {
2528

2629
[SerializeField] public UnityEngine.FixedJoint _fixedJoint;
30+
public Rigidbody _connectedBody;
2731

2832
/// <summary>
2933
/// The Unity fixed joint linked to this ZOSim fixed joint.
@@ -64,6 +68,10 @@ public float Effort {
6468
set { }
6569
}
6670

71+
/// <summary>
72+
/// The connected rigid body. If null then it is the world.
73+
/// </summary>
74+
/// <value></value>
6775
/// <summary>
6876
/// The connected rigid body. If null then it is the world.
6977
/// </summary>
@@ -73,7 +81,30 @@ public Rigidbody ConnectedBody {
7381
return UnityFixedJoint.connectedBody;
7482
}
7583
set {
76-
UnityFixedJoint.connectedBody = value;
84+
if (UnityFixedJoint.connectedBody != value) {
85+
_connectedBody = value;
86+
UnityFixedJoint.connectedBody = value;
87+
}
88+
89+
// update the name
90+
if (string.IsNullOrEmpty(Name)) {
91+
Name = Type;
92+
93+
ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();
94+
if (occurrence) {
95+
Name = Name + "_from_" + occurrence.Name;
96+
}
97+
98+
if (UnityFixedJoint.connectedBody) {
99+
ZOSimOccurrence connected_occurrence = UnityFixedJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();
100+
101+
if (connected_occurrence) {
102+
Name = Name + "_to_" + connected_occurrence.Name;
103+
}
104+
}
105+
106+
}
107+
77108
}
78109
}
79110

@@ -91,6 +122,11 @@ public ZOSimOccurrence ConnectedOccurrence {
91122
#endregion
92123

93124

125+
protected override void ZOOnValidate() {
126+
base.ZOOnValidate();
127+
ConnectedBody = _connectedBody;
128+
}
129+
94130

95131
/// <summary>
96132
/// Reset is a MonoBehavior method that gets called on creation of this component.
@@ -128,7 +164,6 @@ public void CreateRequirements() {
128164
}
129165

130166

131-
#region ZOSerializationInterface
132167
public string Type {
133168
get { return "joint.fixed"; }
134169
}
@@ -143,19 +178,6 @@ private set {
143178
}
144179
}
145180

146-
private JObject _json;
147-
public JObject JSON {
148-
get {
149-
// if (_json == null) {
150-
// _json = BuildJSON();
151-
// }
152-
return _json;
153-
154-
}
155-
}
156-
157-
158-
#endregion
159181

160182
}
161183

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

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -134,6 +134,12 @@ public UnityEngine.Quaternion UnityQuaternion {
134134
get { return ToUnityQuaternion(); }
135135
set { FromUnityQuaternion(value); }
136136
}
137+
138+
[Newtonsoft.Json.JsonIgnore]
139+
public UnityEngine.Quaternion ROSQuaternion {
140+
get { return new UnityEngine.Quaternion((float)this.x, (float)this.y, (float)this.z, (float)this.w); }
141+
}
142+
137143
}
138144

139145
/// <summary>

‎Runtime/Scripts/ROS/MessageTypes/Nav/ZOROSNavMessages.cs‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,11 @@ namespace ZO.ROS.MessageTypes.Nav {
1212
public class OdometryMessage : ZOROSMessageInterface {
1313

1414
[Newtonsoft.Json.JsonIgnore]
15-
public string MessageType { get { return "nav_msgs/Odometry"; } }
15+
public string MessageType { get { return OdometryMessage.Type; } }
16+
17+
[Newtonsoft.Json.JsonIgnore]
18+
public static string Type = "nav_msgs/Odometry";
19+
1620

1721
public HeaderMessage header { get; set; }
1822
public string child_frame_id { get; set; }

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

Lines changed: 194 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,194 @@
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.ROS.Unity;
8+
using ZO.ROS.MessageTypes.Nav;
9+
using ZO.Math;
10+
using ZO.Document;
11+
using ZO.Sensors;
12+
13+
14+
namespace ZO.ROS.Publisher {
15+
16+
/// <summary>
17+
/// Publish fake Odometry by just taking the transform of whatever this script is.
18+
///
19+
/// Implemented noise mode: https://blog.lxsang.me/post/id/16
20+
/// </summary>
21+
[RequireComponent(typeof(Rigidbody))]
22+
public class ZOROSFakeOdometryPublisher : ZOROSUnityGameObjectBase {
23+
24+
public bool _applyNoise = false;
25+
public float _a1 = 0.05f;
26+
public float _a2 = 15.0f;
27+
public float _a3 = 0.05f;
28+
public float _a4 = 0.01f;
29+
30+
public Rigidbody Rigidbody {
31+
get {
32+
return gameObject.GetComponent<Rigidbody>();
33+
}
34+
}
35+
36+
private ZOGaussianNoiseModel _noiseModel = new ZOGaussianNoiseModel();
37+
private OdometryMessage _currentOdometryMessage = null;
38+
private OdometryMessage _previousOdometryMessage = null;
39+
private TransformStampedMessage _transformStampedMessage = new TransformStampedMessage();
40+
41+
public override string Type {
42+
get { return "publisher.odometry"; }
43+
}
44+
45+
46+
protected override void ZOOnValidate() {
47+
base.ZOOnValidate();
48+
if (string.IsNullOrEmpty(ROSTopic) == true) {
49+
ROSTopic = "/odom";
50+
}
51+
52+
if (UpdateRateHz == 0) {
53+
UpdateRateHz = 20;
54+
}
55+
56+
}
57+
58+
protected override void ZOFixedUpdateHzSynchronized() {
59+
base.ZOFixedUpdateHzSynchronized();
60+
61+
// publish odometry
62+
if (ZOROSBridgeConnection.Instance.IsConnected) {
63+
64+
// See: http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
65+
66+
_currentOdometryMessage = new OdometryMessage();
67+
68+
// Publish odom on TF as well
69+
_transformStampedMessage.header.Update();
70+
_transformStampedMessage.header.frame_id = ZOROSUnityManager.Instance.WorldFrameId; // connect to the world
71+
_transformStampedMessage.child_frame_id = "odom";
72+
_transformStampedMessage.UnityLocalTransform = Rigidbody.transform;
73+
ZOROSUnityManager.Instance.BroadcastTransform(_transformStampedMessage);
74+
75+
76+
_currentOdometryMessage.Update(); // update times stamps
77+
78+
// BUGBUG: not super clear on where the pose should be?
79+
_currentOdometryMessage.pose.pose.GlobalUnityTransform = Rigidbody.transform;
80+
81+
// get velocity in /odom frame
82+
Vector3 linear = Rigidbody.velocity;
83+
_currentOdometryMessage.twist.twist.angular.z = -Rigidbody.angularVelocity.y; // NOTE: negating velocity?
84+
85+
float yaw = Rigidbody.transform.localRotation.eulerAngles.y * Mathf.Deg2Rad;
86+
_currentOdometryMessage.twist.twist.linear.x = Mathf.Cos(yaw) * linear.z + Mathf.Sin(yaw) * linear.x;
87+
_currentOdometryMessage.twist.twist.linear.y = Mathf.Cos(yaw) * linear.x - Mathf.Sin(yaw) * linear.z;
88+
// set covariance
89+
// see: https://robotics.stackexchange.com/questions/15265/ros-gazebo-odometry-issue
90+
// # Odometry covariances for the encoder output of the robot. These values should
91+
// # be tuned to your robot's sample odometry data, but these values are a good place
92+
// # to start
93+
// pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
94+
// twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
95+
_currentOdometryMessage.pose.covariance[0] = 1e-3;
96+
_currentOdometryMessage.pose.covariance[7] = 1e-3;
97+
_currentOdometryMessage.pose.covariance[14] = 1e6;
98+
99+
_currentOdometryMessage.pose.covariance[21] = 1e6;
100+
_currentOdometryMessage.pose.covariance[28] = 1e6;
101+
_currentOdometryMessage.pose.covariance[35] = 1e3;
102+
103+
_currentOdometryMessage.twist.covariance[0] = 1e-3;
104+
_currentOdometryMessage.twist.covariance[7] = 1e-3;
105+
_currentOdometryMessage.twist.covariance[14] = 1e6;
106+
_currentOdometryMessage.twist.covariance[21] = 1e6;
107+
_currentOdometryMessage.twist.covariance[28] = 1e6;
108+
_currentOdometryMessage.twist.covariance[35] = 1e3;
109+
110+
// BUGBUG: not super clear on this being a child of map?
111+
_currentOdometryMessage.header.frame_id = ZOROSUnityManager.Instance.WorldFrameId;
112+
_currentOdometryMessage.child_frame_id = GetComponent<ZOSimOccurrence>().Name;
113+
114+
if (_applyNoise) {
115+
if (_previousOdometryMessage != null) {
116+
ApplyNoise(ref _currentOdometryMessage, _previousOdometryMessage);
117+
}
118+
}
119+
120+
ZOROSBridgeConnection.Instance.Publish<OdometryMessage>(_currentOdometryMessage, ROSTopic);
121+
_previousOdometryMessage = _currentOdometryMessage;
122+
}
123+
124+
}
125+
126+
127+
128+
129+
private void Initialize() {
130+
// advertise
131+
ROSBridgeConnection.Advertise(ROSTopic, OdometryMessage.Type);
132+
133+
}
134+
135+
public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
136+
Debug.Log("INFO: ZOImagePublisher::OnROSBridgeConnected");
137+
Initialize();
138+
139+
}
140+
141+
public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
142+
Debug.Log("INFO: ZOImagePublisher::OnROSBridgeDisconnected");
143+
ROSBridgeConnection?.UnAdvertise(ROSTopic);
144+
}
145+
146+
/// <summary>
147+
/// Apply noise to OdometryMessage.
148+
///
149+
/// See: https://blog.lxsang.me/post/id/16
150+
/// </summary>
151+
/// <param name="currentOdomMessage"></param>
152+
/// <param name="previousOdomMessage"></param>
153+
private void ApplyNoise(ref OdometryMessage currentOdomMessage, OdometryMessage previousOdomMessage) {
154+
155+
double dx = currentOdomMessage.pose.pose.position.x - previousOdomMessage.pose.pose.position.x;
156+
double dy = currentOdomMessage.pose.pose.position.y - previousOdomMessage.pose.pose.position.y;
157+
double trans = System.Math.Sqrt(dx * dx + dy * dy);
158+
QuaternionMessage quaternionMessage = previousOdomMessage.pose.pose.orientation;
159+
Quaternion q = quaternionMessage.UnityQuaternion;
160+
double r = q.eulerAngles.z * Mathf.Deg2Rad;
161+
double p = q.eulerAngles.x * Mathf.Deg2Rad;
162+
double theta1 = q.eulerAngles.y * Mathf.Deg2Rad;
163+
q = currentOdomMessage.pose.pose.orientation.UnityQuaternion;
164+
double theta2 = q.eulerAngles.y * Mathf.Deg2Rad;
165+
166+
double rot1 = System.Math.Atan2(dy, dx) - theta1;
167+
double rot2 = theta2 - theta1 - rot1;
168+
169+
double sd_rot1 = _a1 * System.Math.Abs(rot1) + (_a2 * Mathf.Deg2Rad) * trans;
170+
double sd_rot2 = _a1 * System.Math.Abs(rot2) + (_a2 * Mathf.Deg2Rad) * trans;
171+
double sd_trans = _a3 * trans + _a4 * (System.Math.Abs(rot1) + System.Math.Abs(rot2));
172+
173+
_noiseModel.StdDev = sd_trans * sd_trans;
174+
trans = _noiseModel.Apply(trans);
175+
176+
_noiseModel.StdDev = sd_rot1 * sd_rot1;
177+
rot1 = _noiseModel.Apply(rot1);
178+
179+
_noiseModel.StdDev = sd_rot2 * sd_rot2;
180+
rot2 = _noiseModel.Apply(rot2);
181+
182+
currentOdomMessage.pose.pose.position.x += trans * System.Math.Cos(theta1 + rot1);
183+
currentOdomMessage.pose.pose.position.y += trans * System.Math.Sin(theta1 + rot1);
184+
185+
theta2 += (rot1 + rot2);
186+
187+
Quaternion yawQuaternion = Quaternion.EulerAngles(0, (float)theta2, 0);
188+
currentOdomMessage.pose.pose.orientation.FromUnityQuaternion(yawQuaternion);
189+
190+
}
191+
192+
193+
}
194+
}

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSFakeOdometryPublisher.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/Util/Math/ZOGaussianNoiseModel.cs‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -18,26 +18,32 @@ public double StdDev {
1818
public double _biasMean = 0;
1919
public double BiasMean {
2020
get { return _biasMean; }
21-
set { _biasMean = value; _needToCalculateBias = true; }
21+
set {
22+
_biasMean = value;
23+
_needToCalculateBias = System.Math.Abs(value) > 0 ? true : false;
24+
}
2225
}
23-
26+
2427
public double _biasStdDev = 0;
2528
public double BiasStdDev {
2629
get { return _biasStdDev; }
27-
set { _biasStdDev = value; _needToCalculateBias = true; }
30+
set {
31+
_biasStdDev = value;
32+
_needToCalculateBias = System.Math.Abs(value) > 0 ? true : false;
33+
}
2834
}
2935

3036
private System.Random _random = new System.Random();
31-
private bool _needToCalculateBias = true;
37+
private bool _needToCalculateBias = false;
3238
private double _bias = 0.0;
3339

34-
public double Apply(double v) {
40+
public double Apply(double v) {
3541
if (_needToCalculateBias == true) { // initialize the bias
3642
_bias = ZORandom.NextGaussian(_random, _biasMean, _biasStdDev);
3743
if (_random.NextDouble() < 0.5) {
3844
_bias = -_bias;
3945
}
40-
}
46+
}
4147
double whiteNoise = ZO.Math.ZORandom.NextGaussian(_random, _mean, _stdDev);
4248
double output = v + _bias + whiteNoise;
4349
return output;

0 commit comments

Comments
 (0)