데브코스 - 4. ROS - Rviz 차량제어

프로그래머스에서 진행중인 자율주행 데브코스 - Planning & Control : ROS 기초부터 정리를 시작한다.

강의에서는 Ubuntu 18.04 - ROS Melodic로 진행하였으나
필자는 WSL - Ubuntu 20.04 - ROS Noetic 환경에서 강의를 정리하고 다시 코드를 실행하려 한다.

---

0.사전언급

강의 내옹중에는 xycar의 8자 주행 패키지 제작 과정이 있으나, 실물xycar이 없기에 rviz상에서 virtual vehicle을 8자 주행하는 과정을 수행한다.

---

1. Rviz Visualizer

첫번째로 ROS의 Rviz로 구동가능한 virtual vehicle을 조작하는 기본 패키지의 생성은 아래 명령어를 통해 ROS패키지를 생성한다

$ catkin_create_pkg rviz_xycar_01 geometry_msgs rospy rviz tf urdf xacro

1-1. 의존성 패키지 설명

여기서 의존성 패키지의 리스트가 꽤 되는데 하나하나 살펴보자면
geometry_msgs : 이동로봇의 모션과 관련된 ROS 메세지 표준 규격이 정의되어 있는 패키지이다. 이동로봇 위치, 자세제어 등의 작업이 rviz visualizer에서 수행될 때 해당 패키지에 정의된 메세지 규격을 사용하여 정의한다 보면 된다.
rviz : ROS의 3D시각화 툴인 Rviz를 실행하기 위해 패키지에 연동하는 의존성 패키지로 Rviz는 ROS Visualization Tool의 약자이다. 해당 툴에는 연계해서 사용하는 의존성 패키지로 URDF가 있는데 이는 이동로봇의 로봇 모델링을 기술하는데 사용되는 표준 포맷으로 Unified Robot Description Format의 줄임말이다.

즉, Rviz는 3D 시각화 툴을 제공하는 프로그램이고, 해당 프로그램에 로봇 모델을 불러올 때, 로봇 모델의 코드는 URDF규격으로 작성된다. 라고 보면 된다.
물론 이 두가지를 ROS에서 사용하기 위해 의존성 정보를 기술하는 것이다.

xacro : XML + macro의 약어로 위 언급된 로봇 모델링 표준 규격 URDF를 작성하는데 XML언어를 사용한다.
이 XML언어를 통해 URDF 규격으로 로봇 모델을 기술하는데 작성하다 보면 길게 작성하는 부분이 발생한다. 이를 약어로 처리할 수 있는데 이때 이 '약어(매크로)'에 대한 내용이 기술된 패키지가 xacro라 보면 된다.
tf : transform(좌표변환)의 줄임말로 로봇을 설계하면 '관절'이 생기는데 이를 URDF표현식으로 보면 아래 그림과 같다.

URDF표현식으로 보면 Link - Joint - Link 순으로 Joint가 '관절'역활로 부분 몸체인 Link를 연결짓는데
이 Link, Joint 모두 하나하나 작은 좌표계가 모두 적용된다.

이게 그림으로 보면 위 로봇 모델처럼 각각의 joint 부분에 x,y,z 3차원 좌표계가 도식화된 tf가 적용된다 볼 수 있는데 tf끼리 좌표 변환이 빈번하게 이뤄진다
(자율차로 내용을 한정한다면 자율차에 임의의 Sensor을 장착하면 Sensor을 기준으로 하는 좌표계 Sensor Frame이 생성되고, 나중에 차량의 Body Frame으로 좌표계 변환(Coordinate Transformations)을 수행하고, 이게 Local Coordinate와 Global Coordinate 비교를 통해 차량의 주변 환경을 모델링한다...) 아무튼 길게 썻는데 좌표계 변환이 항상 발생할 수밖에 없고 이것과 관련된 패키지가 ROS TF라 보면 된다.

