[ROS]RViz 3D자동차 주행프로그래밍

Nodazi·2024년 1월 7일

ROS

목록 보기

5/21

🤔0.개요

지난시간에는 RViz에 3D모델링 된 자동차를 띄우고, 바퀴를 제어하는 프로그래밍을 진행하였다. 이번 시간에는 odom이라는 토픽을 만들어, RViz 오도메트리 프로그램에 전송하여 자동차를 가상공간에서 직접 움직여보고, 궤적을 남기는것을 추가하여 주행하는 프로그램을 작성하고 실제로 어떤식으로 움직이는지 확인해보자. (아래 사진은 지난시간 제어프로그래밍 사진이다.)

🚗1.동작 방식

동작 방식은 위와 같다. driver노드에서 xycar_motor라는 모터 제어 토픽을 convertor노드에 전송하고, 전송 받은 토픽을 가공하여, joint_states토픽으로 RViz viewer 및 rviz_odom이라는 노드에 전송한다. RViz viewer는 가상 자동차 모델의 바퀴의 움직임을 제어하는 역할을 하고, rviz_odom노드는 가상 자동차 모델을 실제로 움직이는 odom토픽으로 전송한다.

📬1-1.Msg 구성

각 토픽이 어떤 메세지를 주고받는지 터미널 명령어를 통해 알아보자.

xycar_motor 에는 header 및 angle, speed 로 구성되어있다.

JointState는 header 및 name, position,effort 로 구성되어있으며, xycar_motor 으로 부터 받은 토픽을 해석하여 다시 토픽에 담아서 다음 토픽으로 전달한다.
odem
odem 은 position,orientation 및 선속도, 각속도 정보를 담고 있다.
코드를 통해 해당 메세지들이 어떻게 동작하는지 알아보자.

💻1-2.코드 구성

🚀rviz_odom.launch

<launch>
    <param name="robot_description" textfile="$(find rviz_xycar)/urdf/xycar_3d.urdf"/>
    <param name="use_gui" value="true"/>

    <!-- rviz display -->
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
                args="-d $(find rviz_xycar)/rviz/rviz_odom.rviz"/>

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

    <node name="driver" pkg="rviz_xycar" type="odom_8_drive.py" />
    <node name="odometry" pkg="rviz_xycar" type="rviz_odom.py" />
    <node name="converter" pkg="rviz_xycar" type="converter.py" />

</launch>

🐍odom_8_drive.py

import rospy
import time
from xycar_motor.msg import xycar_motor

motor_control = xycar_motor()

rospy.init_node('driver')
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size=1)

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(20):
        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(20):
        motor_pub(angle, speed) 
        time.sleep(0.1)

8자 주행을 할 코드를 작성한다. angle , speed 값을 xycar_motor 토픽으로 전달한다.
🐍convertor.py

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

global pub
global msg_joint_states
global l_wheel, r_wheel

def callback(data):
    global msg_joint_states,l_wheel, r_wheel, pub
    Angle = data.angle
    msg_joint_states.header.stamp = rospy.Time.now()

    steering = math.radians(Angle * -0.4)

    if l_wheel > 3.14:
        l_wheel = -3.14
        r_wheel = -3.14
    else:
        l_wheel += 0.01
        r_wheel += 0.01

    msg_joint_states.position = [steering, steering, r_wheel, l_wheel, 
                                    r_wheel, l_wheel]    
    pub.publish(msg_joint_states)

def start():
    global msg_joint_states,l_wheel, r_wheel, pub    
    rospy.init_node('converter')
    pub = rospy.Publisher('joint_states', JointState, queue_size=10)

    msg_joint_states = JointState()
    msg_joint_states.header = Header()
    msg_joint_states.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']
    msg_joint_states.velocity = []
    msg_joint_states.effort = []
    l_wheel = -3.14
    r_wheel = -3.14
    rospy.Subscriber("xycar_motor", xycar_motor, callback)
    rospy.spin()

if __name__ == '__main__':
    start()

xycar_motor의 토픽을 받아서 joint_states 토픽 형태로 가공하여 토픽을 보낸다.

🐍rviz_odom.py

import math
from math import sin, cos, pi
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from sensor_msgs.msg import JointState

global Angle

def callback(msg):  
       global Angle 
       Angle = msg.position[msg.name.index("front_left_hinge_joint")]

rospy.Subscriber('joint_states', JointState, callback)

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)

odom_broadcaster = tf.TransformBroadcaster()

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(30.0)

current_speed = 0.4
wheel_base = 0.2
x_ = 0
y_ = 0
yaw_ = 0
Angle = 0

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

    # compute odometry in a typical way given the velocities of the robot
    dt = (current_time - last_time).to_sec()

    current_steering_angle = Angle
    current_angular_velocity = current_speed * math.tan(current_steering_angle) / wheel_base

    x_dot = current_speed * cos(yaw_)
    y_dot = current_speed * sin(yaw_)   
    x_ += x_dot * dt;
    y_ += y_dot * dt;
    yaw_ += current_angular_velocity * dt 

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, yaw_)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x_, y_, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    #odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, yaw_))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()