• 3次元点群の処理
• Point Cloud の表示
  • Xtion PRO Live の場合
  • YVT-35LX の場合
• PCL（Point Cloud Library）による３次元点群処理
• ビルド＆実行
  • Xtion PRO Live の場合
  • YVT-35LX の場合
• 補足 rsj_pointcloud_test_node.cpp について

3次元点群の処理

Xtion PRO Live や YVT-35LX から得られる３次元点群PointCloudに対する基本的な処理を実習します。

Point Cloud の表示

各センサから得られる点群を RViz によって可視化します。センサごとに実行するコマンドが異なりますので、お手持ちのセンサに応じて次のコマンドを実行してください。

Xtion PRO Live の場合

1
2
3
$ cd ~/catkin_ws/
$ source devel/setup.bash
$ roslaunch rsj_pointcloud_to_laserscan rsj_pointcloud_to_laserscan.launch

別のターミナルを開き

1
2
$ cd ~/catkin_ws/src/rsj_pointcloud_to_laserscan/config/rviz
$ rosrun rviz rviz -d view_points.rviz

図のようにPointCloudが表示されれば成功です。

起動した２つのターミナルを Ctrl+c で終了してください。

YVT-35LX の場合

1
2
3
$ cd ~/catkin_ws/
$ source devel/setup.bash
$ roslaunch rsj_pointcloud_to_laserscan rsj_pointcloud_to_laserscan_3durg.launch

別のターミナルを開き

1
2
$ cd ~/catkin_ws/src/rsj_pointcloud_to_laserscan/config/rviz
$ rosrun rviz rviz -d view_points_3durg.rviz

図のようにPointCloudが表示されれば成功です。

起動した２つのターミナルを Ctrl+c で終了してください。

PCL（Point Cloud Library）による３次元点群処理

ロボットでオドメトリのデータを利用したときと同じように３次元センサが出力したPointCloudのデータを受け取るプログラムを作成しましょう。 PointCloudの処理を独自に書こうとすると大変な労力が必要です。 ２次元画像処理用にOpenCVというライブラリがあるように、３次元点群処理にはPCL(Point Cloud Library) があります。 PCL を利用するプログラムを作成してみましょう。

端末を開き雛形をダウンロードしてください。

1
2
$ cd ~/catkin_ws/src
$ git clone https://github.com/KMiyawaki/rsj_pointcloud_test.git

テキストエディタでrsj_pointcloud_test_node.cppを開いてください。

1
2
$ cd ~/catkin_ws/src/rsj_pointcloud_test/src
任意のテキストエディタで rsj_pointcloud_test_node.cpp を開く

RsjPointcloudTestNodeクラスにある、PointCloud用のコールバック関数cbPointsを編集します。

