class CrazyflieZMQ
Allow control of a {www.bitcraze.io/crazyflie-2/ Crazyflie} drone using the ZMQ protocol.
To use this you need to have a ZMQ server running, it is started using the crazyflie python clients API ({github.com/bitcraze/crazyflie-clients-python/ crazyflie-clients-python}):
crazyflie-clients-python/bin/cfzmq --url tcp://* -d
Small demonstration:
$cf = CrazyflieZMQ.new('tcp://_ip_address_') # Connect to ZMQ sockets $cf.connect('radio://0/80/1M/E7E7E7E7E7') # Connect to drone $cf.log_create(:range, # Create log block 'ranging.distance0', 'ranging.distance1', 'ranging.distance2') $cf['posCtlPid','xKp']= 1.0 # Parameter (group, name notation) $cf['posCtlPid.yKp' ]= 1.0 # Paramerer (name dotted notation) $cf.log_start(:range, file: :csv) # Log to csv generated file sleep(120) # Wait 2 minutes $cf.log_stop(:range) # Stop logging of :range $cf.disconnect # Disconnect from drone
@see wiki.bitcraze.io/doc:crazyflie:client:cfzmq:index @see wiki.bitcraze.io/doc:crazyflie:dev:arch:logparam @see wiki.bitcraze.io/projects:crazyflie:firmware:comm_protocol @see wiki.bitcraze.io/doc:crazyflie:crtp:log
Constants
- VERSION
Public Class Methods
Create a {CrazyflieZMQ} instance
@param url [String] the url to connect to the ZMQ socket
# File lib/crazyflie-zmq.rb, line 57 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(_json_fix(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(_json_fix(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
Public Instance Methods
Get a parameter value from the crazyflie
If a parameter group is not specified it is possible to use a .
in the parameter name to indicate it: +“group.name”+
@param group [String,nil] group on which the parameter belongs @param name [String] parameter name
# File lib/crazyflie-zmq.rb, line 295 def [](group=nil, name) group, name = name.split('.', 2) if group.nil? @param.dig(group, name, 'value') end
Assign a parameter value to the crazyflie
If a parameter group is not specified it is possible to use a .
in the parameter name to indicate it: +“group.name”+
@param group [String,nil] group on which the parameter belongs @param name [String] parameter name @param value
# File lib/crazyflie-zmq.rb, line 281 def []=(group=nil, name, value) name = [ group, name ].join('.') if group _request(cmd: :param, name: name, value: value) value end
Establish a connection with a crazyflie
@param uri [String] crazyflie URI @param log_blocks [Hash{Symbol=>Hash}] predefined log blocks @return [self]
# File lib/crazyflie-zmq.rb, line 132 def connect(uri, log_blocks: nil) toc = _request(cmd: :connect, uri: uri) @param = toc['param'] || {} @log = toc['log' ] || {} @connected = Time.now.freeze @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 if !variables.nil? && !variables.empty? self.log_create(key, *variables, period: period || 100) end } self end
Disconnect from the crazyflie
@return [self]
# File lib/crazyflie-zmq.rb, line 158 def disconnect() @log_blocks&.each_key {|key| self.log_delete(key) } _request(cmd: :disconnect) @connected = @param = @log = @log_blocks = nil self end
Ensure we are in a connect state
@raise [NotConnected] if not connected to a crazyflie @return [self]
# File lib/crazyflie-zmq.rb, line 176 def is_connected! raise NotConnected unless is_connected? self end
Are we connected to the crazyflie
@return [Boolean] connection status
# File lib/crazyflie-zmq.rb, line 168 def is_connected? !@connected.nil? end
Create a log block
@note logging is usually done through the crazyflie radio link
so you are limited in the number of variables that you can log at the same time as well as the minimal logging period that you can use.
@param name [Symbole,String] log block name @param variables [Array<String>] name of the variable to logs @param period [Integer] milliseconds between consecutive logs @return [self]
# File lib/crazyflie-zmq.rb, line 193 def log_create(name, *variables, period: 1000) _request(cmd: :log, action: :create, name: name, variables: variables, period: period) self end
Delete a registerd log block @param name [Symbol,String] name of the log block @return [self]
# File lib/crazyflie-zmq.rb, line 266 def log_delete(name) _request(cmd: :log, action: :delete, name: name) self end
Start logging information
It is possible to automatically create a log file using the `file` parameter, in this case you can specify the file name to use for logging (must end in .csv as for now only CSV format is supported), or you can use :csv and a filename will be generated using timestamp and counter
@param name [Symbol, String] name of the log block to start @param file [String, :csv, nil] name or type of file where to automatically log data @yield log data @return [self]
# File lib/crazyflie-zmq.rb, line 211 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
Stop logging of the specified log block @param name [Symbol,String] name of the log block @return [self]
# File lib/crazyflie-zmq.rb, line 256 def log_stop(name) _request(cmd: :log, action: :stop, name: name) @log_data_cb.delete(name) @log_file.delete(name)&.close self end
Returns the list of available crazyflie @return [Array<Hash{String=>String}>] list of available interfaces
# File lib/crazyflie-zmq.rb, line 122 def scan() _request(cmd: :scan).dig('interfaces') end
Private Instance Methods
# File lib/crazyflie-zmq.rb, line 303 def _json_fix(str) str.gsub(/(?<=:) (?:[\+\-]?Infinity|NaN) (?=,|\})/x, 'null') end
# File lib/crazyflie-zmq.rb, line 315 def _request(data) data = data.merge(version: 1) resp = '' @client_sock.send_string(data.to_json) @client_sock.recv_string(resp) resp = JSON.parse(_json_fix(resp)) version = resp.delete('version') status = resp.delete('status') unless status.nil? || status.zero? raise RequestError, "#{status}: #{resp['msg']}" end resp end
# File lib/crazyflie-zmq.rb, line 309 def _zmq_ok!(rc, msg = nil) if !ZMQ::Util.resultcode_ok?(rc) raise ZMQError, msg end end