쉽게 ROS로 로봇 모델링 작업을 수행한다면 rviz, URDF, Xacro, tf 4개는 꼭 따라다닌다 보면 된다.

---

1-2. 폴더 구성 및 코드 작성

패키지를 작성후 폴더, 파일은 아래와 같이 생성 및 작성한다.

첫번째로 xycar_3d.launch 코드는 아래와 같이 작성한다

<?xml version="1.0" encoding="utf-8"?>
<launch>

    <param name="robot_description" textfile="$(find rviz_xycar_01)/urdf/xycar_3d.urdf"/>

    <param name="use_gui" value="ture"/>
   
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
                args="-d $(find rviz_xycar_01)/rviz/xycar_3d.rviz"/>

    <node name="robot_state_publisher" pkg="robot_state_publisher" 
                type="robot_state_publisher"/>

    <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui"
                type="joint_state_publisher_gui"/>
</launch>

xycar_3d.launch코드를 한줄 한줄 살펴보면 아래와 같다.

<param name="robot_description" textfile="$(find rviz_xycar_01)/urdf/xycar_3d.urdf"/>

roslaunch에서 로봇 모델 기술 정보에 해당하는 URDF파일을 불러오는 것은 param태그로 수행하며
여기서 불러오는 URDF파일인 xycar_3d.urdf는 https://drive.google.com/file/d/1E1JRFVJJg0HMnlRvgwZClYRXoqGxBWMb/view?usp=drive_link
위 링크에 접속하여 다운로드가 가능하다.
URDF파일에 대한 코드 설명은 따로 진행하지 않겠다.

<node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" args="-d $(find rviz_xycar_01)/rviz/xycar_3d.rviz"/>

다음으로 불러온 로봇 모델은 Rviz에서 구동이 가능한데, Rviz에서 구동한 로봇 모델은 config 정보를 *.rviz형식의 확장자로 저장이 가능하다.
따라서 xycar_3d.rviz는 config 정보가 저장된 파일이며,
매번 config를 수행하는 것이 번거롭기에 이 저장된 파일을 roslaunch에 불러오는 것으로 기술하는 것이다.
위 rviz 파일의 경우 해당파일이 없어도 roslaunch 기동에는 문제가 없으며,
rviz파일을 만드는 방식은 아래와 같다.

rviz파일의 작성은 GUI로 작성하는 것이며 따로 코드수행으로 만드는 것은 아니다

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>

다음으로 2개의 노드를 구동해야 하는데
하나는 로봇 모델에서 '관절'에 해당하는 Joint를 GUI로 직접 조정하기 위한 GUI툴을 불러오는 노드인
이를 받아서 로봇 모델에 반영시켜주는
joint_state_publisher_gui 두개의 노드를 구동한다.

노드 정보를 기술한 후 roslaunch를 수행하면 위 gif와 같이 불러온 로봇 모델에 대한 GUI조작 창과 GUI조작된 정보가 rviz상에 불러온 로봇 모델에 적용됨을 알 수 있다.

---

2. py파일 작성으로 로봇 조작

다음으로 rviz viewer에서 보이는 robot를 앞선 과정에서는 joint_state_publisher_gui로 조작을 수행했다면, 이번에는 별도의 *.py파일을 작성해서 임의의 node를 생성 후 해당 노드로 로봇을 조작하고자 한다.

파일 구성 및 move_Joint.launch 코드 작성

<?xml version="1.0" encoding="utf-8"?>
<launch>
    <param name="robot_description" textfile="$(find rviz_xycar_01)/urdf/xycar_3d.urdf"/>
    <param name="use_gui" value="ture"/>
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
                args="-d $(find rviz_xycar_01)/rviz/xycar_3d.rviz"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" 
                type="robot_state_publisher"/>
    <node name="move_joint_node" pkg="rviz_xycar_01" type="move_joint.py"/>
</launch>

roslaunch파일은 joint_state_publisher_gui만
move_joint.py파일로 변경한다.

