rosruby 0.0.1
Sign up to get free protection for your applications and to get access to all the features.
- data/bin/rubyroscore +5 -0
- data/lib/ros.rb +25 -0
- data/lib/ros/duration.rb +63 -0
- data/lib/ros/graph_manager.rb +408 -0
- data/lib/ros/log.rb +72 -0
- data/lib/ros/master.rb +408 -0
- data/lib/ros/master_proxy.rb +256 -0
- data/lib/ros/message.rb +65 -0
- data/lib/ros/name.rb +88 -0
- data/lib/ros/node.rb +442 -0
- data/lib/ros/package.rb +144 -0
- data/lib/ros/parameter_manager.rb +127 -0
- data/lib/ros/parameter_subscriber.rb +47 -0
- data/lib/ros/publisher.rb +96 -0
- data/lib/ros/rate.rb +41 -0
- data/lib/ros/ros.rb +10 -0
- data/lib/ros/roscore.rb +29 -0
- data/lib/ros/service.rb +37 -0
- data/lib/ros/service_client.rb +83 -0
- data/lib/ros/service_server.rb +92 -0
- data/lib/ros/slave_proxy.rb +153 -0
- data/lib/ros/subscriber.rb +119 -0
- data/lib/ros/tcpros/client.rb +108 -0
- data/lib/ros/tcpros/header.rb +89 -0
- data/lib/ros/tcpros/message.rb +74 -0
- data/lib/ros/tcpros/server.rb +137 -0
- data/lib/ros/tcpros/service_client.rb +104 -0
- data/lib/ros/tcpros/service_server.rb +132 -0
- data/lib/ros/time.rb +109 -0
- data/lib/ros/topic.rb +47 -0
- data/lib/ros/xmlrpcserver.rb +40 -0
- data/samples/add_two_ints_client.rb +25 -0
- data/samples/add_two_ints_server.rb +20 -0
- data/samples/gui.rb +126 -0
- data/samples/sample_log.rb +16 -0
- data/samples/sample_param.rb +20 -0
- data/samples/sample_publisher.rb +20 -0
- data/samples/sample_subscriber.rb +19 -0
- data/scripts/genmsg_ruby.py +1135 -0
- data/scripts/genmsg_ruby.pyc +0 -0
- data/scripts/gensrv_ruby.py +105 -0
- data/scripts/gensrv_ruby.pyc +0 -0
- data/scripts/rosruby_genmsg.py +67 -0
- data/scripts/run-test.rb +21 -0
- data/test/test_header.rb +36 -0
- data/test/test_log.rb +45 -0
- data/test/test_master_proxy.rb +73 -0
- data/test/test_message.rb +13 -0
- data/test/test_node.rb +166 -0
- data/test/test_package.rb +10 -0
- data/test/test_param.rb +27 -0
- data/test/test_pubsub.rb +154 -0
- data/test/test_rate.rb +16 -0
- data/test/test_service.rb +34 -0
- data/test/test_slave_proxy.rb +49 -0
- data/test/test_time.rb +39 -0
- metadata +170 -0
data/bin/rubyroscore
ADDED
data/lib/ros.rb
ADDED
@@ -0,0 +1,25 @@
|
|
1
|
+
# ros.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
|
8
|
+
#
|
9
|
+
# start up file.
|
10
|
+
# add rospackage paths to $:.
|
11
|
+
#
|
12
|
+
|
13
|
+
require 'ros/package'
|
14
|
+
ROS::load_manifest('rosruby')
|
15
|
+
["#{ENV['HOME']}/.ros/rosruby/msg_gen/ruby", "#{ENV['HOME']}/.ros/rosruby/srv_gen/ruby"].each do |path|
|
16
|
+
if File.exists?(path)
|
17
|
+
if not $:.include?(path)
|
18
|
+
$:.push(path)
|
19
|
+
end
|
20
|
+
end
|
21
|
+
end
|
22
|
+
require 'ros/node'
|
23
|
+
|
24
|
+
# ensure shutdown all nodes
|
25
|
+
END {ROS::Node.shutdown_all_nodes}
|
data/lib/ros/duration.rb
ADDED
@@ -0,0 +1,63 @@
|
|
1
|
+
# ros/duration.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
# Duration object for ROS.
|
8
|
+
#
|
9
|
+
|
10
|
+
require 'ros/time'
|
11
|
+
|
12
|
+
module ROS
|
13
|
+
|
14
|
+
##
|
15
|
+
# == ROS Duration object
|
16
|
+
# This is used as msg object for duration
|
17
|
+
class Duration < TimeValue
|
18
|
+
|
19
|
+
# @example
|
20
|
+
# d1 = ROS::Duration.new(0.1) # => @nsecs=100000000, @secs=0
|
21
|
+
# d2 = ROS::Duration.new(1, 100) # => @nsecs=100, @secs=1
|
22
|
+
# @overload initialize(secs)
|
23
|
+
# @param [Float] secs initialize from seconds
|
24
|
+
# @overload initialize(secs, nsecs)
|
25
|
+
# @param [Integer] secs seconds
|
26
|
+
# @param [Integer] nsecs nano seconds
|
27
|
+
def initialize(secs=0, nsecs=nil)
|
28
|
+
@secs = secs.to_i
|
29
|
+
if nsecs
|
30
|
+
@nsecs = nsecs
|
31
|
+
else
|
32
|
+
@nsecs = ((secs - @secs) * 1e9.to_i).to_i
|
33
|
+
end
|
34
|
+
canonicalize
|
35
|
+
end
|
36
|
+
|
37
|
+
# create a new duration
|
38
|
+
# @param [Duration] duration Duration for adding
|
39
|
+
# @return [Duration] added duration
|
40
|
+
def +(duration)
|
41
|
+
tm = ::ROS::Duration.new
|
42
|
+
tm.secs = @secs + duration.secs
|
43
|
+
tm.nsecs = @nsecs + duration.nsecs
|
44
|
+
tm.canonicalize
|
45
|
+
end
|
46
|
+
|
47
|
+
# create a new duration
|
48
|
+
# @param [Duration] other Duration for substituting
|
49
|
+
# @return [Duration] substituted duration
|
50
|
+
def -(other)
|
51
|
+
d = ::ROS::Duration.new
|
52
|
+
d.secs = @secs - other.secs
|
53
|
+
d.nsecs = @nsecs - other.nsecs
|
54
|
+
d.canonicalize
|
55
|
+
end
|
56
|
+
|
57
|
+
# sleep while this duration
|
58
|
+
def sleep
|
59
|
+
Kernel.sleep(to_sec)
|
60
|
+
end
|
61
|
+
end
|
62
|
+
|
63
|
+
end
|
@@ -0,0 +1,408 @@
|
|
1
|
+
# ros/graph_manager.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
#=Manager of ROS graph
|
8
|
+
#
|
9
|
+
# this contains all subscribers, publishers, service_servers of a node.
|
10
|
+
# Master API document is http://ros.org/wiki/ROS/Master_API
|
11
|
+
# Slave API is http://ros.org/wiki/ROS/Slave_API
|
12
|
+
#
|
13
|
+
require 'ros/xmlrpcserver'
|
14
|
+
require 'ros/master_proxy'
|
15
|
+
require 'timeout'
|
16
|
+
|
17
|
+
module ROS
|
18
|
+
|
19
|
+
#Manager of ROS graph
|
20
|
+
#
|
21
|
+
#This contains all subscribers, publishers, service_servers of a node.
|
22
|
+
#It connects with master and manage pub/sub and services.
|
23
|
+
#- Master API document is http://ros.org/wiki/ROS/Master_API
|
24
|
+
#- Slave API is http://ros.org/wiki/ROS/Slave_API
|
25
|
+
class GraphManager
|
26
|
+
|
27
|
+
# for canonicalize_name(parameter_key)
|
28
|
+
include Name
|
29
|
+
|
30
|
+
# @return [Array] all Publisher of this node
|
31
|
+
attr_reader :publishers
|
32
|
+
# @return [Array] all Subscriber of this node
|
33
|
+
attr_reader :subscribers
|
34
|
+
# @return [Array] all ServiceServer of this node
|
35
|
+
attr_reader :service_servers
|
36
|
+
# @return [Array] all ParameterSubscriber of this node
|
37
|
+
attr_reader :parameter_subscribers
|
38
|
+
|
39
|
+
##
|
40
|
+
# current running all nodes.
|
41
|
+
# This is used for shutdown all nodes.
|
42
|
+
@@all_nodes = []
|
43
|
+
|
44
|
+
# @return [String] value hostname of this node
|
45
|
+
attr_reader :host
|
46
|
+
# @return [Integer] value port number of this node
|
47
|
+
attr_reader :port
|
48
|
+
|
49
|
+
##
|
50
|
+
# add xmlrpc handlers for slave connections.
|
51
|
+
# Then start serve thread.
|
52
|
+
# @param [String] caller_id caller_id of this node
|
53
|
+
# @param [String] master_uri URI of ROS Master
|
54
|
+
# @param [String] host hostname of this node
|
55
|
+
def initialize(caller_id, master_uri, host)
|
56
|
+
@caller_id = caller_id
|
57
|
+
@host = host
|
58
|
+
@port = get_available_port
|
59
|
+
@master_uri = master_uri
|
60
|
+
@is_ok = true
|
61
|
+
@master = MasterProxy.new(@caller_id, @master_uri, get_uri)
|
62
|
+
@server = XMLRPCServer.new(@port, @host)
|
63
|
+
@publishers = []
|
64
|
+
@subscribers = []
|
65
|
+
@service_servers = []
|
66
|
+
@parameter_subscribers = []
|
67
|
+
|
68
|
+
add_handlers
|
69
|
+
|
70
|
+
@thread = Thread.new do
|
71
|
+
@server.serve
|
72
|
+
end
|
73
|
+
|
74
|
+
@@all_nodes.push(self)
|
75
|
+
end
|
76
|
+
|
77
|
+
# shutdown all nodes
|
78
|
+
def self.shutdown_all
|
79
|
+
@@all_nodes.each do |node|
|
80
|
+
if node.is_ok?
|
81
|
+
node.shutdown
|
82
|
+
end
|
83
|
+
end
|
84
|
+
end
|
85
|
+
|
86
|
+
##
|
87
|
+
# check if this node is running or not.
|
88
|
+
# @return [Boolean] true if node is running.
|
89
|
+
def is_ok?
|
90
|
+
@is_ok
|
91
|
+
end
|
92
|
+
|
93
|
+
##
|
94
|
+
# get available port number by opening port 0.
|
95
|
+
# @return [Integer] port_num
|
96
|
+
#
|
97
|
+
def get_available_port
|
98
|
+
server = TCPServer.open(0)
|
99
|
+
saddr = server.getsockname
|
100
|
+
port = Socket.unpack_sockaddr_in(saddr)[0]
|
101
|
+
server.close
|
102
|
+
port
|
103
|
+
end
|
104
|
+
|
105
|
+
##
|
106
|
+
# get this slave node's URI
|
107
|
+
# @return [String] uri ('http://host:port')
|
108
|
+
#
|
109
|
+
def get_uri
|
110
|
+
"http://" + @host + ":" + @port.to_s + "/"
|
111
|
+
end
|
112
|
+
|
113
|
+
##
|
114
|
+
# wait until service is available
|
115
|
+
# @param [String] service_name name of service for waiting
|
116
|
+
# @param [Float] timeout_sec wait for this seconds, then time out
|
117
|
+
# @return [Boolean] true: available, false: time out
|
118
|
+
def wait_for_service(service_name, timeout_sec)
|
119
|
+
begin
|
120
|
+
timeout(timeout_sec) do
|
121
|
+
while @is_ok
|
122
|
+
if @master.lookup_service(service_name)
|
123
|
+
return true
|
124
|
+
end
|
125
|
+
sleep(0.1)
|
126
|
+
end
|
127
|
+
end
|
128
|
+
rescue Timeout::Error
|
129
|
+
puts "time outed for wait service #{service_name}"
|
130
|
+
return nil
|
131
|
+
rescue
|
132
|
+
raise "connection with master failed. master = #{@master_uri}"
|
133
|
+
end
|
134
|
+
end
|
135
|
+
|
136
|
+
##
|
137
|
+
# register a service to master,
|
138
|
+
# and add it in the controlling server list.
|
139
|
+
# raise if fail.
|
140
|
+
# @param [ServiceServer] service_server ServiceServer to be added
|
141
|
+
# @return [ServiceServer] service_server
|
142
|
+
def add_service_server(service_server)
|
143
|
+
@master.register_service(service_server.service_name,
|
144
|
+
service_server.service_uri)
|
145
|
+
service_server.set_manager(self)
|
146
|
+
@service_servers.push(service_server)
|
147
|
+
service_server
|
148
|
+
end
|
149
|
+
|
150
|
+
|
151
|
+
##
|
152
|
+
# register a subscriber to master. raise if fail.
|
153
|
+
# @param [Subscriber] subscriber Subscriber to be added
|
154
|
+
# @return [Subscriber] subscriber
|
155
|
+
def add_subscriber(subscriber)
|
156
|
+
uris = @master.register_subscriber(subscriber.topic_name,
|
157
|
+
subscriber.topic_type.type)
|
158
|
+
subscriber.set_manager(self)
|
159
|
+
uris.each do |publisher_uri|
|
160
|
+
subscriber.add_connection(publisher_uri)
|
161
|
+
end
|
162
|
+
@subscribers.push(subscriber)
|
163
|
+
subscriber
|
164
|
+
end
|
165
|
+
|
166
|
+
##
|
167
|
+
# register callback for paramUpdate
|
168
|
+
# @param [ParameterSubscriber] subscriber ParameterSubscriber instance to be added
|
169
|
+
# @return [ParameterSubscriber] subscriber
|
170
|
+
def add_parameter_subscriber(subscriber)
|
171
|
+
subscriber.set_manager(self)
|
172
|
+
@parameter_subscribers.push(subscriber)
|
173
|
+
@master.subscribe_param(subscriber.key)
|
174
|
+
subscriber
|
175
|
+
end
|
176
|
+
|
177
|
+
##
|
178
|
+
# register a publisher. raise if fail.
|
179
|
+
# @param [Publisher] publisher Publisher instance to be added
|
180
|
+
# @return [Publisher] publisher
|
181
|
+
def add_publisher(publisher)
|
182
|
+
@master.register_publisher(publisher.topic_name,
|
183
|
+
publisher.topic_type.type)
|
184
|
+
publisher.set_manager(self)
|
185
|
+
@publishers.push(publisher)
|
186
|
+
publisher
|
187
|
+
end
|
188
|
+
|
189
|
+
##
|
190
|
+
# process all messages of subscribers.
|
191
|
+
# This means that callbacks for all queued messages are called.
|
192
|
+
def spin_once
|
193
|
+
@subscribers.each {|subscriber| subscriber.process_queue}
|
194
|
+
end
|
195
|
+
|
196
|
+
##
|
197
|
+
# shutdown a publisher.
|
198
|
+
# @param [Publisher] publisher Publisher to be shutdown
|
199
|
+
def shutdown_publisher(publisher)
|
200
|
+
begin
|
201
|
+
@master.unregister_publisher(publisher.topic_name)
|
202
|
+
ensure
|
203
|
+
@publishers.delete(publisher) do |pub|
|
204
|
+
raise "publisher not found"
|
205
|
+
end
|
206
|
+
publisher.close
|
207
|
+
end
|
208
|
+
end
|
209
|
+
|
210
|
+
##
|
211
|
+
# shutdown a subscriber.
|
212
|
+
# @param [Subscriber] subscriber Subscriber to be shutdown
|
213
|
+
def shutdown_subscriber(subscriber)
|
214
|
+
begin
|
215
|
+
@master.unregister_subscriber(subscriber.topic_name)
|
216
|
+
@subscribers.delete(subscriber) do |pub|
|
217
|
+
raise "subscriber not found"
|
218
|
+
end
|
219
|
+
ensure
|
220
|
+
subscriber.close
|
221
|
+
end
|
222
|
+
end
|
223
|
+
|
224
|
+
##
|
225
|
+
# shutdown a service server.
|
226
|
+
# @param [ServiceServer] service ServiceServer to be shutdown
|
227
|
+
def shutdown_service_server(service)
|
228
|
+
begin
|
229
|
+
@master.unregister_service(service.service_name,
|
230
|
+
service.service_uri)
|
231
|
+
@service_servers.delete(service) do |pub|
|
232
|
+
raise "service_server not found"
|
233
|
+
end
|
234
|
+
ensure
|
235
|
+
service.close
|
236
|
+
end
|
237
|
+
end
|
238
|
+
|
239
|
+
##
|
240
|
+
# shutdown a parameter subscriber.
|
241
|
+
# @param [Subscriber] subscriber ParameterSubscriber to be shutdown
|
242
|
+
def shutdown_parameter_subscriber(subscriber)
|
243
|
+
begin
|
244
|
+
@master.unsubscribe_param(subscriber.key)
|
245
|
+
@parameter_subscribers.delete(subscriber) do |sub|
|
246
|
+
raise "parameter server not found"
|
247
|
+
end
|
248
|
+
end
|
249
|
+
end
|
250
|
+
|
251
|
+
##
|
252
|
+
# shutdown this slave node.
|
253
|
+
# shutdown the xmlrpc server and all pub/sub connections.
|
254
|
+
# and delelte all pub/sub instance from connection list
|
255
|
+
# @return [GraphManager] self
|
256
|
+
def shutdown
|
257
|
+
begin
|
258
|
+
@is_ok = false
|
259
|
+
@server.shutdown
|
260
|
+
if not @thread.join(0.1)
|
261
|
+
Thread::kill(@thread)
|
262
|
+
end
|
263
|
+
rescue
|
264
|
+
puts 'fail while shutdown'
|
265
|
+
Thread::kill(@thread)
|
266
|
+
end
|
267
|
+
|
268
|
+
begin
|
269
|
+
@publishers.each do |publisher|
|
270
|
+
@master.unregister_publisher(publisher.topic_name)
|
271
|
+
publisher.close
|
272
|
+
end
|
273
|
+
rescue
|
274
|
+
ensure
|
275
|
+
@publishers = nil
|
276
|
+
end
|
277
|
+
|
278
|
+
begin
|
279
|
+
@subscribers.each do |subscriber|
|
280
|
+
@master.unregister_subscriber(subscriber.topic_name)
|
281
|
+
subscriber.close
|
282
|
+
end
|
283
|
+
rescue
|
284
|
+
ensure
|
285
|
+
@subscribers = nil
|
286
|
+
end
|
287
|
+
begin
|
288
|
+
@service_servers.each do |service|
|
289
|
+
@master.unregister_service(service.service_name,
|
290
|
+
service.service_uri)
|
291
|
+
service.close
|
292
|
+
end
|
293
|
+
rescue
|
294
|
+
ensure
|
295
|
+
@service_servers = nil
|
296
|
+
end
|
297
|
+
begin
|
298
|
+
@parameter_subscribers.each do |subscriber|
|
299
|
+
@master.unsubscribe_param(subscriber.key)
|
300
|
+
end
|
301
|
+
rescue
|
302
|
+
ensure
|
303
|
+
@parameter_subscribers = nil
|
304
|
+
end
|
305
|
+
@@all_nodes.delete(self)
|
306
|
+
|
307
|
+
self
|
308
|
+
end
|
309
|
+
|
310
|
+
private
|
311
|
+
|
312
|
+
##
|
313
|
+
# add all handers
|
314
|
+
def add_handlers #:nodoc:
|
315
|
+
@server.set_default_handler do |method, *args|
|
316
|
+
puts "unhandled call with #{method}, #{args}"
|
317
|
+
[0, "I DON'T KNOW", 0]
|
318
|
+
end
|
319
|
+
|
320
|
+
@server.add_handler('getBusStats') do |caller_id|
|
321
|
+
pubstats = @publishers.map do |pub|
|
322
|
+
[pub.topic_name, pub.topic_type.type, pub.get_connection_data]
|
323
|
+
end
|
324
|
+
substats = @subscribers.map do |sub|
|
325
|
+
[sub.topic_name, sub.get_connection_data]
|
326
|
+
end
|
327
|
+
servstats = @service_servers.map do |service|
|
328
|
+
[service.get_connection_data]
|
329
|
+
end
|
330
|
+
[1, "stats", [pubstats, substats, servstats]]
|
331
|
+
end
|
332
|
+
|
333
|
+
@server.add_handler('getMasterUri') do |caller_id|
|
334
|
+
[1, "master", @master_uri]
|
335
|
+
end
|
336
|
+
|
337
|
+
@server.add_handler('getSubscriptions') do |caller_id|
|
338
|
+
topic_list = @subscribers.map do |sub|
|
339
|
+
[sub.topic_name, sub.topic_type.type]
|
340
|
+
end
|
341
|
+
[1, "ok", topic_list]
|
342
|
+
end
|
343
|
+
|
344
|
+
@server.add_handler('getPublications') do |caller_id|
|
345
|
+
topic_list = @publishers.map do |pub|
|
346
|
+
[pub.topic_name, pub.topic_type.type]
|
347
|
+
end
|
348
|
+
[1, "ok", topic_list]
|
349
|
+
end
|
350
|
+
|
351
|
+
@server.add_handler('paramUpdate') do |caller_id, parameter_key, parameter_value|
|
352
|
+
@parameter_subscribers.each do |param|
|
353
|
+
# parameter_key has / in the end
|
354
|
+
if param.key == canonicalize_name(parameter_key)
|
355
|
+
param.call(parameter_value)
|
356
|
+
end
|
357
|
+
end
|
358
|
+
[1, "ok", 0]
|
359
|
+
end
|
360
|
+
|
361
|
+
@server.add_handler('requestTopic') do |caller_id, topic, protocols|
|
362
|
+
message = [0, "I DON'T KNOW", 0]
|
363
|
+
protocols.select {|x| x[0] == 'TCPROS'}.each do |protocol|
|
364
|
+
@publishers.select {|pub| pub.topic_name == topic}.each do |publisher|
|
365
|
+
connection = publisher.add_connection(caller_id)
|
366
|
+
message = [1, "OK! WAIT!!!", ['TCPROS',
|
367
|
+
connection.host,
|
368
|
+
connection.port]]
|
369
|
+
end
|
370
|
+
end
|
371
|
+
message
|
372
|
+
end
|
373
|
+
|
374
|
+
@server.add_handler('shutdown') do |caller_id, msg|
|
375
|
+
# puts "shutting down by master request: #{msg}"
|
376
|
+
shutdown
|
377
|
+
[1, 'shutdown ok', 0]
|
378
|
+
end
|
379
|
+
|
380
|
+
@server.add_handler('getPid') do |caller_id|
|
381
|
+
[1, "pid ok", Process.pid]
|
382
|
+
end
|
383
|
+
|
384
|
+
@server.add_handler('getBusInfo') do |caller_id|
|
385
|
+
info = []
|
386
|
+
@publishers.each do |publisher|
|
387
|
+
info.concat(publisher.get_connection_info)
|
388
|
+
end
|
389
|
+
@subscribers.each do |subscriber|
|
390
|
+
info.concat(subscriber.get_connection_info)
|
391
|
+
end
|
392
|
+
[1, "getBusInfo ok", info]
|
393
|
+
end
|
394
|
+
|
395
|
+
@server.add_handler('publisherUpdate') do |caller_id, topic, publishers|
|
396
|
+
@subscribers.select {|sub| sub.topic_name == topic}.each do |sub|
|
397
|
+
publishers.select {|uri| not sub.has_connection_with?(uri)}.each do |uri|
|
398
|
+
sub.add_connection(uri)
|
399
|
+
end
|
400
|
+
sub.get_connected_uri.select {|uri| not publishers.include?(uri)}.each do |uri|
|
401
|
+
sub.drop_connection(uri)
|
402
|
+
end
|
403
|
+
end
|
404
|
+
[1, "OK! Updated!!", 0]
|
405
|
+
end
|
406
|
+
end
|
407
|
+
end
|
408
|
+
end
|