RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_016日目_SLAM_Google CartoGrapher_オドメトリの調整と地図生成と位置推定

zumo32u4　

◆ 前回記事

RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_015日目_SLAM_自律走行の前準備_CartoGrapherによる自宅の地図作成 【成功！！】　の続き

◆ はじめに

前回 は RaspberryPi(Raspbian Stretch) で Google CartoGrapher のオドメトリを無効にしてLiDARの点群データだけで高精度の地図を生成したが、今回は将来的にナビゲーションを実施することを見据えて、真面目にオドメトリを調整しようと思う。

＜機体の外観＞

結果を先に公開するが、オドメトリを使用したうえでも下図のような動きへ大幅に改善した。

機体のプロットを正しく矯正できている。
突如ワープしたり、大きく明後日の方向へ移動したりはしなくなった。
角度、移動距離、共に申し分ない。
足回りのセンサー精度が若干あやしく、ほんの少しだけ歪んだ地図になってしまったが、かなりの精度で地図生成と自己位置推定を同時に実施できるようになった。
地図単体で見ると、LiDARのみで生成したときのほうが精度が高かった ため、要件によっては
（１） LiDAR単体で綺麗に地図生成
（２） LiDARとオドメトリ、IMUで位置推定
を別々の段取りで実施するのが良策かもしれない。
Google CartoGrapher の性能が良いので、自動で誤差を補正してくれる仕様にはかなり助けられている。

● 地図生成環境の概要図

　

● 環境

1. Ubuntu 16.04 (rvizによる地図可視化用PC)
2. Windows10 + TeraTerm (ロボット操作用PC)
3. Zumo32u4 (ロボット本体)
4. RaspberryPi3 + Raspbian Stretch (ロボット本体、CartoGrapher)
5. RPLidar A1M8 (ロボット本体、測域)
6. ELP-USB8MP02G-L75 (USBカメラ、動画撮影用)

◆ 作業に掛かる前の前提条件

Raspbian Stretch+NCS(Neural Compute Stick)+YoloV2+Webカメラ＋ROSによるリアルタイム複数動体検知環境の構築 と ００２日目 ００３日目 ００４日目 ００５日目 ００６日目 ００８日目 ００９日目 ０１４日目 ０１５日目 の記事を参考に、全ての作業を実施済みであり、導入済みパッケージの種類と数の同期がとれていることを前提とする。

◆ センサー値や物理仕様の関連性の理解

もはや、算数程度の数値計算の理解ぐらいは避けて通れないと思った。
大したことは出来ないが、メーカーの部品表や設計図を読み漁り、自分にとって分り易い簡単な絵を書いて、プログラム中に記載してある数式の意味を一つづつ紐付けしながら、チューニングすべき箇所を特定していった。

まずは、距離・スピード・回転角・機体の向き、を正しく計算するために、部品単位のサイズや、出力仕様を整理する必要がると考えたため、稚拙だが下記のようにラフに整理してみた。

ロボットの機体・部品仕様の再整理

モーターの減速比： 100.37：1
モーターの減速比 ＝ モーター100.37回転でタイヤ(スプロケット)が1回転

スプロケットサイズ ＝ 35mm
スプロケット ＋ キャタピラ風ゴムの厚み ＝ 39mm ＝ タイヤの直径(自重による沈み込みは非考慮)

タイヤ１回転で進む距離 ＝ タイヤの直径 × 3.14(円周率)
タイヤ１回転で進む距離 ＝ 39mm × 3.14(円周率) ＝ 122.46mm

エンコーダ分解能 ＝ 12パルス(エンコーダ1回転)

タイヤ１回転あたりのパルス数 ＝ 100.37 × 12 ＝ 1204.44パルス(エンコーダカウント)

　　　１パルスで進む距離 ＝ 122.46mm / 1204.44パルス(エンコーダカウント) ＝ 0.10mm
　　１０パルスで進む距離 ＝ 1mm
　１００パルスで進む距離 ＝ 1cm(10mm)
１０００パルスで進む距離 ＝ 10cm(100mm)

