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

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

デッドレコニング説明資料
- 以下の説明資料をダウンロードして読んでください。これをGazebo上のTurtlebot3に実装していきましょう。
  - deadReckoning.pdf
テンプレートファイル

// ファイル名　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_make
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
- 以下のように表示されれば成功です。

演習

準　備
- ホームディレクトリの名前が日本語の場合はコマンド操作がやりづらいので以下のコマンドで英語に変更する。
  - $ 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
- 下のような建物とロボットが表示されたら終わり。右奥のボールがゴール。
演　習（レポート２）
- 基本動作
  - Turtlebot3を指定の距離[m]だけ直進して停止するプログラムを作ろう
  - Turtlebot3を指定の角度[°]だけ回転して停止するプログラムを作ろう
  - Turtllebot3を矩形軌道を移動するプログラムを作ろう
- ウェイポイントナビゲーション
  - スタート地点からゴールまで進むプログラムを作ろう。
- デッドレコニング
  - デッドレコニングを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)

終わり