ROS演習9-2019：ナビゲーションとアクションプログラム

この記事は私が金沢工業大学ロボティクス学科で担当している2019年後学期開講の講義ロボットプログラミングⅡ用です。ROS演習8ではrvizを使いGUIでロボットを動かしましたが、今回はROSのActionLibを使ったアクションプログラムを学びます。以下のチュートリアルも参考にしてください。

SendingSimpleGoals

ソース

 
// 本プログラムは
// http://wiki.ros.org/ja/navigation/Tutorials/SendingSimpleGoals
// を改変しています。
#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

struct MyPose {
  double x;
  double y;
  double yaw;
};

int main(int argc, char** argv){
  MyPose way_point[] = {{-0.15, -0.54, 0.0 * M_PI}, {1.6, -0.6, M_PI}, 
	                {1.6, 1.5,  0.0 * M_PI},{999, 999, 999}};

  ros::Publisher  pub;

  ros::init(argc, argv, "simple_goal");

  ros::NodeHandle nh;
  pub= nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 2, true);

  // 時間の取得
  ros::Time tmp_time = ros::Time::now();
  //create msg
  geometry_msgs::PoseWithCovarianceStamped initPose;

  // 初期位置の設定 turtlebot3_worldの初期位置が(-2.0, -0.5)なのでそれに設定
  initPose.header.stamp = tmp_time;  //  時間
  initPose.header.frame_id = "map";  //  フレーム
  initPose.pose.pose.position.x = -2.0;
  initPose.pose.pose.position.y = -0.5;
  initPose.pose.pose.position.z =  0;
  initPose.pose.pose.orientation.w = 1.0;

  // パブリッシュ amclパッケージに初期位置を送る
  pub.publish(initPose);
  
  // アクションクライアンを作成。1番目の引数は接続するアクションサーバー名。
  // アクションサーバーが立ち上がっていないとだめ。
  // ２番目の引数はtrueならスレッドを自動的に回す(ros::spin()。
  MoveBaseClient ac("move_base", true);
  // アクションサーバーが起動するまで待つ。引数はタイムアウトする時間(秒）。
  // この例では５秒間待つ(ブロックされる)
  while(!ac.waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the move_base action server to come up");
  }

  ROS_INFO("The server comes up");
  move_base_msgs::MoveBaseGoal goal;
  // base_link座標系（ロボット座標系)
  goal.target_pose.header.frame_id = "base_link";
  // 現在時刻                                                                       
  goal.target_pose.header.stamp = ros::Time::now();


  int i = 0;
  while (ros::ok()) {
    // ROSではロボットの進行方向がx座標、左方向がy座標、上方向がz座標
    goal.target_pose.pose.position.x =  way_point[i].x;
    goal.target_pose.pose.position.y =  way_point[i].y;
    goal.target_pose.pose.orientation.w = 1; 

    ROS_INFO("Sending goal: No.%d", i+1);
    // サーバーにgoalを送信
    ac.sendGoal(goal);

    // 結果が返ってくるまで30.0[s] 待つ。ここでブロックされる。
    bool succeeded = ac.waitForResult(ros::Duration(30.0));

    // 結果を見て、成功ならSucceeded、失敗ならFailedと表示
    actionlib::SimpleClientGoalState state = ac.getState();

    if(succeeded) {
      ROS_INFO("Succeeded: No.%d (%s)",i+1, state.toString().c_str());
    }
    else {
      ROS_INFO("Failed: No.%d (%s)",i+1, state.toString().c_str());
    }
    i++;  
  }
  return 0;
}

packageのディレクトリ毎、圧縮したファイル
- simple_goal
展開・ビルド方法
- $ cp simple_goal.tar ~/catkin_ws/src
- $ cd ~/catkin_ws/src
- $ tar xvf simple_goal.tar
- $ cd ~/catkin_ws
- $ catkin build

ハンズオン

上で展開したsimple_goalを次の要領で実行しよう。
- まず、geditで~/.bashrcを開き、最後の行に以下を追加し保存する。これにより、毎回入力する手間がはぶける。
  - export TURTLEBOT3_MODEL=burger
- まず、端末を開き次のコマンドを実行。
  - $ roslaunch turtlebot3_gazebo turtlebot3_world.launch
- この地図map.tarファイルをダウンロードして、次のようにホームディレクトリで展開してください。
  - $ cd
  - $ tar xvf maps.tar
  - 展開するとmapsディレクトリができ、その中に、mymap.pgmとmymap.yamlの２つのファイルがあります。pgmファイルは地図画像ファイルで、yamlファイルは設定ファイルです。mymap.yamlファイルの1行目に画像ファイルの場所を指定する必要があります。ダウンロードしたファイルは以下のようにimage: /home/demulab/maps/mymap.pgmになっています。demulabの部分を自分のユーザ名に変更、保存してください。
- ２個目の端末を開き次のコマンドを実行。
  - $ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/maps/mymap.yaml
- ３個目の端末を開き、catkin_makeして生成したsimple_goalノードを起動する。
  - $ rosrun simple_goal simple_goal

演　習（レポート２：ROS演習7-2019の演習と同じ）
- 準備
  - 以下のROS C++スタイルガイドを参考にRobotクラスを作ろう。
    - ROS C++ スタイルガイド – ROS Wiki – ROS.org
- 基本動作
  1. Turtlebot3を指定速度[m/s]で直進する以下のメンバ関数を作ろう。
    - void Robot::moveAtSpeed(double linear_vel)
  2. Turtlebot3を指定角速度[rad/s]で回転する以下のメンバ関数を作ろう。
    - void Robot::turnAtSpeed(double ang_vel)
  3. Turtlebot3を指定速度[m/s]で指定の距離[m]だけ直進して停止する以下のメンバ関数を作ろう。
    - void Robot::moveToDistance(double linear_vel, double dist)
  4. Turtlebot3を指定角速度[°/s]で指定の角度[°]だけ回転して停止する以下のメンバ関数を作ろう。
    - void Robot::turnToAngle(double ang_vel, double angle)
  5. Turtlebot3を指定位置（ロボット座標系）へ移動する以下のメンバ関数を作ろう。なお、ROSの座標系なのでロボットの進行方向がx、左方向がy軸の正方向です。
    - void Robot::moveToPoint(double x, double y)
- ウェイポイントナビゲーション
  - スタート地点からゴールまで進むプログラムを作ろう。ロボットが通過するウェイポイントとその地点での姿勢を配列として実装しなさい。ロボットはウェイポイントで停止してもしなくても良いが、指定された姿勢を取ること。
- デッドレコニング
  - デッドレコニングをcbMyOdom関数に実装しよう。rvizの場合は/odomトピックと値を比較し、gazeboが動く場合はシミュレータ上の真の位置Real Posと比較しよう。
ヒント
- Gazeboを起動するとTurtlebot3に速度指令を送らなくても滑って動く場合があります。その場合は次のパラメータを変更してみてください。
  - turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro
    - 11,12, 21,22行目パラメータ摩擦係数mu1,mu2を0.1から1以下の大きな値。
      - <mu1>1.0</mu1>
        <mu2>1.0</mu2>
    - mu1, mu2を変化させてもスリップする場合はkp, kdのパラメータを変えましょう。gazeboは動力学計算にODEを使っており、ODEのdt(時間ステップ）、erp、cfmとは次の関係があります。
      - kp = erp/ (dt* cfm)
      - kd = (1.0- erp)/cfm
    - kp, kd, dtからerp, cfmを求める式
      - erp = dt * kp / (dt * kp + kd)
      - cfm = 1.0 / (dt * kp + kd)

終わり