Class: Orocos::ROS::NameService

Inherits:
NameServiceBase show all
Includes:
Namespace
Defined in:
lib/orocos/ros/name_service.rb

Overview

A name service implementation that allows to enumerate all ROS nodes

Constant Summary

Constants included from Namespace

Namespace::DELIMATOR

Instance Attribute Summary collapse

Attributes inherited from NameServiceBase

#name

Instance Method Summary collapse

Methods included from Namespace

#basename, #map_to_namespace, #namespace, #namespace=, #same_namespace?, #split_name, split_name, validate_namespace_name, #verify_same_namespace

Methods inherited from NameServiceBase

#each_task, #find_one_running, #get_provides, #ior, #reachable?, #same_namespace?, #task_reachable?

Constructor Details

#initialize(uri = ROS.default_ros_master_uri, caller_id = ROS.caller_id, options = Hash.new) ⇒ NameService

Returns a new instance of NameService



112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
# File 'lib/orocos/ros/name_service.rb', line 112

def initialize(uri = ROS.default_ros_master_uri, caller_id = ROS.caller_id, options = Hash.new)
    options = Kernel.validate_options options,
        :poll_period => 1

    @uri = uri
    @caller_id = caller_id
    @ros_graph = NodeGraph.new
    @mutex = Mutex.new
    @ros_master_sync = Mutex.new
    @updated_graph_signal = ConditionVariable.new
    @update_time = Time.at(0)
    @poll_period = options[:poll_period]
    super()

    @ros_master = ROSMaster.new(uri, caller_id)
    @thread_pool = Utilrb::ThreadPool.new(0, 2)
    poll_system_state
end

Dynamic Method Handling

This class handles dynamic methods through the method_missing method

#method_missing(m, *args, &block) ⇒ Object

Provide thread-safe access to the ROS graph API



301
302
303
304
305
306
307
308
# File 'lib/orocos/ros/name_service.rb', line 301

def method_missing(m, *args, &block)
    access_ros_graph do
        if ros_graph.respond_to?(m)
            return ros_graph.send(m, *args, &block)
        end
    super
    end
end

Instance Attribute Details

#caller_idObject (readonly)

Returns the value of attribute caller_id



98
99
100
# File 'lib/orocos/ros/name_service.rb', line 98

def caller_id
  @caller_id
end

#poll_periodObject

Returns the value of attribute poll_period



105
106
107
# File 'lib/orocos/ros/name_service.rb', line 105

def poll_period
  @poll_period
end

#ros_graphObject (readonly)

Returns the value of attribute ros_graph



104
105
106
# File 'lib/orocos/ros/name_service.rb', line 104

def ros_graph
  @ros_graph
end

#thread_poolUtilrb::ThreadPool (readonly)

The Utilrb::ThreadPool object that handles the asynchronous update of the ROS node graph

Returns:

  • (Utilrb::ThreadPool)


110
111
112
# File 'lib/orocos/ros/name_service.rb', line 110

def thread_pool
  @thread_pool
end

#update_timeTime (readonly)

The time of the last update to ros_graph. This is the time at which the XMLRPC request has been made

Returns:

  • (Time)


103
104
105
# File 'lib/orocos/ros/name_service.rb', line 103

def update_time
  @update_time
end

#uriObject (readonly)

Returns the value of attribute uri



97
98
99
# File 'lib/orocos/ros/name_service.rb', line 97

def uri
  @uri
end

Instance Method Details

#==(other) ⇒ Object



131
132
133
134
# File 'lib/orocos/ros/name_service.rb', line 131

def ==(other)
    other.class == self.class &&
        other.uri == self.uri
end

#access_ros_graphvoid

This method returns an undefined value.

Gives thread-safe access to the ROS graph

Raises:

  • any error that has occured during ROS graph update



286
287
288
289
290
291
# File 'lib/orocos/ros/name_service.rb', line 286

def access_ros_graph
    @mutex.synchronize do
        process_ros_master_exception
        yield if block_given?
    end
end

#done_system_state(result, exception) ⇒ Object



165
166
167
168
169
170
# File 'lib/orocos/ros/name_service.rb', line 165

def done_system_state(result, exception)
    time, graph = *result
    update_system_state(time, graph, exception)
    sleep(poll_period)
    poll_system_state
end

#find_topic_by_name(topic_name) ⇒ Topic

Returns the Topic object that matches the given topic name. If that topic has more than one publisher (yuk), it picks the first one.

Returns:



