ur-sock 0.1
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +7 -0
- data/COPYING +165 -0
- data/README.md +34 -0
- data/Rakefile +21 -0
- data/lib/conf.rb +21 -0
- data/lib/rtde.rb +348 -0
- data/lib/serialize.rb +185 -0
- data/lib/ur-sock.rb +2 -0
- data/ur-sock.gemspec +23 -0
- metadata +73 -0
checksums.yaml
ADDED
@@ -0,0 +1,7 @@
|
|
1
|
+
---
|
2
|
+
SHA256:
|
3
|
+
metadata.gz: 4d864734b08ba388db84746d04f4fec4357f280011f064b22ffba985b4d7b799
|
4
|
+
data.tar.gz: 7fe3132f97b38b9cdf422856cebb172dcbe2a5b26c65c98abfb767d5de6ff046
|
5
|
+
SHA512:
|
6
|
+
metadata.gz: b5c2273bc22a68ae3e15b5a2eb57dde5a510529b1992b10c706dd9a732d20b45e99cff04ceef3bb5c3b29b8b4019ed5d1ee21d430450e15da921f4bb7295f4a8
|
7
|
+
data.tar.gz: 85a81702f09a06e978a38a79853f5f621b283e82d9903f30d523f2f51fc5bd5cceef4a24a7da38f7771bcf20f74a35e75093757cf51ab594492d63bfac77817d
|
data/COPYING
ADDED
@@ -0,0 +1,165 @@
|
|
1
|
+
GNU LESSER GENERAL PUBLIC LICENSE
|
2
|
+
Version 3, 29 June 2007
|
3
|
+
|
4
|
+
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
|
5
|
+
Everyone is permitted to copy and distribute verbatim copies
|
6
|
+
of this license document, but changing it is not allowed.
|
7
|
+
|
8
|
+
|
9
|
+
This version of the GNU Lesser General Public License incorporates
|
10
|
+
the terms and conditions of version 3 of the GNU General Public
|
11
|
+
License, supplemented by the additional permissions listed below.
|
12
|
+
|
13
|
+
0. Additional Definitions.
|
14
|
+
|
15
|
+
As used herein, "this License" refers to version 3 of the GNU Lesser
|
16
|
+
General Public License, and the "GNU GPL" refers to version 3 of the GNU
|
17
|
+
General Public License.
|
18
|
+
|
19
|
+
"The Library" refers to a covered work governed by this License,
|
20
|
+
other than an Application or a Combined Work as defined below.
|
21
|
+
|
22
|
+
An "Application" is any work that makes use of an interface provided
|
23
|
+
by the Library, but which is not otherwise based on the Library.
|
24
|
+
Defining a subclass of a class defined by the Library is deemed a mode
|
25
|
+
of using an interface provided by the Library.
|
26
|
+
|
27
|
+
A "Combined Work" is a work produced by combining or linking an
|
28
|
+
Application with the Library. The particular version of the Library
|
29
|
+
with which the Combined Work was made is also called the "Linked
|
30
|
+
Version".
|
31
|
+
|
32
|
+
The "Minimal Corresponding Source" for a Combined Work means the
|
33
|
+
Corresponding Source for the Combined Work, excluding any source code
|
34
|
+
for portions of the Combined Work that, considered in isolation, are
|
35
|
+
based on the Application, and not on the Linked Version.
|
36
|
+
|
37
|
+
The "Corresponding Application Code" for a Combined Work means the
|
38
|
+
object code and/or source code for the Application, including any data
|
39
|
+
and utility programs needed for reproducing the Combined Work from the
|
40
|
+
Application, but excluding the System Libraries of the Combined Work.
|
41
|
+
|
42
|
+
1. Exception to Section 3 of the GNU GPL.
|
43
|
+
|
44
|
+
You may convey a covered work under sections 3 and 4 of this License
|
45
|
+
without being bound by section 3 of the GNU GPL.
|
46
|
+
|
47
|
+
2. Conveying Modified Versions.
|
48
|
+
|
49
|
+
If you modify a copy of the Library, and, in your modifications, a
|
50
|
+
facility refers to a function or data to be supplied by an Application
|
51
|
+
that uses the facility (other than as an argument passed when the
|
52
|
+
facility is invoked), then you may convey a copy of the modified
|
53
|
+
version:
|
54
|
+
|
55
|
+
a) under this License, provided that you make a good faith effort to
|
56
|
+
ensure that, in the event an Application does not supply the
|
57
|
+
function or data, the facility still operates, and performs
|
58
|
+
whatever part of its purpose remains meaningful, or
|
59
|
+
|
60
|
+
b) under the GNU GPL, with none of the additional permissions of
|
61
|
+
this License applicable to that copy.
|
62
|
+
|
63
|
+
3. Object Code Incorporating Material from Library Header Files.
|
64
|
+
|
65
|
+
The object code form of an Application may incorporate material from
|
66
|
+
a header file that is part of the Library. You may convey such object
|
67
|
+
code under terms of your choice, provided that, if the incorporated
|
68
|
+
material is not limited to numerical parameters, data structure
|
69
|
+
layouts and accessors, or small macros, inline functions and templates
|
70
|
+
(ten or fewer lines in length), you do both of the following:
|
71
|
+
|
72
|
+
a) Give prominent notice with each copy of the object code that the
|
73
|
+
Library is used in it and that the Library and its use are
|
74
|
+
covered by this License.
|
75
|
+
|
76
|
+
b) Accompany the object code with a copy of the GNU GPL and this license
|
77
|
+
document.
|
78
|
+
|
79
|
+
4. Combined Works.
|
80
|
+
|
81
|
+
You may convey a Combined Work under terms of your choice that,
|
82
|
+
taken together, effectively do not restrict modification of the
|
83
|
+
portions of the Library contained in the Combined Work and reverse
|
84
|
+
engineering for debugging such modifications, if you also do each of
|
85
|
+
the following:
|
86
|
+
|
87
|
+
a) Give prominent notice with each copy of the Combined Work that
|
88
|
+
the Library is used in it and that the Library and its use are
|
89
|
+
covered by this License.
|
90
|
+
|
91
|
+
b) Accompany the Combined Work with a copy of the GNU GPL and this license
|
92
|
+
document.
|
93
|
+
|
94
|
+
c) For a Combined Work that displays copyright notices during
|
95
|
+
execution, include the copyright notice for the Library among
|
96
|
+
these notices, as well as a reference directing the user to the
|
97
|
+
copies of the GNU GPL and this license document.
|
98
|
+
|
99
|
+
d) Do one of the following:
|
100
|
+
|
101
|
+
0) Convey the Minimal Corresponding Source under the terms of this
|
102
|
+
License, and the Corresponding Application Code in a form
|
103
|
+
suitable for, and under terms that permit, the user to
|
104
|
+
recombine or relink the Application with a modified version of
|
105
|
+
the Linked Version to produce a modified Combined Work, in the
|
106
|
+
manner specified by section 6 of the GNU GPL for conveying
|
107
|
+
Corresponding Source.
|
108
|
+
|
109
|
+
1) Use a suitable shared library mechanism for linking with the
|
110
|
+
Library. A suitable mechanism is one that (a) uses at run time
|
111
|
+
a copy of the Library already present on the user's computer
|
112
|
+
system, and (b) will operate properly with a modified version
|
113
|
+
of the Library that is interface-compatible with the Linked
|
114
|
+
Version.
|
115
|
+
|
116
|
+
e) Provide Installation Information, but only if you would otherwise
|
117
|
+
be required to provide such information under section 6 of the
|
118
|
+
GNU GPL, and only to the extent that such information is
|
119
|
+
necessary to install and execute a modified version of the
|
120
|
+
Combined Work produced by recombining or relinking the
|
121
|
+
Application with a modified version of the Linked Version. (If
|
122
|
+
you use option 4d0, the Installation Information must accompany
|
123
|
+
the Minimal Corresponding Source and Corresponding Application
|
124
|
+
Code. If you use option 4d1, you must provide the Installation
|
125
|
+
Information in the manner specified by section 6 of the GNU GPL
|
126
|
+
for conveying Corresponding Source.)
|
127
|
+
|
128
|
+
5. Combined Libraries.
|
129
|
+
|
130
|
+
You may place library facilities that are a work based on the
|
131
|
+
Library side by side in a single library together with other library
|
132
|
+
facilities that are not Applications and are not covered by this
|
133
|
+
License, and convey such a combined library under terms of your
|
134
|
+
choice, if you do both of the following:
|
135
|
+
|
136
|
+
a) Accompany the combined library with a copy of the same work based
|
137
|
+
on the Library, uncombined with any other library facilities,
|
138
|
+
conveyed under the terms of this License.
|
139
|
+
|
140
|
+
b) Give prominent notice with the combined library that part of it
|
141
|
+
is a work based on the Library, and explaining where to find the
|
142
|
+
accompanying uncombined form of the same work.
|
143
|
+
|
144
|
+
6. Revised Versions of the GNU Lesser General Public License.
|
145
|
+
|
146
|
+
The Free Software Foundation may publish revised and/or new versions
|
147
|
+
of the GNU Lesser General Public License from time to time. Such new
|
148
|
+
versions will be similar in spirit to the present version, but may
|
149
|
+
differ in detail to address new problems or concerns.
|
150
|
+
|
151
|
+
Each version is given a distinguishing version number. If the
|
152
|
+
Library as you received it specifies that a certain numbered version
|
153
|
+
of the GNU Lesser General Public License "or any later version"
|
154
|
+
applies to it, you have the option of following the terms and
|
155
|
+
conditions either of that published version or of any later version
|
156
|
+
published by the Free Software Foundation. If the Library as you
|
157
|
+
received it does not specify a version number of the GNU Lesser
|
158
|
+
General Public License, you may choose any version of the GNU Lesser
|
159
|
+
General Public License ever published by the Free Software Foundation.
|
160
|
+
|
161
|
+
If the Library as you received it specifies that a proxy can decide
|
162
|
+
whether future versions of the GNU Lesser General Public License shall
|
163
|
+
apply, that proxy's public statement of acceptance of any version is
|
164
|
+
permanent authorization for you to choose that version for the
|
165
|
+
Library.
|
data/README.md
ADDED
@@ -0,0 +1,34 @@
|
|
1
|
+
# ur-smart
|
2
|
+
|
3
|
+
RTDE interface implementation in ruby
|
4
|
+
|
5
|
+
## Getting Started
|
6
|
+
|
7
|
+
Uses the RTDE socekt of universal robots. The commands are sent using TCP socket on port 30002.
|
8
|
+
|
9
|
+
### Prerequisites & Intallation
|
10
|
+
|
11
|
+
To run the server we need the following packages:
|
12
|
+
|
13
|
+
|
14
|
+
```
|
15
|
+
|
16
|
+
```
|
17
|
+
|
18
|
+
|
19
|
+
|
20
|
+
## Contributing
|
21
|
+
|
22
|
+
Please read [CONTRIBUTING.md](https://gist.github.com/PurpleBooth/b24679402957c63ec426) for details on our code of conduct, and the process for submitting pull requests to us.
|
23
|
+
|
24
|
+
## Authors
|
25
|
+
|
26
|
+
* **Florian Pauker** - ** -
|
27
|
+
* **Jürgen Mangler** - ** -
|
28
|
+
|
29
|
+
See also the list of [contributors](https://intra.acdp.at/gogs/fpauker/ua4ur/contributors) who participated in this project.
|
30
|
+
|
31
|
+
## License
|
32
|
+
|
33
|
+
This project is licensed under the LGPL3 License - see the [LICENSE.md](LICENSE.md) file for details
|
34
|
+
|
data/Rakefile
ADDED
@@ -0,0 +1,21 @@
|
|
1
|
+
require 'rubygems/package_task'
|
2
|
+
|
3
|
+
spec = eval(File.read('ur-sock.gemspec'))
|
4
|
+
|
5
|
+
task :default => [:gem]
|
6
|
+
|
7
|
+
pkg = Gem::PackageTask.new(spec) { |pkg|
|
8
|
+
pkg.need_zip = true
|
9
|
+
pkg.need_tar = true
|
10
|
+
`mkdir pkg`
|
11
|
+
`rm pkg/* -rf`
|
12
|
+
`ln -sf #{pkg.name}.gem pkg/#{spec.name}.gem`
|
13
|
+
}
|
14
|
+
|
15
|
+
task :push => :gem do |r|
|
16
|
+
`gem push pkg/ur-sock.gem`
|
17
|
+
end
|
18
|
+
|
19
|
+
task :install => :gem do |r|
|
20
|
+
`gem install pkg/ur-sock.gem`
|
21
|
+
end
|
data/lib/conf.rb
ADDED
@@ -0,0 +1,21 @@
|
|
1
|
+
require 'xml/smart'
|
2
|
+
|
3
|
+
module UR
|
4
|
+
|
5
|
+
class XMLConfigFile
|
6
|
+
def initialize(filename)
|
7
|
+
@names = {}
|
8
|
+
@types = {}
|
9
|
+
doc = XML::Smart.open(filename)
|
10
|
+
doc.find('/rtde_config/recipe/@key').each do |key|
|
11
|
+
@names[key.value] = doc.find("//field/@name").map {|x| x.to_s }
|
12
|
+
@types[key.value] = doc.find("//field/@type").map {|x| x.to_s }
|
13
|
+
end
|
14
|
+
end
|
15
|
+
|
16
|
+
def get_recipe(key)
|
17
|
+
return @names[key], @types[key] if @types.include?(key) && @names.include?(key)
|
18
|
+
end
|
19
|
+
end
|
20
|
+
|
21
|
+
end
|
data/lib/rtde.rb
ADDED
@@ -0,0 +1,348 @@
|
|
1
|
+
require_relative 'serialize'
|
2
|
+
require 'socket'
|
3
|
+
require 'logger'
|
4
|
+
require 'uri'
|
5
|
+
|
6
|
+
module UR
|
7
|
+
|
8
|
+
class Rtde
|
9
|
+
PROTOCOL_VERSION = 2
|
10
|
+
|
11
|
+
module Command #{{{
|
12
|
+
RTDE_REQUEST_PROTOCOL_VERSION = 86 # ASCII V
|
13
|
+
RTDE_GET_URCONTROL_VERSION = 118 # ASCII V
|
14
|
+
RTDE_TEXT_MESSAGE = 77 # ASCII M
|
15
|
+
RTDE_DATA_PACKAGE = 85 # ASCII U
|
16
|
+
RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ASCII O
|
17
|
+
RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ASCII I
|
18
|
+
RTDE_CONTROL_PACKAGE_START = 83 # ASCII S
|
19
|
+
RTDE_CONTROL_PACKAGE_PAUSE = 80 # ascii p
|
20
|
+
end #}}}
|
21
|
+
module ConnectionState #{{{
|
22
|
+
DISCONNECTED = 0
|
23
|
+
CONNECTED = 1
|
24
|
+
STARTED = 2
|
25
|
+
PAUSED = 3
|
26
|
+
end #}}}
|
27
|
+
|
28
|
+
def initialize(host, logger=Logger.new(STDOUT,level: :INFO)) #{{{
|
29
|
+
host = '//' + host if host !~ /\/\//
|
30
|
+
uri = URI::parse(host)
|
31
|
+
@logger = logger
|
32
|
+
@hostname = uri.host
|
33
|
+
@port = uri.port.nil? ? 30004 : uri.port
|
34
|
+
@conn_state = ConnectionState::DISCONNECTED
|
35
|
+
@sock = nil
|
36
|
+
@output_config = nil
|
37
|
+
@input_config = {}
|
38
|
+
end #}}}
|
39
|
+
|
40
|
+
def connect #{{{
|
41
|
+
return if @sock
|
42
|
+
|
43
|
+
@buf = '' # buffer data in binary format
|
44
|
+
begin
|
45
|
+
@sock = Socket.new Socket::AF_INET, Socket::SOCK_STREAM
|
46
|
+
@sock.setsockopt Socket::SOL_SOCKET, Socket::SO_REUSEADDR, 1
|
47
|
+
@sock = TCPSocket.new(@hostname, @port)
|
48
|
+
@conn_state = ConnectionState::CONNECTED
|
49
|
+
rescue
|
50
|
+
@sock = nil
|
51
|
+
raise
|
52
|
+
end
|
53
|
+
if not negotiate_protocol_version
|
54
|
+
raise RuntimeError.new 'Unable to negotiate protocol version'
|
55
|
+
end
|
56
|
+
self
|
57
|
+
end #}}}
|
58
|
+
|
59
|
+
def disconnect #{{{
|
60
|
+
if @sock
|
61
|
+
@sock.close
|
62
|
+
@sock = nil
|
63
|
+
@conn_state = ConnectionState::DISCONNECTED
|
64
|
+
@logger.info "Connection closed " + @hostname +":" + @port.to_s
|
65
|
+
true
|
66
|
+
else
|
67
|
+
false
|
68
|
+
end
|
69
|
+
end #}}}
|
70
|
+
|
71
|
+
def connected? #{{{
|
72
|
+
@conn_state != ConnectionState::DISCONNECTED
|
73
|
+
end #}}}
|
74
|
+
|
75
|
+
def controller_version #{{{
|
76
|
+
cmd = Command::RTDE_GET_URCONTROL_VERSION
|
77
|
+
version = send_and_receive cmd
|
78
|
+
@logger.debug 'Controller Version: ' + version.major.to_s + '.' + version.minor.to_s + '.' + version.bugfix.to_s + '.' + version.build.to_s
|
79
|
+
if version
|
80
|
+
if version.major == 3 && version.minor <=2 && version.bugfix < 19171
|
81
|
+
@logger.error 'Upgrade your controller to version higher than 3.2.19171'
|
82
|
+
exit
|
83
|
+
end
|
84
|
+
[version.major, version.minor, version.bugfix, version.build]
|
85
|
+
else
|
86
|
+
[nil, nil, nil, nil]
|
87
|
+
end
|
88
|
+
end #}}}
|
89
|
+
|
90
|
+
def negotiate_protocol_version #{{{
|
91
|
+
cmd = Command::RTDE_REQUEST_PROTOCOL_VERSION
|
92
|
+
payload = [PROTOCOL_VERSION].pack 'S>'
|
93
|
+
send_and_receive cmd, payload
|
94
|
+
end #}}}
|
95
|
+
|
96
|
+
def send(input_data) #{{{
|
97
|
+
if @conn_state != ConnectionState::STARTED
|
98
|
+
@logger.error 'Cannot send when RTDE synchroinization is inactive'
|
99
|
+
return
|
100
|
+
end
|
101
|
+
if not @input_config.key?(input_data.recipe_id)
|
102
|
+
@logger.error 'Input configuration id not found: ' + @input_data.recipe_id
|
103
|
+
return
|
104
|
+
end
|
105
|
+
config = @input_config[input_data.recipe_id]
|
106
|
+
send_all Command::RTDE_DATA_PACKAGE, config.pack(input_data)
|
107
|
+
end #}}}
|
108
|
+
def send_and_receive(cmd, payload = '') #{{{
|
109
|
+
@logger.debug 'Start send_and_receive'
|
110
|
+
send_all(cmd, payload) ? recv(cmd) : nil
|
111
|
+
end #}}}
|
112
|
+
def send_all(command, payload = '') #{{{
|
113
|
+
fmt = 'S>C'
|
114
|
+
size = ([0,0].pack fmt).length + payload.length
|
115
|
+
buf = [size, command].pack(fmt) + payload
|
116
|
+
@logger.debug 'send_all.size: ' +size.to_s
|
117
|
+
@logger.debug 'send_all.buf: ' + buf.to_s + "\n"
|
118
|
+
if !@sock
|
119
|
+
@logger.error 'Unable to send: not connected to Robot'
|
120
|
+
return false
|
121
|
+
end
|
122
|
+
|
123
|
+
_, writable, _ = IO.select([], [@sock], [])
|
124
|
+
if writable.length > 0
|
125
|
+
#@logger.debug 'buffer: ' + buf
|
126
|
+
@sock.sendmsg(buf)
|
127
|
+
@logger.debug 'sending ok'
|
128
|
+
true
|
129
|
+
else
|
130
|
+
trigger_disconnected
|
131
|
+
false
|
132
|
+
end
|
133
|
+
end #}}}
|
134
|
+
def send_message(message, source = 'Ruby Client', type = Serialize::Message::INFO_MESSAGE) #{{{
|
135
|
+
cmd = Command::RTDE_TEXT_MESSAGE
|
136
|
+
fmt = 'Ca%dCa%dC' % [message.length, source.length]
|
137
|
+
payload = struct.pack(fmt, message.length, message, source.length, source, type)
|
138
|
+
send_all(cmd, payload)
|
139
|
+
end #}}}
|
140
|
+
def send_start #{{{
|
141
|
+
@logger.debug 'Start send_start'
|
142
|
+
cmd = Command::RTDE_CONTROL_PACKAGE_START
|
143
|
+
if send_and_receive cmd
|
144
|
+
@logger.info 'RTDE synchronization started'
|
145
|
+
@conn_state = ConnectionState::STARTED
|
146
|
+
true
|
147
|
+
else
|
148
|
+
@logger.error 'RTDE synchronization failed to start'
|
149
|
+
false
|
150
|
+
end
|
151
|
+
end
|
152
|
+
#}}}
|
153
|
+
def send_pause #{{{
|
154
|
+
cmd = Command::RTDE_CONTROL_PACKAGE_PAUSE
|
155
|
+
success = send_and_receive(cmd)
|
156
|
+
if success
|
157
|
+
@logger.info 'RTDE synchronization paused'
|
158
|
+
@conn_state = ConnectionState::PAUSED
|
159
|
+
else
|
160
|
+
@logger.error('RTDE synchronization failed to pause')
|
161
|
+
end
|
162
|
+
success
|
163
|
+
end #}}}
|
164
|
+
def send_input_setup(variables, types=[]) #{{{
|
165
|
+
cmd = Command::RTDE_CONTROL_PACKAGE_SETUP_INPUTS
|
166
|
+
payload = variables.join ','
|
167
|
+
result = send_and_receive cmd, payload
|
168
|
+
if types.length != 0 && result.types != types
|
169
|
+
@logger.error(
|
170
|
+
'Data type inconsistency for input setup: ' +
|
171
|
+
types.to_s + ' - ' +
|
172
|
+
result.types.to_s
|
173
|
+
)
|
174
|
+
return nil
|
175
|
+
end
|
176
|
+
|
177
|
+
result.names = variables
|
178
|
+
@input_config[result.id] = result
|
179
|
+
Serialize::DataObject.create_empty variables, result.id
|
180
|
+
end #}}}
|
181
|
+
def send_output_setup(variables, types=[], frequency = 125) #{{{
|
182
|
+
@logger.debug 'Start send_output_setup'
|
183
|
+
@logger.debug 'variables: ' + variables.to_s
|
184
|
+
@logger.debug 'types: ' + types.to_s + "\n"
|
185
|
+
cmd = Command::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS
|
186
|
+
payload = [frequency].pack 'G'
|
187
|
+
payload = payload + variables.join(',')
|
188
|
+
result = send_and_receive cmd, payload
|
189
|
+
if types.length != 0 && result.types != types
|
190
|
+
@logger.error(
|
191
|
+
'Data type inconsistency for output setup: ' +
|
192
|
+
types.to_s + ' - ' +
|
193
|
+
result.types.to_s
|
194
|
+
)
|
195
|
+
return false
|
196
|
+
end
|
197
|
+
result.names = variables
|
198
|
+
@output_config = result
|
199
|
+
@logger.debug 'result:' + @output_config.to_s
|
200
|
+
return true
|
201
|
+
end #}}}
|
202
|
+
|
203
|
+
def receive #{{{
|
204
|
+
@logger.debug 'Start receive'
|
205
|
+
if !@output_config
|
206
|
+
@logger.error 'Output configuration not initialized'
|
207
|
+
nil
|
208
|
+
end
|
209
|
+
return nil if @conn_state != ConnectionState::STARTED
|
210
|
+
recv Command::RTDE_DATA_PACKAGE
|
211
|
+
end #}}}
|
212
|
+
def recv(command) #{{{
|
213
|
+
@logger.debug 'Start recv' + @buf.to_s
|
214
|
+
while connected?
|
215
|
+
readable, _, xlist = IO.select([@sock], [], [@sock])
|
216
|
+
@logger.debug 'Readable: ' + readable.to_s
|
217
|
+
if readable.length > 0
|
218
|
+
@logger.debug 'readable.length >0: ' + readable.length.to_s
|
219
|
+
more = @sock.recv(4096)
|
220
|
+
if more.length == 0
|
221
|
+
trigger_disconnected
|
222
|
+
return nil
|
223
|
+
end
|
224
|
+
@buf += more
|
225
|
+
end
|
226
|
+
|
227
|
+
if xlist.length > 0 || readable.length == 0
|
228
|
+
@logger.info 'lost connection with controller'
|
229
|
+
trigger_disconnected
|
230
|
+
return nil
|
231
|
+
end
|
232
|
+
while @buf.length >= 3
|
233
|
+
@logger.debug '@buf>=3'
|
234
|
+
packet_header = Serialize::ControlHeader.unpack(@buf)
|
235
|
+
|
236
|
+
if @buf.length >= packet_header.size
|
237
|
+
@logger.debug '@buf.length >= packet_header.size' + @buf.length.to_s + ">=" + packet_header.size.to_s
|
238
|
+
packet, @buf = @buf[3..packet_header.size], @buf[packet_header.size..-1]
|
239
|
+
#@logger.debug 'Packet:' + packet.to_s
|
240
|
+
@logger.debug 'Packet_Header_Command: ' + packet_header.command.to_s + "\n"
|
241
|
+
data = on_packet(packet_header.command, packet)
|
242
|
+
@logger.debug 'DATA:' + data.to_s
|
243
|
+
if @buf.length >= 3 && command == Command::RTDE_DATA_PACKAGE
|
244
|
+
@logger.debug '@buf.length >= 3 && command == Command::RTDE_DATA_PACKAGE'
|
245
|
+
next_packet_header = Serialize::ControlHeader.unpack(@buf)
|
246
|
+
if next_packet_header.command == command
|
247
|
+
@logger.info 'skipping package(1)'
|
248
|
+
next
|
249
|
+
end
|
250
|
+
end
|
251
|
+
if packet_header.command == command
|
252
|
+
@logger.debug 'returning becuase of packet_header.command == command'
|
253
|
+
return data
|
254
|
+
else
|
255
|
+
@logger.info 'skipping package(2)'
|
256
|
+
end
|
257
|
+
else
|
258
|
+
break
|
259
|
+
end
|
260
|
+
end
|
261
|
+
end
|
262
|
+
nil
|
263
|
+
end #}}}
|
264
|
+
|
265
|
+
def on_packet(cmd, payload) #{{{
|
266
|
+
return unpack_protocol_version_package(payload) if cmd == Command::RTDE_REQUEST_PROTOCOL_VERSION
|
267
|
+
return unpack_urcontrol_version_package(payload) if cmd == Command::RTDE_GET_URCONTROL_VERSION
|
268
|
+
return unpack_text_message(payload) if cmd == Command::RTDE_TEXT_MESSAGE
|
269
|
+
return unpack_setup_outputs_package(payload) if cmd == Command::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS
|
270
|
+
return unpack_setup_inputs_package(payload) if cmd == Command::RTDE_CONTROL_PACKAGE_SETUP_INPUTS
|
271
|
+
return unpack_start_package(payload) if cmd == Command::RTDE_CONTROL_PACKAGE_START
|
272
|
+
return unpack_pause_package(payload) if cmd == Command::RTDE_CONTROL_PACKAGE_PAUSE
|
273
|
+
return unpack_data_package(payload, @output_config) if cmd == Command::RTDE_DATA_PACKAGE
|
274
|
+
@logger.error 'Unknown package command' + cmd.to_s
|
275
|
+
end #}}}
|
276
|
+
|
277
|
+
def has_data? #{{{
|
278
|
+
timeout = 0
|
279
|
+
readable, _, _ = IO.select([@sock], [], [], timeout)
|
280
|
+
readable.length != 0
|
281
|
+
end #}}}
|
282
|
+
|
283
|
+
def trigger_disconnected #{{{
|
284
|
+
@logger.info 'RTDE disconnected'
|
285
|
+
disconnect
|
286
|
+
end #}}}
|
287
|
+
|
288
|
+
def unpack_protocol_version_package(payload) #{{{
|
289
|
+
@logger.debug 'unpaking protocol version package'
|
290
|
+
return nil if payload.length != 1
|
291
|
+
Serialize::ReturnValue.unpack(payload).success
|
292
|
+
end #}}}
|
293
|
+
def unpack_urcontrol_version_package(payload) #{{{
|
294
|
+
@logger.debug 'unpack urcontrol_version'
|
295
|
+
return nil if payload.length != 16
|
296
|
+
@logger.debug 'packet lenght ok'
|
297
|
+
Serialize::ControlVersion.unpack payload
|
298
|
+
end #}}}
|
299
|
+
def unpack_text_message(payload) #{{{
|
300
|
+
return nil if payload.length < 1
|
301
|
+
msg = Serialize::Message.unpack payload
|
302
|
+
@logger.error (msg.source + ':' + msg.message) if msg.level == Serialize::Message::EXCEPTION_MESSAGE || msg.level == Serialize::Message::ERROR_MESSAGE
|
303
|
+
@logger.warning(msg.source + ':' + msg.message) if msg.level == Serialize::Message::WARNING_MESSAGE
|
304
|
+
@logger.info (msg.source + ':' + msg.message) if msg.level == Serialize::Message::INFO_MESSAGE
|
305
|
+
end #}}}
|
306
|
+
def unpack_setup_outputs_package(payload) #{{{
|
307
|
+
@logger.debug 'Start unpack_setup_outputs_package'
|
308
|
+
if payload.length < 1
|
309
|
+
@logger.error 'RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload'
|
310
|
+
return nil
|
311
|
+
end
|
312
|
+
@logger.debug 'Payload for unpack: ' + payload.to_s
|
313
|
+
Serialize::DataConfig.unpack_recipe payload
|
314
|
+
end #}}}
|
315
|
+
def unpack_setup_inputs_package(payload) #{{{
|
316
|
+
if payload.length < 1
|
317
|
+
@logger.error 'RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload'
|
318
|
+
return nil
|
319
|
+
end
|
320
|
+
Serialize::DataConfig.unpack_recipe payload
|
321
|
+
end #}}}
|
322
|
+
def unpack_start_package(payload) #{{{
|
323
|
+
if payload.length != 1
|
324
|
+
@logger.error 'RTDE_CONTROL_PACKAGE_START: Wrong payload size'
|
325
|
+
return nil
|
326
|
+
end
|
327
|
+
Serialize::ReturnValue.unpack(payload).success
|
328
|
+
end #}}}
|
329
|
+
def unpack_pause_package(payload) #{{{
|
330
|
+
if payload.length != 1
|
331
|
+
@logger.error 'RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size'
|
332
|
+
return nil
|
333
|
+
end
|
334
|
+
Serialize::ReturnValue.unpack(payload).success
|
335
|
+
end #}}}
|
336
|
+
def unpack_data_package(payload, output_config) #{{{
|
337
|
+
if !output_config
|
338
|
+
@logger.error 'RTDE_DATA_PACKAGE: Missing output configuration'
|
339
|
+
return nil
|
340
|
+
end
|
341
|
+
@logger.debug "outputconfig: " + output_config.to_s
|
342
|
+
@logger.debug "payload: " + payload.to_s
|
343
|
+
output_config.unpack payload
|
344
|
+
end #}}}
|
345
|
+
|
346
|
+
end
|
347
|
+
|
348
|
+
end
|
data/lib/serialize.rb
ADDED
@@ -0,0 +1,185 @@
|
|
1
|
+
#!/usr/bin/env ruby
|
2
|
+
|
3
|
+
module UR
|
4
|
+
module Serialize
|
5
|
+
module ControlHeader
|
6
|
+
Data = Struct.new(:size, :command)
|
7
|
+
def self.unpack str
|
8
|
+
Data.new *str.unpack('S>C')
|
9
|
+
end
|
10
|
+
end
|
11
|
+
|
12
|
+
module ControlVersion
|
13
|
+
Data = Struct.new(:major, :minor, :bugfix, :build)
|
14
|
+
def self.unpack(str)
|
15
|
+
Data.new *str.unpack('I>I>I>I>')
|
16
|
+
end
|
17
|
+
end
|
18
|
+
|
19
|
+
module ReturnValue
|
20
|
+
Data = Struct.new :success
|
21
|
+
def self.unpack(str)
|
22
|
+
Data.new *str.unpack('C')
|
23
|
+
end
|
24
|
+
end
|
25
|
+
|
26
|
+
module Message
|
27
|
+
Data = Struct.new :level, :message, :source
|
28
|
+
EXCEPTION_MESSAGE = 0
|
29
|
+
ERROR_MESSAGE = 1
|
30
|
+
WARNING_MESSAGE = 2
|
31
|
+
INFO_MESSAGE = 3
|
32
|
+
|
33
|
+
def self.unpack
|
34
|
+
msg_length = buf.unpack('C')
|
35
|
+
msg = buf.unpack('x' + C * msg_length).pack('C*')
|
36
|
+
src_length = buf.unpack('x' + 'x' * msg_length + 'C')
|
37
|
+
src = buf.unpack('x' + 'x' * msg_length + 'x' + 'C' * src_length).pack('C*')
|
38
|
+
lvl = buf.unpack('x' + 'x' * msg_length + 'x' + 'x' * src_length + 'C')
|
39
|
+
Data.new(level,lvl,msg,src)
|
40
|
+
end
|
41
|
+
end
|
42
|
+
|
43
|
+
def self.get_item_size(data_type)
|
44
|
+
if data_type.start_with? 'VECTOR6'
|
45
|
+
6
|
46
|
+
elsif data_type.start_with? 'VECTOR3'
|
47
|
+
3
|
48
|
+
else
|
49
|
+
1
|
50
|
+
end
|
51
|
+
end
|
52
|
+
|
53
|
+
def self.unpack_field(data, offset, data_type)
|
54
|
+
size = self.get_item_size(data_type)
|
55
|
+
if data_type == 'VECTOR6D' or data_type == 'VECTOR3D'
|
56
|
+
data[offset...offset+size].map(&:to_f)
|
57
|
+
elsif data_type == 'VECTOR6UINT32'
|
58
|
+
data[offset...offset+size].map(&:to_i)
|
59
|
+
elsif data_type == 'DOUBLE'
|
60
|
+
data[offset].to_f
|
61
|
+
elsif data_type == 'UINT32' or data_type == 'UINT64'
|
62
|
+
data[offset].to_i
|
63
|
+
elsif data_type == 'VECTOR6INT32'
|
64
|
+
data[offset...offset+size].map(&:to_i)
|
65
|
+
elsif data_type == 'INT32' or data_type == 'UINT8'
|
66
|
+
data[offset].to_i
|
67
|
+
else
|
68
|
+
raise TypeError.new('unpack_field: unknown data type: ' + data_type)
|
69
|
+
end
|
70
|
+
end
|
71
|
+
def [](item)
|
72
|
+
@values[item]
|
73
|
+
end
|
74
|
+
|
75
|
+
class DataObject
|
76
|
+
attr_reader :values
|
77
|
+
attr_accessor :recipe_id
|
78
|
+
|
79
|
+
def initialize
|
80
|
+
@values = {}
|
81
|
+
@recipe_id = nil
|
82
|
+
end
|
83
|
+
|
84
|
+
def [](item)
|
85
|
+
@values[item]
|
86
|
+
end
|
87
|
+
def []=(item,value)
|
88
|
+
@values[item] = value
|
89
|
+
end
|
90
|
+
|
91
|
+
def self.unpack(data, names, types)
|
92
|
+
if names.length != types.length
|
93
|
+
raise RuntimeError.new('List sizes are not identical.')
|
94
|
+
end
|
95
|
+
obj = DataObject.new
|
96
|
+
offset = 0
|
97
|
+
obj.recipe_id = data[0]
|
98
|
+
|
99
|
+
#puts "Datat:" + data.to_s
|
100
|
+
|
101
|
+
names.each_with_index do |name,i|
|
102
|
+
#obj.values[i] = data[1..-1].unpack('x' * offset + types[i])
|
103
|
+
#puts unpack_field(data[1..-1], offset, types[i])
|
104
|
+
|
105
|
+
obj.values[name] = Serialize.unpack_field(data[1..-1], offset, types[i])
|
106
|
+
#puts "obj"
|
107
|
+
#puts obj.values[i]
|
108
|
+
offset += Serialize::get_item_size(types[i])
|
109
|
+
end
|
110
|
+
#puts "obj:" + obj.values.to_s
|
111
|
+
obj
|
112
|
+
end
|
113
|
+
|
114
|
+
def self.create_empty(names, recipe_id)
|
115
|
+
obj = DataObject.new
|
116
|
+
names.each do |i|
|
117
|
+
obj.values[i] = nil
|
118
|
+
end
|
119
|
+
obj.recipe_id = recipe_id
|
120
|
+
obj
|
121
|
+
end
|
122
|
+
|
123
|
+
def pack(names, types)
|
124
|
+
if names.length != types.length
|
125
|
+
raise RuntimeError.new('List sizes are not identical.')
|
126
|
+
end
|
127
|
+
l = []
|
128
|
+
l.append @recipe_id unless @recipe_id
|
129
|
+
names.each do |i|
|
130
|
+
raise RuntimeEror.new('Uninitialized parameter: ' + i) unless @values[i]
|
131
|
+
l.push *@values[i]
|
132
|
+
end
|
133
|
+
l
|
134
|
+
end
|
135
|
+
end
|
136
|
+
|
137
|
+
class DataConfig < Struct.new(:id, :names, :types, :fmt)
|
138
|
+
def self.unpack_recipe(buf)
|
139
|
+
rmd = DataConfig.new
|
140
|
+
rmd.id = buf.unpack('C')[0]
|
141
|
+
rmd.types = buf[1..-1].split(',')
|
142
|
+
rmd.fmt = 'C'
|
143
|
+
#puts "DataConfig: " +rmd.to_s
|
144
|
+
rmd.types.each do |i|
|
145
|
+
if i == 'INT32'
|
146
|
+
rmd.fmt += 'i>'
|
147
|
+
elsif i == 'UINT32'
|
148
|
+
rmd.fmt += 'I>'
|
149
|
+
elsif i == 'VECTOR6D'
|
150
|
+
rmd.fmt += 'G'*6
|
151
|
+
elsif i == 'VECTOR3D'
|
152
|
+
rmd.fmt += 'G'*3
|
153
|
+
elsif i == 'VECTOR6INT32'
|
154
|
+
rmd.fmt += 'i>'*6
|
155
|
+
elsif i == 'VECTOR6UINT32'
|
156
|
+
rmd.fmt += 'I>'*6
|
157
|
+
elsif i == 'DOUBLE'
|
158
|
+
rmd.fmt += 'G'
|
159
|
+
elsif i == 'UINT64'
|
160
|
+
rmd.fmt += 'Q>'
|
161
|
+
elsif i == 'UINT8'
|
162
|
+
rmd.fmt += 'C'
|
163
|
+
elsif i == 'IN_USE'
|
164
|
+
raise TypeError 'An input parameter is already in use.'
|
165
|
+
else
|
166
|
+
raise TypeError 'Unknown data type: ' + i
|
167
|
+
end
|
168
|
+
end
|
169
|
+
@fmt = rmd.fmt
|
170
|
+
rmd
|
171
|
+
end
|
172
|
+
|
173
|
+
def pack(state)
|
174
|
+
l = state.pack(self.names, self.types)
|
175
|
+
l.pack(self.fmt)
|
176
|
+
end
|
177
|
+
|
178
|
+
def unpack(data)
|
179
|
+
li = data.unpack(self.fmt)
|
180
|
+
DataObject.unpack(li, self.names, self.types)
|
181
|
+
end
|
182
|
+
end
|
183
|
+
|
184
|
+
end
|
185
|
+
end
|
data/lib/ur-sock.rb
ADDED
data/ur-sock.gemspec
ADDED
@@ -0,0 +1,23 @@
|
|
1
|
+
Gem::Specification.new do |s|
|
2
|
+
s.name = "ur-sock"
|
3
|
+
s.version = "0.1"
|
4
|
+
s.platform = Gem::Platform::RUBY
|
5
|
+
s.license = "LGPL-3.0"
|
6
|
+
s.summary = "Preliminary release of Universal Robot (UR) Socket Communication."
|
7
|
+
|
8
|
+
s.description = "see https://github.com/fpauker/ur-sock"
|
9
|
+
|
10
|
+
s.files = Dir['{example/**/*,lib/**/*.rb,contrib/logo*}'] + %w(COPYING Rakefile ur-sock.gemspec README.md)
|
11
|
+
s.extensions = Dir["ext/**/extconf.rb"]
|
12
|
+
s.require_path = 'lib'
|
13
|
+
s.extra_rdoc_files = ['README.md']
|
14
|
+
|
15
|
+
s.required_ruby_version = '>=2.3.0'
|
16
|
+
|
17
|
+
s.authors = ['Florian Pauker','Juergen eTM Mangler']
|
18
|
+
|
19
|
+
s.email = 'florian.pauker@gmail.com'
|
20
|
+
s.homepage = 'https://github.com/fpauker/ur-sock'
|
21
|
+
|
22
|
+
s.add_runtime_dependency 'xml-smart', '>=0.3.6', '~>0'
|
23
|
+
end
|
metadata
ADDED
@@ -0,0 +1,73 @@
|
|
1
|
+
--- !ruby/object:Gem::Specification
|
2
|
+
name: ur-sock
|
3
|
+
version: !ruby/object:Gem::Version
|
4
|
+
version: '0.1'
|
5
|
+
platform: ruby
|
6
|
+
authors:
|
7
|
+
- Florian Pauker
|
8
|
+
- Juergen eTM Mangler
|
9
|
+
autorequire:
|
10
|
+
bindir: bin
|
11
|
+
cert_chain: []
|
12
|
+
date: 2019-05-17 00:00:00.000000000 Z
|
13
|
+
dependencies:
|
14
|
+
- !ruby/object:Gem::Dependency
|
15
|
+
name: xml-smart
|
16
|
+
requirement: !ruby/object:Gem::Requirement
|
17
|
+
requirements:
|
18
|
+
- - ">="
|
19
|
+
- !ruby/object:Gem::Version
|
20
|
+
version: 0.3.6
|
21
|
+
- - "~>"
|
22
|
+
- !ruby/object:Gem::Version
|
23
|
+
version: '0'
|
24
|
+
type: :runtime
|
25
|
+
prerelease: false
|
26
|
+
version_requirements: !ruby/object:Gem::Requirement
|
27
|
+
requirements:
|
28
|
+
- - ">="
|
29
|
+
- !ruby/object:Gem::Version
|
30
|
+
version: 0.3.6
|
31
|
+
- - "~>"
|
32
|
+
- !ruby/object:Gem::Version
|
33
|
+
version: '0'
|
34
|
+
description: see https://github.com/fpauker/ur-sock
|
35
|
+
email: florian.pauker@gmail.com
|
36
|
+
executables: []
|
37
|
+
extensions: []
|
38
|
+
extra_rdoc_files:
|
39
|
+
- README.md
|
40
|
+
files:
|
41
|
+
- COPYING
|
42
|
+
- README.md
|
43
|
+
- Rakefile
|
44
|
+
- lib/conf.rb
|
45
|
+
- lib/rtde.rb
|
46
|
+
- lib/serialize.rb
|
47
|
+
- lib/ur-sock.rb
|
48
|
+
- ur-sock.gemspec
|
49
|
+
homepage: https://github.com/fpauker/ur-sock
|
50
|
+
licenses:
|
51
|
+
- LGPL-3.0
|
52
|
+
metadata: {}
|
53
|
+
post_install_message:
|
54
|
+
rdoc_options: []
|
55
|
+
require_paths:
|
56
|
+
- lib
|
57
|
+
required_ruby_version: !ruby/object:Gem::Requirement
|
58
|
+
requirements:
|
59
|
+
- - ">="
|
60
|
+
- !ruby/object:Gem::Version
|
61
|
+
version: 2.3.0
|
62
|
+
required_rubygems_version: !ruby/object:Gem::Requirement
|
63
|
+
requirements:
|
64
|
+
- - ">="
|
65
|
+
- !ruby/object:Gem::Version
|
66
|
+
version: '0'
|
67
|
+
requirements: []
|
68
|
+
rubyforge_project:
|
69
|
+
rubygems_version: 2.7.6.2
|
70
|
+
signing_key:
|
71
|
+
specification_version: 4
|
72
|
+
summary: Preliminary release of Universal Robot (UR) Socket Communication.
|
73
|
+
test_files: []
|