move_joint.py

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

import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header

rospy.init_node('move_joint_node')
pub = rospy.Publisher('joint_states', JointState, queue_size=10)

xycar_msg = JointState()
xycar_msg.header = Header()

xycar_msg.name = ['front_right_hinge_joint', 'front_left_hinge_joint',
                  'front_right_wheel_joint', 'front_left_wheel_joint',
                  'rear_right_wheel_joint', 'rear_left_wheel_joint',]
xycar_msg.velocity = []
xycar_msg.effort = []

f_r_w_angle = -3.14
l_r_w_angle = -3.14

rate = rospy.Rate(50) #메세지 발행 간격은 50Hz

while not rospy.is_shutdown():
    xycar_msg.header.stamp = rospy.Time.now()

    if f_r_w_angle >= 3.14:
        f_r_w_angle = -3.14
    else:
        f_r_w_angle += 0.01
  
    if l_r_w_angle >= 3.14:
        l_r_w_angle = -3.14
    else:
        l_r_w_angle += 0.01

    xycar_msg.position = [0, 0, f_r_w_angle, l_r_w_angle, 0, 0]
    pub.publish(xycar_msg)
    rate.sleep()

코드 설명을 진행하도록 하겠다.

from sensor_msgs.msg import JointState
from std_msgs.msg import Header

첫번째로 import하는 메세지 표준 규격인 JointStae와 Header를 import하는 것으로 메세지 규격은 아래와 같다.

JointState 메세지는 하위 메세지 항목으로 Header 메세지를 포함하고 있으며, Header 메세지는 time_stamp 정보를 담는 메세지 헤더라 볼 수 있다.
그리고 JointState 메세지는 URDF파일에서 정의되어 있는 Joint의 name을 읽어 각 Joint에 대해 Position, Velocity, Effort(힘)을 부여할 수 있도록 메세지 포맷을 정의하고 있다.
이 때, 불러온 Joint는 Position, Velocity, Effort(힘) 3가지 정보를 받을 수 있도록 *.urdf파일에 재대로 기술되어야 하는데 현재 불러온 xycar_3d.urdf파일은 Position 정보만 재대로 반영한다

위 메세지 규격에 따라서 move_joint.py파일이 퍼블리싱 하고자 하는 토픽 Joint_states은 아래의 코드처럼 메세지 계층 구조에 맞게 메세지를 인스턴스화 한다.

xycar_msg = JointState() #상위 JointState 메세지 인스턴스
xycar_msg.header = Header() #JointState의 하위 항목 Header 인스턴스

다음으로 xycar_3d.urdf에 정의되어 있는 5개의 Joint name을 찾아서 이를 move_joint.py코드가 사용할 수 있도록 매핑해준다

xycar_msg.name = ['front_right_hinge_joint', 'front_left_hinge_joint',
                  'front_right_wheel_joint', 'front_left_wheel_joint',
                  'rear_right_wheel_joint', 'rear_left_wheel_joint',]

xycar_3d.urdf파일에서 joint name= 을 검색하면 6개가 뜨는데
동일하게 이름을 찾아서 move_joint.py에 매핑한다.

참고로 xycar_3d.urdf에서 기술된 정보는
position 항목만 재대로 조작이 가능하고 나머지 velocity effort는 값을 넣어도 반영이 안되니 그냥 빈칸으로 정의한다.

position 항목의 경우 선형 움직임일 시에는 m 단위, 회전 움직임일 시에는 rad 단위로 항목을 받아들인다.

xycar_3d.urdf에서 조작 대상 Joint인 'front_right_wheel_joint', 'front_left_wheel_joint'은 회전운동을 하기에 rad단위로 받으며
rad값의 범위는 -3.14 ~ 3.14이다.

따라서 while 구문 내에 작성한 회전운동 알고리즘은
아래와 같이 기동함을 확인 할 수 있다.

