PR2のグリッパを動かす

今回はPR2のグリッパです。

http://www.ros.org/wiki/pr2/Tutorials/Moving%20the%20gripper

今回も上位レベルからの操作です。
直接動かすのはできるだろ？
今回はもうちょっと賢くやるぜ。
って書いてあるけど、直接動かす方法わかんないですね。
まあ、そのうち分かるでしょうから気にせず進みます。

まずは必要なソフトをmakeします。


$ rosdep install pr2_gripper_action
$ rosmake pr2_gripper_action

で、チュートリアル用パッケージsimple_gripperを作ります。
$ roscd sandbox

$ roscreate-pkg simple_gripper roscpp actionlib pr2_controllers_msgs

$ roscd simple_gripper

で、まずシミュレータを走らせます。

$ roslaunch gazebo empty_world.launch
$ roslaunch pr2_gazebo pr2.launch
ですね。

で、今回はgripper_controllerを使います。
で、このコントローラーが生きているか確かめます。
以前もやりましたが、

$ rosrun pr2_controller_manager pr2_controller_manager list
とすると、

r_gripper_controller ( stopped )
っていう感じになっていると思います。

r_gripper_controllerがロードされているが、動いていない状態です。
これを動かすために

$ rosrun pr2_controller_manager pr2_controller_manager start r_gripper_controller

としましょう。
もっかい、listで確かめてみると、

r_gripper_controller ( running )

になっていますね。

pr2_controller_managerはこんな感じで各種のコントローラーを統括するプログラムのようです。

チュートリアルには

$ rosnode list r_gripper_controller

とすると、/r_gripper_controller/gripper_action_node

が見えるはず、でなきゃおかしいぞ、って言ってますが、

rosnode: error: invalid arguments 'r_gripper_controller'

というエラーがでますね。

rosnode listの使い方を見てもこういう利用法はないので、今回は気にせず先に進みます。

（チュートリアルは対象バージョンが古いのかな）

どちらにせよ、
$ rosnode list
すると、

/r_gripper_controller/gripper_action_node
がありますね。

では以下のソースをsrc/simple_gripper.cppとして保存してビルドしましょう。

#include <ros/ros.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <actionlib/client/simple_action_client.h>

// Our Action interface type, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

class Gripper{
private:
  GripperClient* gripper_client_;  

public:
  //Action client initialization
  Gripper(){

    //Initialize the client for the Action interface to the gripper controller
    //and tell the action client that we want to spin a thread by default
    gripper_client_ = new GripperClient("r_gripper_controller/gripper_action", true);
    
    //wait for the gripper action server to come up 
    while(!gripper_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
    }
  }

  ~Gripper(){
    delete gripper_client_;
  }

  //Open the gripper
  void open(){
    pr2_controllers_msgs::Pr2GripperCommandGoal open;
    open.command.position = 0.08;
    open.command.max_effort = -1.0;  // Do not limit effort (negative)
    
    ROS_INFO("Sending open goal");
    gripper_client_->sendGoal(open);
    gripper_client_->waitForResult();
    if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper opened!");
    else
      ROS_INFO("The gripper failed to open.");
  }

  //Close the gripper
  void close(){
    pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
    squeeze.command.position = 0.0;
    squeeze.command.max_effort = 50.0;  // Close gently
    
    ROS_INFO("Sending squeeze goal");
    gripper_client_->sendGoal(squeeze);
    gripper_client_->waitForResult();
    if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper closed!");
    else
      ROS_INFO("The gripper failed to close.");
  }
};

int main(int argc, char** argv){
  ros::init(argc, argv, "simple_gripper");

  Gripper gripper;

  gripper.open();
  gripper.close();

  return 0;
}

ではソースを見ていきますか。
今回はactionlibを使っていますね。



typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

としてactionlibをtypedefして使っています。

//Initialize the client for the Action interface to the gripper controller
    //and tell the action client that we want to spin a thread by default
    gripper_client_ = new GripperClient("r_gripper_controller/gripper_action", true);
    
    //wait for the gripper action server to come up 
    while(!gripper_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
    }

ここでActionClientを立ち上げて接続を待ちます。
r_gripper_controller/gripper_action
というアクションに接続していますね。

pr2_controllers_msgs::Pr2GripperCommandGoal open;
    open.command.position = 0.08;
    open.command.max_effort = -1.0;  // Do not limit effort (negative)

この辺でactionlibのゴール設定をしています。
$ rosmsg show Pr2GripperCommandGoal
とすると確かに

[pr2_controllers_msgs/Pr2GripperCommandGoal]:
pr2_controllers_msgs/Pr2GripperCommand command
  float64 position
  float64 max_effort

となっています。

ROS_INFO("Sending squeeze goal");
    gripper_client_->sendGoal(squeeze);
    gripper_client_->waitForResult();
    if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper closed!");
    else
      ROS_INFO("The gripper failed to close.");

このへんで指令を実際に投げて結果を待ちます。

そしていつものようにビルドします。
以下をCMakeLists.txtに追加してmakeですね。

rosbuild_add_executable(simple_gripper src/simple_gripper.cpp)

$ rostopic echo /r_gripper_controller/gripper_action/feedback
としておいて、
他のターミナルでbin/simple_gripperを実行しましょう。

おー、開いたり閉じたりしてます。
ちょっとうれしい。

非常に簡単でしたね。
今回は以上です。