PR2の台車が動かない？

以前、PR2の台車を動かすチュートリアルを紹介しました。
記事：http://ros-robot.blogspot.com/2010/01/pr2_06.html
本家チュートリアル：http://www.ros.org/wiki/pr2/Tutorials/Using%20the%20robot%20base%20controllers%20to%20drive%20the%20robot

これが現在動きません。

この原因を探っていくと、
~/ros/pkgs/pr2_controllers/pr2_mechanism_controllers/src/
にある
pr2_base_controller.cppの、


  //  cmd_sub_deprecated_ = root_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pr2BaseController::commandCallback, this);
  cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController::commandCallback, this);

という記述があります。

以前は

cmd_vel

というトピックで動かされていたのですが、これがdeprecatedになり、

現在は

command

というトピックに変更になったようです。

また、ネームスペースも切られていて、

$ rostopic list

とすると

/base_controller/command

というのがあります。

試しに動かすために、

$ rostopic pub /base_controller/command geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0]' 

とすると、ロボットが動きました。

ということで、このチュートリアルをどう変更すればいいか分かりますか？

私は以下のようにしました。

#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>("command", 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 = ros::NodeHandle("base_controller");

  RobotDriver driver(nh);

  driver.driveKeyboard();

}

赤いところが変更したところです。

ROSの開発にドキュメントがついていけてない感じですね。。。