_ROS1_rviz

rviz는 ROS의 시각화 툴로 토픽들을 시각화 할 수 있는 도구이다.

이번에는 URDF(Unified Robot Description Format)제공 파일을 가지고 rviz에서 토픽들을 시각화 해보려고 한다.

주어진 rviz_xycar 패키지 내부의 urdf를 rviz에서 구현하고 joint states gui로 구동을 확인해보려고 한다.

우선적으로 joint states publisher패키지를 다음 명령어로 설치할 수 있다.

$ sudo apt install ros-melodic-joint-state-publisher
$ roslaunch rviz_xycar xycar_3d.launch

설치 및 실행후 joint_state_publisher로 rviz 상의 자동차를 제어할 수 있다.

이번에는 move_joint.launch를 실행해보면 앞바퀴가 조금씩 회전하는 모습을 볼 수 있다.

이번에는 바퀴에 토픽을 보내서 8자 주행을 해보려고 한다.

이번에는 새롭게 converter.py와 rviz_8_drive.py를 작성하여 문제를 해결하고자 한다.

Python

- 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, motor_control

    motor_control.angle = angle
    motor_control.speed = speed

    pub.publish(motor_control)


speed = 3
while not rospy.is_shutdown():
    angle = -50
    for _ in range(80):
        motor_pub(angle, speed)
        time.sleep(0.1)

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

    angle = 50
    for _ in range(80):
        motor_pub(angle, speed)
        time.sleep(0.1)

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

 

- converter.py

#!/usr/bin/env python

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, msg_joint_states, l_wheel, r_wheel

def callback(data):
    global pub, msg_joint_states, l_wheel, r_wheel
    msg_joint_states.header.stamp = rospy.Time.now()

    Angle = data.angle

    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)

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

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

 

- rviz_drive.launch

<launch>
    <param name="robot_description" textfile="$(find rviz_xycar)/urdf/xycar_3d.urdf"/>
    <param name="use_gui" value="false"/>

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

 

결과

바퀴가 좌우로 회전하며 움직이는 것을 볼 수 있다.

 

---

Odometry

Odometry는 바퀴가 달린 차량, 로봇이 주행하며 이동한 거리를 측정할 수 있는 센서이다.

해당 센서는 원의 둘레의 길이인 원주와 바퀴의 회전수로 이동 거리를 계산한다.

하지만, 자동차의 경우 앞 바퀴에서 회전시 꺾이는 각도, 그리고 이동 거리도 다르기 때문에 Ackermann Steering에 대해 공부를 해보면 좋을 것 같다.

https://en.wikipedia.org/wiki/Ackermann_steering_geometry

Ackermann steering geometry - Wikipedia

 

해당 페이지에서는 rviz에서 Odometry가 어떻게 사용되는지만 볼 것이다.

odom topic 발행 예제코드는 아래의 링크에서 제공하고 있으므로 처음이라면 한번 해보는 것을 추천한다.

https://gist.github.com/atotto/f2754f75bedb6ea56e3e0264ec405dcf

Publishing Odometry Information over ROS (python)

이 예제를 바탕으로 Odometry가 어떻게 동작하는지 볼 것이다.

제공된 실행파일을 실행하면 Odom을 기준으로 막대가 회전하는 것을 볼 수 있다.

rviz에서 실행된 시뮬레이션 모습

rqt_graph

rviz에서 8자 주행

지금까지 사용한 토픽들로 rviz상에서 8자 주행을 해보자.

이전에 만든 converter.py는 그대로 사용하며, 추가로 odom_8_drive.py와 rviz_odom.py를 추가하여 코드를 작성할 것이다.

odom_8_drive.py도 기존의 rviz_8_drive.py의 코드를 가져와서 사용해도 된다.

- rviz_odom.py

#!/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('odom_pub')
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()

cur_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(30.0)

cur_speed = 0.4
wheel_base = 0.2
x_ = 0
y_ = 0
yaw_ = 0
Angle = 0

while not rospy.is_shutdown():
    cur_time = rospy.Time.now()

    dt = (cur_time - last_time).to_sec()

    cur_steering_angle = Angle
    cur_angular_velocity = cur_speed * math.tan(cur_steering_angle) / wheel_base

    x_dot = cur_speed * cos(yaw_)
    y_dot = cur_speed * sin(yaw_)
    x_ += x_dot * dt
    y_ += y_dot * dt
    yaw_ += cur_angular_velocity * dt

    odom_quat = tf.transformations.quaternion_from_euler(0, 0, yaw_)

    odom_broadcaster.sendTransform(
    (x_, y_, 0.),
    odom_quat,
    cur_time,
    "base_link",
    "odom"
    )
    
    odom = Odometry()
    odom.header.stamp = cur_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(x_, y_, 0), Vector3(0, 0, yaw_))

    odom_pub.publish(odom)

    last_time = cur_time
    r.sleep()

 

- rviz_odom.launch

<launch>
    <param name="robot_description" textfile="$(find rviz_xycar)/urdf/xycar_3d.urdf"/>
    <param name="use_gui" value="false"/>

    <!-- 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="state_publisher"/>

    <node name="driver" pkg="rviz_xycar" type="odom_8_drive.py" />
    <node name="odometry" pkg="rviz_xycar" type="rviz_odom.py" />
    <node name="converter" pkg="rviz_xycar" type="converter.py" />

</launch>

 

결과