Skip to content

Commit 8d14267

Browse files
micahpearlmanmicahpearlman
committed
start of spot controller
1 parent 5550017 commit 8d14267

File tree

6 files changed

+154
-47
lines changed

6 files changed

+154
-47
lines changed

‎Editor/CustomEditors/ZOSpotControllerEditor.cs‎

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

‎Editor/CustomEditors/ZOSpotControllerEditor.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/Controllers/Robots/Spot/ZOSpotController.cs‎

Lines changed: 40 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,32 @@
11
using System.Collections;
22
using System.Collections.Generic;
3+
using System.Threading.Tasks;
34
using UnityEngine;
45
using ZO.Util;
56
using ZO.Physics;
67
using ZO.Sensors;
8+
using ZO.ROS;
79
using ZO.ROS.Publisher;
10+
using ZO.ROS.Unity;
11+
using ZO.ROS.MessageTypes;
12+
using ZO.ROS.MessageTypes.Geometry;
13+
814

915
namespace ZO.Controllers {
1016

1117
[RequireComponent(typeof(ZOROSJointStatesPublisher))]
12-
public class ZOSpotController : ZOGameObjectBase {
18+
public class ZOSpotController : ZOROSUnityGameObjectBase {
19+
20+
21+
public string _TwistTopicSubscription = "/cmd_vel";
22+
private TwistMessage _twistMessage = new TwistMessage();
1323

14-
1524

1625
protected override void ZOOnValidate() {
1726
base.ZOOnValidate();
1827
UpdateRateHz = 50;
1928
}
29+
2030
protected override void ZOStart() {
2131
base.ZOStart();
2232

@@ -44,7 +54,35 @@ protected override void ZOUpdateHzSynchronized() {
4454

4555

4656
protected override void ZOOnGUI() {
57+
base.ZOOnGUI();
58+
}
59+
60+
public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
61+
Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeConnected");
62+
63+
// subscribe to Twist Message
64+
ZOROSBridgeConnection.Instance.Subscribe<TwistMessage>(Name, _TwistTopicSubscription, _twistMessage.MessageType, OnROSTwistMessageReceived);
4765

4866
}
67+
68+
public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
69+
ZOROSBridgeConnection.Instance.UnAdvertise(_TwistTopicSubscription);
70+
Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeDisconnected");
71+
}
72+
73+
74+
/// <summary>
75+
/// Handles subscribed to `TwistMessage` which controls the differential control steering and drive.
76+
/// </summary>
77+
/// <param name="rosBridgeConnection">ROS Bridge Connection</param>
78+
/// <param name="msg">TwistMessage</param>
79+
/// <returns></returns>
80+
public Task OnROSTwistMessageReceived(ZOROSBridgeConnection rosBridgeConnection, ZOROSMessageInterface msg) {
81+
_twistMessage = (TwistMessage)msg;
82+
// Debug.Log("INFO: Twist Message Received: linear " + _twistMessage.linear.ToString() + " angular: " + _twistMessage.angular.ToString());
83+
84+
return Task.CompletedTask;
85+
}
86+
4987
}
5088
}

‎Runtime/Scripts/ROS/Unity/Controllers/ZODifferentialDriveController.cs‎

Lines changed: 8 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -32,17 +32,8 @@ namespace ZO.ROS.Controllers {
3232
/// See: https://github.com/ros-controls/ros_controllers/blob/indigo-devel/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
3333
/// </reference>
3434
/// TODO: Make this a ZOROSUnityGameObjectBase and a controller interface
35-
public class ZODifferentialDriveController : ZOGameObjectBase {
35+
public class ZODifferentialDriveController : ZOROSUnityGameObjectBase {
3636

37-
public String _name;
38-
public string Name {
39-
get {
40-
if (string.IsNullOrEmpty(_name))
41-
_name = gameObject.name + "_" + Type;
42-
return _name;
43-
}
44-
private set => _name = value;
45-
}
4637

4738
public ZOSimOccurrence _connectedBody;
4839

@@ -99,24 +90,19 @@ public float AngularVelocity {
9990
get => _angularVelocity;
10091
}
10192

102-
private JObject _json;
103-
public JObject JSON {
104-
get => _json;
105-
set => _json = value;
106-
}
10793

10894

10995

110-
public string Type {
96+
public override string Type {
11197
get {
11298
return "controller.differential_drive";
11399
}
114100
}
115101

116102
void OnValidate() {
117103
// make sure we have a name
118-
if (string.IsNullOrEmpty(_name)) {
119-
_name = gameObject.name + "_" + Type;
104+
if (string.IsNullOrEmpty(Name)) {
105+
Name = gameObject.name + "_" + Type;
120106
}
121107

122108
}
@@ -125,33 +111,13 @@ protected override void ZOAwake() {
125111
base.ZOAwake();
126112

127113
// make sure we have a name
128-
if (string.IsNullOrEmpty(_name)) {
129-
_name = gameObject.name + "_" + Type;
114+
if (string.IsNullOrEmpty(Name)) {
115+
Name = gameObject.name + "_" + Type;
130116
}
131117

132118
}
133119

134-
// Start is called before the first frame update
135-
protected override void ZOStart() {
136-
base.ZOStart();
137-
// auto-connect to ROS Bridge connection and disconnect events
138-
ZOROSUnityManager.Instance.ROSBridgeConnectEvent += OnROSBridgeConnected;
139-
ZOROSUnityManager.Instance.ROSBridgeDisconnectEvent += OnROSBridgeDisconnected;
140-
141-
if (ZOROSBridgeConnection.Instance.IsConnected) {
142-
// subscribe to Twist Message
143-
ZOROSBridgeConnection.Instance.Subscribe<TwistMessage>(Name, _ROSTopicSubscription, _twistMessage.MessageType, OnROSTwistMessageReceived);
144120

145-
// adverise Odometry Message
146-
ZOROSBridgeConnection.Instance.Advertise("/odom", _odometryMessage.MessageType);
147-
148-
}
149-
}
150-
151-
protected override void ZOOnDestroy() {
152-
ZOROSUnityManager.Instance.ROSBridgeConnectEvent -= OnROSBridgeConnected;
153-
ZOROSUnityManager.Instance.ROSBridgeDisconnectEvent -= OnROSBridgeDisconnected;
154-
}
155121

156122
protected override void ZOFixedUpdate() {
157123
// update the motors from the twist message
@@ -356,7 +322,7 @@ private void CalculateOdometryOpenLoop() {
356322
private TransformStampedMessage _transformMessage = new TransformStampedMessage();
357323

358324

359-
public void OnROSBridgeConnected(object rosUnityManager) {
325+
public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
360326
Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeConnected");
361327

362328
// subscribe to Twist Message
@@ -366,7 +332,7 @@ public void OnROSBridgeConnected(object rosUnityManager) {
366332
ZOROSBridgeConnection.Instance.Advertise("/odom", _odometryMessage.MessageType);
367333
}
368334

369-
public void OnROSBridgeDisconnected(object rosUnityManager) {
335+
public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
370336
ZOROSBridgeConnection.Instance.UnAdvertise(_ROSTopicSubscription);
371337
Debug.Log("INFO: ZODifferentialDriveController::OnROSBridgeDisconnected");
372338
}

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

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -181,15 +181,12 @@ private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel,
181181
return Task.CompletedTask;
182182
}
183183

184-
#region ZOSerializationInterface
185184
public override string Type {
186185
get { return "ros.publisher.imu"; }
187186
}
188187

189188

190189

191-
#endregion // ZOSerializationInterface
192-
193190
}
194191

195192
}

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

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -650,6 +650,11 @@ PrefabInstance:
650650
m_Modification:
651651
m_TransformParent: {fileID: 0}
652652
m_Modifications:
653+
- target: {fileID: 570012235415932074, guid: 895d6b116cd343cc29b67a0ef2535776,
654+
type: 3}
655+
propertyPath: m_Constraints
656+
value: 112
657+
objectReference: {fileID: 0}
653658
- target: {fileID: 2619807027348755217, guid: 895d6b116cd343cc29b67a0ef2535776,
654659
type: 3}
655660
propertyPath: m_Name
@@ -797,3 +802,64 @@ MonoBehaviour:
797802
_fixedJoint: {fileID: 1492638518}
798803
_connectedBody: {fileID: 1492638516}
799804
_name: joint.fixed_from_body
805+
--- !u!1 &1492638520 stripped
806+
GameObject:
807+
m_CorrespondingSourceObject: {fileID: 2619807027348755217, guid: 895d6b116cd343cc29b67a0ef2535776,
808+
type: 3}
809+
m_PrefabInstance: {fileID: 1492638513}
810+
m_PrefabAsset: {fileID: 0}
811+
--- !u!114 &1492638521
812+
MonoBehaviour:
813+
m_ObjectHideFlags: 0
814+
m_CorrespondingSourceObject: {fileID: 0}
815+
m_PrefabInstance: {fileID: 0}
816+
m_PrefabAsset: {fileID: 0}
817+
m_GameObject: {fileID: 1492638520}
818+
m_Enabled: 1
819+
m_EditorHideFlags: 0
820+
m_Script: {fileID: 11500000, guid: ff33c65ed3465152791a1661420ca26e, type: 3}
821+
m_Name:
822+
m_EditorClassIdentifier:
823+
_updateRateHz: 50
824+
_debug: 0
825+
_currentUpdateHz: 0
826+
_currentFixedUpdateHz: 0
827+
_ROSTopic:
828+
_name: spot
829+
_TwistTopicSubscription: /cmd_vel
830+
--- !u!114 &1492638522
831+
MonoBehaviour:
832+
m_ObjectHideFlags: 0
833+
m_CorrespondingSourceObject: {fileID: 0}
834+
m_PrefabInstance: {fileID: 0}
835+
m_PrefabAsset: {fileID: 0}
836+
m_GameObject: {fileID: 1492638520}
837+
m_Enabled: 1
838+
m_EditorHideFlags: 0
839+
m_Script: {fileID: 11500000, guid: 129fe926cb5f9784893878adb84b7b04, type: 3}
840+
m_Name:
841+
m_EditorClassIdentifier:
842+
_updateRateHz: 25
843+
_debug: 0
844+
_currentUpdateHz: 0
845+
_currentFixedUpdateHz: 0
846+
_ROSTopic: /joint_states
847+
_name: ros.publisher.joint_states_spot
848+
--- !u!143 &1492638523
849+
CharacterController:
850+
m_ObjectHideFlags: 0
851+
m_CorrespondingSourceObject: {fileID: 0}
852+
m_PrefabInstance: {fileID: 0}
853+
m_PrefabAsset: {fileID: 0}
854+
m_GameObject: {fileID: 1492638517}
855+
m_Material: {fileID: 0}
856+
m_IsTrigger: 0
857+
m_Enabled: 1
858+
serializedVersion: 2
859+
m_Height: 0.56
860+
m_Radius: 0.5
861+
m_SlopeLimit: 45
862+
m_StepOffset: 0.3
863+
m_SkinWidth: 0.08
864+
m_MinMoveDistance: 0.001
865+
m_Center: {x: 0, y: -0.16, z: 0}

0 commit comments

Comments
 (0)