1
2
3
4
5
6
7
8
9
10
11
12
13
void cbPoints(const PointCloud::ConstPtr &msg)
{
  try
  {
    (略)
    // ここに cloud_src に対するフィルタ処理を書く
    ROS_INFO("width: %u, height: %u", cloud_src->width, cloud_src->height);
  }
  catch (std::exception &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

cbPoints関数はセンサからPointCloudを受け取るたびに呼び出される関数です。

なお、今回の実習で利用している三次元センサのドライバノードから出力される点群データは、元々は ROS のsensor_msgs/PointCloud2という型ですが、後述するpcl_rosというライブラリがPCLのPointCloud型にsensor_msgs/PointCloud2型と互換性を持たせる実装を追加することで、特別にそのまま ROS Message 型と同様に使えるようにしています。 サブスクライバの初期化コードで指定するコールバック関数が任意の型を受け取れるわけではないことに注意してください。

ファイルを保存してエディタを閉じます。

ビルド＆実行

まず、catkin_wsでcatkin_makeを実行して、追加したコードをビルドします。

1
2
$ cd ~/catkin_ws
$ catkin_make 

次にお手持ちの３次元センサごとに次のようにノードを起動します。

Xtion PRO Live の場合

ターミナルでセンサドライバノードを起動します。

1
2
3
$ cd ~/catkin_ws/
$ source devel/setup.bash
$ roslaunch rsj_pointcloud_to_laserscan rsj_pointcloud_to_laserscan.launch

新しいターミナルを開き、rsj_pointcloud_test_nodeを起動します。

1
2
3
4
5
6
7
8
$ cd ~/catkin_ws/
$ source devel/setup.bash
$ rosrun  rsj_pointcloud_test rsj_pointcloud_test_node \
    _target_frame:=camera_link _topic_name:=/camera/depth_registered/points
[ INFO] [1524039160.481736901]: target_frame='camera_link'
[ INFO] [1524039160.481783905]: topic_name='/camera/depth_registered/points'
[ INFO] [1524039160.485222004]: Hello Point Cloud!
[ INFO] [1524039161.311438819]: width: 640, height: 480

YVT-35LX の場合

ターミナルでセンサドライバノードを起動します。

1
2
3
$ cd ~/catkin_ws/
$ source devel/setup.bash
$ roslaunch rsj_pointcloud_to_laserscan rsj_pointcloud_to_laserscan_3durg.launch

新しいターミナルを開き、rsj_pointcloud_test_nodeを起動します。

1
2
3
4
5
6
7
8
9
10
$ cd ~/catkin_ws/
$ source devel/setup.bash
$ rosrun rsj_pointcloud_test rsj_pointcloud_test_node \
    _target_frame:= _topic_name:=/hokuyo3d/hokuyo_cloud2
[ INFO] [1528006896.162315502]: target_frame=''
[ INFO] [1528006896.162660037]: topic_name='/hokuyo3d/hokuyo_cloud2'
[ INFO] [1528006896.178381795]: Hello Point Cloud!
[ INFO] [1528006896.378943557]: width: 2228, height: 1
[ INFO] [1528006896.412205921]: width: 2670, height: 1
[ INFO] [1528006896.478866703]: width: 2674, height: 1

このようにwidth: xxx, height: xxxというメッセージが表示されればPointCloudは受信できています。

補足 rsj_pointcloud_test_node.cpp について

プログラムの先頭には ROS 内で PCL を扱うためのヘッダファイルに関するinclude文と実習で使うPointCloudの型宣言が記述されています。

1
2
3
4
5
6
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <visualization_msgs/MarkerArray.h>

typedef pcl::PointXYZ PointT
typedef pcl::PointCloud<PointT> PointCloud;

PCL ではpcl::PointCloud<T>という C++ のテンプレートで点群を扱うデータ型を表現しています。 Tの部分には座標と色情報を持つpcl::PointXYZRGBなど様々な点の型を与えることが可能です。 今回は色情報のない、位置だけの点pcl::PointXYZを使います。 最終行のtypedef宣言ではpcl::PointCloud<PointT>に対し今回の実習で利用する点群の型PointCloudという別名をつけ、プログラムが書きやすくなるようにしています。

なお、#include <visualization_msgs/MarkerArray.h>は点群処理結果を可視化するために必要となる ROS に含まれるヘッダファイルです。

RsjPointcloudTestNodeクラスの冒頭には、sub_odom_と同じようにPointCloud用のサブスクライバを宣言しています。

1
2
3
4
5
class RsjPointcloudTestNode 
{
private:
  (略)
  ros::Subscriber sub_points_;

RsjPointcloudTestNodeのコンストラクタには、このノードが必要としているパラメータの取得やPointCloud用のサブスクライバ初期化コードが記述されています。

1
2
3
4
5
6
7
8
9
10
RsjPointcloudTestNode()
  : nh_()
  , pnh_("~")
{
  std::string topic_name;
  pnh_.param("target_frame", target_frame_, std::string(""));
  pnh_.param("topic_name", topic_name, std::string("/camera/depth_registered/points"));
  ROS_INFO("target_frame = '%s'", target_frame_.c_str());
  ROS_INFO("topic_name = '%s'", topic_name.c_str());
  sub_points_ = nh_.subscribe(topic_name, 5, &RsjPointcloudTestNode::cbPoints, this);

pnh_.param関数は、実行時に与えられたパラメータを読み込みます。 ここでpnh_は、rsj_robot_testのソースコードで解説した nh_と同様のros::NodeHandleです。 ただしpnh_は、"~"を引数に初期化することで、このノードのプライベートな名前空間を使う設定になっています。 この1つ目の例では、/rsj_pointcloud_test_node/target_frame というパラメータをtarget_frame_変数に読み込み、もし指定されていなければstd::string("")をデフォルト値として用いることを意味します。

topic_nameはセンサが出力するPointCloudのトピック名を、target_frame_は得られた点群を処理しやすい座標系に変換する際の座標系の名前を示しています。 特に Xtion PRO Live の場合、点群の座標系はロボットのローカル座標系と異なっているため、cbPoints関数の冒頭で座標変換をしています。

この座標変換は ROS のtfによるものです。 tfによって座標系の情報がノード間で共有され、時刻と座標系の名前に基づいて様々なデータの座標変換を実現することができます。

target_frame_が空白の場合は座標変換を行いません（YVT-35LX の場合）。

CMakeLists.txtでは PCL を ROS で扱えるようにしています。

1
2
3
4
5
6
7
8
find_package(catkin REQUIRED COMPONENTS
  pcl_ros  # 注: pcl_ros を追加している
  roscpp
  rospy
  sensor_msgs
  std_msgs
  visualization_msgs
)

終了したら PCL を使った点群処理に関する実習に進んでください。

PointCloud に対するフィルタ