Class: CrazyflieZMQ

Inherits:
Object
  • Object
show all
Defined in:
lib/crazyflie-zmq.rb

Defined Under Namespace

Classes: Error, NotConnected, RequestError, ZMQError

Constant Summary collapse

VERSION = 
"0.1.0"

Instance Attribute Summary collapse

Instance Method Summary collapse

Constructor Details

#initialize(url, log: nil) ⇒ CrazyflieZMQ

Returns a new instance of CrazyflieZMQ.



17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
 
# File 'lib/crazyflie-zmq.rb', line 17

def initialize(url, log: nil)
    @url          = url
    @log_cb       = log
    @log_data_cb  = {}
    @log_file     = {}
    @log_count    = {}
    @log_blocks   = nil
    
    @param  = {}
    @log    = {}
    @connected = nil
    
    @ctx    = ZMQ::Context.create(1)

    @client_sock = @ctx.socket(ZMQ::REQ)
    _zmq_ok!(@client_sock.setsockopt(ZMQ::LINGER, 0),  "client setsockopt")
    _zmq_ok!(@client_sock.connect("#{@url}:2000"),     "client connect"   )

    
    @param_sock = @ctx.socket(ZMQ::SUB)
    _zmq_ok!(@param_sock.setsockopt(ZMQ::LINGER, 0),   "param setsockopt" )
    _zmq_ok!(@param_sock.setsockopt(ZMQ::SUBSCRIBE,''),"param setsockopt" )
    _zmq_ok!(@param_sock.connect("#{@url}:2002"),      "param connect"    )

    @param_thr = Thread.new {
        loop {
            data = ''
            @param_sock.recv_string(data)
            resp = JSON.parse(data)
            version     = resp.delete('version'  )
            name        = resp.delete('name'     )
            value       = resp.delete('value'    )
            group, name = name.split('.', 2)
            @param.dig(group, name)&.merge('value' => value.to_s)
        }
    }
    @param_thr.abort_on_exception = true

    
    @log_sock = @ctx.socket(ZMQ::SUB)
    _zmq_ok!(@log_sock.setsockopt(ZMQ::LINGER, 0),     "log setsockopt"   )
    _zmq_ok!(@log_sock.setsockopt(ZMQ::SUBSCRIBE, ''), "log setsockopt"   )
    _zmq_ok!(@log_sock.connect("#{@url}:2001"),        "log connect"      )

    @log_thr = Thread.new {
        loop {
            data = ''
            @log_sock.recv_string(data)
            resp = JSON.parse(data)
            version   = resp.delete('version'  )
            event     = resp.delete('event'    ).to_sym
            name      = resp.delete('name'     ).to_sym
            timestamp = resp.delete('timestamp')
            resp = Hash[resp.map {|key, value| [ key.to_sym, value ] }]
            @log_cb&.(event, name, timestamp, resp)
            @log_data_cb[name]&.each {|cb|
                cb.(timestamp, resp) } if event == :data
        }
    }
    @log_thr.abort_on_exception = true
end

Instance Attribute Details

#connected ⇒ Object (readonly)

Returns the value of attribute connected.



15
16
17
 
# File 'lib/crazyflie-zmq.rb', line 15

def connected
  @connected
end

Instance Method Details

#[](group = nil, name) ⇒ Object 



192
193
194
195
 
# File 'lib/crazyflie-zmq.rb', line 192

def [](group=nil, name)
    group, name = name.split('.', 2) if group.nil?
    @param.dig(group, name, 'value')
end

#[]=(group = nil, name, value) ⇒ Object 



186
187
188
189
190
 
# File 'lib/crazyflie-zmq.rb', line 186

def []=(group=nil, name, value)
    name = [ group, name ].join('.') if group
    _request(cmd: :param, name: name, value: value)
    value
end

#connect(uri, log_blocks: nil) ⇒ Object 



87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
 
# File 'lib/crazyflie-zmq.rb', line 87

def connect(uri, log_blocks: nil)
    toc = _request(cmd: :connect, uri: uri)
    @param     = toc['param'] || {}
    @log       = toc['log'  ] || {}
    @connected = Time.now.freeze

    if @log_blocks = log_blocks
        @log_blocks.each {|key, data|
            variables, period =
                case data
                when Hash   then [ data[:variables], data[:period] ]
                when Array  then [ data ]
                when String then [ [ data ] ]
                end

            next if variables.nil? || variables.empty?
            self.log_create(key, *variables, period: period || 100)
        }
    end
    
    self
end

#disconnect ⇒ Object 



110
111
112
113
114
115
 
# File 'lib/crazyflie-zmq.rb', line 110

def disconnect()
    @log_blocks&.each_key {|key| self.log_delete(key) }
    _request(cmd: :disconnect)
    @connected = @param = @log = @log_blocks = nil
    self
end

#is_connected! ⇒ Object

Raises:



121
122
123
 
# File 'lib/crazyflie-zmq.rb', line 121

def is_connected!
    raise NotConnected unless is_connected?
end

#is_connected? ⇒ Boolean

Returns:

  • (Boolean) 


117
118
119
 
# File 'lib/crazyflie-zmq.rb', line 117

def is_connected?
    !@connected.nil?
end

#log_create(name, *variables, period: 1000) ⇒ Object 



127
128
129
130
131
 
# File 'lib/crazyflie-zmq.rb', line 127

def log_create(name, *variables, period: 1000)
    _request(cmd: :log, action: :create, name: name,
             variables: variables, period: period)
    self
end

#log_delete(name) ⇒ Object 



181
182
183
184
 
# File 'lib/crazyflie-zmq.rb', line 181

def log_delete(name)
    _request(cmd: :log, action: :delete, name: name)
    self
end

#log_start(name, file: nil, &block) ⇒ Object 



133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
 
# File 'lib/crazyflie-zmq.rb', line 133

def log_start(name, file: nil, &block)
    count = (@log_count[name] || 0) + 1

    if block
        (@log_data_cb[name] ||= []) << block
    end

    if file
        case file
        when String
            if ! file.end_with?('.csv')
                raise ArgumentError,
                      "only file with csv extension/format is supported"
            end
        when :csv
            prefix = [ @connected.strftime("%Y%m%dT%H%M"),
                       count
                     ].join('-')
            file   = "#{prefix}-#{name}.csv"
        else
            raise ArgumentError, "unsupported file specification"
        end
        
        variables = case data = @log_blocks[name]
                    when Array then data
                    when Hash  then data[:variables]
                    end
        io = @log_file[name] =
            CSV.open(file, 'wb',
                     :write_headers => true,
                     :headers       => [ 'timestamp' ] + variables)
        (@log_data_cb[name] ||= []) << ->(timestamp, variables:) {
            io << variables.merge('timestamp' => timestamp)
        }
    end

    _request(cmd: :log, action: :start,  name: name)
    @log_count[name] = count
    self
end

#log_stop(name) ⇒ Object 



174
175
176
177
178
179
 
# File 'lib/crazyflie-zmq.rb', line 174

def log_stop(name)
    _request(cmd: :log, action: :stop,   name: name)
    @log_data_cb.delete(name)
    @log_file.delete(name)&.close
    self
end

#scan ⇒ Object 



81
82
83
 
# File 'lib/crazyflie-zmq.rb', line 81

def scan()
    _request(cmd: :scan)
end