코드에서 전방 wheel만 무한궤도로 회전하도록 알고리즘을 작성했기에
이에 맞게 rviz viewer에서 정상적으로 무한회전함을 확인할 수 있다.

---

2. 과제 코드 수행

강의에서는 Virtual Vehicle을 8자 주행시키는 코드인 driver.py와 이를 로봇 모델을 제어하기 위한 메세지로 변조 시켜주는 converter.py 두개의 노드 코드를 새로 작성하지만, xycar_3d.urdf는 position 조작만 가능하고 velocity, effort 조작이 불가능하기에 8자 주행 알고리즘을 구동시켜도 로봇 모델은 스티어 조작만 하지 실제 주행은 하지 않는다.

로봇을 주행시키려면 Rviz에 불러온 로봇 모델이 odometry까지 포함해야 하기에 아래와 같이 새로 패키지를 하나 만들고 파일 구성을 설정한다.

$ catkin_create_pkg rviz_xycar_02 geometry_msgs rospy rviz tf urdf xacro

과제 수행을 위해 총 3개의 py파일을 작성하며, 각각 로봇제어(steer, speed)로 명령 생성, rviz에서 구동되는 virtual vehicle을 제어하는 메세지 규격인 JointState메세지 규격으로 변환, virtual vehicle의 시간에 따른 위치 변화를 추정하는데 사용되는 Odometry 메세지규격을 출력하는 노드의 코드이다.

첫번째로 패키지 내 작성한 노드 및 rviz viewer 구동 및 UDRF 코드 참조를 위한 Launch파일을 먼저 작성한다

exam_rviz_odom.launch

<?xml version="1.0" encoding="utf-8"?>
<launch>
    <param name="robot_description" textfile="$(find rviz_xycar_02)/urdf/xycar_3d.urdf"/>
    <param name="use_gui" value="ture"/>
   
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
                args="-d $(find rviz_xycar_02)/rviz/rviz_odom.rviz"/>

    <node name="robot_state_publisher" pkg="robot_state_publisher" 
                type="robot_state_publisher"/>
  
    <node name="Drive_node" pkg="rviz_xycar_02" type="exam_odom_8_drive.py"/>

    <node name="Converter_node" pkg="rviz_xycar_02" type="converter.py"/>

    <node name="rviz_controller_node" pkg="rviz_xycar_02" type="exam_rviz_odom.py"/>
</launch>

Launch파일의 param 및 node 태그의 설명은 앞에서 중복되게 설명했기에 생략한다

첫번째로 8자 주행 알고리즘인 exam_odom_8_drive.py코드 설명이다.

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

import rospy
import time
from xycar_motor.msg import xycar_motor

motor_control = xycar_motor()

rospy.init_node('drive_node')
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size=10)

def motor_pub(angle, speed):
    global pub
    global motor_control

    motor_control.angle = angle
    motor_control.speed = speed

    pub.publish(motor_control)

speed = 3
while not rospy.is_shutdown():
    angle = -50
    for i in range(70):
        motor_pub(angle, speed)
        time.sleep(0.1)
   
    angle = 0
    for i in range(30):
        motor_pub(angle, speed)
        time.sleep(0.1)
   
    angle = 50
    for i in range(70):
        motor_pub(angle, speed)
        time.sleep(0.1)
   
    angle = 0
    for i in range(30):
        motor_pub(angle, speed)
        time.sleep(0.1)

코드 구문별 설명은 아래와 같다.

from xycar_motor.msg import xycar_motor
motor_control = xycar_motor()

위 코드는 차량의 8자 주행을 수행하는데 입력되는 명령어로 Angle, Speed 두가지 정보를 받게끔 만든 임의의 커스텀 메세지로 xycar 로봇 제어에 사용되는 커스텀 메세지를 활용했다.

커스텀 메세지의 규격에 대한 설명은 넘어가고 그냥 간단하게 차량의 헤딩각도인 Angle, 차량의 속도인 Speed를 제어한다.

