목표

8자 주행하는 모델을 rviz를 통해 시각화 하기

설명

8_drive.py -> converter.py -> odom.py 순서로 메시지를 전달하고 처리한다.

• 8_drive.py
  publish angle message -50 ~ 50 and fixed speed
  -50 ~ 50도를 기준으로 조향각 메시지를 발행한다.
• converter.py
  get angle message -50 ~ 50 -> -20 ~ 20
  -50 ~ 50도의 조향각을 -20 ~ 20도의 조향각으로 스케일링 후 라디안으로 변환한다.
  $\text{angle} = \text{angle} * \frac{20}{50} * \frac{math.pi}{180}$

1. package make

$ catkin_create_pkg my_motor rospy
$ mkdir ~/xycar_ws/src/my_motor/launch
$ gedit 8_drive.launch
$ cd ./src/
$ gedit 8_drive.py

2. 8_drive.launch 파일 만들기

<!--- ~/xycar_ws/src/my_motor/launch/8_drive.launch-->
<launch>
    <!---motor node-->
    <include file = "$(find xycar_motor)/launch/xycar_motor.launch"/>

    <!---auto driver -->
    <node name="auto_driver" pkg="my_motor" type="8_drive.py" output="screen">
</launch>

3. src 폴더에 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("auto_driver")

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

def motor(angle, speed):
    global pub
    global motor_control

    motor_control.angle = angle
    motor_control.speed = speed

    pub.publish(motor_control)

speed = 3                       # 구동속도 3으로 설정
while not rospy.is_shutdown():
    angle = -50                 # 좌회전, 조향각 최대(핸들 최대한 왼쪽) 
    for _ in range(60):
        motor(angle, speed)
        time.sleep(0.1)

    angle = 0                   # 직진(핸들 중앙)
    for _ in range(30):
        motor(angle, speed)
        time.sleep(0.1)

    angle = 50                  # 우회전, 조향각 최대(핸들 최대한 오른쪽)
    for _ in range(60):
        motor(angle, speed)
        time.sleep(0.1)

    angle = 0                   # 직진(핸들 중앙)
    for _ in range(30):
        motor(angle, speed)
        time.sleep(0.1)

4. rviz를 통해 차량의 바퀴 회전과 조향모습 시각화 하기

#! /usr/bin/env python
#converter.py
import rospy
from xycar_motor.msg import xycar_motor
from sensor_msgs.msg import JointState

from std_msgs.msg import Header
import math

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)  # 20 / 50 = 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)

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


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, r_wheel = -3.14, -3.14

rospy.Subscriber("xycar_motor", xycar_motor, callback)

rospy.spin()

<!--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>

5. 오도메트리와 tf를 이용한 주행 시각화

#! /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_pub.publish(odom)
    last_time = current_time
    r.sleep()

<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" />
    <node name="odom" pkg="rviz_xycar" type="rviz_odom.py" />
</launch>

실행 결과