粗大メモ置き場

個人用,たまーに来訪者を意識する雑記メモ

ROS deliver argument to callback function in subscribers C++

Just a memo.

Code Example

// in callback
void chatterCallback(const sensor_msgs::Imu::ConstPtr& msg, boost::numeric::ublas::vector<double>& pose)
{
  ROS_INFO("Imu Seq: [%d]", msg->header.seq);
  ROS_INFO("Imu Orientation x: [%f], y: [%f], z: [%f], w: [%f]", msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w); 
  pose(0) += 0.1;
  ROS_INFO("Pose test: [%f]", pose(0));
  
}
// in main()
  // init pose with 0  
  using namespace boost::numeric::ublas;

  vector<double> pose(3);
  ros::Subscriber sub = n.subscribe<sensor_msgs::Imu>("imu/data", 1000, boost::bind(chatterCallback, _1, pose));

Read yaml file with yaml-cpp in ROS C++ / yamlファイルをC++で開く

This article both contains both Japanese and English explanations.
需要とか考えた結果、英語と日本語で書きます。

Goal: Read .yaml calibration files and then publish camera info

以下のようなyamlファイルをC++で読んでPublishするのが目的です。

image_width: 640
image_height: 480
camera_name: narrow_stereo/left
camera_matrix:
  rows: 3
  cols: 3
  data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
projection_matrix:
  rows: 3
  cols: 4
  data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]


And My environment is ROS kinetic and Ubuntu16.04 on amd64 cpu.
環境は上に示したとおりです。

Method: Using yaml-cpp

Installation

Download and make from source. Follow their installation guide.
GitHub - jbeder/yaml-cpp: A YAML parser and emitter in C++

ROS setup (Kinetic)

Here shows how to add dependency on rosmake.
yaml_cpp - ROS Wiki

But I use catkin_make, so I have to change the "package.xml" and "CmakeList" instead.

In your CmakeList in project, you should add

target_link_libraries(${PROJECT_NAME} yaml-cpp)

If you have already added linker, you can write as below:

target_link_libraries(${PROJECT_NAME} <your_already_added_link> yaml-cpp)

Then open your package.xml and add dependency as below:

...
  <build_depend>common_rosdeps</build_depend>
...
  <run_depend>common_rosdeps</run_depend>
...


Now you can edit your project.

Usage

I recommend you to understand what json/yaml format represents and how you read the data in python for introduction.
まずはPythonなどでファイルを開く方法を調べたほうが理解がスムーズに行くと思います。

Sample code

You need to include

#include <yaml-cpp/yaml.h>

and then you can read files like below:

// Load file
YAML::Node lconf = YAML::LoadFile(filename.yaml);

// how to read data
int iw = lconf["image_width"].as<int>();
std::string model = lconf["distortion_model"].as<std::string>();
std::vector<double> distdata = lconf["distortion_coefficients"]["data"].as<std::vector<double>>();

If the type of value is different from you declared, you will see the error about that.

Application: read yaml file and publish camera_info

