cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

安価なIMU(AHRS)センサのBosch BNO055USBStickを試す(その3)

前回、BNO055USBStickのデータを自前のプログラムで取得することはできるようになりました。でも、数値だけを見ていても正しいかどうか判断できませんので、いつものようにopenFrameworksで可視化していきたいと思います。

ofxBNO055USBStick

外部addonとしてofxGui.hを使っていますので、ProjectGeneratorで追加してください。

ofApp.h

#pragma once

#include "bno055_usb_stick/bno055_usb_stick.hpp"
#include "bno055_usb_stick/decoder.hpp"
#include "bno055_usb_stick_msgs/Output.h"

#include <boost/asio/io_service.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

#include "ofMain.h"
#include "ofThread.h"
#include "ofEasyCam.h"
#include "ofxGui.h" 

class ofxBNO055USBStick : public ofThread
{
public:
	void setup() {
		const std::string fixed_frame_id("fixed");
		device = std::make_shared<bno055_usb_stick::BNO055USBStick>(asio_service, boost::bind(&ofxBNO055USBStick::publish, this, _1, fixed_frame_id));
	}

	void threadedFunction() {
		while (1) {
			asio_service.run_one();
		}
	}

	void publish(const bno055_usb_stick_msgs::Output &output, const std::string &fixed_frame_id) {
		mutex.lock();
		current = output;
		mutex.unlock();
	}

	bno055_usb_stick_msgs::Output & getOutput() { return current;  }

protected:
	boost::asio::io_service asio_service;
	std::shared_ptr<bno055_usb_stick::BNO055USBStick> device;

	ofMutex mutex;
	bno055_usb_stick_msgs::Output current;
};

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);

		ofxBNO055USBStick bno055;
		bno055_usb_stick_msgs::Output output;

		ofEasyCam cam;
		ofLight light;
		ofBoxPrimitive box;
		ofCylinderPrimitive cylinder;

		ofxPanel gui;
		ofParameter<ofVec3f> acceleration;
		ofParameter<ofVec3f> magnetometer;
		ofParameter<ofVec3f> gyroscope;
		ofParameter<ofVec3f> euler_angles;
		ofParameter<ofQuaternion> quaternion;
		ofParameter<ofVec3f> linear_acceleration;
		ofParameter<ofVec3f> gravity_vector;
		ofParameter<double> temperature;
		ofParameter<ofVec4f> calibration_status;
};

ofApp.cpp

#include "ofApp.h"

//--------------------------------------------------------------
void ofApp::setup(){
	// sensor settings
	bno055.setup();
	bno055.startThread();

	// draw settings
	box.set(0.5f, 0.8f, 0.2f);
	cam.setTarget(box);
	cam.setNearClip(0.001f);
	cam.setFarClip(1000.f);
	cam.setDistance(1.f);
	light.setParent(cam);
	cylinder.set(0.1f, 0.7f);

	// gui settings
	gui.setup("bno055");
	gui.add(acceleration.set("acceleration", ofVec3f(), ofVec3f(-100.f), ofVec3f(100.f)));
	gui.add(magnetometer.set("magnetometer", ofVec3f(), ofVec3f(-1000.f), ofVec3f(1000.f)));
	gui.add(gyroscope.set("gyroscope", ofVec3f(), ofVec3f(-100.f), ofVec3f(100.f)));
	gui.add(euler_angles.set("euler_angles", ofVec3f(), ofVec3f(-10.f), ofVec3f(10.f)));
	gui.add(temperature.set("temperature", 0., -100., 100.));
	gui.add(calibration_status.set("calibration_status", ofVec4f(), ofVec4f(0), ofVec4f(3)));
}

//--------------------------------------------------------------
void ofApp::update(){
	output = bno055.getOutput();

	acceleration = output.acceleration;
	magnetometer = output.magnetometer;
	gyroscope = output.gyroscope;
	euler_angles = output.euler_angles;
	quaternion = output.quaternion;
	linear_acceleration = output.linear_acceleration;
	gravity_vector = output.gravity_vector;
	temperature = output.temperature;
	calibration_status = output.calibration_status;
}

//--------------------------------------------------------------
void ofApp::draw(){
	ofBackgroundGradient(ofColor::black, ofColor::grey);
	ofEnableLighting();
	ofEnableDepthTest();
	light.enable();

	cam.begin();
	ofPushMatrix();
	ofMatrix4x4 mat;
	mat.setRotate(output.quaternion);
	ofMultMatrix(mat);
	box.drawAxes(0.5f);
	ofPushStyle();
	ofSetColor(ofColor::white, 200);
	box.draw();
	ofPopStyle();
	ofPopMatrix();
	cam.end();

	ofDisableLighting();
	ofDisableDepthTest();

	gui.draw();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){

}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}

実行結果

サクサク、かなり高速に安定に動きます。60Hz程度の更新レートならば、4千円のBNO055USBStickで十分ですね。キャリブレーション状態が頻繁に変わるので、正しくキャリブレーションが動いているのかどうか分かりませんが、姿勢のずれは特に感じられません。
f:id:cvl-robot:20180806191733p:plain

Addon化

openFrameworksのAddonにしました。
github.com

今日の京都まんが

2人の主人公のそれぞれの視点を描いたチヒロのこととユキチのことと、の2シリーズが刊行されているかなり実験的な漫画。アクションコミックスとヤングチャンピオンと掲載誌も違ったため、コミックスの本屋の棚も違うので探そうとすると難易度高いことがある。

古都こと―チヒロのこと―(2) (アクションコミックス)

古都こと―チヒロのこと―(2) (アクションコミックス)

古都こと―チヒロのこと―(3) (アクションコミックス)

古都こと―チヒロのこと―(3) (アクションコミックス)

古都ことーユキチのことー 3 (ヤングチャンピオンコミックス)

古都ことーユキチのことー 3 (ヤングチャンピオンコミックス)