演習課題 回答例

演習課題の回答例を掲載しています。

課題A URGのデータを使ったロボット制御

A-1 直進し、正面に壁が近づいたら停止する

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

#include <tf/transform_datatypes.h>

class RSJRobotTestNode
{
private:
  ros::Subscriber sub_odom_;
  ros::Subscriber sub_scan_;
  ros::Publisher pub_twist_;

  sensor_msgs::LaserScan latest_scan_;

  void cbOdom(const nav_msgs::Odometry::ConstPtr &msg)
  {
  }
  void cbScan(const sensor_msgs::LaserScan::ConstPtr &msg)
  {
    // 受け取ったメッセージをコピーしておく
    latest_scan_ = *msg;
  }
public:
  RSJRobotTestNode()
  {
    ros::NodeHandle nh;
    pub_twist_ = nh.advertise<geometry_msgs::Twist>(
        "cmd_vel", 5);
    sub_odom_ = nh.subscribe("odom", 5,
        &RSJRobotTestNode::cbOdom, this);
    sub_scan_ = nh.subscribe("scan", 5, 
        &RSJRobotTestNode::cbScan, this);
  }
  void mainloop()
  {
    ROS_INFO("Hello ROS World!");

    ros::Rate rate(10.0);
    while (ros::ok())
    {
      ros::spinOnce();

      if (latest_scan_.ranges.size() > 0)
      {
        // LaserScanメッセージをすでに受け取っている場合a

        geometry_msgs::Twist cmd_vel;
        int i = (-latest_scan_.angle_min) / latest_scan_.angle_increment;
        if (latest_scan_.ranges[i] < latest_scan_.range_min || // エラー値の場合
            latest_scan_.ranges[i] > latest_scan_.range_max || // 測定範囲外の場合
            std::isnan(latest_scan_.ranges[i])) // 無限遠の場合
        {
          // 正面に十分な距離がある (測定範囲以上)
          ROS_INFO("front-range: measurement error");
          cmd_vel.linear.x = 0.2;
          cmd_vel.angular.z = 0.0;
        }
        else
        {
          ROS_INFO("front-range: %0.3f", latest_scan_.ranges[i]);
          if (latest_scan_.ranges[i] > 0.5)
          {
            // 50cm以上距離がある
            cmd_vel.linear.x = 0.2;
            cmd_vel.angular.z = 0.0;
          }
          else
          {
            // 50cm以下になった
            cmd_vel.linear.x = 0.0;
            cmd_vel.angular.z = 0.0;
          }
        }
        pub_twist_.publish(cmd_vel);
      }

      rate.sleep();
    }
  }
};

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "rsj_robot_test_node");

  RSJRobotTestNode robot_test;

  robot_test.mainloop();
}

センサ正面方向の1点のみの距離を取得し、のこりの距離が0.5mより大きい場合は前進、0.5m以下になったら停止します。正面1方向のみでなく、距離を確認する範囲に幅を持たせると、より確実に障害物の手前で止まれるようになります。

A-2 曲がり角を含む壁に沿って走行する

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

#include <tf/transform_datatypes.h>

class RSJRobotTestNode
{
private:
  ros::Subscriber sub_odom_;
  ros::Subscriber sub_scan_;
  ros::Publisher pub_twist_;

  sensor_msgs::LaserScan latest_scan_;
  float left_prev_;
  geometry_msgs::Twist cmd_vel_;

  void cbOdom(const nav_msgs::Odometry::ConstPtr &msg)
  {
  }
  void cbScan(const sensor_msgs::LaserScan::ConstPtr &msg)
  {
    // 受け取ったメッセージをコピーしておく
    latest_scan_ = *msg;
  }
public:
  RSJRobotTestNode()
  {
    ros::NodeHandle nh;
    pub_twist_ = nh.advertise<geometry_msgs::Twist>(
        "cmd_vel", 5);
    sub_odom_ = nh.subscribe("odom", 5,
        &RSJRobotTestNode::cbOdom, this);
    sub_scan_ = nh.subscribe("scan", 5, 
        &RSJRobotTestNode::cbScan, this);
  }
  void mainloop()
  {
    ROS_INFO("Hello ROS World!");

    ros::Rate rate(10.0);
    left_prev_ = FLT_MAX;
    while (ros::ok())
    {
      ros::spinOnce();

      if (latest_scan_.ranges.size() > 0)
      {
        // LaserScanメッセージをすでに受け取っている場合

        float front = FLT_MAX, left = FLT_MAX;

        // theta-range 座標系から x-y 座標系に変換
        for(unsigned int i = 0; i < latest_scan_.ranges.size(); i ++)
        {
          if (!(latest_scan_.ranges[i] < latest_scan_.range_min ||
              latest_scan_.ranges[i] > latest_scan_.range_max ||
              std::isnan(latest_scan_.ranges[i])))
          {
            // 距離値がエラーでない場合

            float theta = latest_scan_.angle_min + i * latest_scan_.angle_increment;
            // x-y 座標系に変換
            float x = latest_scan_.ranges[i] * cosf(theta);
            float y = latest_scan_.ranges[i] * sinf(theta);

            if (fabs(y) < 0.25 && x > 0.05)
            {
              // ロボット正面方向のデータについて最小距離を計算
              if (front > x) front = x;
            }
            else if (fabs(x) < 0.25 && y > 0.25)
            {
              // ロボット左方向のデータについて最小距離を計算
              if (left > y) left = y;
            }
          }
        }

        if (front > 0.5)
        {
          // 正面の距離に余裕がある場合

          ROS_INFO("Following left wall (distance %0.3f)", left);
          cmd_vel_.linear.x = 0.1;

          if (left > 1.0) left = 1.0;
          // 角速度指令値を0に近づけるようにフィードバック
          cmd_vel_.angular.z += -cmd_vel_.angular.z * 0.01;
          // 左方向の距離を0.5mに近づけるようにフィードバック
          cmd_vel_.angular.z += (left - 0.5) * 0.02;
          // 距離の変化量(壁の向きを表す)を0に近づけるようにフィードバック
          if (left_prev_ < 1.0)
            cmd_vel_.angular.z += (left - left_prev_) * 4.0;

          if (cmd_vel_.angular.z > 0.3) cmd_vel_.angular.z = 0.3;
          else if (cmd_vel_.angular.z < -0.3) cmd_vel_.angular.z = -0.3;
        }
        else
        {
          ROS_INFO("Something in front");
          cmd_vel_.linear.x = 0.0;
          cmd_vel_.angular.z = -0.2;
        }
        left_prev_ = left;

        pub_twist_.publish(cmd_vel_);
      }

      rate.sleep();
    }
  }
};

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "rsj_robot_test_node");

    RSJRobotTestNode robot_test;

    robot_test.mainloop();
}

