3D 자동차 제어 프로그래밍

rviz_xycar 패키지

  • RVIZ에서 자동차를 3D 형상으로 모델링함
  • 핸들 조작하고 바퀴를 굴릴 수 있도록 제작된 패키지

image

  • 준비된 디렉터리들을 src 밑에 옮기고 $ cm으로 빌드

  • $ sudo apt install ros-melodic-joint-state-publisher-gui로 Joint_state_publisher UI를 다운로드

    • Could not find the GUI, install the ‘joint_state_publisher_gui’ package 이런 에러가 뜸

  • $ roslaunch rviz_xycar xycar_3d.launch로 RVIZ 실행

실행결과
  • joint_state_publisher의 슬라이드바를 움직여서 바퀴를 움직일 수 있음

image

  • $ rqt_graph
  • $ rostopic info joint_states
  • $ rosmsg show sensors_msgs/JoinState

image

  • ` $ rostopic echo joint_states`

image

move_joint 파일로 joint_state_publisher 제어 창을 대신해보자

image

launch 파일, 파이썬 코드 작성 및 실행
  • move_joint.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/xycar_3d.rviz"/>

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

    <node name= "move_joint" pkg="rviz_xycar" type= "move_joint.py" /> 
 
</launch>
  • move_joint.py
#!/usr/bin/env python

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

def talker():
    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
    rospy.init_node('move_joint')
    rate = rospy.Rate(50) 

    hello_xycar = JointState()
    hello_xycar.header = Header()
    hello_xycar.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']
    hello_xycar.velocity = []
    hello_xycar.effort = [] 
  
    a = -3.14
    b = -3.14

    while not rospy.is_shutdown():
      hello_xycar.header.stamp = rospy.Time.now()
      
      if a >= 3.14:
         a = -3.14
         b = -3.14
      else:
         a += 0.01 # 0.01 라디안은 약 6도
         b += 0.01

      hello_xycar.position = [0, 0, a, b, 0, 0] # 3,4번째는 각각 앞 바퀴 왼쪽과 오른쪽

      pub.publish(hello_xycar)
      rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

  • $ chmod +x move_joint.py

  • $ cm

  • $ roslaunch rviz_xycar move_joint.launch

실행결과
  • 앞 바퀴의 왼쪽 오른쪽 바퀴가 회전하는 것을 확인할 수 있음

image

  • $ rqt_graph
    • move_joint 파이썬 코드에서 만든 노드인 move_joint에서 joint_states 토픽을 /robot_state_publisher에게 보내는 것을 확인할 수 있음

image

RVIZ에서 3D 자동차 8자 주행하기

  • converter 노드가 Xycar의 속도와 조향각을 제어하는 토픽(xycar_motor)을 받아 변환하여 변경한 토픽(joint_states)을 RVIZ로 보냄

image

  • 토픽 변환

image

launch 파일, 파이썬 코드 작성 및 실행
  • rviz_8_drive.py
#!/usr/bin/env python

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
#!/usr/bin/env python

import rospy
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()
  • 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>
실행결과
  • RobotModel의 Status Error가 뜸

image

  • converter.py의 callback 함수에 ` msg_joint_states.header.stamp = rospy.Time.now()`를 추가

다시 실행결과

  • 정상 작동하는 것을 확인할 수 있음

image-20230403193659019