2
Help us understand the problem. What are the problem?

posted at

updated at

把持位置の推定(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を簡単に試すことができます。

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

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

実行
roslaunch jsk_pcl_ros_samples detect_graspable_poses_pcabase.launch

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

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

を設定しています。

Screenshot from 2021-03-21 15-01-49.png

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

参考

  • ドキュメント

  • 本家サンプル

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
2
Help us understand the problem. What are the problem?