ロボットの左側約0.5mに壁がある状態からスタートすることで動作を確認できます。

左方向の物体までの距離を測定し、これを0.5mにする(つまり、壁から 0.5mの位置を走る)ように制御します。壁からの距離の誤差、距離の誤差の微分値(壁の向きに対するロボットの向きを表す)、角速度指令値に適当なゲインをかけて、角加速度にフィードバックしています。すなわち、これは、講義1中で説明した、直線追従制御系の簡易的な実装になっています。また、正面がふさがっている場合は、右に旋回することで、常に左の壁に沿って走行を続けます。

A-3 前を歩いている人について走行する

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

#include <tf/transform_datatypes.h>

class RSJRobotTestNode
{
private:
  ros::Subscriber sub_odom_;
  ros::Subscriber sub_scan_;
  ros::Publisher pub_twist_;

  sensor_msgs::LaserScan latest_scan_;

  void cbOdom(const nav_msgs::Odometry::ConstPtr &msg)
  {
  }
  void cbScan(const sensor_msgs::LaserScan::ConstPtr &msg)
  {
    // 受け取ったメッセージをコピーしておく
    latest_scan_ = *msg;
  }
public:
  RSJRobotTestNode()
  {
    ros::NodeHandle nh;
    pub_twist_ = nh.advertise<geometry_msgs::Twist>(
        "cmd_vel", 5);
    sub_odom_ = nh.subscribe("odom", 5,
        &RSJRobotTestNode::cbOdom, this);
    sub_scan_ = nh.subscribe("scan", 5, 
        &RSJRobotTestNode::cbScan, this);
  }
  void mainloop()
  {
    ROS_INFO("Hello ROS World!");

    ros::Rate rate(20.0);
    while (ros::ok())
    {
      ros::spinOnce();

      if (latest_scan_.ranges.size() > 0)
      {
        // LaserScanメッセージをすでに受け取っている場合

        float hx = 0, hy = 0;
        int hnum = 0;

        // theta-range 座標系から x-y 座標系に変換
        for(unsigned int i = 0; i < latest_scan_.ranges.size(); i ++)
        {
          if (!(latest_scan_.ranges[i] < latest_scan_.range_min ||
                latest_scan_.ranges[i] > latest_scan_.range_max ||
                std::isnan(latest_scan_.ranges[i])))
          {
            // 距離値がエラーでない場合

            float theta = latest_scan_.angle_min + i * latest_scan_.angle_increment;
            // x-y 座標系に変換
            float x = latest_scan_.ranges[i] * cosf(theta);
            float y = latest_scan_.ranges[i] * sinf(theta);

            if (fabs(y) < 0.5 && x > 0.05 && x < 1.05)
            {
              // ロボット正面方向、1m四方の領域にある点の重心を計算
              hx += x;
              hy += y;
              hnum ++;
            }
          }
        }
        geometry_msgs::Twist cmd_vel;
        if (hnum > 0)
        {
          hy /= hnum;
          hx /= hnum;
          float dir = atan2(hy, hx);
          float dist = hypotf(hy, hx);

          ROS_INFO("Following the object in front (dist %0.3f, dir %0.3f)",
              dist, dir);

          // 角加速度0.6rad/ssで、向きを最短時間制御
          cmd_vel.angular.z = sqrtf(2.0 * 0.6 * fabs(dir));
          if (dir < 0) cmd_vel.angular.z *= -1;
          if (cmd_vel.angular.z > 0.4) cmd_vel.angular.z = 0.4;
          else if (cmd_vel.angular.z < -0.4) cmd_vel.angular.z = -0.4;

          // 加速度0.3m/ssで、距離を最短時間制御
          cmd_vel.linear.x = sqrtf(2.0 * 0.3 * fabs(dist - 0.5));
          if (dist - 0.5 < 0) cmd_vel.linear.x *= -1;
          if (cmd_vel.linear.x > 0.2) cmd_vel.linear.x = 0.2;
          else if (cmd_vel.linear.x < -0.2) cmd_vel.linear.x = -0.2;
        }

        pub_twist_.publish(cmd_vel);
      }

      rate.sleep();
    }
  }
};

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "rsj_robot_test_node");

  RSJRobotTestNode robot_test;

  robot_test.mainloop();
}

