Class: ROS::Master

Inherits:

Object

• Object
• ROS::Master

Includes:

Name

Defined in:

lib/ros/master.rb

Overview

ROS Master

See Also:

• http://ros.org/wiki/ROS/Master_API

Defined Under Namespace

Classes: Parameter, ParameterSubscriber, Publisher, ServiceServer, Subscriber

Constant Summary

Constants included from Name

Name::SEP

Instance Method Summary

• #initialize(master_uri = ) ⇒ Master constructor

  A new instance of Master.

• #kill_same_name_node(caller_id, api) ⇒ Object

  kill old node if the same caller_id node is exits.

• #start ⇒ Object

  start server and set default parameters.

Methods included from Name

#anonymous_name, #canonicalize_name, #expand_local_name, #resolve_name_with_call_id

Constructor Details

#initialize(master_uri = ) ⇒ Master

Returns a new instance of Master.

Parameters:

• master_uri (String) (defaults to: ) —

  uri of master

# File 'lib/ros/master.rb', line 139

def initialize(master_uri=ENV['ROS_MASTER_URI'])
  @master_uri = master_uri
  uri = URI.parse(@master_uri)
  @services = []
  @publishers = []
  @subscribers = []
  @parameters = []
  @param_subscribers = []
  @server = XMLRPCServer.new(uri.port, uri.host)

  @server.set_default_handler do |method, *args|
    puts "unhandled call with #{method}, #{args}"
    [0, "I DON'T KNOW", 0]
  end
  
  @server.add_handler('registerService') do |caller_id, service_name, service_api, caller_api|
    service_name = canonicalize_name(service_name)
    kill_same_name_node(caller_id, caller_api)
    @services.push(ServiceServer.new(caller_id, service_name, caller_api, service_api))
    [1, "registered", 0]
  end

  @server.add_handler('unregisterService') do |caller_id, service_name, service_api|
    service_name = canonicalize_name(service_name)
    before = @services.length
    @services.delete_if {|x| x.name == service_name and x.caller_id == caller_id}
    after = @services.length
    [1, "deleted", before - after]
  end

  @server.add_handler('registerSubscriber') do |caller_id, topic_name, type, api|
    topic_name = canonicalize_name(topic_name)
    kill_same_name_node(caller_id, api)
    @subscribers.push(Subscriber.new(caller_id, topic_name, type, api))
    pub_apis = @publishers.select {|x|x.name == topic_name and x.type == type}.map {|x| x.api }
    [1, "registered", pub_apis]
  end

  @server.add_handler('unregisterSubscriber') do |caller_id, topic_name, api|
    topic_name = canonicalize_name(topic_name)
    before = @subscribers.length
    @subscribers.delete_if {|x| x.name == topic_name and x.caller_id == caller_id}
    after = @subscribers.length
    [1, "deleted", before - after]
  end

  @server.add_handler('registerPublisher') do |caller_id, topic_name, type, api|
    topic_name = canonicalize_name(topic_name)
    kill_same_name_node(caller_id, api)
    @publishers.push(Publisher.new(caller_id, topic_name, type, api))
    sub_apis = @subscribers.select {|x|x.name == topic_name and x.type == type}.map {|x| x.api }
    pub_apis = @publishers.select {|x|x.name == topic_name and x.type == type}.map {|x| x.api }
    proxy = SlaveProxy.new('/master', api)
    begin
      proxy.publisher_update(topic_name, pub_apis)
    rescue
    end
    if not sub_apis
      sub_apis = []
    end
    [1, "registered", sub_apis]
  end

  @server.add_handler('unregisterPublisher') do |caller_id, topic_name, api|
    topic_name = canonicalize_name(topic_name)
    before = @publishers.length
    @publishers.delete_if {|x| x.name == topic_name and x.caller_id == caller_id}
    pub_apis = @publishers.select {|x|x.name == topic_name and x.caller_id == caller_id}.map {|x| x.api }
    proxy = SlaveProxy.new('/master', api)
    begin
      code, status, ignore = proxy.publisher_update(topic_name, pub_apis)
    rescue
      #
    end
    after = @publishers.length
    [1, "deleted", before - after]
  end

  @server.add_handler('getPid') do |caller_id|
    [1, "ok", $$]
  end

  @server.add_handler('lookupNode') do |caller_id, node_name|
    pub = @publishers.select {|x| x.caller_id == node_name}
    sub = @subscribers.select {|x| x.caller_id == node_name}
    service = @services.select {|x| x.caller_id == node_name}
    if not pub.empty?
      [1, "found", pub.first.api]
    elsif not sub.empty?
      [1, "found", sub.first.api]
    elsif not service.empty?
      [1, "found", service.first.api]
    else
      [0, "not found", 0]
    end
  end
  
  @server.add_handler('getPublishedTopics') do |caller_id, subgraph|
    if subgraph == ''
      [1, "ok", @publishers.map {|x| [x.name, x.type]}]
    else
      [1, "ok", @publishers.select {|x| not x.caller_id.scan(/^#{subgraph}/).empty?}
         .map {|x| [x.name, x.type]}]
    end
  end
  
  @server.add_handler('getSystemState') do |caller_id|
    def convert_info_to_list(info)
      list = []
      info.keys.each do |key|
        list.push([key, info[key]])
      end
      list
    end

    pub_info = {}
    @publishers.each do |pub|
      if pub_info[pub.name]
        pub_info[pub.name].push(pub.caller_id)
      else
        pub_info[pub.name]= [pub.caller_id]
      end
    end
    
    sub_info = {}
    @subscribers.each do |sub|
      if sub_info[sub.name]
        sub_info[sub.name].push(sub.caller_id)
      else
        sub_info[sub.name]= [sub.caller_id]
      end
    end
    
    ser_info = {}
    @services.each do |ser|
      if ser_info[ser.name]
        ser_info[ser.name].push(ser.caller_id)
      else
        ser_info[ser.name]= [ser.caller_id]
      end
    end

    [1, "ok", [convert_info_to_list(pub_info),
               convert_info_to_list(sub_info),
               convert_info_to_list(ser_info)]]
  end

  @server.add_handler('getUri') do |caller_id|
    [1, "ok", @master_uri]
  end

  @server.add_handler('lookupService') do |caller_id, service|
    ser = @services.select {|x| x.name == service}
    if ser.empty?
      [0, "fail", 0]
    else
      [1, "ok", ser.first.service_api]
    end
  end

  ## parameters
  @server.add_handler('deleteParam') do |caller_id, key|
    key = canonicalize_name(key)
    before = @parameters.length
    @parameters.delete_if {|x| x.key == key}
    after = @parameters.length

    if before == after
      [0, "[#{key}] is not exists", 0]
    else
      [1, "ok", 0]
    end
  end

  @server.add_handler('setParam') do |caller_id, key, value|
    key = canonicalize_name(key)
    exist_param = @parameters.select {|x| x.key == key}
    if exist_param.empty?
      @parameters.push(Parameter.new(key, value))
    else
      exist_param.first.value = value
    end
    @param_subscribers.each do |x|
      if x.key == key
        begin
          proxy = SlaveProxy.new('/master', x.api)
          proxy.param_update(key, value)
        rescue
        end
      end
    end
    [1, "ok", 0]
  end

  @server.add_handler('getParam') do |caller_id, key|
    key = canonicalize_name(key)
    param = @parameters.select {|x| x.key == key}
    if param.empty?
      [0, "no such param [#{key}]", 0]
    else
      [1, "ok", param.first.value]
    end
  end

  @server.add_handler('searchParam') do |caller_id, key|
    key = canonicalize_name(key)
    if key == ''
      param = @parameters
    else
      param = @parameters.select {|x| not x.key.scan(/#{key}/).empty?}
    end
    if param.empty?
      [-1, "no param", 0]
    else
      [1, "ok", param.first.key]
    end
  end
  
  @server.add_handler('subscribeParam') do |caller_id, caller_api, key|
    key = canonicalize_name(key)
    @param_subscribers.push(ParameterSubscriber.new(caller_id, caller_api, key))
    params = @parameters.select {|x| x.key == key}
    if params.empty?
      [1, "ok", []]
    else
      [1, "ok", params.map {|x| x.value}]
    end
  end

  @server.add_handler('unsubscribeParam') do |caller_id, caller_api, key|
    key = canonicalize_name(key)
    before = @param_subscribers.length
    @param_subscribers.delete_if {|x| x.api == caller_api and x.key == key}
    after = @param_subscribers.length
    [1, "ok", before - after]
  end

  @server.add_handler('hasParam') do |caller_id, key|
    key = canonicalize_name(key)
    params = @parameters.select {|x| x.key == key}
    if params.empty?
      [0, "no", false]
    else
      [1, "ok", params.first.value]
    end
  end
  
  @server.add_handler('getParamNames') do |caller_id|
    [1, "ok", @parameters.map{|x| x.key}]
  end

  # not documented api
  @server.add_handler('getTopicTypes') do |caller_id|
    [1, "ok", @publishers.map {|x| [x.name, x.type]} | @subscribers.map {|x| [x.name, x.type]}]
  end
  
end

Instance Method Details

#kill_same_name_node(caller_id, api) ⇒ Object

kill old node if the same caller_id node is exits.

Parameters:

• caller_id (String) —

  new node’s caller_id

• api (String) —

  new node’s XMLRPC URI

# File 'lib/ros/master.rb', line 115

def kill_same_name_node(caller_id, api)
  delete_api = nil
  [@publishers, @subscribers, @services].each do |list|
    list.each do |pub|
      if pub.caller_id == caller_id and pub.api != api
        delete_api = pub.api
        break
      end
    end
  end
  if delete_api
    proxy = SlaveProxy.new('/master', delete_api)
    begin
      proxy.shutdown("registered new node #{delete_api}")
    rescue
    end
    # delete
    [@publishers, @subscribers, @services].each do |list|
      list.delete_if {|x| x.api == delete_api}
    end
  end
end

#start ⇒ Object

start server and set default parameters

# File 'lib/ros/master.rb', line 398

def start
  puts "=== starting Ruby ROS master @#{@master_uri} ==="
  @parameters.push(Parameter.new('/rosversion', '1.8.6'))
  @parameters.push(Parameter.new('/rosdistro', 'fuerte'))
  
  @server.serve
  self
end