Skip to content

Commit caa542c

Browse files
micahpearlmanmicahpearlman
committed
added claim and release services
1 parent b94eb14 commit caa542c

File tree

8 files changed

+218
-30
lines changed

8 files changed

+218
-30
lines changed

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

Lines changed: 68 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
using ZO.ROS.Unity;
1111
using ZO.ROS.MessageTypes;
1212
using ZO.ROS.MessageTypes.Geometry;
13-
using ZO.ROS.MessageTypes.Nav;
13+
using ZO.ROS.MessageTypes.StdSrvs;
1414

1515

1616
namespace ZO.Controllers {
@@ -24,9 +24,12 @@ public class ZOSpotController : ZOROSUnityGameObjectBase {
2424
private TwistMessage _twistMessage = new TwistMessage();
2525
private TwistWithCovarianceStampedMessage _twistPublishMessage = new TwistWithCovarianceStampedMessage();
2626
private ZOROSFakeOdometryPublisher _odomPublisher = null;
27+
private bool IsClaimed {
28+
get; set;
29+
} = false;
30+
2731

2832

29-
3033

3134

3235
protected override void ZOOnValidate() {
@@ -38,7 +41,7 @@ protected override void ZOStart() {
3841
base.ZOStart();
3942

4043
_odomPublisher = GetComponentInChildren<ZOROSFakeOdometryPublisher>(true);
41-
44+
4245

4346
}
4447

@@ -59,11 +62,11 @@ protected override void ZOFixedUpdate() {
5962
protected override void ZOUpdateHzSynchronized() {
6063
base.ZOUpdateHzSynchronized();
6164

62-
if (_odomPublisher && ZOROSBridgeConnection.Instance.IsConnected == true) {
63-
_twistPublishMessage.header = _odomPublisher.CurrentOdometryMessage.header;
65+
if (_odomPublisher && ROSBridgeConnection.IsConnected == true) {
66+
_twistPublishMessage.header = _odomPublisher.CurrentOdometryMessage.header;
6467
_twistPublishMessage.twist = _odomPublisher.CurrentOdometryMessage.twist;
65-
ZOROSBridgeConnection.Instance.Publish<TwistWithCovarianceStampedMessage>(_twistPublishMessage, "/odometry/twist");
66-
68+
ROSBridgeConnection.Publish<TwistWithCovarianceStampedMessage>(_twistPublishMessage, "/odometry/twist");
69+
6770
}
6871
}
6972

@@ -76,16 +79,68 @@ protected override void ZOOnGUI() {
7679
public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
7780
Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeConnected");
7881

79-
// subscribe to Twist Message
80-
ZOROSBridgeConnection.Instance.Subscribe<TwistMessage>(Name, _TwistTopicSubscription, _twistMessage.MessageType, OnROSTwistMessageReceived);
81-
82-
// advertise twist
83-
ZOROSBridgeConnection.Instance.Advertise("/odometry/twist", TwistWithCovarianceStampedMessage.Type);
82+
// subscribe to Twist Message (i.e. control)
83+
ROSBridgeConnection.Subscribe<TwistMessage>(Name, _TwistTopicSubscription, _twistMessage.MessageType, OnROSTwistMessageReceived);
84+
85+
// advertise odometry twist
86+
ROSBridgeConnection.Advertise("/odometry/twist", TwistWithCovarianceStampedMessage.Type);
87+
88+
// advertise services
89+
ROSBridgeConnection.AdvertiseService<TriggerServiceRequest>("claim",
90+
TriggerServiceRequest.Type,
91+
(rosBridge, msg, id) => {
92+
TriggerServiceRequest triggerServiceRequest = msg as TriggerServiceRequest;
93+
94+
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest");
95+
96+
if (IsClaimed == true) {
97+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Already claimed"), "claim", false, id);
98+
99+
} else {
100+
IsClaimed = true;
101+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "success"), "claim", false, id);
102+
103+
104+
}
105+
106+
107+
108+
return Task.CompletedTask;
109+
110+
});
111+
112+
// advertise services
113+
ROSBridgeConnection.AdvertiseService<TriggerServiceRequest>("release",
114+
TriggerServiceRequest.Type,
115+
(rosBridge, msg, id) => {
116+
TriggerServiceRequest triggerServiceRequest = msg as TriggerServiceRequest;
117+
118+
Debug.Log("INFO: ZOSpotController::TriggerServiceRequest");
119+
120+
if (IsClaimed == false) {
121+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(false, "Not claimed"), "release", false, id);
122+
123+
} else {
124+
IsClaimed = false;
125+
ROSBridgeConnection.ServiceResponse<TriggerServiceResponse>(new TriggerServiceResponse(true, "success"), "release", false, id);
126+
127+
128+
}
129+
130+
131+
132+
return Task.CompletedTask;
133+
134+
});
135+
84136

85137
}
86138

87139
public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
88-
ZOROSBridgeConnection.Instance.UnAdvertise("/odometry/twist");
140+
ROSBridgeConnection.UnAdvertise("/odometry/twist");
141+
ROSBridgeConnection.UnAdvertiseService("claim");
142+
ROSBridgeConnection.UnAdvertiseService("release");
143+
89144
Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeDisconnected");
90145
}
91146

‎Runtime/Scripts/ROS/MessageTypes/Spot.meta‎

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

‎Runtime/Scripts/ROS/MessageTypes/Spot/ZOSpotMessages.cs‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
namespace ZO.ROS.MessageTypes.Nav {
2+
}

‎Runtime/Scripts/ROS/MessageTypes/Spot/ZOSpotMessages.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/MessageTypes/StdSrvs.meta‎

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

‎Runtime/Scripts/ROS/MessageTypes/StdSrvs/ZOROSStdSrvsMessages.cs‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
namespace ZO.ROS.MessageTypes.StdSrvs {
2+
3+
/// <summary>
4+
/// See: http://docs.ros.org/en/api/std_srvs/html/srv/Trigger.html
5+
/// </summary>
6+
public class TriggerServiceRequest : ZOROSMessageInterface {
7+
8+
[Newtonsoft.Json.JsonIgnore]
9+
public string MessageType { get { return TriggerServiceRequest.Type; } }
10+
11+
[Newtonsoft.Json.JsonIgnore]
12+
public static string Type = "std_srvs/Trigger";
13+
14+
15+
public TriggerServiceRequest() {
16+
}
17+
18+
}
19+
20+
/// <summary>
21+
/// See: http://docs.ros.org/en/api/std_srvs/html/srv/Trigger.html
22+
/// </summary>
23+
public class TriggerServiceResponse : ZOROSMessageInterface {
24+
25+
[Newtonsoft.Json.JsonIgnore]
26+
public string MessageType { get { return TriggerServiceResponse.Type; } }
27+
28+
[Newtonsoft.Json.JsonIgnore]
29+
public static string Type = "std_srvs/Trigger";
30+
31+
32+
/// <summary>
33+
/// Indicate successful run of triggered service
34+
/// </summary>
35+
/// <value></value>
36+
public bool success { get; set; }
37+
38+
/// <summary>
39+
/// Informational, e.g. for error messages
40+
/// </summary>
41+
/// <value></value>
42+
public string message { get; set; }
43+
44+
public TriggerServiceResponse() {
45+
this.success = true;
46+
this.message = "";
47+
}
48+
49+
public TriggerServiceResponse(bool success, string message) {
50+
this.success = success;
51+
this.message = message;
52+
}
53+
}
54+
55+
56+
}

‎Runtime/Scripts/ROS/MessageTypes/StdSrvs/ZOROSStdSrvsMessages.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.

‎Samples~/ZeroSimSamples/Robots/Spot/spot_rigged.prefab‎

Lines changed: 54 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -196,7 +196,7 @@ HingeJoint:
196196
m_EnablePreprocessing: 1
197197
m_MassScale: 1
198198
m_ConnectedMassScale: 1
199-
--- !u!114 &662680489
199+
--- !u!114 &662680477
200200
MonoBehaviour:
201201
m_ObjectHideFlags: 0
202202
m_CorrespondingSourceObject: {fileID: 0}
@@ -205,18 +205,21 @@ MonoBehaviour:
205205
m_GameObject: {fileID: 705516937584561444}
206206
m_Enabled: 1
207207
m_EditorHideFlags: 0
208-
m_Script: {fileID: 11500000, guid: 6013e0556113f1e9f95189e724e765fc, type: 3}
208+
m_Script: {fileID: 11500000, guid: b28630d63f8f5fb68a139c2eebaae78f, type: 3}
209209
m_Name:
210210
m_EditorClassIdentifier:
211-
_updateRateHz: 10
211+
_updateRateHz: 20
212212
_debug: 0
213213
_currentUpdateHz: 0
214214
_currentFixedUpdateHz: 0
215-
_ROSTopic:
216-
_name: ros.publisher.transform_body
217-
_frameId: map
218-
_childFrameId: body
219-
--- !u!114 &662680477
215+
_ROSTopic: /odometry
216+
_name: publisher.odometry_body
217+
_applyNoise: 0
218+
_a1: 0.05
219+
_a2: 15
220+
_a3: 0.05
221+
_a4: 0.01
222+
--- !u!114 &662680489
220223
MonoBehaviour:
221224
m_ObjectHideFlags: 0
222225
m_CorrespondingSourceObject: {fileID: 0}
@@ -225,20 +228,17 @@ MonoBehaviour:
225228
m_GameObject: {fileID: 705516937584561444}
226229
m_Enabled: 1
227230
m_EditorHideFlags: 0
228-
m_Script: {fileID: 11500000, guid: b28630d63f8f5fb68a139c2eebaae78f, type: 3}
231+
m_Script: {fileID: 11500000, guid: 6013e0556113f1e9f95189e724e765fc, type: 3}
229232
m_Name:
230233
m_EditorClassIdentifier:
231-
_updateRateHz: 20
234+
_updateRateHz: 10
232235
_debug: 0
233236
_currentUpdateHz: 0
234237
_currentFixedUpdateHz: 0
235-
_ROSTopic: /odometry
236-
_name: publisher.odometry_body
237-
_applyNoise: 0
238-
_a1: 0.05
239-
_a2: 15
240-
_a3: 0.05
241-
_a4: 0.01
238+
_ROSTopic:
239+
_name: ros.publisher.transform_body
240+
_frameId: map
241+
_childFrameId: body
242242
--- !u!114 &1690162258
243243
MonoBehaviour:
244244
m_ObjectHideFlags: 0
@@ -289,6 +289,43 @@ MonoBehaviour:
289289
m_Name:
290290
m_EditorClassIdentifier:
291291
_spotCharacterController: {fileID: 971641630}
292+
--- !u!114 &820716398
293+
MonoBehaviour:
294+
m_ObjectHideFlags: 0
295+
m_CorrespondingSourceObject: {fileID: 0}
296+
m_PrefabInstance: {fileID: 0}
297+
m_PrefabAsset: {fileID: 0}
298+
m_GameObject: {fileID: 7918743296464092362}
299+
m_Enabled: 1
300+
m_EditorHideFlags: 0
301+
m_Script: {fileID: 11500000, guid: 129fe926cb5f9784893878adb84b7b04, type: 3}
302+
m_Name:
303+
m_EditorClassIdentifier:
304+
_updateRateHz: 10
305+
_debug: 0
306+
_currentUpdateHz: 0
307+
_currentFixedUpdateHz: 0
308+
_ROSTopic: /joint_states
309+
_name: ros.publisher.joint_states_spot_rigged
310+
--- !u!114 &820716399
311+
MonoBehaviour:
312+
m_ObjectHideFlags: 0
313+
m_CorrespondingSourceObject: {fileID: 0}
314+
m_PrefabInstance: {fileID: 0}
315+
m_PrefabAsset: {fileID: 0}
316+
m_GameObject: {fileID: 7918743296464092362}
317+
m_Enabled: 1
318+
m_EditorHideFlags: 0
319+
m_Script: {fileID: 11500000, guid: ff33c65ed3465152791a1661420ca26e, type: 3}
320+
m_Name:
321+
m_EditorClassIdentifier:
322+
_updateRateHz: 50
323+
_debug: 0
324+
_currentUpdateHz: 0
325+
_currentFixedUpdateHz: 0
326+
_ROSTopic:
327+
_name: spot_rigged
328+
_TwistTopicSubscription: /cmd_vel
292329
--- !u!1001 &5313802871329697755
293330
PrefabInstance:
294331
m_ObjectHideFlags: 0

0 commit comments

Comments
 (0)