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

この記事は私が金沢工業大学ロボティクス学科で担当している2018年度後学期開講の講義ロボットプログラミングⅡ用です。今回はTurtlebot3にデッドレコニングを実装します。

• テンプレートファイル

// ファイル名　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 <iostream>
using namespace std;

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



void callbackOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
  ROS_INFO("Seq: %d", msg->header.seq);
  ROS_INFO("Position (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("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);
												  
}


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

  //subscriberの作成。トピック/cmd_velを購読する。バッファ数は10。
  ros::Subscriber sub  = nh.subscribe("/cmd_vel", 10, callbackVel);
  ros::Subscriber sub2 = nh.subscribe("/odom", 1000, callbackOdom);
  
  // コールバック関数を繰り返し呼び出す。whileループが必要な場合はspinOnce()を使う。
  ros::Rate rate(100);

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


  return 0;
}

• 以下ファイルをダウンロードして~/catkin_ws/srcの下にコピーする。
  • my_odom.tar
• ビルドする
  • $ cd ~/catkin_ws/src
  • $ tar xvf my_odom.tar
  • $ cd ~/catkin_ws
  • $ catkin_make
• 実行
  • $ rosrun my_odom my_odom

演  習

• 準　備
  • $ cd
  • $ mkdir downloads
  • fmt_world.tarダウンロードして~/Downloadsの中に保存。
  • $ cd downloads
  • $ tar xvf  fmt_world.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
  • 下のような建物とロボットが表示されたら終わり。右奥のボールがゴール。