2010年2月11日木曜日

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の開発にドキュメントがついていけてない感じですね。。。

1 件のコメント:

  1. 本家編集しちゃいましたんで、本家コピペで動くようになってると思います。

    返信削除