Here show the sequence I publish these parameter in ros.

    // fill the camera data before running
    sensor_msgs::CameraInfoPtr lcam(new sensor_msgs::CameraInfo());
    sensor_msgs::CameraInfoPtr rcam(new sensor_msgs::CameraInfo());

    // read calib file
    std::string fnamel,fnamer;
    
    if( argc != 3 ){ // argc should be 2 for correct execution
        fnamel = "/home/ubuntu/calibres/left1.yml";
        fnamer = "/home/ubuntu/calibres/right1.yml";
        std::cout << "Load defaults! " <<  std::endl;
    }else {
        fnamel=argv[1];
        fnamer=argv[2];
        std::cout << "Now load..." << " " << fnamel << " and " << fnamer << std::endl;
    }
    
    YAML::Node lconf = YAML::LoadFile(fnamel);
    YAML::Node rconf = YAML::LoadFile(fnamer);
    
    // start read left camera configure! 
    lcam->width = lconf["image_width"].as<int>();
    lcam->height = lconf["image_height"].as<int>();
    lcam->distortion_model = lconf["distortion_model"].as<std::string>();    
    // get value
    std::vector<double> dd,kk,rr,pp,buf;
    dd = lconf["distortion_coefficients"]["data"].as<std::vector<double>>();
    kk = lconf["camera_matrix"]["data"].as<std::vector<double>>();
    rr = lconf["rectification_matrix"]["data"].as<std::vector<double>>();
    pp = lconf["projection_matrix"]["data"].as<std::vector<double>>();
    // conversion
    boost::array<double, 9ul> kl;
    std::memcpy(&kl[0], &kk[0], sizeof(double)*9);
    boost::array<double, 9ul> rl;
    std::memcpy(&rl[0], &rr[0], sizeof(double)*9);
    boost::array<double, 12ul> pl;
    std::memcpy(&pl[0], &pp[0], sizeof(double)*12);
    // put value
    lcam->D =  dd; 
    lcam->K =  kl; 
    lcam->R =  rl; 
    lcam->P =  pl;

How to split string using multiple delimeters / and convert to double array in C++

GOAL: get double array from "[1.1, 3.3]"

Unfortunately std::string do not have split function.

I've heard boost/ library has split function but I do not want to use that.

Split string into string vector

Here is sample code.
The original code is from below:
c++ - Split a string into words by multiple delimiters - Stack Overflow

#include <iostream>
#include <string>
#include <vector>

int main(){
std::string str = "[0.0, 2.1, 22.3]";
   std::vector<std::string> v ; //Use vector to add the words

    std::size_t prev_pos = 0, pos;
    while ((pos = str.find_first_of("[] ,", prev_pos)) != std::string::npos)
    {
        if (pos > prev_pos)
            v.push_back(str.substr(prev_pos, pos-prev_pos));
        prev_pos= pos+1;
    }
    if (prev_pos< str.length())
        v.push_back(str.substr(prev_pos, std::string::npos));


std::cout << v << std::endl;
std::cout << v[0] << std::endl;
std::cout << v[1] << std::endl;

}

Out put become:

[0.0, 2.1, 22.3]
0.0
2.1

Give it try

You can try this code in CodePad.

C++ code - 21 lines - codepad

Convert string vector to vector of double

Just using while loop. It will be safer.
Code is like below:

#include <iostream>
#include <string>
#include <vector>

int main(){
std::string str = "[0.0, 2.1, 22.3]";

// copied from stackoverflow
std::vector<std::string> v ; //Use vector to add the words

    std::size_t prev_pos = 0, pos;
    while ((pos = str.find_first_of("[] ,", prev_pos)) != std::string::npos)
    {
        if (pos > prev_pos)
            v.push_back(str.substr(prev_pos, pos-prev_pos));
        prev_pos= pos+1;
    }
    if (prev_pos< str.length())
        v.push_back(str.substr(prev_pos, std::string::npos));


std::cout << v[1] << std::endl;
std::cout << v.size() << std::endl;

double* num;
num = new double[v.size()];

for(unsigned int i=0;i<v.size();i++){
    num[i] = std::stod(v[i]);
} 

std::cout << num[1] << std::endl;

}

try?

Codepad could not compile this due to compiler dependency or some error.
There is another page to test this code.
Online C++ Compiler - online editor

Written as a Function

#include <iostream>
#include <string>
#include <vector>

void string2doublearray(std::string str, std::string delim, double *&num){
    
    std::vector<std::string> v ; //Use vector to add the words

    std::size_t prev_pos = 0, pos;
    while ((pos = str.find_first_of("[] ,", prev_pos)) != std::string::npos)
    {
        if (pos > prev_pos)
            v.push_back(str.substr(prev_pos, pos-prev_pos));
        prev_pos= pos+1;
    }
    if (prev_pos< str.length())
        v.push_back(str.substr(prev_pos, std::string::npos));

    num = new double[v.size()];

    for(unsigned int i=0;i<v.size();i++){
        num[i] = std::stof(v[i]);
    } 
    
    std::cout<< num[1] << std::endl;
}

