rostmsPlugin for choreonoid

先に起動するパッケージ

roslaunch tms_db_manager tms_db_manager.launch 

• Simulationの場合

rosrun tms_rc_smartpal_virtual_control smartpal_virtual_control

• RealTimeの場合

rosrun tms_rc_smartpal_control smartpal_control

choreonoidとtms_rp_actionの起動

cd ~/choreonoid/bin/
choreonoid 

Choreonoidのバーの説明

• Simulationの場合

1. Click the "smartpal5_1" in items box.
2. Click the "SetRobot" button.
3. Select "0", and click the "OK" button.
4. Click "simulation" button.
5. Call service like below.

rosservice call /rp_move "robot_id: 2002 x: 7000.0 y: 4000.0 theta: -90.0"

• RealTimeの場合　(環境の３Dビューアの機能を含めて)

1. Click the "smartpal5_1" in items box.
2. Click the "SetRobot" button.
3. Select "0", and click the "OK" button.
4. Click "standby" button.

rosservice call /rp_move "robot_id: 2002 x: 7000.0 y: 4000.0 theta: -90.0"

About Subtask(source: tms_rp_action/src/tms_rp_rp.cpp)

• before using these subtasks, please run db_manager, vicon_publisher and choreonoid

• in Simulation case: push "S" button, in ReamTime case: push "ROS"button

• srv file: rp_cmd.srv

int32 command // subtask's ID
bool type // non-use
int32 robot_id
float64[] arg
---
int32 result
float32[] val

実装済みサブタスク

move(command:9001) : move from present location to destination

:star: About argument

• 第1引数が「-1」のとき 座標値を与える場合.arg[-1,goal_x(mm), goal_y(mm), goal_yaw(deg)]

• robot_id(20012999) or furniture_id(60016999) DB【id_table】の該当IDの,etc_dataに格納された移動候補点(記述についてはDB_wiki参照)

• object_id(7001~7999) DB 【data_table】のplace情報に格納された家具のDB【id_table】etc_dataに格納された移動候補点

grasp(command:9002) : grab at an object

• argument :object_id(7001~7999)

• SmartPalVの場合,choreonoid上で右の第2関節を-10にする (choreonoid起動時には第2関節が0で,腕と胴体が接触しているため)

• KXPでgraspを行うとき(graspでchoreonoidがおちる場合)

~/catkin_ws/src/ros_tms/tms_rp/tms_models/robot/KXP/Plugin以下の ソースファイルとヘッダファイル,CMakeLists.txtを ~/choreonoid/extplugin/graspPlugin/RobotModels/KXP/Plugin以下にコピーしてコンパイルし, 生成された「.so」「.so.0」「.so.0.0.0」ファイルを ~/catkin_ws/src/ros_tms/tms_rp/tms_models/robot/KXP/Plugin以下に置き換えて 実行してみてください．

コンパイルするときは,choreonoidで ccmake .をし,GRASP_ROBOT_MODEL_PLUGINSに「KXP/Plugin」が追加されていることを確認してください．

give(command:9003) : give handling object to person

:star: 先に以下のノードを起動しておく.

• rosrun tms_rp_rps_planner rps_give

open(command:9004) : open the refrigerator

• arg情報は使いませんが,argが空だとエラーになる仕様なので,ダミーの値をいれてください． :star2: TSを介すと勝手に0が代入されます

close(command:9005) : close the refrigerator

• arg情報は使いませんが,argが空だとエラーになる仕様なので,ダミーの値をいれてください．

move2(command:9006) : kobuki's random walker

• kobukiドライバを立ち上げておく．
• kobukiのrandom walkerをsystem関数で起動

sensing(command:9007) : kinect's sensing for patrol

• kobuki,kinectのドライバを立ち上げておく．

Example

// move in front of the furniture
1. rosservice call /rp_cmd "command: 9001 robot_id: 2002 arg: [6011]"
 // arg[0]=-1: directly teach destination
 1'. rosservice call /rp_cmd "command: 9001 robot_id: 2002 arg: [-1, 7000, 4000, -90.0]"

2. rosservice call /rp_cmd "command: 9002 robot_id: 2002 arg: [7001]"
3. rosservice call /rp_cmd "command: 9003 robot_id: 2002 arg: [1001]"

How to add other subtasks

1. subtask関数に新しいサブタスクを追加(ID:9001~9999)
2. スレッドで呼び出すサブタスクの処理を実装

bool tms_rp::TmsRpSubtask::name_of_subtask(void) {
        // tsがサブタスクを実行しているスレッド数を管理する必要があるので,state_controlサービスを
	tms_msg_ts::ts_state_control s_srv;
	s_srv.request.type = 1; // for subtask state update;
	s_srv.request.state = 0; // 失敗

        // 実際の処理

        // エラーが起きたとき：TSに通知
	s_srv.request.error_msg = "Error message";
	state_client.call(s_srv);
	return false;

	// 正常終了時
	s_srv.request.state = 1; // 成功
	state_client.call(s_srv);
	return true;
}