Rvizに表示されている点群を選択し，選択した点群情報をpublishする

# 概要

Rvizで表示している点群を選択して，選択された点群の情報を簡単に取得するRvizプラグイン([selected_points_publisher](https://github.com/tu-rbo/turbo-ros-pkg/tree/master/selected_points_publisher))を見つけました．
しかしそのままだと動かなかったため，修正して使えるようにしてみました．
[Github: kazuki21057/selected_points_publisher](https://github.com/kazuki21057/selected_points_publisher)

本記事は修正した`selected_points_publisher`の導入方法と使い方を紹介します．（最後に修正内容も紹介します）

# 環境

- Ubuntu: 16.04
- ROS: kinetic
- rviz: 1.12.17
- Qt: 5.5.1
- OGRE: 1.9.0 (Ghadamon)

# 導入方法

## インストール

```
$ cd ${your-catkin-workspace-root}/src
$ git clone https://github.com/kazuki21057/selected_points_publisher.git
$ catkin_make # or $ catkin build
```

## ブランチ

`SelectedPointsPublisher`には3つのブランチがあります．それぞれのブランチで扱える点群情報の種類と数が変わります．

- `master`: 位置情報のみ(x, y, z) [**オススメ**]
- `withIntensity`: 位置情報と反射強度 (x, y, z, intensity)
- `withIntensityRing`: 位置情報，反射強度，レーザID (x, y, z, intensity, ring)

お使いのLiDARの点群情報が一致している人は`withIntensity`，`withIntensityRing`ブランチをお使いください．
そうでない人，よくわからない人はとりあえず`master`ブランチを使えば位置情報は取得できます．
（コードの修正が適当なためこのような形になってしまいました...）

## Rvizに表示

プラグインツールとして新たに追加します．  

1. Rvizを起動し，上部の"+"ボタンをクリック
![add.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/329862/64d1d886-8d92-7fb2-54ae-996a42e9b223.png)

2. 選択項目の中から`SelectedPointsPublisher`を選択
![select.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/329862/58750045-6fd7-f02d-45cf-4fa8793a7ab6.png)

導入は以上です．簡単ですね．

# 使い方

使い方は`Select`ツールと同じです．

1. Rviz上部に追加された`SelectedPointsPublisher`をクリック
![finish.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/329862/f20f565d-1ae6-8778-e3cf-d3f2d6e9754f.png)

2. 情報が欲しい点群を長方形で選択

3. `/rviz_selected_points`をSubscribe．

# 使っている様子

選択した点群の反射強度の合計と平均値を取得するコードを作成し，実際に使ってみた様子がこちら．

![ezgif.com-video-to-gif.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/329862/6fbf1f76-c85b-229e-29c8-f582baa0c41d.gif)

<details><summary>サンプルコードはこちら．</summary><div>

```cpp:ave_intensity.cpp
/* ROS */
#include <ros/ros.h>
/* PointCloud */
#include <sensor_msgs/PointCloud2.h>
#include <velodyne_pointcloud/point_types.h>
/* PCL */
#include <pcl_ros/pcl_nodelet.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/impl/common.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>


void pcCallback (const sensor_msgs::PointCloud2ConstPtr& msg)
{
    pcl::PointCloud<velodyne_pointcloud::PointXYZIR> points_in, remain;
    pcl::fromROSMsg(*msg, points_in);
    remain.header = points_in.header;
    remain.reserve(points_in.size());

    float intensity = 0.0;
    for (auto& point: points_in)
    {
        intensity += point.intensity;
    }

    std::printf("sum: %d\n", static_cast<int>(intensity));
    std::printf("ave: %f\n", intensity/static_cast<float>(points_in.size()));
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ave_intensity");
    ros::NodeHandle nh;
    ros::Subscriber subPoints = nh.subscribe("rviz_selected_points", 1, &pcCallback);

    ros::spin();

    return 0;
}
```
</div></details>

# 元パッケージのエラー原因

Rvizプラグイン初心者なりにデバッグしたのでその記録．

### box_clipper3D.hpp の問題

エラー内容

```
usr/include/pcl-1.7/pcl/filters/impl/box_clipper3D.hpp:186:1: error: prototype for ‘void pcl::BoxClipper3D::clipPlanarPolygon3D(const std::vector<T, Eigen::aligned_allocator >&, std::vector<T, Eigen::aligned_allocator >&) const’ does not match any in class ‘pcl::BoxClipper3D’
```

[issue](https://github.com/tu-rbo/turbo-ros-pkg/issues/7)でも質問されていますが，未だに回答はないようです．

`src/SlectedPointsPublisher.cpp`の28行目の下記インクルードをコメントアウトすると解決しました．

```cpp
#include <pcl/filters/impl/box_clipper3D.hpp>
```

### Segmentation fault

`pcl::BoxClipper3D`のエラーを解決するとコンパイルは通るのですが，いざ使ってみると`Segmentation fault`が発生し情報を取得することができませんでした．
発生場所は209行目の`model->rowCount()`でした．（rowの値が取れないことが原因?）

こちらも[issue1](https://github.com/tu-rbo/turbo-ros-pkg/issues/6), [issue2](https://github.com/tu-rbo/turbo-ros-pkg/issues/5)にて質問されています．
このうち，issue2にて質問者が修正コードを公開してくれていたので，ありがたく使わていただきました．

<details><summary>修正後の`SelectedPointsPublisher::_processSelectedAreaAndFindPoints()`の一部</summary><div>  

```cpp:SelectedPointsPublisher.cpp
    rviz::SelectionManager* sel_manager = context_->getSelectionManager();
    rviz::M_Picked selection = sel_manager->getSelection();
    rviz::PropertyTreeModel *model = sel_manager->getPropertyModel();

    // Generate a ros point cloud message with the selected points in rviz
    sensor_msgs::PointCloud2 selected_points_ros;
    selected_points_ros.header.frame_id = context_->getFixedFrame().toStdString();
    selected_points_ros.height = 1;
    selected_points_ros.point_step = 4 * 4;
    selected_points_ros.is_dense = false;
    selected_points_ros.is_bigendian = false;
    selected_points_ros.fields.resize( 4 );

    selected_points_ros.fields[0].name = "x";
    selected_points_ros.fields[0].offset = 0;
    selected_points_ros.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
    selected_points_ros.fields[0].count = 1;

    selected_points_ros.fields[1].name = "y";
    selected_points_ros.fields[1].offset = 4;
    selected_points_ros.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
    selected_points_ros.fields[1].count = 1;

    selected_points_ros.fields[2].name = "z";
    selected_points_ros.fields[2].offset = 8;
    selected_points_ros.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
    selected_points_ros.fields[2].count = 1;

    selected_points_ros.fields[3].name = "intensity";
    selected_points_ros.fields[3].offset = 12;
    selected_points_ros.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
    selected_points_ros.fields[3].count = 1;

    selected_points_ros.fields[3].name = "ring";
    selected_points_ros.fields[3].offset = 16;
    selected_points_ros.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
    selected_points_ros.fields[3].count = 1;

    int i=0;
    while (model->hasIndex(i, 0))
    {
      selected_points_ros.row_step = (i+1) * selected_points_ros.point_step;
      selected_points_ros.data.resize( selected_points_ros.row_step );

      QModelIndex child_index = model->index( i, 0 );

      rviz::Property* child = model->getProp( child_index );
      rviz::VectorProperty* subchild = (rviz::VectorProperty*) child->childAt( 0 );
      Ogre::Vector3 vec = subchild->getVector();

      uint8_t* ptr = &selected_points_ros.data[0] + i * selected_points_ros.point_step;
      *(float*)ptr = vec.x;
      ptr += 4;
      *(float*)ptr = vec.y;
      ptr += 4;
      *(float*)ptr = vec.z;
      ptr += 4;

      /* get intensity */
      rviz::Property* intensityChild = (rviz::Property*) child->childAt( 1 );
      QVariant intensity = intensityChild->getValue();
      *(float*)ptr = intensity.toFloat();
      ptr += 4;

      /* get ring */
      rviz::Property* ringChild = (rviz::Property*) child->childAt( 2 );
      QVariant ring = ringChild->getValue();
      *(float*)ptr = ring.toFloat();
      ptr += 4;

      i++;
    }

    ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher._processSelectedAreaAndFindPoints", "Number of points in the selected area: " << i);

    selected_points_ros.width = i;
    selected_points_ros.header.stamp = ros::Time::now();
    rviz_selected_pub_.publish( selected_points_ros );

```

</div></details>

ちなみに，提示されていたコードも，intensityを取得する部分がうまく機能しなかったため，下記のように決め打ちでデータを取りに行くことでとりあえず解決しています．

```cpp
      /* get intensity */
      rviz::Property* intensityChild = (rviz::Property*) child->childAt( 1 );
      QVariant intensity = intensityChild->getValue();
      *(float*)ptr = intensity.toFloat();
      ptr += 4;

      /* get ring */
      rviz::Property* ringChild = (rviz::Property*) child->childAt( 2 );
      QVariant ring = ringChild->getValue();
      *(float*)ptr = ring.toFloat();
      ptr += 4;
```

この部分がもう少しうまくかければ，いろいろな種類の点群情報を格納することができそうです．（誰か教えてください）


# まとめ

特定エリアの3次元点群情報をピンポイントで取得するのは大変なので，個人的にはとても便利なプラグインだと思いました．

# 参考

- [Github turbo/turbo-ros-pkg/selected_points_publisher](https://github.com/tu-rbo/turbo-ros-pkg/tree/master/selected_points_publisher)