Skip to content

Commit c392598

Browse files
micahpearlmanmicahpearlman
committed
fixed lidar 3d orientation update
1 parent 822c64a commit c392598

File tree

5 files changed

+93
-17
lines changed

5 files changed

+93
-17
lines changed

‎Runtime/Scripts/Controllers/Robots/Spot/ZOSpotCharacterController.cs‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,14 @@
44
using System;
55

66
namespace ZO {
7+
/// <summary>
8+
/// Handles all the Unity specific "Character Control" stuff like movement and animation.
9+
/// </summary>
710
public class ZOSpotCharacterController : MonoBehaviour {
811

912
public Rigidbody _rigidBody;
13+
public Transform _frontCollider = null;
14+
public Transform _rearCollider = null;
1015
public float _maxFloorAngle = 45;
1116
public float _maxForwardVelocity = 1.0f;
1217

‎Runtime/Scripts/Controllers/Robots/Spot/ZOSpotController.cs‎

Lines changed: 82 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,10 @@ public bool IsPowered {
3232
get; set;
3333
} = false;
3434

35+
public bool IsStanding {
36+
get; set;
37+
} = false;
38+
3539

3640

3741

@@ -99,11 +103,11 @@ public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
99103
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest claim");
100104

101105
if (IsClaimed == true) {
102-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Already claimed"), "claim", false, id);
106+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Already claimed"), "claim", false, id);
103107

104108
} else {
105109
IsClaimed = true;
106-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "success"), "claim", false, id);
110+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "Success"), "claim", false, id);
107111

108112
}
109113

@@ -120,11 +124,11 @@ public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
120124
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest release");
121125

122126
if (IsClaimed == false) {
123-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Not claimed"), "release", false, id);
127+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Not claimed"), "release", false, id);
124128

125129
} else {
126130
IsClaimed = false;
127-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "success"), "release", false, id);
131+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "Success"), "release", false, id);
128132

129133
}
130134

@@ -141,15 +145,15 @@ public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
141145
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest power_on");
142146

143147
if (IsPowered == true) {
144-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Already powered on"), "power_on", false, id);
148+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Already powered on"), "power_on", false, id);
145149

146150
} else {
147151
if (IsClaimed == true) {
148152
IsPowered = true;
149-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "success"), "power_on", false, id);
153+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "Success"), "power_on", false, id);
150154

151155
} else { // can't power if not claimed
152-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Not claimed. Run /claim service"), "power_on", false, id);
156+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Not claimed. Run /claim service"), "power_on", false, id);
153157

154158
}
155159

@@ -168,15 +172,83 @@ public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
168172
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest power_off");
169173

170174
if (IsPowered == false) {
171-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Already powered off"), "power_off", false, id);
175+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Already powered off"), "power_off", false, id);
172176

173177
} else {
174178
if (IsClaimed == true) {
175179
IsPowered = false;
176-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "success"), "power_off", false, id);
180+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "Success"), "power_off", false, id);
181+
182+
} else { // can't power if not claimed
183+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Not claimed. Run /claim service"), "power_off", false, id);
184+
185+
}
186+
187+
}
188+
189+
return Task.CompletedTask;
190+
191+
});
192+
193+
// stand
194+
ROSBridgeConnection.AdvertiseService<TriggerServiceRequest>("stand",
195+
TriggerServiceRequest.Type,
196+
(rosBridge, msg, id) => {
197+
TriggerServiceRequest triggerServiceRequest = msg as TriggerServiceRequest;
198+
199+
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest stand");
200+
201+
if (IsStanding == true) {
202+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Already standing"), "stand", false, id);
203+
204+
} else {
205+
if (IsClaimed == true) {
206+
if (IsPowered == true) {
207+
IsStanding = true;
208+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "Success"), "stand", false, id);
209+
210+
} else { // not powereed
211+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Not powered. Need to power on"), "stand", false, id);
212+
213+
214+
}
215+
216+
} else { // can't power if not claimed
217+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Not claimed. Run /claim service"), "stand", false, id);
218+
219+
}
220+
221+
}
222+
223+
return Task.CompletedTask;
224+
225+
});
226+
227+
// power off
228+
ROSBridgeConnection.AdvertiseService<TriggerServiceRequest>("sit",
229+
TriggerServiceRequest.Type,
230+
(rosBridge, msg, id) => {
231+
TriggerServiceRequest triggerServiceRequest = msg as TriggerServiceRequest;
232+
233+
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest sit");
234+
235+
if (IsStanding == true) {
236+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Already standing"), "sit", false, id);
237+
238+
} else {
239+
if (IsClaimed == true) {
240+
if (IsPowered == true) {
241+
IsStanding = false;
242+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "Success"), "sit", false, id);
243+
244+
} else { // not powereed
245+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Error: Not powered. Need to power on"), "sit", false, id);
246+
247+
248+
}
177249

178250
} else { // can't power if not claimed
179-
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Not claimed. Run /claim service"), "power_on", false, id);
251+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Not claimed. Run /claim service"), "sit", false, id);
180252

181253
}
182254

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

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -122,15 +122,11 @@ private Task OnPublishLidarScanDelegate(ZOLIDAR3D lidar, string name, NativeArra
122122
return Task.CompletedTask;
123123
}
124124

125-
#region ZOSerializationInterface
126125
public override string Type {
127126
get { return "ros.publisher.point_cloud2"; }
128127
}
129128

130129

131-
132-
#endregion // ZOSerializationInterface
133-
134130
}
135131

136132
}

‎Runtime/Scripts/Sensors/LIDAR/ZOLIDAR3D.cs‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,13 +16,14 @@ namespace ZO.Sensors {
1616
[BurstCompile(CompileSynchronously = true)]
1717
struct ZOPrepareRaycastCommands : IJobParallelFor {
1818
public Vector3 Position;
19+
public Quaternion Orientation;
1920
public float Distance;
2021
public NativeArray<RaycastCommand> RaycastCommands;
2122
[ReadOnly]
2223
public NativeArray<Vector3> RaycastDirections;
2324

2425
public void Execute(int i) {
25-
RaycastCommands[i] = new RaycastCommand(Position, RaycastDirections[i], Distance);
26+
RaycastCommands[i] = new RaycastCommand(Position, Orientation * RaycastDirections[i], Distance);
2627
}
2728
};
2829

@@ -147,7 +148,8 @@ protected override void ZOStart() {
147148
}
148149

149150

150-
private void OnDestroy() {
151+
private void ZOOnDestroy() {
152+
base.ZOOnDestroy();
151153
_transformHitsJobHandle.Complete();
152154
_rayDirections.Dispose();
153155
_hitNormals.Dispose();
@@ -185,6 +187,7 @@ protected override async void ZOFixedUpdateHzSynchronized() {
185187
// SetupRaycasts();
186188
var setupRaycastsJob = new ZOPrepareRaycastCommands() {
187189
Position = transform.position,
190+
Orientation = transform.rotation,
188191
Distance = _maxRange,
189192
RaycastDirections = _rayDirections,
190193
RaycastCommands = _raycastBatchJob.RaycastCommands

‎package.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
{
22
"name": "com.fsstudio.zerosim",
3-
"version": "0.1.14",
3+
"version": "0.1.15",
44
"displayName": "ZeroSim",
55
"description": "ZeroSim ROS robotic simulator in Unity.",
66
"unity": "2020.1",

0 commit comments

Comments
 (0)