int main(){
std::string str = "[0.0, 2.1, 22.3]";
std::string delim = "[] ,";
double *d;

string2doublearray(str,delim,d);

std::cout<< d[1] << std::endl;

}

ROS publish the camera info in C++ (PerceptIn Camera)

I'm now using PerceptIn camera to make ROS wrapper.

Publish camera image

See the sample code online_feature.cpp to check how to get image from perceptIn camera.

Publish Camera info

Initialization and put value

Use cameraInfo.h.
sensor_msgs/CameraInfo Documentation
First, include and add your value.

Part of an example is below:

//init 
sensor_msgs::CameraInfoPtr lcam(new sensor_msgs::CameraInfo());
// put value
lcam->height = 480;
lcam->width = 640;
lcam->distortion_model = "plumb_bob";
lcam->D = { -0.361976, 0.110510, 0.001014, 0.000505, 0.000000};
lcam->K = {438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000};
lcam->R = {0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972};
lcam->P = {393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000};

Of course, it is better to read a calibration file to fill these information.

Publishing

When you publish the data, you should synchronize the camera image and camera info (many ROS packages yield that).

Sample code is like below:

lcam->header.stamp = imageLeftMsg->header.stamp;
lcam->header.frame_id = imageLeftMsg->header.frame_id;

imageLeftMsg means image data is for data to be published.

When debugging

Be careful [rostopic echo] will not visualize your CameraInfo until the synchronized image is subscribed by another topic.
github.com

Python,c++でyamlファイル読み込み

jsonに引き続きyamlファイルの読み込みをメモ。

Python

以下の通り。
How can I parse a YAML file in Python - Stack Overflow

import ruamel.yaml as yaml

with open("example.yaml") as stream:
    try:
        print(yaml.load(stream))
    except yaml.YAMLError as exc:
        print(exc)

C++

今回はOpencvにおいてROSの保存したキャリブレーションファイルを読み込みたいので
一番目の方法を採用します。

opencvを使って真面目に読む方法

cv::FileNodeというクラスを使って頑張って読めるようです。
c++ - Read data from yaml file issue - opencv - Stack Overflow

yaml++.hを使う

こちらの一番上の解決策を使います。
Parse YAML Files in C++ - Stack Overflow

yaml.cppを使う

インストールしてビルドする必要がありますが、一番シンプルにかけそうです。
使用例がないので日和って使いませんでしたが。

GitHub - jbeder/yaml-cpp: A YAML parser and emitter in C++

料理初心者でもできる?白ワインとチキンブロスで作る簡単リゾット

というわけで実際に簡単飯?を試してみました。

元レシピ

レシピは以下のサイトからとってきました。
ちゃんと作るなら下のサイトを見た方が安全です。
おいしいチーズリゾット – おいしいアメリカ:アメリカ料理レシピと食べ歩き情報満載!

材料について

元レシピで材料は

  • チキンストック 1½カップ (360cc)
  • 水 1½カップ (360cc)
  • バター 大さじ2
  • たまねぎ 1個(みじん切り)
  • アーボリオ米 1カップ
  • 白ワイン 1/2カップ (120cc)
  • パルメジャンチーズ 1/2-1カップ(削ったもの)
  • 塩こしょう

と言われています。チキンストックとは、平たく言うと液状のコンソメの元的なSomethingのはずです。
チキンブロスとかブイヨンとか仲間が多くてあまり区別がついていないですが。

すべて比較的簡単に手に入るはずですがバターとかワインとか代用がきくのではないでしょうか。



買い出し

コメを7kg買って持って帰るのがきつかった...

買った材料

  • チキンブロス 1缶 (350ccくらい?)
  • 玉ねぎ: 拳くらい 1個
  • バター:1/5ブロック(つまり適当)
  • 米:250ccくらい コップ一杯
  • 白ワイン:適当
  • モッツァレラチーズ:好きなだけ