다음으로

while not rospy.is_shutdown():
	...

위 while구문 내에 들어 있는 알고리즘이 로봇의 8자 주행을 위한 코드이다.
time.sleep(0.1)로 명령 입력 주기를 설정하고
Angle, Speed값을 변환한 뒤 motor_pub(angle, speed)함수를 호출해서 변환한 제어명령값을 입력, 해당 함수에서는 ROS 메세지 발행까지 수행한다.

두번째로 위 exam_odom_8_drive.py의 메세지 토픽은 xycar_motor이기에 rviz viewer에서 구동되는 로봇 모델의 구동에는 적합한 메세지 토픽이 아니다. 따라서 Vritual Vehicle을 구동하기 위한 JointState메세지 토픽으로 변환해주는 converty.py코드를 구동한다.

converty.py

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

import rospy, rospkg
import math
from sensor_msgs.msg import JointState
from std_msgs.msg import Header

from xycar_motor.msg import xycar_motor

global pub
global Joint_msg
global l_wheel, r_wheel

def callback(xycar_motor_msg):
    global pub, Joint_msg, l_wheel, r_wheel
   
    Angle = xycar_motor_msg.angle
    Speed = xycar_motor_msg.speed

    Steer_Angle = math.radians(Angle * 0.4)
    Vehicle_speed = Speed * 0.1
   
    if l_wheel >= 3.14:
        l_wheel = -3.14
    else:
        l_wheel += 0.01
   
    if r_wheel >= 3.14:
        r_wheel = -3.14
    else:
        r_wheel += 0.01
   
    Joint_msg.position = [Steer_Angle, Steer_Angle,
                          r_wheel, l_wheel, 
                          r_wheel, l_wheel]
    Joint_msg.velocity = [Vehicle_speed, Vehicle_speed,
                          Vehicle_speed, Vehicle_speed,
                          Vehicle_speed, Vehicle_speed]

    Joint_msg.header.stamp = rospy.Time.now()
   
    pub.publish(Joint_msg)    

rospy.init_node('Converter_node')
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.Subscriber('xycar_motor', xycar_motor, callback)

Joint_msg = JointState()
Joint_msg.header = Header()
Joint_msg.name = ['front_right_hinge_joint', 'front_left_hinge_joint',
                  'front_right_wheel_joint', 'front_left_wheel_joint',
                  'rear_right_wheel_joint', 'rear_left_wheel_joint',]
Joint_msg.effort = []

l_wheel = -3.14
r_wheel = -3.14 #바퀴 초기화

rospy.spin()

코드는 앞서 JointState메세지 토픽을 사용한 move_joint.py코드와 거의 유사하나, 다른 부분은

def callback(xycar_motor_msg):
    global pub, Joint_msg, l_wheel, r_wheel
    
    Angle = xycar_motor_msg.angle
    Speed = xycar_motor_msg.speed
    
    Steer_Angle = math.radians(Angle * 0.4)
    Vehicle_speed = Speed * 0.1

위 부분처럼 xycar_motor에서 받은 토픽 명령 중 Angle, Speed항목을 저장하고, 이를 virtual vehicle의 조작이 가능하도록 데이터 컨버팅을 수행한다.
이때 Steer_Angle은 xycar_motor의 단위가 degree 이고, JointState의 단위는 rad이기에 math.radians(degree) 함수로 rad 각도로 변환해준다.

다음으로

	Joint_msg.position = [Steer_Angle, Steer_Angle,
                          r_wheel, l_wheel, 
                          r_wheel, l_wheel]
    Joint_msg.velocity = [Vehicle_speed, Vehicle_speed,
                          Vehicle_speed, Vehicle_speed,
                          Vehicle_speed, Vehicle_speed]