左右のタイヤ間の距離：84mm (左右のタイヤを半分に輪切りにしたときの中心部同士の距離)

delta(差分時間)：直近センサー値取得からの経過時間
theta(角度)：回転角度

Roll(ロール)：前後方向の軸に対しての回転（側転のイメージ）
Pitch(ピッチ)：左右方向の軸に対しての回転（前転・後転のイメージ）
Yaw(ヨー)：上下方向の軸に対しての回転（右折・左折のイメージ）

tf.transformations.quaternion_from_euler(Roll, Pitch, Yaw)
↓ Yaw方向に対する回転のみTF通知
tf.transformations.quaternion_from_euler(0, 0, self.theta)

3.14ラジアン＝180°＝全周の半分＝0.042m×2×3.14＝0.26376m
1.57ラジアン＝90°＝４分の１周＝0.13188m

1ラジアン＝0.084m

タイヤ１周＝100.37×12エンコーダカウント
タイヤ外周＝1204.44カウント＝0.039m×3.14＝0.12246m

タイヤ１周＝0.12246m÷0.084m＝
1.4579ラジアン


90°(４分の１周)はタイヤ何回転か＝0.13188m÷0.12246m＝1.0769回転
何カウントか＝1.0769回転×1204.44＝1,297.0614カウント
何ラジアンか＝1.4579×1.0769＝1.57ラジアン

180°(半周)はタイヤ何回転か＝0.26376m÷0.12246m＝2.1538回転
何カウントか＝2.1538回転×1204.44＝2,594.1229カウント
何ラジアンか＝1.4579×2.1538＝3.14ラジアン

ラジアンは、半径を１としたときの円周の長さを角度と言い直した単位
90°＝1/4×2πラジアン
180°＝1/2×2πラジアン
270°＝3/4×2πラジアン
360°＝1×2πラジアン
450°＝5/4×2πラジアン

左右車輪が排他的に動く場合のラジアン計算
前進時の左右エンコーダ平均＝左エンコーダ正、右エンコーダ正→左右の絶対値を足して左輪の符号を付与÷２
後進時の左右エンコーダ平均＝左エンコーダ負、右エンコーダ負→左右の絶対値を足して左輪の符号を付与÷２
右折時の左右エンコーダ平均＝左エンコーダ正、右エンコーダ負→左右の絶対値を足して左輪の符号を付与÷２
左折時の左右エンコーダ平均＝左エンコーダ負、右エンコーダ正→左右の絶対値を足して左輪の符号を付与÷２

数式化
絶対値：abs(x)
((a>0)×1+(a<0)×-1)×(abs(a)+abs(b))÷2
((a>0)-(a<0))×(abs(a)+abs(b))÷2

エンコーダ１カウントあたりのラジアン＝3.14÷2,594.1229＝0.001210428388＝0.00121043

回転角(ラジアン)＝((a>0)-(a<0))×(abs(a)+abs(b))÷2×0.00121043

　
　©2014 ACE COMBAT INFINITY

Pythonのロジック上の定数や数式には大きな問題となりそうな箇所は見当たらなかったが、調べを進めて新たに理解したことは下記。

1. Zumo32u4に付属するエンコーダは 符号付きInt16 で値を保持する
2. 符号付きInt16 の最大値・最小値は　32,767 〜 -32,768
3. 機体を前進するとエンコーダ値は 0 から 1ずつ増えて、32,767 まで到達すると、オーバーフローして突然 -32,768 にワープする → 地図上で突如ワープして迷子になっていた原因
4. エンコーダのカウントがオーバーフローして突如ワープすることにより、回転角の計算や距離の計算がブッ飛んだ値になる
5. かたや、Python2.7 のfloat型はかなりの範囲の有効桁数を取りうる

ということで、対策を講ずる一筋の光がチラリと見えた気がした。

◆ オドメトリの調整