チキンブラストブロスはこんなやつを買いました。100円いかないくらい。
f:id:ossyaritoori:20170813152312j:plain

ワインも安いやつ(500円未満)を買って飲んだり料理したりします。
f:id:ossyaritoori:20170813152353j:plain

調理開始

大体通しで20分くらいかかります。
「強火」で「5分」とか言われても知るかって感じなので見た目ベースで記録しています。悪しからず。

玉ねぎを切り、フライパンで炒める

玉ねぎを一個まるまるみじん切りにします。
どうせ加熱すると小さくなるのでサイズは適当でいいです。

そのあと、鍋にバターを入れて炒めます。
私は麻雀配1.5個分くらい入れたと思います。
バターは玉ねぎと米にしみこむので多少多い分には問題ないです。

「中火で5分」炒めるそうですがまぁ玉ねぎが半透明になってしんなりするまで炒めればいいんです。うん。

米を投入し半透明になるまで炒める

タイトルの通り米を投入して炒めます。投入前にあまりに鍋がパッサパサならちょっと油なりバターを足してもよさげな気がします。
入れすぎるとたぶん油が跳ねます。たぶん。

生米は白濁していますが、それが透明になるまで炒めます。目安は中火で3分らしいです。

ワインを投入しさらに炒める。

大体の米粒が透明になったら(何粒かはかたくなに白いので無視)コップ1/3くらいのワインをいれて炒めます。
ジュっ!!!って音がします。割とビビる。

この工程そんな重要なのかな?とちょっと思っちゃう。
香り付けとか風味づけとかそういう高尚な行程なのでしょう。

水気が再び飛ぶまで炒めます。2分くらい?

チキンブロスを半分くらい投入。ぐつぐつ煮る。

先ほどのワインの汁気が飛んだらチキンブロスを入れます。
なければ水&コンソメとかで代用してください。


はじめ蓋なしでやっていましたが速攻で水気が飛んでやばかったのできちんと蓋をしましょう。
中火で5~10分くらい煮ます。焦げ付かないように時々開けてかき混ぜるのがいいでしょう。(焦がした人)

残りのチキンブロスをちまちま投入、水気を飛ばす。

蓋を開けると水気が減ってリゾット感が出ている頃かと思います。

ちょっと米をつまんでみて「ちょっと芯が残っているけどまぁかみ砕けるかな」くらいになっていれば仕上げです。
残りのチキンブロスを3回くらいに分けて入れて「水気が飛ぶ」→「入れてかき混ぜる」を繰り返して冷まして完了・

チーズとか胡椒とかお好みで

モッツアレラチーズしかよく知らないので刻んだモッツアレラチーズを入れましたがちょっと粘り気が強すぎる印象。
ネッチョリしないチーズ(パルミジャーノとか?)のほうが良いかもしれません。

きちんと火が通ってさえいればOKなのであとは塩と胡椒で味付けして完成。


見た目は良くないけどまぁまぁです。
f:id:ossyaritoori:20170813152429j:plain

ただ,三人前くらいできます。食べきれないのでホストのお婆ちゃんに少しあげました。

トータルコスト

お金:350円くらい

米:多分300g行かないくらい?=50円強
ワイン:1/8 * 500 = 60円強
バター:1/20 * 600 = 30円強
玉ねぎ:30円くらい
チーズ:1/5 * 300 = 60円くらい
チキンブロス:80円くらい

                              • -

合計:350円くらい

まぁまぁ安い。日本で作っても悪くないくらいの値段です。

時間:けっこうかかる

ただ,買い物でさまよった時間+調理とちょっとした片付けで2時間半かかりました。
この記事書いて3時間半です。これは休日とかにしか頑張れない...

補足

正直見た目が寂しいので玉ねぎを炒める工程で刻んだマッシュルームやアスパラガスを。
コメを炒めた後あたりにトマトとか入れても美味しそう。試してみたら更新します。

今後これを続けるなら手順を簡略化したいですね。

チキンブロスが無い時

