PR2を動かす（ロボットの台車を速度指令で動かす）

今回は台車（車輪）を動かします。
http://www.ros.org/wiki/pr2/Tutorials/Using%20the%20robot%20base%20controllers%20to%20drive%20the%20robot

台車の操作には以下の３つのインタフェースがあります。

• モータへの指令値
• odometry controllerへの入力
• 上位の速度指令

で、今回は最後の上位からの速度指令をやります。

前回のチュートリアルのように、ターミナルを２つ用意して、

$ roslaunch gazebo empty_world.launch

$ roslaunch pr2_gazebo pr2.launch

としてロボットを表示します。

また、もう一つターミナルを用意して、

$ rosrun pr2_controller_manager pr2_controller_manager list

とすると以下のように表示されます。

head_traj_controller ( running )

l_arm_controller ( stopped )

l_gripper_controller ( stopped )

laser_tilt_controller ( running )

pr2_base_controller ( running )

pr2_base_odometry ( running )

r_arm_controller ( stopped )

r_gripper_controller ( stopped )

pr2_base_controllerが動いていますね。

base_controllerはTwistというメッセージをcmd_velというトピックで受け取ります。

単位は m/s, rad/s です。

これを使って台車を動かします。

これで受け付けた速度で一瞬だけ動くので、一定速度でロボットを動かしつづけるためには、このトピックに書き込み続けないといけません。

ではチュートリアル用パッケージを作りましょう。

$ roscd sandbox

$ roscreate-pkg drive_base_tutorial roscpp geometry_msgs

$ roscd drive_base_tutorial

では以下をsrc/drive_base.cpp

として保存しましょう。

今回は速度制御だけです。次回は移動距離を指定する操作方法をやります。

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>class RobotDriver
{
private:
  //! The node handle we'll be using
  ros::NodeHandle nh_;
  //! We will be publishing to the "cmd_vel" topic to issue commands
  ros::Publisher cmd_vel_pub_;

public:
  //! ROS node initialization
  RobotDriver(ros::NodeHandle &nh)
  {
    nh_ = nh;
    //set up the publisher for the cmd_vel topic
    cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  }

  //! Loop forever while sending drive commands based on keyboard input
  bool driveKeyboard()
  {
    std::cout << "Type a command and then press enter.  "
      "Use '+' to move forward, 'l' to turn left, "
      "'r' to turn right, '.' to exit.\n";

    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;

    char cmd[50];
    while(nh_.ok()){

      std::cin.getline(cmd, 50);
      if(cmd[0]!='+' && cmd[0]!='l' && cmd[0]!='r' && cmd[0]!='.')
      {
        std::cout << "unknown command:" << cmd << "\n";
        continue;
      }

      base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0;   
      //move forward
      if(cmd[0]=='+'){
        base_cmd.linear.x = 0.25;
      } 
      //turn left (yaw) and drive forward at the same time
      else if(cmd[0]=='l'){
        base_cmd.angular.z = 0.75;
        base_cmd.linear.x = 0.25;
      } 
      //turn right (yaw) and drive forward at the same time
      else if(cmd[0]=='r'){
        base_cmd.angular.z = -0.75;
        base_cmd.linear.x = 0.25;
      } 
      //quit
      else if(cmd[0]=='.'){
        break;
      }
      
      //publish the assembled command
      cmd_vel_pub_.publish(base_cmd);
    }
    return true;
  }

};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "robot_driver");
  ros::NodeHandle nh;

  RobotDriver driver(nh);
  driver.driveKeyboard();
}

コンストラクタで初期化子を使っていませんね。。。なんでだろ。

Twistの定義は

$ roscd geometry_msg

して、

$ cat msg/Twist.msg

するとみれます。

Vector3  linear

Vector3  angular

ですね。

$ cat msg/Vector3.msg

は

float64 x

float64 y

float64 z

となっていますね。

ではビルドしましょう。以下をCMakeLists.txtに追加してmakeです。

rosbuild_add_executable(drive_base src/drive_base.cpp)

$ cd bin

$ ./drive_base

して、

l

などしてエンターすると動きます。他にr, +が使えます。

とりあえずグリグリして遊びましょう。

今回はこれで終わりです。

とりあえずロボットの台車の速度制御ができますね。

こういうロボットの指令の仕方を標準化したほうがいいと思うのですが、

そういうのはどう考えているんでしょうね。

Twistというメッセージで標準化するのか、

Twistというメッセージでcmd_velというトピック名まで決めるのか、

標準化する気がないのか・・・？

取り合えず新しいロボットを作ったらこのインタフェースにそろえたほうがいいかもしれませんね。