위 구문으로 JointState의 메세지 규격에 스티어값(rad)와 속도값을 넣어준다. 이때 vehicle_speed는 rviz의 로봇에 적용되지 않는다.
이 속도값을 향후 Odometry메세지 토픽을 통해 적용시킨다.

---

2.1 odometry.py 코드 설명

마지막 odometry가 적용된 exam_rviz_odom.py코드는 Odometry메세지 토픽을 포함하는데, 해당 메세지의 경우 이론설명이 조금 필요한 메세지이다.
하여 하위 항목으로 설명을 진행한다.

우선 전체 코드를 먼저 첨부한다.

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

import math
from math import sin, cos, pi

import rospy
import tf.transformations

from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3

from sensor_msgs.msg import JointState

global Angle, Speed

def callback(Joint_msg):
    global Angle, Speed
    Angle = Joint_msg.position[Joint_msg.name.index("front_right_hinge_joint")]

    Speed = Joint_msg.velocity[Joint_msg.name.index("front_right_hinge_joint")]


rospy.init_node('odom_pub_node')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_sub = rospy.Subscriber("joint_states", JointState, callback)

odom_broadcaster = tf.TransformBroadcaster()

r = rospy.Rate(30)

wheel_base = 0.2

x_ = 0.0
y_ = 0.0
yaw_ = 0

Angle = 0
Speed = 0

curr_time = rospy.Time.now()
pre_time = rospy.Time.now()


while not rospy.is_shutdown():
    curr_time = rospy.Time.now()

    dt = (curr_time - pre_time).to_sec()

    curr_ster_angle = Angle
    curr_speed = Speed
    curr_angular_v = curr_speed * math.tan(curr_ster_angle) / wheel_base

    x_dot = curr_speed * cos(yaw_)
    y_dot = curr_speed * sin(yaw_)

    x_ = x_ + x_dot * dt
    y_ = y_ + y_dot * dt
    yaw_ = yaw_ + curr_angular_v * dt


    odom_quat = tf.transformations.quaternion_from_euler(0, 0, yaw_)

    odom_broadcaster.sendTransform(
        (x_, y_, 0.),
        odom_quat,
        curr_time,
        "base_link",
        "odom"
    )

   
    odom_msg = Odometry()

	odom_msg.header.stamp = curr_time
    odom_msg.header.frame_id = "odom"
   
    odom_msg.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat))

    odom_msg.child_frame_id = "base_link"
    odom_msg.twist.twist = Twist(Vector3(x_dot, y_dot, 0), Vector3(0, 0, yaw_))

    odom_pub.publish(odom_msg)

    pre_time = curr_time
    r.sleep()

상단부분의 메세지 포맷에 대한 설명을 먼저 기술하면 아래와 같다.

from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from sensor_msgs.msg import JointState

nav_msgs 패키지의 Odometry 메세지 규격은 위 사진처럼 다양한 하위 메세지 포맷 규격이 존재하며, 주요하게 볼 규격은
Pose - Point : 절대좌표계 world fixed frame에서 로봇의 상대 위치 [x, y, z]
Post - Quaternion : world fixed frame에서 로봇의 각도정보를 쿼터니언으로 표기
Twist - linear[Vector3] : world fixed frame에서 로봇의 x, y, z축별 선속도
Twist - angular[vector3] : world fixed frame에서 로봇의 각속도 [roll rate, pitch rate, yaw rate]
4가지 이다.

우선 로봇의 odometry를 설명하기 위한 좌표계를 도식화 하면 아래와 같다.

rviz viewr는 3D로 로봇의 모델을 시각화 하지만 실제 로봇의 구동은 2차원 좌표계에서 모두 표현이 가능하다.
이를 위 사진처럼 표현이 가능한데, $\theta$는 yaw angle이 되고, $\omega$는 yaw rate와 매핑이 된다.

차량 동역학 및 좌표계의 이론 설명은 넘어가며
rviz viewer에서 로봇의 이동 궤적을 표현하는데는
추가로 2개의 frame이 필요하다
하나는 world fixed frame이 되는 odom, 또 하나는 world fixed frame에서 이동체의 frame이 되는 child_frame_id인 base_link 이다.

