RVIZ 3D 자동차 주행 프로그래밍

  • RVIZ 가상공간에 있는 3D 자동차를 주행시켜보자
  • 동작과정
    1. 8자 주행 프로그램이 모터제어 메시지를 담은 /xycar_motor 토픽을 보냄
    2. 그걸 변환하는 프로그램이 받아서 변환해서 /joint_states 토픽을 만들어 발행
    3. 발행 한 것을 오도메트리 프로그램이 받아서 변환하여 /odom 토픽을 만들어 발행
  • converter 노드에서 RVIZ viewer로 발행을 하면 바퀴가 움직이고, rviz_odom 노드로 발행하면 자동차가 움직임(경로가 보임)

image

파일 구성

image

토픽 3가지

  • /xycar_motor

image

  • /joint_states

image

  • /odom

image

파이썬 코드 작성

  • odom_8_drive_py
    • 8자 주행을 하도록 운전하는 프로그램으로 /xycar_motor 토픽을 발행함
#!/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(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)
  • converter.py
    • /xycar_motor 토픽을 /joint_states 토픽으로 변환하는 프로그램
#!/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_odom.py
    • /joint_states 토픽을 받아서 바퀴의 방향과 회전속도 정보를 획득하고 오도메트리 데이터를 만들어 /odom 토픽에 담아 발행함
#!/usr/bin/env python

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()

	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		
	
	odom_quat = tf.transformations.quaternion_from_euler(0, 0, yaw_)

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


	odom = Odometry()
	odom.header.stamp = current_time
	odom.header.frame_id = "odom"


	odom.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat))

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


	odom_pub.publish(odom)

	last_time = current_time
	r.sleep()
  • 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="robot_state_publisher"/>
  
	<node name="driver" pkg="rviz_xycar" type="odom_8_drive.py" /> 
	<node name="converter" pkg="rviz_xycar" type="converter.py" />
	<node name="odometry" pkg="rviz_xycar" type="rviz_odom.py" />
</launch>

실행

$ roslaunch rviz_xycar rviz_odom.launch

실행결과

  • 자동차의 오도메트리가 8자로 형성되는 것을 확인할 수 있었음

image

  • rqt_graph

image