広い部屋で、壁から離れた位置で実行し、ロボットの正面に立つことで動作を確認できます。

ロボットの正面、1m四方にある計測点の重心位置を求め、その方向を向くような回転方向の最短時間制御と、距離を0.5mにするような並進方向の最短時間制御を独立に行っています。こちらも、講義1中で説明した位置制御系の実装例となっています。

この実装では、正面方向にある点は、すべて追従対象として見てしまうため、壁などの他の物体があると、そちらに吸い寄せられてしまいます。追従する対象をトラッキングするように変更すれば、より多くの環境で動作できるようになります。

課題B(上級編) navigationパッケージの利用

A-3 ナビゲーションパッケージを利用して、複数の目的地に順番に移動する

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

class RSJRobotTestNode
{
private:
  ros::Publisher pub_goal_;
  tf::TransformListener tfl_;

  std::list<geometry_msgs::PoseStamped> goals_;
  geometry_msgs::PoseStamped current_goal_;

public:
  RSJRobotTestNode()
  {
    ros::NodeHandle nh;
    pub_goal_ = nh.advertise<geometry_msgs::PoseStamped>(
        "/move_base_simple/goal", 5, true);
  }
  void addGoal(const float x, const float y, const float yaw)
  {
    geometry_msgs::PoseStamped goal;
    goal.header.frame_id = "map";
    goal.pose.position.x = x;
    goal.pose.position.y = y;
    goal.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
    goals_.push_back(goal);
  }
  bool popGoal()
  {
    if (goals_.size() == 0) return false;

    current_goal_ = goals_.front();
    goals_.pop_front();
    ROS_INFO("Applying goal %0.3f %0.3f %0.3f",
        current_goal_.pose.position.x,
        current_goal_.pose.position.y,
        tf::getYaw(current_goal_.pose.orientation));
    pub_goal_.publish(current_goal_);

    return true;
  }
  void mainloop()
  {
    ROS_INFO("Hello ROS World!");

    if (!popGoal())
    {
      ROS_ERROR("No goal specified");
      return;
    }

    ros::Rate rate(10.0);
    while (ros::ok())
    {
      rate.sleep();
      ros::spinOnce();

      float x, y, yaw;
      try
      {
        tf::StampedTransform trans;
        tfl_.waitForTransform("map", "base_link", 
            ros::Time(0), ros::Duration(0.5));
        tfl_.lookupTransform("map", "base_link", 
            ros::Time(0), trans);
        x = trans.getOrigin().x();
        y = trans.getOrigin().y();
        yaw = tf::getYaw(trans.getRotation());
      }
      catch(tf::TransformException &e)
      {
        ROS_WARN("%s", e.what());
        continue;
      }

      float yaw_goal = tf::getYaw(current_goal_.pose.orientation);
      float yaw_error = yaw - yaw_goal;
      if (yaw > M_PI) yaw -= 2.0 * M_PI;
      else if (yaw < -M_PI) yaw += 2.0 * M_PI;

      if (hypotf(x - current_goal_.pose.position.x,
                 y - current_goal_.pose.position.y) < 0.15 &&
          fabs(yaw_error) < 0.3)
      {
        if (!popGoal())
        {
          ROS_INFO("Finished");
          return;
        }
        ROS_INFO("Next goal applied");
      }
    }
  }
};

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "rsj_robot_test_node");

  RSJRobotTestNode robot_test;

  // 行き先を追加
  robot_test.addGoal(0.3, -0.3, 0.0);
  robot_test.addGoal(0.2, 0.2, 1.57);

  robot_test.mainloop();
}

地図を作ってある環境で、「ROS navigationパッケージを使ってみよう」の章で行ったように、rsj_seminar_navigationパッケージのnavigation.launchを実行している状態で、上記プログラムを動かすと、main関数中で追加した位置・姿勢(x[m], y[m], yaw[rad])を順に訪れる動作を確認できます。

この実装では、障害物などがあってスタックしたような状態はチェックしていません。たとえば、タイムアウト処理を入れて、どうしてもたどり着けない場合は、あきらめて次に進む、などの処理を入れるとよいかもしれません。

また、スタックした場合の処理などは、actionlibというROS用のライブラリを使用するとうまく実装できます。http://wiki.ros.org/ja/actionlib