コンソメでもブイヨンでもブロスでもなんでもいいですが,要は出汁を切らした場合を先程体験しました。
結論やや淡白な味わいになりますが食べれないことはないです。

代わりにケチャップとチリソース(お店からかっぱらってきた)があったのでそれとトマトを入れて作りました。
顆粒タイプの何らかの出汁があればベストですがなければ醤油やケチャップで十分味付け可能のように感じます。
バターやチーズが風味を出してくれるのもでかいのかも。

とにかく,ケチャップは何かと使えるし現地で買っておいて損はないかと思います。チキンライスも作れるだろうし。

出先で作れる超お手軽自炊のノウハウ、が欲しい。

出先など自分の家ではないところで自炊をしたいという背景がございまして。
なお、知見が得られ次第更新します。

求められる条件

  • 調理器具はフライパンと鍋、電子レンジに限定。(オーブンとかないやろ)
  • つまり、煮るか炒めるか和えれば済むものにしたい。
  • 調味料は最低限、塩と油と砂糖。醤油は以外と手に入る(基本中国系)。

この範囲でなるべく安価で普遍的な材料で自炊をしたいです。


主食をどうするか

バナナは主食に数えないことにします。朝食には超有能ですけどね。

選択肢は大きく分けて

  • 麺類
  • パン

でしょうか。

米を主食にする場合。

海外とかに行っても意外と米は手に入ります。中国系のスーパーに行けばジャポニカ種(細長くなく、しっとりもっちりとした米種)も手に入ります。
こっちの近くのスーパーは1500円いかない位で7kg近く買えます。キラキラネーム米が多いけど。

最上!
f:id:ossyaritoori:20170813152453j:plain

熱き日を海に入れたり最上川。はい。
f:id:ossyaritoori:20170813152811j:plain
個人的には「五月雨を集めて早し」の方が有名かなとも思ったり。


ええと,何が問題かというと「炊飯器がない!」です。
石油王ばりにお金持ちなら現地で高い炊飯器を買えばいいですが、
我々労働階級は鍋で自分で炊くor 炒める とかしなければいけません。

炒めると言ったらリゾット。所要時間は大体30分近くを見た方がいいです。
matome.naver.jp

ただ,やっぱりコメは最小入手単位が大きいのと手間がかかるので勇気がいりますね。

チキンストックと塩、白ワインのリゾット

見た感じ材料がシンプルで簡単そうです。マッシュルームとかアスパラとか拡張性が高そうなのもいいですね。
これは候補に入れておくことにします。
おいしいチーズリゾット – おいしいアメリカ:アメリカ料理レシピと食べ歩き情報満載!

早速作ってみました。
ossyaritoori.hatenablog.com

麺を主食にする場合

生めんも乾麺もたくさん売ってます。
これが一番手軽な気がする。


とりあえず、中華系スーパーにある袋麺を試してみました。
卵と袋めんという質素な組み合わせですが満足いく出来でした。
ちょっとしたサラダやお肉系があるとなおよし。

試しに買った袋麺。札幌n番を作るような要領
f:id:ossyaritoori:20170813141157j:plain

パンを主食にする場合

パン食圏の国ならスープとかメインディッシュを作ればフランスパンとかをかじれば満足できるかもしれません。
ただね、ぶっちゃけ近くで売ってるパンが安くないのとあんまりおいしくなさそうなので検討していません。

適応能力が高ければ現地の人に聞いていろいろ作ってみるといいんじゃないでしょうか。(適当)

大正義冷凍食品

正直、ここまで書いていて最も頼れるのがこいつな気がしてきました。
少々お金はかかりますがお店で買うよりははるかに安く済みますし、余力があればもう一品作ればいいですよね。

中国系スーパーでは謎に餃子の種類が豊富でした。
ちなみに結構イケますこれ。

そのほか

ここではサラダとかスープとかを考えています。
手っ取り早いのはできあいのものを買ってくること。
てかこれでよくね。

自分は一品で耐えられるので特に書くことがないです。
簡単なスープのレシピがあったら残しておきたいですね。