ROSのTips（初心者向け）

[ROSConJP 2018](http://roscon.jp/)が先日あったのでインスパイアされて記事を書きました
ROSConJP 2018参加された皆様お疲れ様でした
twitter : https://twitter.com/yukky_saito

##PythonとC++どっちで書いた方がいいのか？
状況によりけりですが、ロボットの中のマシンで動作させるノードを書く場合、リソース制約や時間制約がある場合が多いためC++で書く方がオススメです。
周辺のツールやデバッグはPythonでも構いません。

##C++のROSのノードの書き方は？
公式でのC++ [ROS Tutorial](http://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29)はC++と言いながらC言語っぽい書き方になっています。チュートリアルで勉強した方が、ROSノードを書くとC言語っぽい書き方になってしまい厳しい意見が寄せられる時があります。
例えばですが私は以下のように書く場合が多いです（コンパイルとかしていないのでミスがあるかもです）

```C++
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>

class Sample
{
  public:
    Sample();
    ~Sample(){};

  private:
    void sampleCallback(const sensor_msgs::PointCloud2::ConstPtr &input);
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    ros::Publisher publisher_;
    ros::Subscriber subscriber_;
}

Sample::Sample() : nh_(""), private_nh_("~")
{
    std::string input_topic_name, output_topic_name;
    private_nh_.param("input_topic", input_topic_name, std::string("input"));
    subcriber_ = nh_.subscribe(input_topic_name, 1, &Sample::sampleCallback, this);
    private_nh_.param("output_topic", output_topic_name, std::string("output"));
    publisher_ = nh_.advertise<sensor_msgs::PointCloud>(output_topic_name, 1);
}

void Sample::sampleCallback(const sensor_msgs::PointCloud2::ConstPtr &input){
    if (publisher_.getNumSubscribers() < 1)
        return;
    sensor_msgs::PointCloud2 output;
    output.header = input.header;
    /* 何かの処理 */
    publisher_.publish(output);
}

int main(int argc, char **argv)
{
    Sample sample_node;
    ros::spin();
    return 0;
}
```
その他参考：https://github.com/leggedrobotics/ros_best_practices/wiki


##ノードからどんなトピックが出ているか知りたい
ROSシステムが小さい場合（ノード数10未満程度）の時はrqt_graphで見るのがいいかもしれませんが、
システムが大きくなるとrqt_graphがぐちゃぐちゃになったり、起動が重かったりします。
そんな時は端末から以下のコマンドで調べられます

```
$ rosnode info <ノード名>
```

##トピックをSubsribe, advertiseしているノードが知りたい
端末から以下のコマンドでトピックに関する情報が得られます

```
$ rostopic info <トピック名>
```

##トピックにデータが流れているのか知りたい
端末から以下のコマンドでトピックのデータを表示できます

```
$ rostopic echo <トピック名>
```
もしくは
以下のコマンドで何hzで出ているか確認できます。出ていなければ no new messageと出力されます

```
$ rostopic hz <トピック名>
```
##大きなデータrostopic echoするとやばい感じになる時は？
noarrオプションをつけると配列は表示されなくなります

```
$ rostopic echo --noarr <トピック名>
```

##どんなノードが動作しているか知りたい

```
$ rosnode list
```

##ネームスペースを含めたノード名が知りたい
調べたいノードの検索するヒントがあれば以下のコマンドで調べられます。ノードの数が小さければrosnode listだけれも大丈夫です。

```
$ rosnode list　| grep <検索したい文字列>
```

##ネームスペースを含めたトピック名が知りたい
調べたいトピックの検索するヒントがあれば以下のコマンドで調べられます。トピックの数が小さければrostopic listだけれも大丈夫です。

```
$ rostopic list　| grep <検索したい文字列>
```

##とあるトピック内の一部だけechoしたい
例えばPointCloud2形式の"sample"トピックのheaderだけをechoする場合

```
$ rostopic echo /sample/header
```

##ノードのパラメータを実機を動かしながら調節したい
以下のように静的にパラメータを決めると実際に動かしながら調節ができません

```
    private_nh_.param("input_topic", input_topic_name, std::string("input"));
```
そんな時は[Dynamic reconfigure](http://wiki.ros.org/ja/dynamic_reconfigure/Tutorials)を使いましょう。
余談ですが、Dynamic reconfigureで動的に調節できるパラメータをcfgファイルに書き込んだ場合、private_nh_.param()と書かなくても内部で自動生成されます。つまりlaunchで<param>タグ使えます

##ノード or launchを起動するたびに端末が必要になる
できる限りlaunchとしてまとめるのがいいですが、そうじゃない時はtmuxや
terminatorを使うのがいいと思います。
私はtmuxを使っています
参考：https://qiita.com/nl0_blu/items/9d207a70ccc8467f7bab

##複数のトピックを同期してコールバック関数を呼びたい
センサレベルでの同期はROSフレームワークでは扱えませんが、複数のトピックのタイムスタンプが同じもの（もしくは近いもの）を選んでコールバック関数を呼ぶことは可能です。
その機能は[message filter](http://wiki.ros.org/message_filters)のPolicy-Based Synchronizerのところです。

###タイムスタンプが完全一致しているときコールバック関数が呼びたい場合
タイムスタンプが完全に一致しているときコールバック関数を呼ぶ場合は、ExactTime Policyを使うことで実現できます。（以下のコードのsync_policies::ExactTimeがポリシー）

```C++
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);

  typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
  // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
```

###タイムスタンプが大体一致しているときコールバック関数が呼びたい場合
タイムスタンプが大体一致しているときコールバック関数を呼ぶ場合は、ApproximateTime Policyを使うことで実現できます。（以下のコードのsync_policies::ApproximateTimeがポリシー）

```C++
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);

  typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
```
ApproximateTime Policyのアルゴリズムはこちらで述べられています。重要なことはメッセージは一回使ったら再利用しないということです。簡単なアルゴリズムなので読んでおくことをお勧めします。
http://wiki.ros.org/message_filters/ApproximateTime

##あるトピックを違うトピック名にして再publish（リレー）したい
[topic_tools](http://wiki.ros.org/topic_tools/relay)を使って実現できます。
以下の<input>をリレーする元のトピック、<output>をリレー後のトピック名にすると、再publishされます。これは<input>を<output>にしてメッセージを流しているだけです。

```
$ rosrun topic_tools relay <input> <output>
```


##その他
[2014年でお世話になったROSノウハウまとめ](https://garaemon.github.io/ros/2014/12/31/ros.html)