PR2を動かす（ロボットの台車を距離指定で動かす）

PR2の台車を距離指定で動かします。

http://www.ros.org/wiki/pr2/Tutorials/Using%20the%20base%20controller%20with%20odometry%20and%20transform%20information

前回のチュートリアルでは、速度指令を一瞬なげるというサンプルでした。
ロボットを動かしつづけるには速度指令を投げ続ける必要があります。
これは通信が途切れたときにロボットが停止するための安全設計です。

で、今回はodometry controllerを使ってオドメトリ（ロボットがこれまでの指令値によりどれくらい進んだと思っているか）をゲットします。
ただし、odometry controllerはtfを使って情報をアップデートしてくれるので、tfを使ってロボットがどれくらい移動したかを調べればいいだけです。

では前回のチュートリアルの続きからです。

$ roscd drive_base_tutorial
しましょう。

で、今回はtfライブラリを使うので、
manifest.xmlに

<depend package="tf" />

を追加しましょう。

今回はtf listenerを使うので、を使うには

#include <tf/transform_listener.h>

をインクルードして、

tf::TransformListener listener_;

を作成します。

tfについて勉強しておいてよかったですね。

まず、以下のようにしてtfのデータが発行されるのを待ちます。

listener_.waitForTransform("base_link", "odom", 
                            ros::Time(0), ros::Duration(1.0));

base_linkからodomへのtfを計算します。

で、以下のようにして欲しいデータを取ります。

listener_.lookupTransform("base_link", "odom", 
                            ros::Time(0), start_transform);

で、結局以下のようになるのですが、せっかくなので前回のソースを変更して作りましょう。

#include <iostream>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.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_;
  //! We will be listening ot TF transforms as well
  tf::TransformListener listener_;

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);
  }

  //! Drive forward a specified distance based on odometry information
  bool driveForwardOdom(double distance)
  {
    //wait for the listener to get the first message
    listener_.waitForTransform("base_link", "odom", 
                               ros::Time(0), ros::Duration(1.0));
    
    //we will record transforms here
    tf::StampedTransform start_transform;
    tf::StampedTransform current_transform;

//record the starting transform from the odometry to the base frame
    listener_.lookupTransform("base_link", "odom", 
                              ros::Time(0), start_transform);
    
    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;
    //the command will be to go forward at 0.25 m/s
    base_cmd.linear.y = base_cmd.angular.z = 0;
    base_cmd.linear.x = 0.25;
    
    ros::Rate rate(10.0);
    bool done = false;
    while (!done && nh_.ok())
    {
      //send the drive command
      cmd_vel_pub_.publish(base_cmd);
      rate.sleep();
      //get the current transform
      try
      {
        listener_.lookupTransform("base_link", "odom", 
                                  ros::Time(0), current_transform);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
        break;
      }
      //see how far we've traveled
      tf::Transform relative_transform = 
        start_transform.inverse() * current_transform;
      double dist_moved = relative_transform.getOrigin().length();

      if(dist_moved > distance) done = true;
    }
    if (done) return true;
    return false;
  }
};

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

  RobotDriver driver(nh);
  driver.driveForwardOdom(0.5);
}

そしてビルドして実行してみましょう。

見事指定距離だけ進みました。

でも、たまに
terminate called after throwing an instance of 'tf::LookupException'

  what():  Frame id /odom does not exist! When trying to transform between /odom and /base_link.

というエラーがでて死にます。

本当はこのwaitForTransformのところもtry catchすべきですね。

以前の例ではそうなっています。

うーん、どういうことなんだろう。

回転の例もあるので試してみます。

bool turnOdom(bool clockwise, double radians)
  {
    while(radians < 0) radians += 2*M_PI;
    while(radians > 2*M_PI) radians -= 2*M_PI;

    //wait for the listener to get the first message
    listener_.waitForTransform("base_link", "odom", 
                               ros::Time(0), ros::Duration(1.0));
    
    //we will record transforms here
    tf::StampedTransform start_transform;
    tf::StampedTransform current_transform;

    //record the starting transform from the odometry to the base frame
    listener_.lookupTransform("base_link", "odom", 
                              ros::Time(0), start_transform);
    
    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;
    //the command will be to turn at 0.75 rad/s
    base_cmd.linear.x = base_cmd.linear.y = 0.0;
    base_cmd.angular.z = 0.75;
    if (clockwise) base_cmd.angular.z = -base_cmd.angular.z;
    
    //the axis we want to be rotating by
    tf::Vector3 desired_turn_axis(0,0,1);
    if (!clockwise) desired_turn_axis = -desired_turn_axis;
    
    ros::Rate rate(10.0);
    bool done = false;
    while (!done && nh_.ok())
    {
      //send the drive command
      cmd_vel_pub_.publish(base_cmd);
      rate.sleep();
      //get the current transform
      try
      {
        listener_.lookupTransform("base_link", "odom", 
                                  ros::Time(0), current_transform);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
        break;
      }
      tf::Transform relative_transform = 
        start_transform.inverse() * current_transform;
      tf::Vector3 actual_turn_axis = 
        relative_transform.getRotation().getAxis();
      double angle_turned = relative_transform.getRotation().getAngle();
      if ( fabs(angle_turned) < 1.0e-2) continue;

      if ( actual_turn_axis.dot( desired_turn_axis ) < 0 ) 
        angle_turned = 2 * M_PI - angle_turned;

      if (angle_turned > radians) done = true;
    }
    if (done) return true;
    return false;
  }

こっちもたまに死にます。うーむ。
まずいなぁ。
最初のlookupTransformで死んでいるので、waitができていない見たいです。

タスクの優先度の問題でtfのほうが書き込めなくなっているのだろうか？

ということで最初のlookupTransformの前に

ros::Rate sleep(10.0);

sleep.sleep();

を入れるとうまくいきました。

tfの使い方のサンプルとしてコードの中身を見ておくといいですね。