tms_rp_rps
以下の３つの要素から構成される． 1. rps_checker : 環境監視 2. rps_planner : 動作計画 3. rps_commander : 実行命令
voronoi_path
- make collision map
- select the person, robot, floor928 3D models
- click the "RemoveEnv" button
- file >> Open... >> OpenHRP model file >> collision_obj_hrp.wrl
- click the collision_obj model
- click the "SetCollisionTarget" button
- click the "makeCollisionMap" button
- save name = "use_collision_map.csv" in ros_tms/tms_rp/tms_rp_rps/rps_checker/MAP
- cell_size = 0.1 M
- 実行命令
rosrun tms_rp_rps_checker rps_map_publisher
事前に作成した環境地図を発行するノード． tms_viewerのView::RPS_MAPをOnにするとviewer上に表示可能． (CollisionAreaは発行する地図情報に含めなくなったため表示不可)
rosrun tms_dev_viewer tms_viewer
起動中は常に環境地図情報を発行し続ける． (発行される地図情報は固定なので，地図を利用するplanner側でmap情報が取得できれば不要になる． 端的に言うと，planner起動後少し時間をおいたらmap_publisherは停止しても問題ない．)
地図情報は以下で管理される．
rps_checker/MAP/use_collision_map.csv
地図データの仕様は次のとおり． 1行目：地図の範囲情報 ・x軸方向の下限値 ・x軸方向の上限値(m) ・y軸方向の下限値(m) ・y軸方向の上限値(m) ・1マスの幅(m)
例えば，912の実験環境(x:0～4.5m y:0～4m)に対して，1マスが10cmの幅の地図を作成したとすると以下のようになる． 0 4.5 0 4 0.1
2行目以降：障害物位置情報 行方向をY軸，列方向をX軸とし，各座標に障害物が存在するかを表す． つまり，反時計回りに90度回転すると右向きにX軸，上向きにY軸となる．
912の実験環境モデルと，それに対して10cm幅で作成した環境地図を以下に示す．
- 実行命令
- 事前にFloorSensingSystemを起動しておく必要がある.
rosrun tms_rp_rps_checker rps_map_checker
事前に作成した環境地図(管理場所はmap_publisherと同じ)に， FSSにより検出されたクラスタを障害物として追加して発行するノード． クラスタが検出された時にのみ地図情報を発行する．
(デモ用にロボットやワゴンを障害物から除外するように設定しているので，必要に応じて適宜変更してください．)
※利用時はrps_checkerからあらかじめ地図情報を発行しておくこと．
※基本的にSmartPal5を対象ロボットとした計画
・実行命令
rosrun tms_rp_rps_planner rps_goal_planner
以下の2種の作業位置計画を行うノード．
- ワゴン把持位置 通信路名：rps_grasp_wagon_pos_planning
- 物品受渡し位置 通信路名：rps_give_obj_pos_planning
対応するsrvは共通でtms_rp_msgs::rps_goal_planning
入力：target_id (対象とするワゴンのid 基本は22) 出力：goal_pos (把持可能位置x,y,th 0～4点)
以下の図のような対象ワゴンに対して4箇所の把持位置を求め，そのうち接触位置ではない点を出力する． ワゴンからの距離(Robot_Wagon_Control_Dist)は，rps/rps.h内で変更可能． !grasp_wagon_pos.png!
入力：target_id (対象とする人のid 基本は31) 出力：goal_pos (受け渡し候補位置x,y,th 0～k点 kはrps/rps.h内give_obj_pos_divide_numで変更可能)
対象者の位置情報をもとに以下の図のような物品手渡し位置を計画し，出力する． 対象者のperson_behaviorが4(椅子着座)であれば，椅子(id=23)の位置情報を基準とする． 対象者やロボットの使用するアームの左右切り替えや，腰を曲げるかどうかをrps/rps.h内で変更可能． (計画に用いる可操作度地図をrps_planner/manipilabilityMAP内に保持している)
give_obj_pos
- 実行命令
rosrun tms_rp_rps_planner rps_voronoi_path_planner
対応srv：tms_rp_msgs::rps_voronoi_path_planning 通信路名：rps_voronoi_path_planning
入力： robot_id ： 対象ロボットのid start_pos： 始点x,y,th goal_pos ： 終点x,y,th 出力： VoronoiPath ： 計画経路(x,y,thの配列)
指定した始点・終点間の移動経路を計画するノード． 発行されている地図情報を元にボロノイ図を作成し，その境界線上を基本経路とすることで障害物から最も離れた位置を通る経路を求める． ワゴン(id=22)のstateが1(停止)の場合，ワゴンの足を障害物として扱う． (この情報は発行されない，つまりtms_viewer上に障害物として反映されないので要注意) 接触範囲はrobot_idによって切り替えており，接触領域となる障害物からの距離をrps/rps.h内のxxx_Collision_Thresholdで管理している． あらかじめtms_viewerを立ち上げ，View::PathのRobotPathにチェックを入れておくと，下図のように計画経路がviewer上に表示される．
voronoi_path
※主にrps_voronoi_path_plannerを利用すること！ ※rps_voronoi_path_plannerを起動しておく必要あり
- 実行命令
rosrun tms_rp_rps_planner rps_path_planner
対応srv：tms_rp_msgs::rps_path_planning 通信路名：rps_path_planning
入力： robot_id ： 対象ロボットのid rps_goal_pos_candidate ： 終点x,y,thの配列 出力： rps_path ： 計画経路(x,y,thの配列)の配列
rps_voronoi_path_plannerとの仲介役のようなノード． 始点は対象ロボットの現在位置(tms_dbが保持している値)を設定． 入力した終点の数だけ経路を計画し，その分の計画経路を出力する．
正直あまり使い勝手は良くないのでrps_voronoi_path_plannerを直接利用する方がよいかと． rps_voronoi_path_plannerと機能はほとんど変わらないので利用者がいなくなれば削除してください．
*実行命令
rosrun tms_rp_rps_planner rps_push_wagon_path_planner
対応srv：tms_rp_msgs::rps_push_wagon_path_planning 通信路名：rps_push_wagon_path_planning
入力： robot_id ： 対象ロボットのid wagon_id ： 対象ワゴンのid start_robot_pos ： ロボットの初期位置 start_wagon_pos ： ワゴンの初期位置 goal_robot_pos ： ロボットの移動目標位置x,y,th 出力： RobotPath ： ロボット移動経路(x,y,thの配列) WagonPath ： ワゴン移動経路(x,y,thの配列) control_wagon_th ： ワゴン相対姿勢(th)の配列
指定した始点・終点間のワゴンを押すロボットの移動経路を計画するノード． ロボット経路を先に計画し，ロボット，ワゴンがともにボロノイ境界線を元にした基本経路上にくるようにロボット姿勢，およびワゴン相対姿勢を決定していく． そのため，計画結果のthは入力のthとは一致せしない． あらかじめtms_viewerを立ち上げ，View::PathのRobotPath，WagonPathにチェックを入れておくと，下図のように計画経路がviewer上に表示される．
wagon_path
デモ用に作成したものなので参考程度に
※SmartPal5のみ対応
*実行命令
rosrun tms_rp_rps_commander rps_robot_commander
対応srv：tms_rp_msgs::rps_robot_command 通信路名：rps_robot_command
入力： robot_id ： 対象ロボットのid param_array ： ロボットのパラメータの配列 出力： result ： 結果
入力されたパラメータ配列の変化に応じてロボットに動作命令を出すノード． パラメータのサイズは21で添字の順に以下のようになる． 0～2：ロボット位置(x,y,th) 3～4：腰の角度(3：Low 4：High) 5～11：右腕の各節角 12 ：右グリッパ角度 13～19：左腕の各関節 20 ：左グリッパ角度
#define SEND_COMMAND によりロボットへの出力切り替え可能 その際は事前にロボットを命令待機状態にしておくこと． ※命令は絶対座標系で扱うので動かす際は初期位置の設定に注意
物品運搬のデモ用動作計画のトリガとなるノード． rps_planner内の機能をひと通り利用しているので，各種計画部の呼び出し方の参考にしてください．
物品運搬のデモ用動作計画にもとづいて，choreonoidを利用しながら必要なアーム計画などを行いつつ，robot_commanderへ動作命令を出力するノード． 後述のcnoid_planner_interfaceが必要． choreonoidのplannerとの連携や，robot_commanderの呼び出し方の参考にしてください．