本記事では、ChoreonoidとROSを使ってPointCloud2をPCD形式で出力する手順を紹介します。今回は地図生成にCartographerを使用しました。
Cartographerは、Googleが開発したSLAMのオープンソースライブラリで、複数のプラットフォームやセンサーを統合し、2Dあるいは３Dでリアルタイムに自己位置推定とマップ作成（SLAM）を提供するシステムです。
本記事は、ChoreonoidとROSのインストールを行っていることを前提としています。
インストールを行っていない方は、Choreonoid公式HPを参考に「ROSのインストール」と「Choreonoid関連パッケージのビルド」まで行ってください。
https://choreonoid.org/ja/documents/latest/ros/index.html#
1. 使用した環境
・Ubuntu 20.04 LTS
・Choreonoid 2.0
・ROS Noetic Ninjemys
・Google Cartographer 2.0.0
・map_server 1.17.3
2. Cartographerとmap_serverパッケージのインストール
以下のコマンドを実行し、Cartographerとmap_serverパッケージをインストールします。
Google Cartographer公式ドキュメント：https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html
cd ~/catkin_ws
sudo apt update
sudo apt install -y python3-wstool python3-rosdep ninja-build stow
sudo apt install -y ros-noetic-map-server pcl-tools
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
上記コマンド実行時に以下のエラーが発生した場合、~/catkin_ws/src/cartographer/package.xmlを修正し再実行してください。
【エラー内容】
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
ERROR: the following packages/stacks could not have their rosdep keys resolved to system dependencies:
以下のコマンドでpackage.xmlを開き、46行目をコメントアウトします。
gedit ~/catkin_ws/src/cartographer/package.xml
【package.xml修正内容】
<depend>libboost-iostreams-dev</depend>
<depend>eigen</depend>
<!-- <depend>libabsl-dev</depend> -->
src/cartographer/scripts/install_abseil.sh
catkin build
上記のCartographerパッケージをインストールしていない場合、launchファイル実行時に以下のエラーが発生します。
roslaunch cnoid_turtlebot3_slam cnoid_turtlebot3_world_slam_3d_teleop.launch slam_methods:=cartographer
ERROR: cannot launch node of type [cartographer_ros/cartographer_node]: cartographer_ros
ERROR: cannot launch node of type [cartographer_ros/cartographer_occupancy_grid_node]: cartographer_ros
3. TurtleBot3関連パッケージのダウンロード・インストール
TurtleBot3関連パッケージのダウンロード・インストール方法は、「Choreonoid tips ⑦【gmappingを使ったSLAM】」の項番3～5をご参照ください。
4. cnoid_turtlebot3_pkgsパッケージのダウンロード
cnoid_turtlebot3_pkgs.zipをダウンロードし、ZIPファイルを解凍します。
cnoid_turtlebot3_pkgs
ここでは、ダウンロードディレクトリにcnoid_turtlebot3_pkgs.zipファイルが保存されていることとします。別のディレクトリに格納している場合は適宜ファイルパスを変更してください。ZIPファイルの解凍が完了したら、~/catkin_ws/src以下に対象のディレクトリをコピーします。
cd ~/ダウンロード
unzip cnoid_turtlebot3_pkgs.zip
cp -rfp cnoid_turtlebot3_pkgs ~/catkin_ws/src
5. シンボリック・リンクの追加
cnoid_turtlebot3_bringupパッケージのモデルファイルでturtlebot3とturtlebot3_simulationsパッケージのメッシュファイルを参照しているので、以下のコマンドを実行しシンボリック・リンクを追加します。
cd ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/model/resource/
ln -s ~/catkin_ws/src/turtlebot3/turtlebot3_description/ ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/model/resource/turtlebot3_description
ln -s ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_world/meshes/ ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/model/resource/turtlebot3_world
6. ChoreonoidとTurtleBot3関連パッケージのビルド
以下のコマンドを実行し、ChoreonoidとTurtleBot3関連パッケージのビルドを行います。ChoreonoidからPointCloud2形式でROSトピックを配信する機能もありますがCartographer側でエラーが発生したので、本記事ではPointCloud形式でROSトピックを配信するために、オプションUSE_POINTCLOUD1_IN_BODY_ROS_ITEMをONにしています。
cd ~/catkin_ws
catkin config –append-args -DBUILD_CHOREONOID_EXECUTABLE=OFF -DUSE_POINTCLOUD1_IN_BODY_ROS_ITEM=ON -DCMAKE_BUILD_TYPE=Release
catkin build
source devel/setup.bash
7. TurtleBot3とCartographerによる3次元点群データ出力
以下のコマンドを実行し、TurtleBot3のSLAM用launchファイルを起動します。実行引数に「slam_methods:=cartographer」を忘れずに指定してください。指定していない場合は、gmappingによるSLAMが実行されるので注意してください。
roslaunch cnoid_turtlebot3_slam cnoid_turtlebot3_world_slam_3d_teleop.launch slam_methods:=cartographer
次に、別ターミナルを起動し、TurtleBot3遠隔操作launchを起動します。
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
TurtleBot3の遠隔操作launchを起動したターミナル上で、キーボードからw(直進), x(後退), d(右旋回), a(左旋回), s(停止)を入力することでTurtleBot3を動かし、3次元点群データをPCD形式で出力することができます。PCDファイルは、~/.ros/ディレクトリ直下に出力されています。
Choreonoid画面
VMwareでは、「Displays」→「Cartographer」→「scan_matched_points2」→「Style」で「Points」が選択されている場合、3次元点群が正常に表示されないことがあります。その場合、「Style」を「Points」以外に変更すると正常に表示されます。
Rvizでポイントクラウドを表示
8. 3次元点群データの統合
本記事では、以下のGitHubで公開されている「PCDファイル統合パッケージ」を使用し、複数のPCDファイルをマージしました。使用手順等は以下サイトをご参照ください。
https://github.com/kazuki21057/join_pcd
上記サイトの通りにプログラムをビルドすると以下のエラーが発生するので、join_pcd/CMakeLists.txtのc++のバージョンを11から17に変更しビルドしてください。
【エラー内容】
make
/usr/include/pcl-1.10/pcl/pcl_config.h:7:4: error: #error PCL requires C++14 or above
【CMakeLists.txtの修正内容】
#add_compile_options(-std=c++11)
add_compile_options(-std=c++17)
ビルドが完了したら、以下のコマンドを実行しPCDファイルを統合します。ここでは、ホームディレクトリ直下にjoin_pcdディレクトリをダウンロードしていることとします。PCDファイル名は、各自で出力したファイル名に変更して実行してください。
cd ~/.ros/
~/join_pcd/build/join_pcd -i velodyne_599000.pcd velodyne_20999000.pcd velodyne_41299000.pcd velodyne_62999000.pcd velodyne_84299000.pcd velodyne_106299000.pcd velodyne_127599000.pcd velodyne_148999000.pcd velodyne_169999000.pcd velodyne_190299000.pcd velodyne_211599000.pcd -o merged.pcd
pcl_viewer merged.pcd
pcl_viewerによる統合したPCDファイルの表示
9. モデルファイルの解説
使用しているTurtleBot3モデルは、Waffle_pi_LiDAR_3d.bodyとなります。このモデルファイルにRangeSensorの記述をすることでChoreonoidからLRFの情報をROSトピックとして出力できます。今回は水平方向と垂直方向のPointCloudトピックを出力するので、垂直方向の取得範囲(pitchRange)に0以外を指定しています。
-
type: RangeSensor
name: LiDAR
translation: [ 0.0, 0.0, 0.03635 ]
rotation: [ [ 1, 0, 0, 90 ], [ 0, 1, 0, -90 ] ]
yawRange: 360.0
yawStep: 0.2
pitchRange: 30.0
pitchStep: 2.0
scanRate: 10.0
maxDistance: 100.0
minDistance: 0.2
on: true
10. シンプルコントローラの解説
本記事ではChoreonoidからの3次元点群データの出力にPointCloud形式を使用しているので、シンプルコントローラで”waffle_pi/velodyne/point_cloud”トピックを取得した際に、sensor_msgs::convertPointCloudToPointCloud2関数でPointCloudからPointCloud2形式に変換を行っています。
void pointcloudCallback(const sensor_msgs::PointCloud& msg)
{
sensor_msgs::PointCloud2 pc2_out;
sensor_msgs::PointCloud pc_in;
pc_in.header = msg.header;
pc_in.channels = msg.channels;
for(int i = 0; i < msg.points.size(); i++){
geometry_msgs::Point32 point;
point.x = -msg.points[i].z;
point.y = -msg.points[i].x;
point.z = msg.points[i].y;
pc_in.points.push_back(point);
}
sensor_msgs::convertPointCloudToPointCloud2(pc_in, pc2_out);
pcl2_pub_.publish(pc2_out);
}
11. TurtleBot3のSLAM用launchファイルの解説
cnoid_turtlebot3_world_slam_3d_teleop.launchを実行することでChoreonoidやCartographerなどの関連ノードが起動します。CartographerによるSLAMを行う場合、Cartographerでサブスクライブするトピック名は”/points2”なので、choreonoidノード起動時にトピック名をタグで”/multi_scan_points”から”/points2”に変換しています。
<node pkg="choreonoid_ros" name="choreonoid" type="choreonoid"
args="$(find cnoid_turtlebot3_bringup)/project/$(arg model)_turtlebot3_world_3d_teleop.cnoid --start-simulation">
<remap from="/multi_scan_points" to="/points2" />
</node>
次に、pcl_rosパッケージのpointcloud_to_pcdノードを記述することで、PointCloud2形式のROSトピックをPCD形式に変換し出力を行っています。pointcloud_to_pcdの説明は以下をご参照ください。
https://wiki.ros.org/pcl_ros
<node pkg="pcl_ros" type="pointcloud_to_pcd" name="pointcloud_to_pcd" output="screen">
<remap from="input" to="/scan_matched_points2" />
<!-- prefix : set output folder path. without this, save pcd files in.ros folder. -->
<param name="prefix" value="velodyne_" />
<param name="fixed_frame" value="map" />
<param name="binary" value="false" />
<param name="compressed" value="false" />
</node>
いかがでしたでしょうか。
ChoreonoidとROSで3次元点群データの出力を行いたい方の参考になれば幸いです。