把持位置の推定(jsk_pcl_ros DetectGraspablePosesPcabase)

#はじめに
PCLを使用して認識した物体をロボットから把持するときに、その把持位置を決定する必要があります。その時にjsk_pcl_rosパッケージのDetectGraspablePosesPcabaseが使えそうだと思い使い方を調べてみました。

#ノードの解説
jsk_pcl_rosパッケージのdetect_graspable_poses_pcabase.pyが今回使用するプログラム名で、ターゲットオブジェクトの点群とPCAアルゴリズムを使用し把持姿勢の候補を出力します。

また,ROSインターフェースの名前やデータ型などは以下のようになります。

- Subscribing Topic  
```~input```(```sensor_msgs/PointCloud2```):入力点群(RGBフィールドが必要)

- Publishing Topic  
```~output/can_grasp_poses```(```geometry_msgs/PoseArray```):ロボットが掴むことのできる姿勢のリスト

- Parameters  
```~direction```(Character,default:```x```):この方向からターゲットをつかもうとする  
```~hand_width[m]```(Float,default:```0.13```):ロボットハンドの開くことができる長さ  
```~interval_m[m]```(Float,default:```0.04```):出力する姿勢の間隔,把持姿勢の候補を増やしたい場合はこの値を減らします。

#使ってみる

以下のパッケージを使うとdetect_graspable_poses_pcabaseを簡単に試すことができます。

https://github.com/hoshianaaa/jsk_pcl_ros_samples/blob/master/README.md

以下の手順で実行できます。

```bash:setting
###インストール
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

```

```bash:実行
roslaunch jsk_pcl_ros_samples detect_graspable_poses_pcabase.launch
```

今回作成したlaunchは以下の写真のようになっていて

- 入力するPointCloud2データのデータ名
- 把持方向,ロボットのハンドの幅,把持姿勢候補の間隔

を設定しています。

![Screenshot from 2021-03-21 15-01-49.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/3a767751-8dcd-f644-f778-6d64c75d9be7.png)

```xml:detect_graspable_poses_pcabase.launch
<launch>

	<arg name="file_name" value="$(find jsk_pcl_ros_samples)/pcd/bunny.pcd"/>

	<!-- rate : if publish at 10 Hz, set 0.1 -->

	<arg name="rate" value="0.1"/>
	<node pkg="pcl_ros" type="pcd_to_pointcloud" name="pcd_to_pointcloud" args="$(arg file_name) $(arg rate)" output="screen">
		<remap from="cloud_pcd" to="/points"/>
		<param name="frame_id" value="base_link" />
	</node>

  <arg name="INPUT_CLOUD" value="/points" />

  <node name="detect_graspable_poses_pcabase"
        pkg="jsk_pcl_ros" type="detect_graspable_poses_pcabase.py">
    <remap from="~input" to="$(arg INPUT_CLOUD)" />
    <rosparam subst_value="true">
      direction: z
      hand_size: 0.20
      interval_m: 0.04
    </rosparam>

  </node>

	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find jsk_pcl_ros_samples)/rviz/detect_graspable_poses_pcabase.rviz" required="true" />

</launch>
```

実行するとbunny.pcdの点群の把持姿勢の候補が出力され,その様子が以下の写真のようにrviz上で確認できます。

![Screenshot from 2021-03-21 15-09-07.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/32f10e0b-2765-bccf-b597-ffbae17d8640.png)


#参考

- ドキュメント

https://jsk-docs.readthedocs.io/projects/jsk_recognition/en/latest/jsk_pcl_ros/nodes/detect_graspable_poses_pcabase.html


- 本家サンプル

https://github.com/jsk-ros-pkg/jsk_recognition/blob/master/jsk_pcl_ros/sample/sample_detect_graspable_poses_pcabase.launch

- ソースコード  
https://github.com/jsk-ros-pkg/jsk_recognition/blob/master/jsk_pcl_ros/scripts/detect_graspable_poses_pcabase.py

# 宣伝
## 販売製品

- 3D Vision Controller ： ロボットアームによる3次元ピッキングを実現可能にします


## お仕事依頼

- メールアドレス : ryotaro.hoshina@gmail.com

- coconala : https://coconala.com/users/4162706

- CloudWorks : https://crowdworks.jp/public/employees/5569031