RVIZ_8자 주행

RVIZ에서 8자 주행 코드를 작성해 보자

이렇게.

파일 구성

• 패키지 : rviz_xycar
• 런치 파일 : rviz_drive.launch
• 소스 파일 : converter.py, rviz_8_drive.py

converter.py

converter.py의 역할

• 실제 자이카의 모터가 받는 토픽과, RVIZ의 모터가 받는 토픽의 구조가 다르다.

  

  • 토픽간 형 변환 이라고 생각하면 될 듯 하다.

• 코드 본문

#!/usr/bin/env python

import rospy, rospkg
import math
from xycar_motor.msg import xycar_motor
# JointState, Header 메시지 타입 import
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
    Angle = data.angle

    # xycar motor의 angle은 -50~50 범위
    # 이를 -20도~20도로 변환
    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 각 자리에 맞는 값 적용
    msg_joint_states.position = [steering, steering, r_wheel, l_wheel, r_wheel, l_wheel]
    pub.publish(msg_joint_states)

rospy.init_node("converter")
pub = rospy.Publisher("joint_states", JointState)

# callback 함수에서 채울 msg_joint_states.position 제외하고 나머지 값 채우기
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

# callback(data): 
rospy.Subscriber("xycar_motor", xycar_motor, callback)
rospy.spin()

rviz_8_drive.py

#!/usr/bin/env python

import rospy
import time
from xycar_motor.msg import xycar_motor # 모터 제어를 위한 토픽 메시지 타입 import

motor_control = xycar_motor()

rospy.init_node("driver") # 노드 초기화

pub = rospy.Publisher("xycar_motor", xycar_motor, queue_size=1) # xycar_motor 토픽

# motor_control 토픽의 구조대로, angle과 speed 값을 받아 토픽을 발행
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():
	# 1. 4초간 좌회전
	angle = -50 # -50이 최대
	for i in range(40):
		motor_pub(angle, speed)
		time.sleep(0.1)
		
  # 2. 3초간 직진
	angle = 0
	for i in range(30):
		motor_pub(angle, speed)
		time.sleep(0.1)
		
	# 3. 4초간 우회전
	angle = 50 # 50이 최대
	for i in range(40):
		motor_pub(angle, speed)
		time.sleep(0.1)
	
  # 4. 3초간 직진
	angle = 0
  for i in range(30):
     motor_pub(angle, speed)
     time.sleep(0.1)

실행

# 파일 실행 권한 설정
chmode +x rviz_8_drive.py
chmode +x converter.py

# 빌드
cm

# 실행
roslaunch rviz_xycar rviz_drive.launch

잘 돌아 간다.