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

Intel Euclidという、デプスセンサーRealSenseにUbuntu PCを搭載した小型で優秀なデバイスが机の上に転がっています。発売後すぐに購入してしばらく放おっておいて、そろそろ使おうかと思った矢先に、発売から半年そこらで、intelから開発中止のアナウンスが出されてしまい、本腰を入れて使うわけには行かなくなった不憫なやつです。

Intel Euclidの6DOFはSLAMの出力

Euclidは最初からROSを搭載していて、いくつかのアプリケーションがシナリオという名前で用意されており、ボタンひとつで起動できます。この中に6DOFというSLAMアプリがあり、WEBブラウザで
http://10.42.0.1にアクセスして、Scenarioから6DOFを起動して、monitorからTrajectoryを開くと軌道が可視化されます。

f:id:cvl-robot:20180124122123p:plain — Intel Euclidの6DOF

f:id:cvl-robot:20180124122118p:plain — Intel Euclidの6DOF

今回の記事は、最終的にこのROSの受信機能を含むモニターの部分をopenFrameworksで自作しようというものです。

openFrameworksでROS

まず[1]の記事を参考にします。元になるプロジェクトファイルは、前回紹介したvscode_oFを利用すると便利です。
Makefileに絶対パスを指定するかどうかは趣味に合わせて良いと思います。OF_ROOTの指定は、config.makeの中でも書けます。

config.makeのROS対応

必要なのは、まずconfig.makeの編集です。記事に倣って、

################################################################################
# CONFIGURE PROJECT MAKEFILE (optional)
# This file is where we make project specific configurations.
################################################################################
################################################################################
# OF ROOT
# The location of your root openFrameworks installation
# (default) OF_ROOT = ../../..
################################################################################
# OF_ROOT = ../../..
################################################################################
# PROJECT ROOT
# The location of the project - a starting place for searching for files
# (default) PROJECT_ROOT = . (this directory)
#
################################################################################
# PROJECT_ROOT = .
################################################################################
# PROJECT SPECIFIC CHECKS
# This is a project defined section to create internal makefile flags to
# conditionally enable or disable the addition of various features within
# this makefile. For instance, if you want to make changes based on whether
# GTK is installed, one might test that here and create a variable to check.
################################################################################
# None
################################################################################
# PROJECT EXTERNAL SOURCE PATHS
# These are fully qualified paths that are not within the PROJECT_ROOT folder.
# Like source folders in the PROJECT_ROOT, these paths are subject to
# exlclusion via the PROJECT_EXLCUSIONS list.
#
# (default) PROJECT_EXTERNAL_SOURCE_PATHS = (blank)
#
# Note: Leave a leading space when adding list items with the += operator
################################################################################
# PROJECT_EXTERNAL_SOURCE_PATHS = -I/opt/ros/kinetic/include/
################################################################################
# PROJECT EXCLUSIONS
# These makefiles assume that all folders in your current project directory
# and any listed in the PROJECT_EXTERNAL_SOURCH_PATHS are are valid locations
# to look for source code. The any folders or files that match any of the
# items in the PROJECT_EXCLUSIONS list below will be ignored.
#
# Each item in the PROJECT_EXCLUSIONS list will be treated as a complete
# string unless teh user adds a wildcard (%) operator to match subdirectories.
# GNU make only allows one wildcard for matching. The second wildcard (%) is
# treated literally.
#
# (default) PROJECT_EXCLUSIONS = (blank)
#
# Will automatically exclude the following:
#
# $(PROJECT_ROOT)/bin%
# $(PROJECT_ROOT)/obj%
# $(PROJECT_ROOT)/%.xcodeproj
#
# Note: Leave a leading space when adding list items with the += operator
################################################################################
# PROJECT_EXCLUSIONS =
################################################################################
# PROJECT LINKER FLAGS
# These flags will be sent to the linker when compiling the executable.
#
# (default) PROJECT_LDFLAGS = -Wl,-rpath=./libs
#
# Note: Leave a leading space when adding list items with the += operator
################################################################################
# Currently, shared libraries that are needed are copied to the
# $(PROJECT_ROOT)/bin/libs directory. The following LDFLAGS tell the linker to
# add a runtime path to search for those shared libraries, since they aren't
# incorporated directly into the final executable application binary.
# TODO: should this be a default setting?
PROJECT_LDFLAGS=-Wl,-rpath=./libs
PROJECT_LDFLAGS+=$(SUBLIBS) $(ros_libs_nocolon)
ros_libs = $(shell pkg-config --libs roscpp nav_msgs tf)
ros_libs_nocolon = $(subst -l:,,$(ros_libs))
################################################################################
# PROJECT DEFINES
# Create a space-delimited list of DEFINES. The list will be converted into
# CFLAGS with the "-D" flag later in the makefile.
#
# (default) PROJECT_DEFINES = (blank)
#
# Note: Leave a leading space when adding list items with the += operator
################################################################################
# PROJECT_DEFINES =
################################################################################
# PROJECT CFLAGS
# This is a list of fully qualified CFLAGS required when compiling for this
# project. These CFLAGS will be used IN ADDITION TO the PLATFORM_CFLAGS
# defined in your platform specific core configuration files. These flags are
# presented to the compiler BEFORE the PROJECT_OPTIMIZATION_CFLAGS below.
#
# (default) PROJECT_CFLAGS = (blank)
#
# Note: Before adding PROJECT_CFLAGS, note that the PLATFORM_CFLAGS defined in
# your platform specific configuration file will be applied by default and
# further flags here may not be needed.
#
# Note: Leave a leading space when adding list items with the += operator
################################################################################
# PROJECT_CFLAGS =
################################################################################
# PROJECT OPTIMIZATION CFLAGS
# These are lists of CFLAGS that are target-specific. While any flags could
# be conditionally added, they are usually limited to optimization flags.
# These flags are added BEFORE the PROJECT_CFLAGS.
#
# PROJECT_OPTIMIZATION_CFLAGS_RELEASE flags are only applied to RELEASE targets.
#
# (default) PROJECT_OPTIMIZATION_CFLAGS_RELEASE = (blank)
#
# PROJECT_OPTIMIZATION_CFLAGS_DEBUG flags are only applied to DEBUG targets.
#
# (default) PROJECT_OPTIMIZATION_CFLAGS_DEBUG = (blank)
#
# Note: Before adding PROJECT_OPTIMIZATION_CFLAGS, please note that the
# PLATFORM_OPTIMIZATION_CFLAGS defined in your platform specific configuration
# file will be applied by default and further optimization flags here may not
# be needed.
#
# Note: Leave a leading space when adding list items with the += operator
################################################################################
PROJECT_OPTIMIZATION_CFLAGS_RELEASE = `pkg-config --cflags roscpp nav_msgs tf` -w -O2
PROJECT_OPTIMIZATION_CFLAGS_DEBUG = `pkg-config --cflags roscpp nav_msgs tf` -w -O2
################################################################################
# PROJECT COMPILERS
# Custom compilers can be set for CC and CXX
# (default) PROJECT_CXX = (blank)
# (default) PROJECT_CC = (blank)
# Note: Leave a leading space when adding list items with the += operator
################################################################################
# PROJECT_CXX =
# PROJECT_CC =

