-
Notifications
You must be signed in to change notification settings - Fork 0
/
SawyerVacuumGripper.cs
213 lines (174 loc) · 6.71 KB
/
SawyerVacuumGripper.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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
using com.robotraconteur.robotics.tool;
using RobotRaconteur;
using RobotRaconteur.Companion.Robot;
using RobotRaconteur.Companion.Util;
using ros_csharp_interop;
using ros_csharp_interop.rosmsg.gen.intera_core_msgs;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Text;
using System.Threading;
namespace SawyerRobotRaconteurDriver
{
public class SawyerVacuumGripper : AbstractTool, ISawyerGripper
{
protected ROSNode _ros_node;
protected string _ros_ns_prefix;
protected string _ros_tool_name;
protected Subscriber<IODeviceStatus> _gripper_state_sub;
protected Publisher<IOComponentCommand> _gripper_command_pub;
protected IODeviceStatus _gripper_current_state;
protected Dictionary<string, string> _gripper_current_signals;
protected long _last_gripper_state;
protected double _last_command;
protected string _tool_name;
const double MAX_POSITION = 1;
const double MIN_POSITION = 0.0;
public SawyerVacuumGripper(ToolInfo tool_info, string ros_tool_name, string ros_ns_prefix = "") : base(tool_info)
{
this._ros_ns_prefix = "";
this._ros_tool_name = ros_tool_name;
}
public override void _start_tool()
{
_ros_node = new ROSNode();
_gripper_state_sub = _ros_node.subscribe<IODeviceStatus>(_ros_ns_prefix + "io/end_effector/" + _ros_tool_name + "/state", 1, _gripper_state_cb); ;
_gripper_command_pub = _ros_node.advertise<IOComponentCommand>(_ros_ns_prefix + "io/end_effector/" + _ros_tool_name + "/command", 1, false);
base._start_tool();
}
private void _gripper_state_cb(IODeviceStatus obj)
{
lock(this)
{
_gripper_current_state = obj;
_last_gripper_state = _stopwatch.ElapsedMilliseconds;
if (_gripper_current_signals == null)
{
_gripper_current_signals = new Dictionary<string, string>();
}
else
{
_gripper_current_signals.Clear();
}
foreach (var v in obj.signals)
{
_gripper_current_signals[v.name] = v.data.Trim('[', ']');
}
}
}
private string _get_signal_or_default(string name, string default_)
{
if(!_gripper_current_signals.TryGetValue(name, out var v1))
{
return default_;
}
return v1;
}
protected override void _fill_state(long now, out ToolState rr_tool_state)
{
lock (this)
{
var o = new ToolState();
o.ts = DateTimeUtil.TimeSpec3Now(RobotRaconteurNode.s);
o.seqno = _state_seqno;
o.command = _last_command;
if (_gripper_current_state == null || _gripper_current_signals == null
// TODO: Gripper isn't publishing unless state changes?
//|| (now - _last_gripper_state > 2000)
)
{
o.tool_state_flags = (uint)ToolStateFlags.communication_failure;
o.sensor = new double[0];
}
else
{
o.sensor = new double[] { double.Parse(_get_signal_or_default("right_vacuum_gripper_tip_object_kg", "0")) };
bool has_error = bool.Parse(_get_signal_or_default("has_error", "true"));
bool is_gripping = bool.Parse(_get_signal_or_default("is_gripping", "false"));
uint f = 0;
if (has_error)
{
f |= (uint)ToolStateFlags.error;
}
if (!has_error)
{
f |= (uint)ToolStateFlags.enabled;
}
f |= (uint)ToolStateFlags.ready;
if (is_gripping)
{
f |= (uint)ToolStateFlags.gripping;
}
if (is_gripping)
{
o.position = 1;
_position = o.position;
f |= (uint)ToolStateFlags.closed;
}
else
{
o.position = 0;
_position = o.position;
f |= (uint)ToolStateFlags.opened;
}
o.tool_state_flags = f;
}
rr_tool_state = o;
}
}
private TimeSpan _last_time;
private ros_csharp_interop.rosmsg.ROSTime _now_ros()
{
var o = new ros_csharp_interop.rosmsg.ROSTime();
TimeSpan t = DateTime.UtcNow.ToUniversalTime() - (new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc));
if (t <= _last_time)
{
t = _last_time + TimeSpan.FromMilliseconds(1);
}
_last_time = t;
o.secs = (uint)Math.Round(t.TotalSeconds);
o.nsecs = (uint)Math.IEEERemainder(t.TotalMilliseconds * 1e6, 1e9);
return o;
}
public override void close()
{
ros_csharp_interop.rosmsg.ROSTime t;
lock (this)
{
t = _now_ros();
}
var cmd1 = new IOComponentCommand();
cmd1.time = t;
cmd1.op = "set";
cmd1.args = "{\"signals\": {\"cmd_grip\": {\"data\": [true], \"format\": {\"type\": \"bool\"}}}}";
_gripper_command_pub.publish(cmd1);
lock (this)
{
_command = 1;
}
}
public override void open()
{
ros_csharp_interop.rosmsg.ROSTime t;
lock (this)
{
t = _now_ros();
}
var cmd1 = new IOComponentCommand();
cmd1.time = t;
cmd1.op = "set";
cmd1.args = "{\"signals\": {\"cmd_grip\": {\"data\": [false], \"format\": {\"type\": \"bool\"}}}}";
_gripper_command_pub.publish(cmd1);
lock (this)
{
_command = 0;
}
}
public override void Dispose()
{
base.Dispose();
_gripper_state_sub?.Dispose();
}
}
}