(1) 限界ギリギリまで各種変数がオーバーフローしないように仕様を見直す
(2) オーバーフローする前提の仕様へ変更する
のいずれかで本質的な対策ができそうだ。
小型ロボットごときで短時間では使い切れないほどの幅を持つPythonのfloat型のレンジを有効活用すれば、実用に十分耐えられる仕様になる、と判断し、(1)を採用することにした。

1.Arduinoのエンコーダから、積算値 ではなく ある断面の瞬発的な値 を取るように変更する。

zumo32u4arduino.ino

newLeft = encoders.getCountsAndResetLeft();
newRight = encoders.getCountsAndResetRight();

2.RaspberryPi側のPythonでは、 エンコーダから 積算値 を受け取ることを期待するのではなく、 ある断面の瞬発的な値 を連続で受信し、自分でfloat型へ積算する。

zumo32u4.py

VR=(float(self.sensorvalue[10])-float(self.odomR))/self.COUNT*3.14*self.DIAMETER/self.delta
VL=(float(self.sensorvalue[9])-float(self.odomL))/self.COUNT*3.14*self.DIAMETER/self.delta
self.o.pose.pose.position.x += self.delta*(VR+VL)/2*cos(self.theta)
self.o.pose.pose.position.y += self.delta*(VR+VL)/2*sin(self.theta)
rad = 0.0
if (self.odomL>0.0 and self.odomR<0.0) or (self.odomL<0.0 and self.odomR>0.0):
    rad = ((self.odomL>0)-(self.odomL<0))*(abs(self.odomL)+abs(self.odomR))/2*self.RADIANPERENCODER
self.theta += rad

3.Google CartoGrapher で オドメトリ と IMU の使用を有効化する。

backpack_2d.lua

use_odometry = true,

TRAJECTORY_BUILDER_2D.use_imu_data = true

◆ 修正を加えた各ファイル

zumo32u4.py の内容

zumo32u4.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import traceback
import serial
from math import sqrt, cos, sin
import rospy
import tf
from time import sleep
from geometry_msgs.msg import Twist, Pose
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from std_msgs.msg import String


