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

デッドレコニングをRoombaに実装しましょう。

• ROSの座標系
  • ROSではロボットの進行方向がx軸、左方向がy軸、上方向がz軸の正方向です。回転方向は反時計周りが正(0~π[rad])、時計回りが負(0~-π[rad])となります。
• テンプレートファイル

// ファイル名　my_odom.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("/create1/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("/create1/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[2].position.x,msg->pose[2].position.y, msg->pose[2].position.z);

  tf::Quaternion q(msg->pose[2].orientation.x, msg->pose[2].orientation.y, msg->pose[2].orientation.z, msg->pose[2].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]を表示
// 参考：Roombaの車輪直径0.078 [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_odom");
  ros::NodeHandle nh;

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

  while (ros::ok()) {

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

• ファイルのダウンロード
  • • srcディレクトリへ移動する。
      • $ mkdir ~/src
      • $ cd ~/src
    • ダウンロードする。
      • 学内
        • $ git clone https://github.com/demulab/robot_programming2.git
      • 学外
        • $ git clone https://github.com/demulab/robot_programming2.git -c http.proxy=””
    • ~/catkin_ws/srcの下にコピーする。
      • $ cp -r ~/src/robot_programming2/my_odom ~/catkin_ws/src/.

   

• ビルドする
  • $ cd ~/catkin_ws
  • $ catkin build my_odom
• gazeboでの実行
• • 端末を３つ開き、各端末で以下のコマンドを実行する。
  • $ roslaunch ca_gazebo create_empty_world.launch
  • $ roslaunch ca_tools keyboard_teleop.launch
  • $ rosrun my_odom  my_odom
  • もう、一つ端末を開き、以下のコマンドでノードとトピックの関係を見てみましょう。
    • $ rqt_graph
  • 以下のように表示されれば成功です。
    

演  習

• • 準　備
    • ホームディレクトリの名前が日本語の場合はコマンド操作がやりづらいので以下のコマンドで英語に変更する。以下のウインドウが開くので[UpdateNames]をクリックすると英語名に変わる。
      • $ LANG=C xdg-user-dirs-gtk-update 
      • 
    • $ cd ~/src/robot_programming2/fmt_world
    • $ cp create_fmt_world.launch  ~/catkin_ws/src/create_autonomy/ca_gazebo/launch
    • $ cp  fmt.world  ~/catkin_ws/src/create_autonomy/ca_gazebo/worlds
    • $ cp -r fmt ~/.gazebo/models
    • $ roslaunch ca_gazebo create_fmt_world.launch
    • 下のような建物とロボットが表示されたら終わり。右奥のボールがゴール。
      
  • 演　習（レポート２）
    • 準備
      • 以下のROS C++スタイルガイドを参考にRobotクラスを作ろう。
        • ROS C++ スタイルガイド – ROS Wiki – ROS.org
    •  ウェイポイントナビゲーション
    • • スタート地点からゴールまで進むプログラムを作ろう。ロボットが通過するウェイポイントとその地点での姿勢を配列として実装しなさい。ロボットはウェイポイントで停止してもしなくても良いが、指定された姿勢を取ること。
    • デッドレコニング
      • デッドレコニングをcbMyOdom関数に実装しよう。rvizの場合は/odomトピックと値を比較し、gazeboが動く場合はシミュレータ上の真の位置Real Posと比較しよう。

終わり