223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
# File 'lib/orocos/ros/name_service.rb', line 223

def find_topic_by_name(topic_name)
    retry_after_update_if_nil do
        node_name, direction =
            access_ros_graph do
                ros_graph.node_graph.find do |node_name, (inputs, outputs)|
                    if inputs.include?(topic_name)
                        break([node_name, :input_port])
                    elsif outputs.include?(topic_name)
                        break([node_name, :output_port])
                    end
                end
            end

        if node_name
            return get(node_name).send(direction, topic_name)
        end
        nil
    end
end

#get(name, options = Hash.new) ⇒ Object



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
# File 'lib/orocos/ros/name_service.rb', line 172

def get(name, options = Hash.new)
    options = Kernel.validate_options options, :retry => true, :process => nil
    _, name = split_name(name)
    name = "/#{name}".gsub(/\/\//,'/')
    has_node = access_ros_graph do
        ros_graph.has_node?(name)
    end

    if !has_node
        if options[:retry]
            # Wait for a single update of the graph and try
            # again
            wait_for_update
            get(name, :retry => false)
        else
            raise Orocos::NotFound, "no such ROS node #{name}"
        end
    end

    slave_uri =
        begin
            @ros_master_sync.synchronize do
               @ros_master.lookup_node(name)
            end
        rescue ArgumentError
            raise Orocos::NotFound, "no such ROS node #{name}"
        end
    server = ROSSlave.new(slave_uri, caller_id)
    return Node.new(self, server, name)
end

#namesObject



203
204
205
206
207
# File 'lib/orocos/ros/name_service.rb', line 203

def names
    wait_for_update do
        ros_graph.nodes.dup
    end
end

#poll_system_stateObject



140
141
142
143
144
145
146
147
148
149
150
151
152
# File 'lib/orocos/ros/name_service.rb', line 140

def poll_system_state
    thread_pool.process_with_options(
        Hash[:sync_key => @ros_master,
             :callback => method(:done_system_state)]) do

        @ros_master_sync.synchronize do
            state  = @ros_master.system_state
            topics = @ros_master.topics
            new_update_time = Time.now
            [new_update_time, NodeGraph.from_system_state(state, topics)]
        end
    end
end

#process_ros_master_exceptionObject

Processes the latest ROS master exception caught

It raises the exception and reinitializes the caught as well

It must be called with @mutex locked



250
251
252
253
254
255
# File 'lib/orocos/ros/name_service.rb', line 250

def process_ros_master_exception
    exception, @ros_master_exception = @ros_master_exception, nil
    if exception
        raise exception
    end
end

#retry_after_update_if_nilObject



209
210
211
212
213
214
215
216
# File 'lib/orocos/ros/name_service.rb', line 209

def retry_after_update_if_nil
    result = yield
    if !result
        wait_for_update
        yield
    else result
    end
end

#to_async(options = Hash.new) ⇒ Object



136
137
138
# File 'lib/orocos/ros/name_service.rb', line 136

def to_async(options = Hash.new)
    Async::ROS::NameService.new(uri, caller_id, options)
end

#update_system_state(update_time, graph, exception) ⇒ Object



154
155
156
157
158
159
160
161
162
163
# File 'lib/orocos/ros/name_service.rb', line 154

def update_system_state(update_time, graph, exception)
    @mutex.synchronize do
        if !exception
            @update_time = update_time
            @ros_graph = graph
        end
        @ros_master_exception ||= exception
        @updated_graph_signal.broadcast
    end
end

#validateObject

Validates that this name service can be used

Raises:

  • any error that has occured during ROS graph update



296
297
298
# File 'lib/orocos/ros/name_service.rb', line 296

def validate
    wait_for_update
end

#wait_for_update(barrier = Time.now) { ... } ⇒ Object

Wait for the ROS graph to be updated at least once

Parameters:

  • if (Time, nil)

    given, the new graph should be newer than this time. Otherwise, we simply wait for any update

Yields:

  • in a context where it is safe to access the ROS graph object. The block is optional

Returns:

  • the value returned by the given block, if a block was given

Raises:

  • any error that has occured during ROS graph update



266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
# File 'lib/orocos/ros/name_service.rb', line 266

def wait_for_update(barrier = Time.now)
    result = nil
    @mutex.synchronize do
        while update_time <= barrier
            process_ros_master_exception
            @updated_graph_signal.wait(@mutex)
        end

        if block_given?
            result = yield
        end
        process_ros_master_exception
    end
    result
end