class Zumo:
    def __init__(self):
        self.DIAMETER=0.039              #[Meter]  Diameter of tire
        self.INTERAXIS=0.084             #[Meter]  Distance between left and right tires
        self.COUNT=12                    #[Count]  Resolution of encoder
        self.RADIANPERENCODER=0.00121043 #[Radian] Radian per encoder count 1
        self.temps=0.0                   #[Second] Last measured time
        self.delta=0.0                   #[Second] Elapsed time
        self.theta=3.14                   #[Radian] Radian
        self.command=""                  #[String] Robot operation command
        self.odomL=0
        self.odomR=0

        try:
            self.PORT = rospy.get_param('BLUETOOTH_PORT') 
        except:
            rospy.set_param('BLUETOOTH_PORT',"/dev/rfcomm0")
            self.PORT = rospy.get_param('BLUETOOTH_PORT')

        try:
            self.BAUDRATE = rospy.get_param('BLUETOOTH_BAUDRATE') 
        except:
            rospy.set_param('BLUETOOTH_BAUDRATE', "115200")
            self.BAUDRATE = rospy.get_param('BLUETOOTH_BAUDRATE')

        self.TIMEOUT = 0.01
        self.sensorvalue = list()
        self.speed = []
        self.angle = []

        # Init Odometry
        self.o = Odometry()
        self.o.pose.pose.position.x = 0.0
        self.o.pose.pose.position.y = 0.0
        self.o.pose.pose.position.z = 0.0
        self.o.pose.pose.orientation.z = 3.14
        self.o.header.stamp = rospy.Time.now()
        self.o.header.frame_id = "odom"
        self.o.child_frame_id = "base_link"

        # Init IMU
        self.p = Imu()
        self.p.header.stamp = rospy.Time.now()
        self.p.header.frame_id = "imu_link"

        try:
            self.ser = serial.Serial(self.PORT, self.BAUDRATE, timeout = self.TIMEOUT)
            sleep(1)
            rospy.loginfo("Serial connection established on the port "+str(self.PORT))
        except:
            rospy.logwarn("Serial connection failure")

        self.pub_comm      = rospy.Publisher('command', String, queue_size=10)
        rospy.loginfo("Publisher initialization success /command")
        self.pub_imu       = rospy.Publisher('imu', Imu, queue_size=10)
        rospy.loginfo("Publisher initialization success /imu")
        self.pub_odom      = rospy.Publisher('odom', Odometry, queue_size=10)
        rospy.loginfo("Publisher initialization success /odom")
        self.sub_sensorval = rospy.Subscriber('sensorval', String, self.subsensorval)
        rospy.loginfo("Subscriber initialization success /sensorval")
        self.tf_br         = tf.TransformBroadcaster()
        rospy.loginfo("TransformBroadcaster initialization success")

    def __delete__(self):
        self.ser.close()

    def pubcommand(self):
        try:
            self.ser.flush()
            self.command = ""
            self.command = self.ser.read()
            if self.command != "":
                rospy.loginfo("Command received ["+self.command+"]")
                self.pub_comm.publish(self.command)
        except:
            pass

    def subsensorval(self, svalue):
        try:
            if len(svalue.data) > 0:
                #rospy.loginfo(svalue.data)
                self.sensorvalue = svalue.data.split(',')
                if len(self.sensorvalue) == 14:
                    self.pubimu()
                    self.pubodom()
        except:
            print "subsensorval Error"
            traceback.print_exc()

    def pubimu(self):
        self.p.linear_acceleration.x=4*9.81*(float(self.sensorvalue[1])/2**16)/100
        self.p.linear_acceleration.y=4*9.81*(float(self.sensorvalue[2])/2**16)/100
        self.p.linear_acceleration.z=4*9.81*(float(self.sensorvalue[3])/2**16)/100
        self.p.orientation.x= float(self.sensorvalue[4])
        self.p.orientation.y=float(self.sensorvalue[5])
        self.p.orientation.z=float(self.sensorvalue[6])
        self.p.header.stamp = rospy.Time.now()
        self.pub_imu.publish(self.p)

    def pubodom(self):

        VL=0.0
        VR=0.0

        if float(self.sensorvalue[10])!=self.odomR or float(self.sensorvalue[9])!=self.odomL:
            self.delta=(float(self.sensorvalue[0])-float(self.temps))/1000 #[Second] Elapsed time from latest measurement
            VR=(float(self.sensorvalue[10])-float(self.odomR))/self.COUNT*3.14*self.DIAMETER/self.delta #[Meter] Advance distance of right wheel
            VL=(float(self.sensorvalue[9])-float(self.odomL))/self.COUNT*3.14*self.DIAMETER/self.delta #[Meter] Advance distance of left wheel
            self.odomL=float(self.sensorvalue[9])
            self.odomR=float(self.sensorvalue[10])
            self.temps=self.sensorvalue[0]
            #rospy.loginfo("[odomL] "+str(self.odomL)+" [odomR] "+str(self.odomR)+" [delta] "+str(self.delta)+" [VL] "+str(VL)+" [VR] "+str(VR))
        else :
            VR=0.0
            VL=0.0
            self.temps=self.sensorvalue[0]
        self.o.pose.pose.position.x += self.delta*(VR+VL)/2*cos(self.theta)
        self.o.pose.pose.position.y += self.delta*(VR+VL)/2*sin(self.theta)
        rad = 0.0
        if (self.odomL>0.0 and self.odomR<0.0) or (self.odomL<0.0 and self.odomR>0.0):
            rad = ((self.odomL>0)-(self.odomL<0))*(abs(self.odomL)+abs(self.odomR))/2*self.RADIANPERENCODER
        self.theta += rad

        quat = tf.transformations.quaternion_from_euler(0,0,self.theta)

        self.o.pose.pose.orientation.x = quat[0]
        self.o.pose.pose.orientation.y = quat[1]
        self.o.pose.pose.orientation.z = quat[2]
        self.o.pose.pose.orientation.w = quat[3]
        self.o.twist.twist.linear.x =(VR+VL)/2*cos(self.theta)
        self.o.twist.twist.linear.y =(VR+VL)/2*sin(self.theta)
        self.o.twist.twist.angular.z = rad
        self.o.header.stamp = rospy.Time.now()
        self.pub_odom.publish(self.o)

        pos = (self.o.pose.pose.position.x,
               self.o.pose.pose.position.y,
               self.o.pose.pose.position.z)

        ori = (self.o.pose.pose.orientation.x,
               self.o.pose.pose.orientation.y,
               self.o.pose.pose.orientation.z,
               self.o.pose.pose.orientation.w)       
        self.tf_br.sendTransform(pos, ori, rospy.Time.now(), 'base_link', 'odom')

