[ROS] PointCloud2の領域を切り抜く

はじめに

ROSのPointCloud2を切り取る方法がないかと思い調べました.
jsk-pcl-rosというパッケージのattention_clipperを使用すると指定したbox内の点群を切り取ってくれることがわかりました.
本記事で、その使用方法を記載します

環境

ubuntu18.04
ROS:melodic

実行手順

install

sudo apt-get install -y ros-melodic-jsk-pcl-ros
sudo apt-get install -y ros-melodic-jsk-visualization

以下のようにlauchを作成します.input topic nameとsize、frame idは自身の環境に合うように修正します.

launch

<launch>

  <arg name="gui" default="false" />

  <arg name="input_cloud" value="/camera/depth/color/points" />

  <node name="attention_clipper"
        pkg="nodelet" type="nodelet"
        args="standalone jsk_pcl/AttentionClipper">
    <remap from="~input/points" to="$(arg input_cloud)" />
    <rosparam>
      initial_pos: [0, 0, 0]
      initial_rot: [0, 0, 0]
      dimension_x: 0.2
      dimension_y: 0.2
      dimension_z: 2
      frame_id: camera_depth_optical_frame
    </rosparam>
  </node>

  <node name="extract_indices"
        pkg="jsk_pcl_ros" type="extract_indices">
    <remap from="~input" to="$(arg input_cloud)" />
    <remap from="~indices" to="attention_clipper/output/point_indices" />
  </node>

</launch>

実行すると,以下の写真のように指定したbox内の点群の切り取りができます.

追記:サンプル

以下のパッケージから,attention_clipperを簡単に試すことができます.

使い方を以下に記載します.

install

sudo apt-get install -y ros-melodic-jsk-pcl-ros
sudo apt-get install -y ros-melodic-jsk-visualization
git clone https://github.com/hoshianaaa/jsk_pcl_ros_samples.git
cd ~/catkin_ws/src

catkin build 
または
catkin make

source ~/catkin_ws/devel/setup.bash

実行

roslaunch jsk_pcl_ros_samples attention_clipper.launch

attention_clipperのBoundingBoxはtopicによって移動させたり,拡大させたりすることができます.
以下に例を示します.

BoundingBoxの移動

rostopic pub /attention_clipper/input/pose geometry_msgs/PoseStamped "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: 'base_link'
pose:
  position:
    x: 0.0
    y: 0.1
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 0.0"

BoundingBoxの拡大

rostopic pub /attention_clipper/input/box jsk_recognition_msgs/BoundingBox "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: 'base_link'
pose:
  position: {x: 0.0, y: 0.1, z: 0.0}
  orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
dimensions: {x: 0.2, y: 0.2, z: 0.2}
value: 0.0
label: 0"

また,複数のBoundingBoxを指定することもできます.
サンプルの起動方法は以下のようになります.

サンプル起動方法(BoundingBox複数)

roslaunch jsk_pcl_ros_samples attention_clipper_multi.launch 

以上です,ぜひお試しください!
また動作がうまく行かないまたは、修正点などあればご気軽にご連絡ください.