2010年1月6日水曜日

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

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

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

  • モータへの指令値
  • odometry controllerへの入力
  • 上位の速度指令
で、今回は最後の上位からの速度指令をやります。

前回のチュートリアルのように、ターミナルを2つ用意して、
$ 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というトピック名まで決めるのか、
標準化する気がないのか・・・?

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

1 件のコメント:

  1. http://ros-robot.blogspot.com/2010/02/pr2.html
    を参照ください。

    また、トピックの名前は実行時に変更できるので、型さえ標準化してしまえば問題ないですね。
    台車速度はTwist型でたぶん不変ですから。

    返信削除