cvl-robot's diary

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

openFrameworksをROSのフロントエンドにする(その3)

出来上がったのがこちら。データさえ拾えれば、ROS関係なくて普通のopenFrameworksの開発作業になります。

main.cpp

#include "ofMain.h"
#include "ofApp.h"
#include <ros/ros.h>

//========================================================================
int main(int argc, char *argv[] ){
	ros::init(argc, argv, "subscriber"); // Initialization of name of the ROS node
	ofSetupOpenGL(1024,768, OF_WINDOW);			// <-------- setup the GL context

	// this kicks off the running of my app
	// can be OF_WINDOW or OF_FULLSCREEN
	// pass in width and height too:
	ofRunApp( new ofApp());
}

ofApp.h

#pragma once

#include "ofMain.h"
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <sstream>
#include <string>
#include "Odometry.h"

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);

		int argc;
		char **argv;

		ros::NodeHandle n;
		//ros::Publisher chatter_pub;
		ros::Subscriber chatter_sub;

		void my_callback(const nav_msgs::Odometry::ConstPtr& val);
		void drawFrustum(float clip_near, float clip_far);

		ofEasyCam cam;
		std::vector<nav_msgs::Odometry> trajectory;
};

ofApp.cpp

#include "ofApp.h"
#include <ofMath.h>

//--------------------------------------------------------------
void ofApp::setup(){
    chatter_sub = n.subscribe("realsense/odom", 1000, &ofApp::my_callback, this);

    // for draw
    cam.setNearClip(0.001f);
    cam.setDistance(10.0f);
}

void ofApp::my_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
#if _DEBUG
        ROS_INFO("header: seq[%d] stamp sec[%d] nsec[%d] frame_id[%s]", msg->header.seq,
                                  msg->header.stamp.sec,
                                  msg->header.stamp.nsec,
                                  msg->header.frame_id.c_str());
        ROS_INFO("child_frame_id: [%s]", msg->child_frame_id.c_str());
        ROS_INFO("Pose: position: [%f][%f][%f]", msg->pose.pose.position.x,
                                                 msg->pose.pose.position.y,
                                                 msg->pose.pose.position.z); 
        ROS_INFO("   orientation: [%f][%f][%f][%f]", msg->pose.pose.orientation.x,
                                                     msg->pose.pose.orientation.y,
                                                     msg->pose.pose.orientation.z,
                                                     msg->pose.pose.orientation.w);
        ROS_INFO("    covariance: [%f]", msg->pose.covariance);

        ROS_INFO("Twist:  linear: [%f][%f][%f]", msg->twist.twist.linear.x,
                                                 msg->twist.twist.linear.y,
                                                 msg->twist.twist.linear.z);
        ROS_INFO("       angular: [%f][%f][%f]", msg->twist.twist.angular.x,
                                                 msg->twist.twist.angular.y,
                                                 msg->twist.twist.angular.z);
        ROS_INFO("    covariance: [%f]", msg->twist.covariance);
#endif // _DEBUG
    trajectory.push_back(*msg);
}

//--------------------------------------------------------------
void ofApp::update(){
    ros::spinOnce();
}

void ofApp::drawFrustum(float clip_near, float clip_far)
{
	float fisheye_fov_horizontal = 133.f;
        float fisheye_fov_vertical = 100.f;
        float aw = ofDegToRad(0.5f * fisheye_fov_horizontal);
        float ah = ofDegToRad(0.5f * fisheye_fov_vertical);
	float px0 = clip_near * tan(aw);
	float px1 = clip_far * tan(aw);
	float py0 = clip_near * tan(ah);
	float py1 = clip_far * tan(ah);

	ofDrawLine(px0, py0, clip_near, px1, py1, clip_far);
	ofDrawLine(-px0, py0, clip_near,  -px1, py1, clip_far);
	ofDrawLine(-px0, -py0, clip_near, -px1, -py1, clip_far);
	ofDrawLine(px0, -py0, clip_near, px1, -py1, clip_far);

	ofDrawLine(px0, py0, clip_near, -px0, py0, clip_near);
	ofDrawLine(-px0, py0, clip_near, -px0, -py0, clip_near);
	ofDrawLine(-px0, -py0, clip_near, px0, -py0, clip_near);
	ofDrawLine(px0, -py0, clip_near, px0, py0, clip_near);

	ofDrawLine(px1, py1, clip_far, -px1, py1, clip_far);
	ofDrawLine(-px1, py1, clip_far, -px1, -py1, clip_far);
	ofDrawLine(-px1, -py1, clip_far, px1, -py1, clip_far);
	ofDrawLine(px1, -py1, clip_far, px1, py1, clip_far);
}

//--------------------------------------------------------------
void ofApp::draw(){

    ofBackground(20,20,20);
    ofSetColor(255);

    cam.begin();
    ofDrawAxis(1.f);

    if(trajectory.size() != 0){
        // render trajectory
        auto b = trajectory.back();
        float x = b.pose.pose.position.x;
        float y = b.pose.pose.position.y;
        float z = b.pose.pose.position.z;
        float qx = b.pose.pose.orientation.x;
        float qy = b.pose.pose.orientation.y;
        float qz = b.pose.pose.orientation.z;
        float qw = b.pose.pose.orientation.w;
        
        ofMatrix4x4 m44;
        m44.setRotate(ofQuaternion(qx, qy, qz, qw));
        m44.setTranslation(x, y, z);
	
        ofMatrix4x4 ma(0, 0, 1, 0,
                       0, 1, 0, 0,
                       1, 0, 0, 0,
		       0, 0, 0, 1);
    
        ofMatrix4x4 mb(1, 0, 0, 0,
	    	       0, 0, 1, 0,
                       0, 1, 0, 0,
		       0, 0, 0, 1);

        ofSetColor(ofColor::yellow);
	ofPushMatrix();
	ofMultMatrix(m44);
        ofMultMatrix(ma*mb);
        ofDrawAxis(1.f);
	drawFrustum(0.4, 2.8);
	ofPopMatrix();
    }
    cam.end();
}

//--------------------------------------------------------------
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){ 

}

実行結果はこちら。
f:id:cvl-robot:20180124175938p:plain
これで、一応、お蔵入りする予定だったIntel EuclidをただのSLAMデバイスとして使えるようにすることで有効活用できるようになりました。

ROSにはrvizって言う立派なGUIビューアがあるじゃない、なぜそちらを使わないの?という問いの答えとしては、僕あれ直感的にうまく使えなくて大嫌いなんです。