[ROS] Rviz 를 이용한 3D 자동차 제어 프로그래밍

Nodazi·2024년 1월 5일

ROS

목록 보기

4/21

🤔0.개요

ROS의 Rviz 프로그램을 이용하면 3D자동차 모델을 조작하는 시뮬레이션이 가능하다. 이번시간에는 Rviz를 통해 자동차를 제어하는 시뮬레이션 프로그래밍을 진행한다.
RViz
위의 사진은 RViz 단독 실행 화면이다. 지금은 아무것도 보이지 않지만 코드를 작성하여 자동차 모형 및 조작 시뮬레이션 프로그래밍 코드를 작성해보자.

🖥️1.코드작성

우선 RViz를 구동하기 위해서는 launch 파일을 작성해야한다.
🚀rviz_drive.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_drive.rviz"/>

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

    <node name="driver" pkg="rviz_xycar" type="rviz_8_drive.py" /> 
    <node name="converter" pkg="rviz_xycar" type="converter.py" />

</launch>

robot_description 은 로봇의 기하학적인 모양, 관절, 링크 등을 기술하는 파일이다. 정확한 내용에 대해서는 차후 다루겠다.
그리고 rviz_drive.rviz는 로봇 시뮬레이션의 시각화 설정을 불러 올 수 있다.

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

    angle = 0
    for i in range(30):
        motor_pub(angle, speed) 
        time.sleep(0.1)

코드는 지난 시간 모터제어기 프로그래밍 게시글에서 일부를 수정하였다.
🐍converter.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()