-
Notifications
You must be signed in to change notification settings - Fork 116
/
AGVController.cs
157 lines (141 loc) · 4.59 KB
/
AGVController.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;
using Unity.Robotics.UrdfImporter.Control;
namespace RosSharp.Control
{
public enum ControlMode { Keyboard, ROS};
public class AGVController : MonoBehaviour
{
public GameObject wheel1;
public GameObject wheel2;
public ControlMode mode = ControlMode.ROS;
private ArticulationBody wA1;
private ArticulationBody wA2;
public float maxLinearSpeed = 2; // m/s
public float maxRotationalSpeed = 1;//
public float wheelRadius = 0.033f; //meters
public float trackWidth = 0.288f; // meters Distance between tyres
public float forceLimit = 10;
public float damping = 10;
public float ROSTimeout = 0.5f;
private float lastCmdReceived = 0f;
ROSConnection ros;
private RotationDirection direction;
private float rosLinear = 0f;
private float rosAngular = 0f;
void Start()
{
wA1 = wheel1.GetComponent<ArticulationBody>();
wA2 = wheel2.GetComponent<ArticulationBody>();
SetParameters(wA1);
SetParameters(wA2);
ros = ROSConnection.GetOrCreateInstance();
ros.Subscribe<TwistMsg>("cmd_vel", ReceiveROSCmd);
}
void ReceiveROSCmd(TwistMsg cmdVel)
{
rosLinear = (float)cmdVel.linear.x;
rosAngular = (float)cmdVel.angular.z;
lastCmdReceived = Time.time;
}
void FixedUpdate()
{
if (mode == ControlMode.Keyboard)
{
KeyBoardUpdate();
}
else if (mode == ControlMode.ROS)
{
ROSUpdate();
}
}
private void SetParameters(ArticulationBody joint)
{
ArticulationDrive drive = joint.xDrive;
drive.forceLimit = forceLimit;
drive.damping = damping;
joint.xDrive = drive;
}
private void SetSpeed(ArticulationBody joint, float wheelSpeed = float.NaN)
{
ArticulationDrive drive = joint.xDrive;
if (float.IsNaN(wheelSpeed))
{
drive.targetVelocity = ((2 * maxLinearSpeed) / wheelRadius) * Mathf.Rad2Deg * (int)direction;
}
else
{
drive.targetVelocity = wheelSpeed;
}
joint.xDrive = drive;
}
private void KeyBoardUpdate()
{
float moveDirection = Input.GetAxis("Vertical");
float inputSpeed;
float inputRotationSpeed;
if (moveDirection > 0)
{
inputSpeed = maxLinearSpeed;
}
else if (moveDirection < 0)
{
inputSpeed = maxLinearSpeed * -1;
}
else
{
inputSpeed = 0;
}
float turnDirction = Input.GetAxis("Horizontal");
if (turnDirction > 0)
{
inputRotationSpeed = maxRotationalSpeed;
}
else if (turnDirction < 0)
{
inputRotationSpeed = maxRotationalSpeed * -1;
}
else
{
inputRotationSpeed = 0;
}
RobotInput(inputSpeed, inputRotationSpeed);
}
private void ROSUpdate()
{
if (Time.time - lastCmdReceived > ROSTimeout)
{
rosLinear = 0f;
rosAngular = 0f;
}
RobotInput(rosLinear, -rosAngular);
}
private void RobotInput(float speed, float rotSpeed) // m/s and rad/s
{
if (speed > maxLinearSpeed)
{
speed = maxLinearSpeed;
}
if (rotSpeed > maxRotationalSpeed)
{
rotSpeed = maxRotationalSpeed;
}
float wheel1Rotation = (speed / wheelRadius);
float wheel2Rotation = wheel1Rotation;
float wheelSpeedDiff = ((rotSpeed * trackWidth) / wheelRadius);
if (rotSpeed != 0)
{
wheel1Rotation = (wheel1Rotation + (wheelSpeedDiff / 1)) * Mathf.Rad2Deg;
wheel2Rotation = (wheel2Rotation - (wheelSpeedDiff / 1)) * Mathf.Rad2Deg;
}
else
{
wheel1Rotation *= Mathf.Rad2Deg;
wheel2Rotation *= Mathf.Rad2Deg;
}
SetSpeed(wA1, wheel1Rotation);
SetSpeed(wA2, wheel2Rotation);
}
}
}