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
@@ -0,0 +1,256 @@
|
|
1
|
+
# ros/master_proxy.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
#
|
8
|
+
# == ROS Master Proxy
|
9
|
+
# access to ROS Master
|
10
|
+
#
|
11
|
+
#
|
12
|
+
|
13
|
+
require 'xmlrpc/client'
|
14
|
+
|
15
|
+
module ROS
|
16
|
+
|
17
|
+
# == ROS Master Proxy
|
18
|
+
# access to ROS Master.
|
19
|
+
# @see http://ros.org/wiki/ROS/Master_API
|
20
|
+
# But there are not documented API.
|
21
|
+
#
|
22
|
+
class MasterProxy
|
23
|
+
|
24
|
+
#
|
25
|
+
# @param [String] caller_id caller_id of this node
|
26
|
+
# @param [String] master_uri URI of ROS Master
|
27
|
+
# @param [String] slave_uri slave URI of this node
|
28
|
+
def initialize(caller_id, master_uri, slave_uri)
|
29
|
+
@caller_id = caller_id
|
30
|
+
@master_uri = master_uri
|
31
|
+
@slave_uri = slave_uri
|
32
|
+
@proxy = XMLRPC::Client.new2(@master_uri).proxy
|
33
|
+
end
|
34
|
+
|
35
|
+
# register a service
|
36
|
+
# @param [String] service name of service
|
37
|
+
# @param [String] service_api service api uri
|
38
|
+
# @return [Boolean] true success
|
39
|
+
# @raise RuntimeError
|
40
|
+
def register_service(service, service_api)
|
41
|
+
code, message, val = @proxy.registerService(@caller_id,
|
42
|
+
service,
|
43
|
+
service_api,
|
44
|
+
@slave_uri)
|
45
|
+
if code == 1
|
46
|
+
return true
|
47
|
+
else
|
48
|
+
raise message
|
49
|
+
end
|
50
|
+
end
|
51
|
+
|
52
|
+
# unregister a service
|
53
|
+
# @param [String] service name of service
|
54
|
+
# @param [String] service_api service api uri
|
55
|
+
# @return [Boolean] true success
|
56
|
+
# @raise RuntimeError
|
57
|
+
def unregister_service(service, service_api)
|
58
|
+
code, message, val = @proxy.unregisterService(@caller_id,
|
59
|
+
service,
|
60
|
+
service_api)
|
61
|
+
if code == 1
|
62
|
+
return true
|
63
|
+
elsif code == 0
|
64
|
+
puts message
|
65
|
+
return true
|
66
|
+
else
|
67
|
+
raise message
|
68
|
+
end
|
69
|
+
end
|
70
|
+
|
71
|
+
# register a subscriber
|
72
|
+
# @param [String] topic topic name
|
73
|
+
# @param [String] topic_type topic type
|
74
|
+
# @return [Array] URI of current publishers
|
75
|
+
# @raise [RuntimeError] if error
|
76
|
+
def register_subscriber(topic, topic_type)
|
77
|
+
code, message,val = @proxy.registerSubscriber(@caller_id,
|
78
|
+
topic,
|
79
|
+
topic_type,
|
80
|
+
@slave_uri)
|
81
|
+
if code == 1
|
82
|
+
val
|
83
|
+
elsif code == 0
|
84
|
+
puts message
|
85
|
+
val
|
86
|
+
else
|
87
|
+
raise message
|
88
|
+
end
|
89
|
+
end
|
90
|
+
|
91
|
+
# unregister a subscriber
|
92
|
+
# @param [String] topic name of topic to unregister
|
93
|
+
# @return [Boolean] true
|
94
|
+
# @raise RuntimeError
|
95
|
+
def unregister_subscriber(topic)
|
96
|
+
code, message,val = @proxy.unregisterSubscriber(@caller_id,
|
97
|
+
topic,
|
98
|
+
@slave_uri)
|
99
|
+
if code == 1
|
100
|
+
return true
|
101
|
+
elsif code == 0
|
102
|
+
puts message
|
103
|
+
return true
|
104
|
+
else
|
105
|
+
raise message
|
106
|
+
end
|
107
|
+
end
|
108
|
+
|
109
|
+
# register a publisher
|
110
|
+
# @param [String] topic topic name of topic
|
111
|
+
# @param [String] topic_type type of topic
|
112
|
+
# @return [Array] URI of current subscribers
|
113
|
+
# @raise RuntimeError
|
114
|
+
def register_publisher(topic, topic_type)
|
115
|
+
code, message, uris = @proxy.registerPublisher(@caller_id,
|
116
|
+
topic,
|
117
|
+
topic_type,
|
118
|
+
@slave_uri)
|
119
|
+
if code == 1
|
120
|
+
uris
|
121
|
+
else
|
122
|
+
raise message
|
123
|
+
end
|
124
|
+
end
|
125
|
+
|
126
|
+
# unregister a publisher
|
127
|
+
# @param [String] topic name of topic
|
128
|
+
# @return [Boolean] true
|
129
|
+
# @raise RuntimeError
|
130
|
+
def unregister_publisher(topic)
|
131
|
+
code, message, val = @proxy.unregisterPublisher(@caller_id,
|
132
|
+
topic,
|
133
|
+
@slave_uri)
|
134
|
+
if code == 1
|
135
|
+
return val
|
136
|
+
elsif code == 0
|
137
|
+
puts message
|
138
|
+
return true
|
139
|
+
else
|
140
|
+
raise message
|
141
|
+
end
|
142
|
+
return false
|
143
|
+
end
|
144
|
+
|
145
|
+
##
|
146
|
+
# this method is not described in the wiki.
|
147
|
+
# subscribe to the parameter key.
|
148
|
+
# @param [String] key name of parameter
|
149
|
+
# @return [Boolean] true
|
150
|
+
# @raise [RuntimeError] if fail
|
151
|
+
def subscribe_param(key)
|
152
|
+
code, message, uri = @proxy.subscribeParam(@caller_id, @slave_uri, key)
|
153
|
+
if code == 1
|
154
|
+
return true
|
155
|
+
else
|
156
|
+
raise message
|
157
|
+
end
|
158
|
+
end
|
159
|
+
|
160
|
+
##
|
161
|
+
# unsubscribe to the parameter key.
|
162
|
+
# this method is not described in the wiki.
|
163
|
+
# @param [String] key name of parameter key
|
164
|
+
# @return [Boolean] true
|
165
|
+
# @raise [RuntimeError] if failt
|
166
|
+
def unsubscribe_param(key)
|
167
|
+
code, message, uri = @proxy.unsubscribeParam(@caller_id, @slave_uri, key)
|
168
|
+
if code == 1
|
169
|
+
return true
|
170
|
+
else
|
171
|
+
raise message
|
172
|
+
end
|
173
|
+
end
|
174
|
+
|
175
|
+
# lookup a node by name.
|
176
|
+
# @param [String] node_name
|
177
|
+
# @return [String, nil] URI of the node if it is found. nil not found.
|
178
|
+
def lookup_node(node_name)
|
179
|
+
code, message, uri = @proxy.lookupNode(@caller_id, node_name)
|
180
|
+
if code == 1
|
181
|
+
uri
|
182
|
+
else
|
183
|
+
nil
|
184
|
+
end
|
185
|
+
end
|
186
|
+
|
187
|
+
# get the all published topics
|
188
|
+
# @param [String] subgraph namespace for check
|
189
|
+
# @return [Array] topic names.
|
190
|
+
# @raise
|
191
|
+
def get_published_topics(subgraph='')
|
192
|
+
code, message, topics = @proxy.getPublishedTopics(@caller_id, subgraph)
|
193
|
+
if code == 1
|
194
|
+
return topics
|
195
|
+
elsif
|
196
|
+
raise message
|
197
|
+
end
|
198
|
+
end
|
199
|
+
|
200
|
+
# get system state
|
201
|
+
# @return [Array] state
|
202
|
+
def get_system_state
|
203
|
+
code, message, state = @proxy.getSystemState(@caller_id)
|
204
|
+
if code == 1
|
205
|
+
return state
|
206
|
+
else
|
207
|
+
raise message
|
208
|
+
end
|
209
|
+
end
|
210
|
+
|
211
|
+
# get the master URI
|
212
|
+
# @return [String] uri
|
213
|
+
# @raise
|
214
|
+
def get_uri
|
215
|
+
code, message, uri = @proxy.getUri(@caller_id)
|
216
|
+
if code == 1
|
217
|
+
return uri
|
218
|
+
else
|
219
|
+
raise message
|
220
|
+
end
|
221
|
+
end
|
222
|
+
|
223
|
+
# look up a service by name
|
224
|
+
# @param [String] service name of service
|
225
|
+
# @return [String, nil] URI of service if found, nil not found.
|
226
|
+
def lookup_service(service)
|
227
|
+
code, message, uri = @proxy.lookupService(@caller_id, service)
|
228
|
+
if code == 1
|
229
|
+
uri
|
230
|
+
else
|
231
|
+
false
|
232
|
+
end
|
233
|
+
end
|
234
|
+
|
235
|
+
# Master URI
|
236
|
+
# @return [String]
|
237
|
+
attr_reader :master_uri
|
238
|
+
|
239
|
+
# set the master uri
|
240
|
+
# @param [String] uri master uri
|
241
|
+
# @return [MasterProxy] self
|
242
|
+
def master_uri=(uri)
|
243
|
+
@master_uri = uri
|
244
|
+
@proxy = XMLRPC::Client.new2(@master_uri).proxy
|
245
|
+
self
|
246
|
+
end
|
247
|
+
|
248
|
+
# Slave URI
|
249
|
+
# @return [String]
|
250
|
+
attr_accessor :slave_uri
|
251
|
+
|
252
|
+
# caller id of this node
|
253
|
+
# @return [String]
|
254
|
+
attr_accessor :caller_id
|
255
|
+
end
|
256
|
+
end
|
data/lib/ros/message.rb
ADDED
@@ -0,0 +1,65 @@
|
|
1
|
+
# ros/message.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
# == Message file for msg/srv (de)serialization
|
8
|
+
#
|
9
|
+
#
|
10
|
+
|
11
|
+
module ROS
|
12
|
+
|
13
|
+
##
|
14
|
+
# super class of all msg/srv converted class.
|
15
|
+
# Currently this is none.
|
16
|
+
class Message
|
17
|
+
end
|
18
|
+
|
19
|
+
##
|
20
|
+
# used for serialization (for python like grammar)
|
21
|
+
# this is used by msg/srv converted *.rb files
|
22
|
+
# it can be removed, if there are more effective genmsg_ruby.
|
23
|
+
class Struct
|
24
|
+
|
25
|
+
# @param [String] format
|
26
|
+
def initialize(format)
|
27
|
+
@format = format
|
28
|
+
end
|
29
|
+
|
30
|
+
attr_reader :format
|
31
|
+
|
32
|
+
def self.calc_size(format)
|
33
|
+
array = []
|
34
|
+
start = 0
|
35
|
+
while start < format.length
|
36
|
+
re = /(\w)(\d*)/
|
37
|
+
re =~ format[start..(format.length-1)]
|
38
|
+
number = $2.to_i
|
39
|
+
if number == 0
|
40
|
+
array.push(0)
|
41
|
+
else
|
42
|
+
for i in 1..number
|
43
|
+
array.push(0)
|
44
|
+
end
|
45
|
+
end
|
46
|
+
start += $&.length
|
47
|
+
end
|
48
|
+
array.pack(format).length
|
49
|
+
end
|
50
|
+
|
51
|
+
# pack the data
|
52
|
+
# @param [Array] args
|
53
|
+
def pack(*args)
|
54
|
+
args.pack(@format)
|
55
|
+
end
|
56
|
+
|
57
|
+
# unpack from string
|
58
|
+
# @param [String] arg
|
59
|
+
# @return [Array] unpacked data
|
60
|
+
def unpack(arg)
|
61
|
+
arg.unpack(@format)
|
62
|
+
end
|
63
|
+
|
64
|
+
end
|
65
|
+
end
|
data/lib/ros/name.rb
ADDED
@@ -0,0 +1,88 @@
|
|
1
|
+
# ros/name.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
# == Naming module
|
8
|
+
#
|
9
|
+
# ROS naming module.
|
10
|
+
#
|
11
|
+
|
12
|
+
require 'socket' # for gethostname
|
13
|
+
|
14
|
+
module ROS
|
15
|
+
|
16
|
+
##
|
17
|
+
# this module provides naming functions.
|
18
|
+
#
|
19
|
+
module Name
|
20
|
+
|
21
|
+
# name sparation char ('/')
|
22
|
+
SEP = '/'
|
23
|
+
|
24
|
+
##
|
25
|
+
# start with '/' and use single '/' for names
|
26
|
+
#
|
27
|
+
# @param [String] name input name
|
28
|
+
# @return [String] canonicalized name
|
29
|
+
def canonicalize_name(name)
|
30
|
+
if name == nil or name == SEP
|
31
|
+
return name
|
32
|
+
elsif name[0] == SEP[0]
|
33
|
+
return name.split(/\/+/).join(SEP)
|
34
|
+
else
|
35
|
+
return SEP + name.split(/\/+/).join(SEP)
|
36
|
+
end
|
37
|
+
end
|
38
|
+
|
39
|
+
##
|
40
|
+
# expand ~local_param like names.
|
41
|
+
#
|
42
|
+
# @param [String] caller_id caller id for replacing ~
|
43
|
+
# @param [String] name param name like '~param'
|
44
|
+
# @return [String] expanded name
|
45
|
+
def expand_local_name(caller_id, name)
|
46
|
+
if name[0] == '~'[0]
|
47
|
+
caller_id + SEP + name[1..-1]
|
48
|
+
else
|
49
|
+
name
|
50
|
+
end
|
51
|
+
end
|
52
|
+
|
53
|
+
##
|
54
|
+
# generate anonymous name using input id.
|
55
|
+
# (arange from roslib)
|
56
|
+
# @param [String] id base id (String)
|
57
|
+
# @return [String] return generated id
|
58
|
+
def anonymous_name(id)
|
59
|
+
name = "#{id}_#{Socket.gethostname}_#{Process.pid}_#{rand(1000000)}"
|
60
|
+
name = name.gsub('.', '_')
|
61
|
+
name = name.gsub('-', '_')
|
62
|
+
name.gsub(':', '_')
|
63
|
+
end
|
64
|
+
|
65
|
+
|
66
|
+
##
|
67
|
+
# expand local, canonicalize, remappings
|
68
|
+
# @param [String] caller_id caller_id
|
69
|
+
# @param [String] ns namespace
|
70
|
+
# @param [String] name target name
|
71
|
+
# @param [Array] remappings name remappings
|
72
|
+
# @return [String] resolved name
|
73
|
+
def resolve_name_with_call_id(caller_id, ns, name, remappings)
|
74
|
+
name = canonicalize_name(expand_local_name(caller_id, name))
|
75
|
+
if remappings
|
76
|
+
remappings.each_pair do |key, value|
|
77
|
+
if name == canonicalize_name(key)
|
78
|
+
name = value
|
79
|
+
end
|
80
|
+
end
|
81
|
+
end
|
82
|
+
if ns
|
83
|
+
name = ns + SEP + name
|
84
|
+
end
|
85
|
+
return canonicalize_name(name)
|
86
|
+
end
|
87
|
+
end
|
88
|
+
end
|
data/lib/ros/node.rb
ADDED
@@ -0,0 +1,442 @@
|
|
1
|
+
# ros/node.rb
|
2
|
+
#
|
3
|
+
# License: BSD
|
4
|
+
#
|
5
|
+
# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
|
6
|
+
#
|
7
|
+
# == ROS Node
|
8
|
+
#
|
9
|
+
# user interface of ROS.
|
10
|
+
#
|
11
|
+
require 'ros/parameter_manager'
|
12
|
+
require 'ros/name'
|
13
|
+
require 'ros/graph_manager'
|
14
|
+
require 'ros/publisher'
|
15
|
+
require 'ros/subscriber'
|
16
|
+
require 'ros/parameter_subscriber'
|
17
|
+
require 'ros/service_server'
|
18
|
+
require 'ros/service_client'
|
19
|
+
require 'ros/log'
|
20
|
+
require 'ros/time'
|
21
|
+
require 'ros/rate'
|
22
|
+
require 'ros/duration'
|
23
|
+
|
24
|
+
module ROS
|
25
|
+
|
26
|
+
# main interface of rosruby.
|
27
|
+
#
|
28
|
+
# @example Sample for Publisher
|
29
|
+
# node = ROS::Node.new('/rosruby/sample_publisher')
|
30
|
+
# publisher = node.advertise('/chatter', Std_msgs::String)
|
31
|
+
# sleep(1)
|
32
|
+
# msg = Std_msgs::String.new
|
33
|
+
# i = 0
|
34
|
+
# while node.ok?
|
35
|
+
# msg.data = "Hello, rosruby!: #{i}"
|
36
|
+
# publisher.publish(msg)
|
37
|
+
#
|
38
|
+
# @example Sample for Subscriber
|
39
|
+
# node = ROS::Node.new('/rosruby/sample_subscriber')
|
40
|
+
# node.subscribe('/chatter', Std_msgs::String) do |msg|
|
41
|
+
# puts "message come! = \'#{msg.data}\'"
|
42
|
+
# end
|
43
|
+
#
|
44
|
+
# while node.ok?
|
45
|
+
# node.spin_once
|
46
|
+
# sleep(1)
|
47
|
+
# end
|
48
|
+
#
|
49
|
+
class Node
|
50
|
+
|
51
|
+
# for node shutdown hook
|
52
|
+
@@shutdown_hook = []
|
53
|
+
|
54
|
+
# Naming functions of ROS
|
55
|
+
include Name
|
56
|
+
|
57
|
+
##
|
58
|
+
# initialization of ROS node
|
59
|
+
# get env, parse args, and start slave xmlrpc servers.
|
60
|
+
#
|
61
|
+
# @param [String] node_name name of this node
|
62
|
+
# @param [Hash] options options
|
63
|
+
# @option options [Boolean] :anonymous (false) use anonymous name if true. anonymous node generates a unique name
|
64
|
+
def initialize(node_name, options={})
|
65
|
+
@remappings = {}
|
66
|
+
# @host is rewrited by ARGS[ROS_IP] or ARGS[ROS_HOSTNAME]
|
67
|
+
@host = Socket.gethostname
|
68
|
+
get_env
|
69
|
+
if options[:anonymous]
|
70
|
+
node_name = anonymous_name(node_name)
|
71
|
+
end
|
72
|
+
@node_name = resolve_name(node_name)
|
73
|
+
@remappings = parse_args(ARGV)
|
74
|
+
if not @master_uri
|
75
|
+
raise 'ROS_MASTER_URI is nos set. please check environment variables'
|
76
|
+
end
|
77
|
+
|
78
|
+
@manager = GraphManager.new(@node_name, @master_uri, @host)
|
79
|
+
@parameter = ParameterManager.new(@master_uri, @node_name, @remappings)
|
80
|
+
if not options[:nologger]
|
81
|
+
@logger = ::ROS::Log.new(self)
|
82
|
+
end
|
83
|
+
# because xmlrpc server use signal trap, after serve, it have to trap sig trap_signals
|
84
|
+
ObjectSpace.define_finalizer(self, proc {|id| self.shutdown})
|
85
|
+
end
|
86
|
+
|
87
|
+
##
|
88
|
+
# Is this node running? Please use for 'while loop' and so on..
|
89
|
+
#
|
90
|
+
# @return [Boolean] true if node is running.
|
91
|
+
#
|
92
|
+
def ok?
|
93
|
+
@manager.is_ok?
|
94
|
+
end
|
95
|
+
|
96
|
+
# URI of master
|
97
|
+
# @return [String] uri string of master
|
98
|
+
attr_reader :master_uri
|
99
|
+
|
100
|
+
# hostname of this node.
|
101
|
+
# @return [String] host name
|
102
|
+
attr_reader :host
|
103
|
+
|
104
|
+
# name of this node (caller_id).
|
105
|
+
# @return [String] name of this node (=caller_id)
|
106
|
+
attr_reader :node_name
|
107
|
+
|
108
|
+
##
|
109
|
+
# resolve the name by this node's remapping rule.
|
110
|
+
# @param [String] name name for resolved
|
111
|
+
# @return [String] resolved name
|
112
|
+
#
|
113
|
+
def resolve_name(name)
|
114
|
+
resolve_name_with_call_id(@node_name, @ns, name, @remappings)
|
115
|
+
end
|
116
|
+
|
117
|
+
##
|
118
|
+
# get the param for key.
|
119
|
+
# You can set default value. That is uesed when the key is not set yet.
|
120
|
+
# @param [String] key key for search the parameters
|
121
|
+
# @param [String, Integer, Float, Boolean] default default value
|
122
|
+
# @return [String, Integer, Float, Boolean] parameter value for key
|
123
|
+
#
|
124
|
+
def get_param(key, default=nil)
|
125
|
+
key = expand_local_name(@node_name, key)
|
126
|
+
param = @parameter.get_param(key)
|
127
|
+
if param
|
128
|
+
param
|
129
|
+
else
|
130
|
+
default
|
131
|
+
end
|
132
|
+
end
|
133
|
+
|
134
|
+
##
|
135
|
+
# get all parameters.
|
136
|
+
#
|
137
|
+
# @return [Array] all parameter list
|
138
|
+
#
|
139
|
+
def get_param_names
|
140
|
+
@parameter.get_param_names
|
141
|
+
end
|
142
|
+
|
143
|
+
##
|
144
|
+
# check if the parameter server has the param for 'key'.
|
145
|
+
# @param [String] key key for check
|
146
|
+
# @return [Boolean] true if exits
|
147
|
+
def has_param(key)
|
148
|
+
@parameter.has_param(expand_local_name(@node_name, key))
|
149
|
+
end
|
150
|
+
|
151
|
+
##
|
152
|
+
# delete the parameter for 'key'
|
153
|
+
#
|
154
|
+
# @param [String] key key for delete
|
155
|
+
# @return [Boolean] true if success, false if it is not exist
|
156
|
+
def delete_param(key)
|
157
|
+
@parameter.delete_param(expand_local_name(@node_name, key))
|
158
|
+
end
|
159
|
+
|
160
|
+
##
|
161
|
+
# set parameter for 'key'.
|
162
|
+
# @param [String] key key of parameter
|
163
|
+
# @param [String, Integer, Float, Boolean] value value of parameter
|
164
|
+
# @return [Boolean] return true if succeed
|
165
|
+
def set_param(key, value)
|
166
|
+
@parameter.set_param(expand_local_name(@node_name, key), value)
|
167
|
+
end
|
168
|
+
|
169
|
+
##
|
170
|
+
# start publishing the topic.
|
171
|
+
#
|
172
|
+
# @param [String] topic_name name of topic (string)
|
173
|
+
# @param [Class] topic_type topic class
|
174
|
+
# @param [Hash] options :latched, :resolve
|
175
|
+
# @option options [Boolean] :latched (false) latched topic
|
176
|
+
# @option options [Boolean] :resolve (true) resolve topic_name or not. This is for publish /rosout with namespaced node.
|
177
|
+
# @return [Publisher] Publisher instance
|
178
|
+
def advertise(topic_name, topic_type, options={})
|
179
|
+
if options[:no_resolve]
|
180
|
+
name = topic_name
|
181
|
+
else
|
182
|
+
name = resolve_name(topic_name)
|
183
|
+
end
|
184
|
+
publisher = Publisher.new(@node_name,
|
185
|
+
name,
|
186
|
+
topic_type,
|
187
|
+
options[:latched],
|
188
|
+
@manager.host)
|
189
|
+
@manager.add_publisher(publisher)
|
190
|
+
trap_signals
|
191
|
+
publisher
|
192
|
+
end
|
193
|
+
|
194
|
+
##
|
195
|
+
# start service server.
|
196
|
+
#
|
197
|
+
# @param [String] service_name name of this service (string)
|
198
|
+
# @param [Service] service_type service class
|
199
|
+
# @param [Proc] callback service definition
|
200
|
+
# @return [ServiceServer] ServiceServer instance
|
201
|
+
def advertise_service(service_name, service_type, &callback)
|
202
|
+
server = ::ROS::ServiceServer.new(@node_name,
|
203
|
+
resolve_name(service_name),
|
204
|
+
service_type,
|
205
|
+
callback)
|
206
|
+
@manager.add_service_server(server)
|
207
|
+
trap_signals
|
208
|
+
server
|
209
|
+
end
|
210
|
+
|
211
|
+
##
|
212
|
+
# wait until start the service.
|
213
|
+
# @param [String] service_name name of service for waiting
|
214
|
+
# @param [Float] timeout_sec time out seconds. default infinity.
|
215
|
+
# @return [Boolean] true if success, false if timeouted
|
216
|
+
def wait_for_service(service_name, timeout_sec=nil)
|
217
|
+
@manager.wait_for_service(service_name, timeout_sec)
|
218
|
+
end
|
219
|
+
|
220
|
+
##
|
221
|
+
# create service client.
|
222
|
+
# @param [String] service_name name of this service (string)
|
223
|
+
# @param [Class] service_type service class
|
224
|
+
# @return [ServiceClient] created ServiceClient instance
|
225
|
+
def service(service_name, service_type)
|
226
|
+
ROS::ServiceClient.new(@master_uri,
|
227
|
+
@node_name,
|
228
|
+
resolve_name(service_name),
|
229
|
+
service_type)
|
230
|
+
end
|
231
|
+
|
232
|
+
##
|
233
|
+
# start to subscribe a topic.
|
234
|
+
#
|
235
|
+
# @param [String] topic_name name of topic (string)
|
236
|
+
# @param [Class] topic_type Topic instance
|
237
|
+
# @return [Subscriber] created Subscriber instance
|
238
|
+
def subscribe(topic_name, topic_type, &callback)
|
239
|
+
sub = Subscriber.new(@node_name,
|
240
|
+
resolve_name(topic_name),
|
241
|
+
topic_type,
|
242
|
+
callback)
|
243
|
+
@manager.add_subscriber(sub)
|
244
|
+
trap_signals
|
245
|
+
sub
|
246
|
+
end
|
247
|
+
|
248
|
+
##
|
249
|
+
# subscribe to the parameter.
|
250
|
+
#
|
251
|
+
# @param [String] param name of parameter to subscribe
|
252
|
+
# @param [Proc] callback callback when parameter updated
|
253
|
+
# @return [ParameterSubscriber] created ParameterSubscriber instance
|
254
|
+
def subscribe_parameter(param, &callback)
|
255
|
+
sub = ParameterSubscriber.new(param, callback)
|
256
|
+
@manager.add_parameter_subscriber(sub)
|
257
|
+
sub
|
258
|
+
end
|
259
|
+
|
260
|
+
##
|
261
|
+
# spin once. This invoke subscription/service_server callbacks
|
262
|
+
#
|
263
|
+
def spin_once
|
264
|
+
@manager.spin_once
|
265
|
+
end
|
266
|
+
|
267
|
+
##
|
268
|
+
# spin forever.
|
269
|
+
#
|
270
|
+
def spin
|
271
|
+
while ok?
|
272
|
+
spin_once
|
273
|
+
sleep(0.01)
|
274
|
+
end
|
275
|
+
end
|
276
|
+
|
277
|
+
##
|
278
|
+
# unregister to master and shutdown all connections.
|
279
|
+
# @return [Node] self
|
280
|
+
def shutdown
|
281
|
+
if ok?
|
282
|
+
begin
|
283
|
+
@manager.shutdown
|
284
|
+
rescue => message
|
285
|
+
p message
|
286
|
+
puts 'ignoring errors while shutdown'
|
287
|
+
end
|
288
|
+
end
|
289
|
+
self
|
290
|
+
end
|
291
|
+
|
292
|
+
##
|
293
|
+
# outputs log message for INFO (INFORMATION).
|
294
|
+
# @param [String] message message for output
|
295
|
+
# @return [Node] self
|
296
|
+
def loginfo(message)
|
297
|
+
file, line, function = caller[0].split(':')
|
298
|
+
@logger.log('INFO', message, file, function, line.to_i)
|
299
|
+
self
|
300
|
+
end
|
301
|
+
|
302
|
+
##
|
303
|
+
# outputs log message for DEBUG
|
304
|
+
# @param [String] message message for output
|
305
|
+
# @return [Node] self
|
306
|
+
def logdebug(message)
|
307
|
+
file, line, function = caller[0].split(':')
|
308
|
+
@logger.log('DEBUG', message, file, function, line.to_i)
|
309
|
+
self
|
310
|
+
end
|
311
|
+
|
312
|
+
##
|
313
|
+
# outputs log message for WARN (WARING).
|
314
|
+
#
|
315
|
+
# @param [String] message message for output
|
316
|
+
# @return [Node] self
|
317
|
+
def logwarn(message)
|
318
|
+
file, line, function = caller[0].split(':')
|
319
|
+
@logger.log('WARN', message, file, function, line.to_i)
|
320
|
+
self
|
321
|
+
end
|
322
|
+
|
323
|
+
##
|
324
|
+
# outputs log message for ERROR.
|
325
|
+
#
|
326
|
+
# @param [String] message message for output
|
327
|
+
# @return [Node] self
|
328
|
+
def logerror(message)
|
329
|
+
file, line, function = caller[0].split(':')
|
330
|
+
@logger.log('ERROR', message, file, function, line.to_i)
|
331
|
+
self
|
332
|
+
end
|
333
|
+
|
334
|
+
alias_method :logerr, :logerror
|
335
|
+
|
336
|
+
##
|
337
|
+
# outputs log message for FATAL.
|
338
|
+
#
|
339
|
+
# @param [String] message message for output
|
340
|
+
# @return [Node] self
|
341
|
+
def logfatal(message)
|
342
|
+
file, line, function = caller[0].split(':')
|
343
|
+
@logger.log('FATAL', message, file, function, line.to_i)
|
344
|
+
self
|
345
|
+
end
|
346
|
+
|
347
|
+
##
|
348
|
+
# get all topics by this node.
|
349
|
+
#
|
350
|
+
# @return [Array] topic names
|
351
|
+
def get_published_topics
|
352
|
+
@manager.publishers.map do |pub|
|
353
|
+
pub.topic_name
|
354
|
+
end
|
355
|
+
end
|
356
|
+
|
357
|
+
private
|
358
|
+
|
359
|
+
##
|
360
|
+
# parse all environment variables.
|
361
|
+
#
|
362
|
+
def get_env #:nodoc:
|
363
|
+
@master_uri = ENV['ROS_MASTER_URI']
|
364
|
+
@ns = ENV['ROS_NAMESPACE']
|
365
|
+
if ENV['ROS_IP']
|
366
|
+
@host = ENV['ROS_IP']
|
367
|
+
elsif ENV['ROS_HOSTNAME']
|
368
|
+
@host = ENV['ROS_HOSTNAME']
|
369
|
+
end
|
370
|
+
end
|
371
|
+
|
372
|
+
##
|
373
|
+
# converts strings if it is float and int numbers.
|
374
|
+
# @example
|
375
|
+
# convert_if_needed('10') # => 10
|
376
|
+
# convert_if_needed('0.1') # => 0.1
|
377
|
+
# convert_if_needed('string') # => 'string'
|
378
|
+
# @param [String] value string
|
379
|
+
# @return [Float, Integer, String] return converted value.
|
380
|
+
def convert_if_needed(value) #:nodoc:
|
381
|
+
if value =~ /^[+-]?\d+\.?\d*$/ # float
|
382
|
+
value = value.to_f
|
383
|
+
elsif value =~ /^[+-]?\d+$/ # int
|
384
|
+
value = value.to_i
|
385
|
+
else
|
386
|
+
value
|
387
|
+
end
|
388
|
+
end
|
389
|
+
|
390
|
+
##
|
391
|
+
# parse all args.
|
392
|
+
# @param [Array] args arguments for parse
|
393
|
+
def parse_args(args) #:nodoc:
|
394
|
+
remapping = {}
|
395
|
+
for arg in args
|
396
|
+
splited = arg.split(':=')
|
397
|
+
if splited.length == 2
|
398
|
+
key, value = splited
|
399
|
+
if key == '__name'
|
400
|
+
@node_name = resolve_name(value)
|
401
|
+
elsif key == '__ip'
|
402
|
+
@host = value
|
403
|
+
elsif key == '__hostname'
|
404
|
+
@host = value
|
405
|
+
elsif key == '__master'
|
406
|
+
@master_uri = value
|
407
|
+
elsif key == '__ns'
|
408
|
+
@ns = value
|
409
|
+
elsif key[0] == '_'[0]
|
410
|
+
# local name remaps
|
411
|
+
key[0] = '~'
|
412
|
+
remapping[resolve_name(key)] = convert_if_needed(value)
|
413
|
+
else
|
414
|
+
# remaps
|
415
|
+
remapping[key] = convert_if_needed(value)
|
416
|
+
end
|
417
|
+
end
|
418
|
+
end
|
419
|
+
remapping
|
420
|
+
end
|
421
|
+
|
422
|
+
# trap signals for safe shutdown.
|
423
|
+
def trap_signals #:nodoc:
|
424
|
+
["INT", "TERM", "HUP"].each do |signal|
|
425
|
+
Signal.trap(signal) do
|
426
|
+
ROS::Node.shutdown_all_nodes
|
427
|
+
end
|
428
|
+
end
|
429
|
+
end
|
430
|
+
|
431
|
+
def self.add_shutdown_hook(proc)
|
432
|
+
@@shutdown_hook.push(proc)
|
433
|
+
end
|
434
|
+
|
435
|
+
# shutdown all nodes.
|
436
|
+
def self.shutdown_all_nodes
|
437
|
+
GraphManager.shutdown_all
|
438
|
+
@@shutdown_hook.each {|obj| obj.call}
|
439
|
+
end
|
440
|
+
|
441
|
+
end
|
442
|
+
end
|