if __name__=="__main__":

    rospy.init_node("zumo", anonymous = True)
    zumo = Zumo()

    while not rospy.is_shutdown():
        zumo.pubcommand()
        sleep(0.001)

    rospy.delete_param("BLUETOOTH_PORT")
    rospy.delete_param("BLUETOOTH_BAUDRATE")
    zumo.ser.close()

zumo32u4arduino.ino の内容

zumo32u4arduino.ino

#define USE_USBCON
#include <ros.h>
#include <std_msgs/String.h>
#include <Wire.h>
#include <LSM303.h>
#include <Zumo32U4.h>

long timer=0;              // Elapsed time since program started (milli second)
int vright = 0;            // Left Morter velocity (speed of motor)
int vleft = 0;             // Right Morter velocity (speed of motor)
int basespeed = 150;       // Base speed of Morter (Effective Range: 1 - 150)
int16_t positionLeft  = 0; // For encoder verification
int16_t positionRight = 0; // For encoder verification
int16_t newLeft = 0;       // Value of Encorder
int16_t newRight = 0;      // Value of Encorder
std_msgs::String str_msg;  // Sensor value to be published

LSM303 compass;            // Magnetometer
L3G gyro;                  // Gyrometer
Zumo32U4Motors motors;     // Morter
Zumo32U4Encoders encoders; // Encoder
ros::NodeHandle nh;        // NodeHandler of ROS

void motorcontrol(const std_msgs::String& cmd_msg)
{

  // I : forward    ,  , : backward
  // L : turn right ,  J : turn left
  // S : stop

  String cmd = "";

  //Serial.println(cmd_msg.data);  // Debug Print

  if (strlen(cmd_msg.data) != 0)
  {
    cmd = cmd_msg.data;
  }

  if (cmd == "i")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    vleft = basespeed;
    vright = basespeed;
    motors.setSpeeds(vleft, vright);
  }
  else if (cmd == ",")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    vleft = -1*basespeed;
    vright = -1*basespeed;
    motors.setSpeeds(vleft, vright);
  }
  else if (cmd == "l")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    vleft = (basespeed+230);
    vright = -1*(basespeed+230);
    motors.setSpeeds(vleft, vright);
  }
  else if (cmd == "j")
  {
    motors.setSpeeds(0, 0);
    delay(2);
    vleft = -1*(basespeed+230);
    vright = (basespeed+230);
    motors.setSpeeds(vleft, vright);
  }
  else if (cmd == "s")
  {
    vleft = 0;
    vright = 0;
    motors.setSpeeds(vleft, vright);
    delay(2);
  }

}

ros::Subscriber<std_msgs::String> sub("/command", motorcontrol);
ros::Publisher chatter("/sensorval", &str_msg);

void setup()
{
  //Serial.begin(9600);     // Debug Print

  Wire.begin();

  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();

  nh.initNode();           // Init ROS Node
  nh.advertise(chatter);   // Init ROS Publisher
  nh.subscribe(sub);       // Init ROS Subscriber

  compass.init();          // Init magnetometer
  compass.enableDefault();

  gyro.init();             // Init gyrometer
  gyro.enableDefault();
}

