ROS演習7-2019：デッドレコニングを実装しよう！

今回はTurtlebot3にデッドレコニングを実装します。

デッドレコニング説明資料
- 以下の説明資料をダウンロードして読んでください。これをGazebo上のTurtlebot3に実装していきましょう。
  - deadReckoning.pdf
  - 講義７週PPT（学内のみダウンロード可能）
ROSの座標系
- ROSではロボットの進行方向がx軸、左方向がy軸、上方向がz軸の正方向です。回転方向は反時計周りが正(0~π[rad])、時計回りが負(0~-π[rad])となります。
テンプレートファイル

// ファイル名　my_odom3.cpp
#include <ros/ros.h>  // rosで必要はヘッダーファイル
#include <geometry_msgs/Twist.h> // ロボットを動かすために必要
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <gazebo_msgs/ModelStates.h> 
#include <sensor_msgs/JointState.h>
#include 

using namespace std;

// コールバック関数。並進、回転速度の表示。
void cbVel(const geometry_msgs::Twist::ConstPtr& vel) {
  cout << "Linear :" << vel->linear.x << endl;
  cout << "Angular:" << vel->angular.z << endl; } 

// /odomトピックから位置posと姿勢poseを表示 
void cbOdom(const nav_msgs::Odometry::ConstPtr& msg) { 
  ROS_INFO("Seq: %d", msg->header.seq);
  ROS_INFO("/odom Pos (x:%f, y:%f, z:%f)", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);

  tf::Quaternion q(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);  
  // tf::Quaternion q(quat.x, quat.y, quat.z, quat.w);
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  ROS_INFO("/odom Pose (roll:%f, pitch:%f, yaw:%f) ", roll, pitch, yaw);
  ROS_INFO("Vel (Linear:%f, Angular:%f)", msg->twist.twist.linear.x,msg->twist.twist.angular.z);
												  
}

// /gazebo/model_statesトピックから真の位置Pos(x,y,z)と姿勢Pose(roll, pitch ,yaw)を表示
void cbModelStates(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
  ROS_INFO("Real Pos (x:%f, y:%f, z:%f)", msg->pose[1].position.x,msg->pose[1].position.y, msg->pose[1].position.z);

  tf::Quaternion q(msg->pose[1].orientation.x, msg->pose[1].orientation.y, msg->pose[1].orientation.z, msg->pose[1].orientation.w);  
  // tf::Quaternion q(quat.x, quat.y, quat.z, quat.w);
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  ROS_INFO("Real Pose (roll:%f, pitch:%f, yaw:%f) ", roll, pitch, yaw);
												  
}

// cbMyOdom：この関数に自分のオドメトリを実装しよう！
// /joint_statesトピックから左右のjoint(車輪回転軸)の位置（回転角度)[rad]を表示
// 参考：Turtlebot3の車輪直径0.066 [m]
void cbMyOdom(const sensor_msgs::JointState::ConstPtr& jointstate)
{
  double wheel_right_joint_pos = jointstate->position[0]; // 右車軸の位置[rad]
  double wheel_left_joint_pos  = jointstate->position[1]; // 左車軸の位置[rad]

  // 車軸の位置は積算される
  ROS_INFO("Whell Pos (r:%f, l:%f)", wheel_right_joint_pos,wheel_left_joint_pos);
}




int main(int argc, char **argv)
{
  ros::init(argc, argv, "my_odom3");
  ros::NodeHandle nh;

  //subscriberの作成。トピック/cmd_velを購読する。
  ros::Subscriber sub  = nh.subscribe("/cmd_vel", 10, cbVel);
  ros::Subscriber sub2 = nh.subscribe("/odom", 100, cbOdom);
  ros::Subscriber sub3 = nh.subscribe("/gazebo/model_states", 100, cbModelStates);
  ros::Subscriber sub4 = nh.subscribe("/joint_states", 100, cbMyOdom);
  
  
  // コールバック関数を繰り返し呼び出す。
  ros::Rate rate(100);

  while (ros::ok()) {

    
    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}

以下ファイルをダウンロードして~/catkin_ws/srcの下にコピーする。
- my_odom3.tar
ビルドする
- $ cd ~/catkin_ws/src
- $ tar xvf my_odom3.tar
- $ cd ~/catkin_ws
- $ catkin build
rvizでの実行
- 端末を３つ開き、各端末で以下のコマンドを実行する。
- $ roslaunch turtlebot3_fake turtlebot3_fake.launch
- $ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
- $ rosrun my_odom3 my_odom3
gazeboでの実行
- 端末を３つ開き、各端末で以下のコマンドを実行する。
- $ roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch
- $ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
- $ rosrun my_odom3 my_odom3
- 以下のコマンドでノードとトピックの関係を見てみましょう。
  - $ rqt_graph
- 以下のように表示されれば成功です。

演習

準　備
- ホームディレクトリの名前が日本語の場合はコマンド操作がやりづらいので以下のコマンドで英語に変更する。以下のウインドウが開くので[UpdateNames]をクリックすると英語名に変わる。
  - $ LANG=C xdg-user-dirs-gtk-update
- $ cd
- fmt_world-2.tarをクリックしてダウンロードし~/Downloadsの中に保存。
- $ cd Downloads
- $ tar xvf fmt_world-2.tar
- $ cd fmt_world
- $ cp turtlebot3_fmt_world.launch ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch
- $ cp fmt.world ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worlds
- $ cp -r fmt ~/.gazebo/models
- $ roslaunch turtlebot3_gazebo turtlebot3_fmt_world.launch
- 下のような建物とロボットが表示されたら終わり。右奥のボールがゴール。
演　習（レポート２）
- 準備
  - 以下の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)