ROSトピックのサブスクライバーを練習として書いてみる

[1]の記事では、ROSのmsgのPublisherを紹介してくれていますので、これを受けるSubscriberを、[2]を参照しつつ書いてみたいと思います。

main.cpp

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

//========================================================================
int main(int argc, char *argv[] ){
	ros::init(argc, argv, "ros_tutorial_msg_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>

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

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

		void chatterCallback(const std_msgs::String::ConstPtr& msg);
};

ofApp.cpp

#include "ofApp.h"

void ofApp::chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard: [%s]", msgmak->data.c_str());
}

//--------------------------------------------------------------
void ofApp::setup(){
    // chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    chatter_sub = n.subscribe("chatter", 1000, &ofApp::chatterCallback, this);
}

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

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

    ofBackground(20,200,100);
    ofSetColor(255);
    ofDrawRectangle(100,100,100,100);
}

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

}

別のターミナルを開いて、publisherを実行しておきます。
もうひとつのターミナルで、このsubscriberを実行すると、こんな感じになります。標準出力で、受け取ったメッセージが表示され、GUIではopenFrameworksが実行されていることを確認するための緑と白の枠が表示されます。
f:id:cvl-robot:20180124125544p:plain
openFrameworksのclassのメンバー関数をcallback関数に指定できるのが良いですね[3]。ROSは流石に良く出来ています。
ROSのcallbackにofEventを指定することはできませんが、ofEventで同じ引数を扱うことはできますので、使いようによっては同じcallback関数をROSとoFのループで相乗りするなどできそうです。（やらないほうがいいですが）