void loop()
{
  compass.read();   // Read magnetometer
  gyro.read();      // Read gyrometer
  timer = millis();
  newLeft = encoders.getCountsAndResetLeft();
  newRight = encoders.getCountsAndResetRight();
  if (!(encoders.checkErrorLeft()) && !(encoders.checkErrorRight())) {
    positionLeft = newLeft;
    positionRight = newRight;    
  }
  String s = "";
  s += timer;          // [0]  Elapsed time since program started (milli second)
  s += ',';
  s += compass.a.x;    // [1]  Accelerometer.x
  s += ',';
  s += compass.a.y;    // [2]  Accelerometer.y
  s += ',';
  s += compass.a.z;    // [3]  Accelerometer.z
  s += ',';
  s += compass.m.x;    // [4]  Magnetometer.x
  s += ',';
  s += compass.m.y;    // [5]  Magnetometer.y
  s += ',';
  s += compass.m.z;    // [6]  Magnetometer.z
  s += ',';
  s += vleft;          // [7]  Left Morter velocity (speed of motor)
  s += ',';
  s += vright;         // [8]  Right Morter velocity (speed of motor)
  s += ',';
  s += positionLeft;   // [9]  Left Morter odometry (Rotation angle of motor)
  s += ',';
  s += positionRight;  // [10] Right Morter odometry (Rotation angle of motor)
  s += ',';
  s += gyro.g.x;       // [11] Gyrometer.x
  s += ',';
  s += gyro.g.y;       // [12] Gyrometer.y
  s += ',';
  s += gyro.g.z;       // [13] Gyrometer.z

  //Serial.println(s);  // Debug Print

  str_msg.data = s.c_str();
  chatter.publish(&str_msg);
  nh.spinOnce();
  delay(1);
}

backpack_2d.lua の内容

backpack_2d.lua

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 20.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.huber_scale = 1e3

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)

return options

backpack_2d.launch の内容

backpack_2d.launch

<?xml version="1.0" ?>
<launch>
  <param
      name="robot_description"
      textfile="$(find zumo32u4)/urdf/zumo32u4.urdf"
  />

  <node
     name="horizontal_laser"
     pkg="rplidar_ros"
     type="rplidarNode"
     output="screen">
     <param name="serial_port"      type="string" value="/dev/ttyUSB0"/>
     <param name="serial_baudrate"  type="int"    value="115200"/>
     <param name="frame_id"         type="string" value="horizontal_laser_link"/>
     <param name="inverted"         type="bool"   value="false"/>
     <param name="angle_compensate" type="bool"   value="true"/>
  </node>

  <node
      pkg="tf"
      type="static_transform_publisher"
      name="base_link_connect"
      args="0 0 0 0 0 0 /base_link /horizontal_laser_link 100"
  />

  <node
      pkg="tf"
      type="static_transform_publisher"
      name="imu_link_connect"
      args="0 0 0 0 0 0 /base_link /imu_link 100"
  />

  <node
      name="cartographer_node"
      pkg="cartographer_ros"
      type="cartographer_node"
      args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename backpack_2d.lua"
      output="screen">
  </node>

  <node
      name="cartographer_occupancy_grid_node"
      pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node"
      args="-resolution 0.05"
  />
</launch>

＜現状のTFツリー＞

◆ 本日のまとめ

• Github を随時更新中。
• オドメトリの調整に成功。
• オドメトリを使用することで機体の位置推定の精度が向上した。
• 足回りのセンサが若干ショボいのでこれぐらいの精度が限界かな。
• RaspberryPi、酷使しすぎてモノが完成する前に壊れないことだけを願う。
• Roll、Pitch、Yaw 初代エースコンバット の時代からやり込み続け、意味を理解しないまま今日に至る。
• 久々にエースコンバットがプレイしたくなってきた。
• 今度は エースコンバット２のこの曲 RISING HIGH がグルグルし始めた。

◆ 次回予告

ついに、 ナビゲーション だろうか。
相当ハマりそうだ。。。