아무튼 요약하자면 차량을 2차원 좌표계에서 표현하는데 Odometry 메세지 규격이 필요하며, 총 6개의 하위 메세지 규격 정보를 통해 로봇의 이동궤적을 표현한다 보면 된다.

exam_rviz_odom.py의 import 항목 중 메세지 규격의 설명은 이정도로 하고, callback 함수에 대해 설명하겠다.

def callback(Joint_msg):
    global Angle, Speed
    Angle = Joint_msg.position[Joint_msg.name.index("front_right_hinge_joint")]
    #Joint_state메세지에서 핸들 꺽임 조향 값을 가져와 Angle에 입력
    Speed = Joint_msg.velocity[Joint_msg.name.index("front_right_hinge_joint")]
    #Joint_state메세지에서 차량속도 정보를 가져와 Speed에 입력

위 함수는 converty.py코드에서 출력되는 메세지 토픽이 JointStates규격이니, 여기서 차량의 헤딩 및 속도정보를 추출해서 전역변수 Angle, Speed에 매핑한다.

다음으로 코드 중 중요한 부분은

odom_broadcaster = tf.TransformBroadcaster()

    odom_broadcaster.sendTransform(
            (x_, y_, 0.),
            odom_quat,
            curr_time,
            "base_link",
            "odom"
        )

부분으로 로봇 모델링을 수행하는 xycar_3d.urdf코드는 로봇을 구성하는 모든 부품 간의 관계, 정확히는 각 부품, 관절에 포함된 좌표계 간의 관계를 TF(Transform)으로 표현한다.
그러나, xycar_3d.urdf에 기술되지 않은 world fixed frame인 odom와 로봇의 body frame인 base_link은 서로 좌표계 간의 관계인 TF(Transform)가 정의되어 있지 않다.

이를 정의하는 코드가 위 구문이라 보면 된다.
맨 위 odom_broadcaster = tf.TransformBroadcaster()는 TF관계를 설정하기 위한 클래스의 인스턴스, odom_broadcaster.sendTransform() 함수는 odom, base_link관의 관계를 정의하기 위한 함수라 보면 된다.

이어서

while not rospy.is_shutdown():
~~~~
~~~
~~~
	yaw_ = yaw_ + curr_angular_v * dt

여기까지 구문은 2차원 좌표계의 로봇이 이동한 궤적을 변환하기 위한 알고리즘이라 보면 된다. 따로 설명은 향후 차량동역학 부분에서 설명하며,
연산에 차량의 축거(wheel_base)가 필요한 정도만 알고 넘어간다.
그리고 연산 시간에 dt(Duration), 이산시간으로 연산하기에 단위시간이 필요한데 이것은 curr_time변수와 pre_time변수 간의 발생한 시간차를 통해 연산한다.

마지막으로 코드 하단부의

odom_msg = Odometry()
~~~
~~~

부분은 odom 메세지 규격에 맞춰 각 항목들을 채워넣는 과정으로 코드는 길어서 잘 해석이 안되지만
모두 하위 클래스의 인스턴스 후 매개변수 입력의 과정이라 보면 된다

이 중, odom_msg.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat)) 부분이 특이한데
odom_quat 변수 앞에 *(Asterisk : 아스타리스크)는 클래스 Quaternion에 입력되는 매개변수인 odom_quat이 리스트 형식이 아닌 튜플이나 딕셔너리 형식이어서 아스타리스크를 붙여서 매개변수로 입력되게 만든 거라 보면 된다.

코드설명은 이정도면 될 듯 하고 roslaunch로 구동하면 아래와 같다.

이는 완성된 rviz 설정으로 디스플레이 되는 정보이며, rviz설정 및 저장은 아래의 사진에 나온 내용을 따라하면 된다.