tms_ss_ods - irvs/ros_tms GitHub Wiki

Wiki

  1. Ubuntu上でのXtion起動方法

※ 製品によってはうまくいかない場合あり(例：DELL,SONY)

0.1. ドライバ設定

  1. Xtion Pro Live 付属CD「Xtion PRO Series」を挿入

  2. SDK/Linux内の圧縮ファイルをホームフォルダーにコピー (64bit → Linux-x64-20.～.rar)

  3. 圧縮ファイルを解凍

rar e Linux-x64-20.4.2.20.rar

rarが入っていない場合は

sudo apt-get install rar

でインストールする 解凍後，3つの圧縮ファイルができる(OpenNI, Sensor, NITE)

4.3つのファイルを解凍

tar -xvf OpenNI~
tar -xvf Sensor~
tar -xvf NITE~

5.OpenNI→Sensor→NITEの順にインストール

cd OpenNI~
sudo sh install.sh
  
cd ../Sensor~
sudo sh install.sh
  
cd ../NITE~
sudo sh install.sh

0.2.ROSを用いたXtion起動

  1. ROS (ここではgroovyとする) をインストール ROSのHP(http://wiki.ros.org/ROS/Installation )を参考

  2. openni-camera と openni-launch の取得

sudo apt-get install ros-groovy-openni-camera ros-groovy-openni-launch

これでXtionを起動できるようになる.

another method for ROS hydro

sudo apt-get install ros-hydro-openni2-camera ros-hydro-openni2-launch

<!-- Hardware depth registration -->
<arg name="depth_registration" default="true" />
  1. Xtion起動
roslaunch openni_launch openni.launch

roslaunch openni2_launch openni2.launch

画像の確認は

rosrun image_view image_view image:=/camera/rgb/image_raw

または，

rosrun rviz rviz

ただし，デフォルトではRGBカメラとDepthカメラのキャリブレーションは行われていない． キャリブレーションを行うには，openni_launch内のファイルを書き換える必要がある.

cd /opt/ros/groovy/share/openni_launch/launch/
sudo gedit openni.launch

25行目を

<arg name="depth_registration" default="true" />

に書き換える.

1. 変化箇所検出プログラム

環境 OS:Ubuntu 12.04 ROS:hydro PCL:ver1.7.0

以下のプログラムはPCLを用いている. PCLの使い方はgeminiのCityフォルダ内のpptファイルを参照(準備中)

ロボット搭載RGBDカメラによって家具上物品の検出を行う方法として， 家具上の以前の状態を記憶した3次元点群情報(記憶情報)と新たに取得した3次元点群情報との変化に着目し， その変化箇所を物品として検出するプログラムである.

1.0 RGBDカメラによるデータ取得

ods_capture/ods_capture

KinectやXtionなどのデータを取得する. 受け取ったID番号に応じて，取得する情報は異なる. 取得できるデータは，RGB画像，距離画像，3次元点群などがある.

rosrun ods_capture ods_capture

以下で紹介する機能は，ほとんどKinect・Xtionのデータを必要とするので 使用する機能と一緒に，この"ods_capture"も起動しておくように.

1.1. 変化箇所

1.1.1. 前提

用いるカメラはRGBDカメラ(Kinect,Xtion)とする. TMSでロボットや家具の位置，記憶情報，家具点群モデルが管理されているとする.

1.1.2. 変化検出アルゴリズム

  1. 取得情報と家具点群モデルの重ね合わせ ロボットと家具の位置情報をもとに， 座標変換により取得情報と家具点群モデルとを重ね合わせる.

  2. ICPアルゴリズムによる位置合わせ ICPアルゴリズムを用いて，取得情報と家具点群モデルとを精密に位置合わせする. その後，家具モデルと重なる点群を除去する.

  3. セグメンテーション 残った点群に対して，セグメンテーションを行う. この結果は新たな記憶情報として管理される.

  4. セグメンテーション間対応付け セグメントごとに，記憶情報のセグメントとの対応付けを行う. 対応付けられなかったセグメントを変化箇所とする. 対応付けの条件には，重心位置やセグメントの高さを記述したエレベーションマップを用いる.

1.2. 対象家具：テーブル(table/table)

rosrun ods_change_detection ods_changedt_table

1.3. 対象家具：棚(shelf/shelf)

rosrun ods_change_detection ods_changedt_shelf

1.4. 対象家具：椅子(chair/chair)

rosrun ods_change_detection ods_changedt_chair

1.5. 対象家具：ベッド(bed/bed)

rosrun ods_change_detection ods_changedt_bed

1.6. TMSとの連結部

ods_chg_dt/ods_chg_dt

ここではTMSから依頼を受け，Kinectの実行と対象家具に応じた変化検出プログラムを選択，実行する. 検出のトリガは，TMSで環境内情報の分析の結果，消失物品があると判断されたとき， TMS側から対象家具のIDと記憶情報，ロボットと家具の位置情報，家具点群モデルを受け取る. 家具IDからテーブル上あるいは棚内の変化検出プログラムを選択し実行する 実行後，変化箇所の物品IDとその位置を受け取り，TMSに渡す.

rosrun ods_change_detection ods_change_detection

1.7. 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_change_detection.srvを用いて行われる. トピック名は『ods_change_detection』

ods_change_detection.srv

uint32 furniture_id
sensor_msgs/PointCloud2 cloud
sensor_msgs/PointCloud2 model
geometry_msgs/Pose furniture
geometry_msgs/Pose2D robot
geometry_msgs/Pose sensor
---
sensor_msgs/PointCloud2 cloud
time tMeasuredTime
geometry_msgs/PoseArray objects

2. SmartPal5に関連する機能

2.1. ワゴンの検出

ods_wagon/ods_wagon

2.1.1. 概要

ロボットがワゴンを動かすために，SmartPalに搭載されたXtionで持つべきグリップを検出し，その位置からワゴンの位置を計算する. 現状，扱うワゴンは下の画像のもののみとしている. グリップの検出には色情報を用いる. 簡単に，黒色の部分を抽出した後，セグメンテーションを行い，グリップを確定する. 検出したグリップの数とそれぞれの位置からワゴン位置を計算する． (Choreonoidにグリップの情報を反映させるには,絶対座標系における位置が必要 ただし，グリップ把持の成否はロボットとグリップとの相対位置の精度による. つまり，グリップの絶対座標の精度にはよらない) この機能に必要な情報は,色付き点群情報とロボットの位置・姿勢，Xtionの姿勢，ロボットとXtionの位置関係である.

wagon

rosrun ods_wagon ods_wagon

2.1.2. 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_wagon.srvを用いて行われる. wagon.srvの中身は以下のようになっている. トピック名は『ods_wagon』

wagon.srv

geometry_msgs/Pose2D robot
geometry_msgs/Pose sensor
---
geometry_msgs/Pose2D wagon

入力にはロボットの位置，姿勢情報が必要 出力は，ワゴンの位置(絶対座標系)

2.1.3. 実行結果

images/wagon_gripper.png

2.2. ワゴン内の物品検出

ods_wagon/ods_wagon_object

2.2.1. 概要

ロボットがワゴン内の物品を取り出すために，ワゴン内の物品の位置情報をXtionから計測する. SmartPalが物品をとりやすいようにするために，ワゴンの荷台に板が取り付けられているという前提をおく. 物品の検出では，まず平面検出により荷台を除去する. その後セグメンテーションを行い，セグメントの中から荷台上にあるものだけを選ぶ. 出力は絶対座標系における各セグメントの重心位置

rosrun ods_wagon ods_wagon_object

2.2.2. 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_wagon_object.srvを用いて行われる. トピック名は『ods_wagon_object』

ods_wagon_object.srv

geometry_msgs/Pose2D robot
geometry_msgs/Pose sensor
---
geometry_msgs/PoseArray objects

2.2.3. 実行結果

images/wagon_object.png

2.3. 平面上(テーブル上，床上)の円筒形物品検出

ods_cylinder/ods_cylinder

2.3.1. 概要

テーブル上や床上などの平面上にある物品を検出する. この機能を作るにあたって，円筒形物品の半径を出力してほしいとの要望があったが， 物品識別は扱っていないので，物品は円筒形であるという前提で行っている. 出力には物品の位置と物品の半径が含まれている. 検出方法は，RANSACを用いた平面検出より平面を除去した後，セグメンテーションを行う. 各セグメントに対し，以下の2つの方法で物品の半径を求める. ただし，物品識別が行えるのであれば，識別した時点でその物品の半径はデータベースから得られる.

  1. RANSAC (入力: id == 1) RANSACを用いて，円筒形モデルとのレジストレーションを行う. 得られるパラメータは7つあり，円筒形の半径は7番目(coefficients->values[6])の値である. RANSACでは円筒形の側面の情報を用いるので，物品を横から撮影するテーブル上を推奨する.

  2. エレベーションマップ (id == 2) 点の平面上からの高さ，つまりセグメントの上面の情報を記述したエレベーションマップを作成する. マップの値から物品の半径を求める. エレベーションマップの作成には，物品上面を撮影できることが条件戸なるので，上からの撮影を行う床上を推奨する.

円筒形物品を前提においていたが，半径を無視すればどの形であっても平面上の物品を検出することができる.

rosrun ods_cylinder ods_cylinder

2.3.2. 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_cylinder.srvを用いて行われる. wagon.srvの中身は以下のようになっている. トピック名は『ods_cylinder_detection』

uint32 id
---
geometry_msgs/PoseArray objects
float64[] radious

入力のidが1であればRANSAC，2であればエレベーションマップを用いて半径を求める. 出力は，物品の位置と半径

2.4. ロボット自己位置修正

ods_robot_position ods_robot_position

2.4.1. 概要

FSSにより得られたロボットの位置情報には大きな誤差を含む可能性があり， 物品の把持時などに障害物との衝突する危険がある. そこで，ロボットに搭載されたカメラを用いて，ロボット位置の修正を行う.

修正するタイミングは物品の把持前を想定しており， カメラ内に固定家具が撮影されていることを前提とする. 修正前のロボットと固定家具の位置情報から家具の点群モデルを取得した3次元点群情報に重ねる. その後，ICPアルゴリズムにより位置合わせを行う. この時得られる変換行列からカメラの視点の移動を計算する. ロボットとカメラの位置関係を用いて，移動後のカメラ視点からロボット位置を計算する.

rosrun ods_robot_position ods_robot_position

2.4.2 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_get_robots_pos.srvを用いて行われる. ods_get_robots_pos.srvの中身は以下のようになっている. トピック名は『ods_robot_position』

sensor_msgs/PointCloud2 model
geometry_msgs/Pose furniture
geometry_msgs/Pose2D robot
geometry_msgs/Pose sensor
---
geometry_msgs/Pose2D m_robot

入力は，家具点群モデル，ロボットの位置，家具の位置，センサの位置(ロボットからの相対位置)

3. KKPに関連する機能

3.0. 異常事態の検出

人が倒れるという異常事態を検出し，倒れた人の顔画像を取得，外部に送信する.

アルゴリズム

  1. 倒れた人の検出 ods_person_detection
  2. 顔方向の決定 ods_skin_color ods_skin_color
  3. 顔認識 ods_face_detection ods_face_detection

3.1. 倒れた人の検出

person_detection/person_detection　 person_detection/realtime_persondt

3.1.1. 概要

KKPの下部に搭載されたXtionを用いて，環境内で倒れた人の検出を行う. 床面ぎりぎりに設置されているXtionでは，床面を構成する点群情報は取得できない. もし，床面の点群情報が取得できる場合は，プログラムに平面除去を加える. 床上の点群情報に対して，床上より一定の高さでフィルタをかける. 残った点群に対し，セグメンテーションを行う. 各セグメントにおいてセグメントの端点を求める. オクルージョン等により1つの物体が複数のセグメントに分離することを考慮して， 任意の2つのセグメント間における端点の距離を求め，この距離が一定値以内であれば， 端点間の画素の距離値をみて，手前に障害物があるのなら1つのセグメントとしてグルーピングする. グルーピングの結果，人と考えうる大きさのセグメントの端点を出力する. ノード『ods_person_detection』では，ROSサービスで通信を行う(1フレームの検出). ノード『ods_realtime_persondt』では，ROSメッセージで通信を行う(リアルタイム検出).

rosrun ods_person_detection ods_person_detection

または

rosrun ods_person_detection ods_realtime_persondt

※ods_realtime_persondtではods_captureを起動する必要無し

3.1.2. ods_person_detectionの利用方法

ods_person_detectionの出力はtms_msgs/tms_sp_msgs/msg/ods_person_detection.srvを用いて行われる. ods_person_detection.srvの中身は以下のようになっている. トピック名は『ods_person_detection』

ods_person_detection.srv

---
float32 p1_x
float32 p1_y
float32 p1_z
float32 p2_x
float32 p2_y
float32 p2_z
sensor_msgs/Image image

p1，p2はそれぞれカメラからみて左側，右側の端点の相対位置座標を表す.

3.1.3. ods_realtime_persondtの利用方法

ods_realtime_persondtの出力はtms_msgs/tms_sp_msgs/msg/ods_person_dt.msgを用いて行われる. ods_person_dt.msgの中身は以下のようになっている. トピック名は『ods_realtime_persondt』

ods_person_dt.msg

float32 p1_x
float32 p1_y
float32 p1_z
float32 p2_x
float32 p2_y
float32 p2_z

3.2. 肌色抽出による顔方向の決定

ods_skin_color/ods_skin_color

3.2.1. 概要

倒れた人の顔画像の取得のため，ロボットの移動先を決定する必要がある. 決定方法は，倒れた人の全身を撮影できる位置に移動した後， 取得した画像の肌色領域を抽出する. 多くの肌色領域が存在する方向を顔のある方向と定め.移動先位置を決定する.

rosrun ods_skin_color ods_skin_color

3.2.2. 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_skincolor_extraction.srvを用いて行われる. ods_skincolor_extraction.srvの中身は以下のようになっている. トピック名は『ods_skin_color』

ods_skincolor_extraction.srv

---
uint32 direction
sensor_msgs/Image image

出力のdirectionは，1であれば右，2であれば左を表す. imageは，顔検出失敗用の全身画像

3.3. 顔検出

ods_face_detection/ods_face_detection

3.3.1. 概要

倒れた人の顔画像取得のための顔検出を行う. OpenCVの顔検出プログラムを利用する. 識別器は直立した顔のみのものなので，倒れた人の顔も検出できるように画像を回転させる.

rosrun ods_face_detection ods_face_detection

3.3.2. 利用方法

出力はtms_msgs/tms_sp_msgs/srv/ods_face_detection.srvを用いて行われる. ods_face_detection.srvの中身は以下のようになっている. トピック名は『ods_face_detection』

---
uint32 result
sensor_msgs/Image image

出力は顔検出の成否を表すresult (0→失敗，1→成功)と顔画像imgaeを送信する.