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

概要

Rvizで表示している点群を選択して，選択された点群の情報を簡単に取得するRvizプラグイン(selected_points_publisher)を見つけました．
しかしそのままだと動かなかったため，修正して使えるようにしてみました．
Github: 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を起動し，上部の"+"ボタンをクリック
   

2. 選択項目の中からSelectedPointsPublisherを選択
   

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

使い方

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

1. Rviz上部に追加されたSelectedPointsPublisherをクリック
   

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

3. /rviz_selected_pointsをSubscribe．

使っている様子

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

サンプルコードはこちら．

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

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

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でも質問されていますが，未だに回答はないようです．

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

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

Segmentation fault

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

こちらもissue1, issue2にて質問されています．
このうち，issue2にて質問者が修正コードを公開してくれていたので，ありがたく使わていただきました．

修正後の`SelectedPointsPublisher::_processSelectedAreaAndFindPoints()`の一部

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

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

      /* 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