[2023-04-04] 3. RVIZ 라이다 정보 Range 표시

April 9, 2023

RVIZ 라이다 정보 Range 표시

데이터 전달 흐름

lidar_topic.bag 파일에서 저장된 라이다 토픽을 ROSBAG으로 하나씩 발행
scan 토픽에서 장애물까지의 거리정보를 꺼내 scan1, scan2, scan3, scan4 4개의 토픽에 각각 담아 발행
RVIZ 에서 Range 형식으로 거리정보를 시각화하여 보여줌

파일 구성

lidar_urdf.launch
lidar_urdf.py
lidar_urdf.urdf (모델링 URDF 파일)
lidar_urdf.rviz (rviz 설정)

lidar_urdf.py

LaserScan 타입의 데이터를 Range 타입으로 변경
scan 토픽을 받아, 4개의 scan1~4의 토픽을 발행
$ rosmsg show sensor_msgs/LaserScan
$ rosmsg show sensor_msgs/Range

lidar_urdf.py

#!/usr/bin/env python

import serial, time, rospy
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Range
from std_msgs.msg import Header # LaserScan, Range, Header 임포트

lidar_points = None

def lidar_callback(data): # 라이다 토픽이 도착하면 실행되는 콜백 함수
	global lidar_points
	lidar_points = data.ranges # 거리 정보를 lidar_points에 옮겨 담기

rospy.init_node('lidar') # 노드 생성 
rospy.Subscriber("/scan", LaserScan, lidar_callback, queue_size = 1) # 구독 준비

pub1 = rospy.Publisher('scan1', Range, queue_size = 1)
pub2 = rospy.Publisher('scan2', Range, queue_size = 1)
pub3 = rospy.Publisher('scan3', Range, queue_size = 1)
pub4 = rospy.Publisher('scan4', Range, queue_size = 1) # 4개의 토픽 발행 준비

msg = Range()
h = Header() # Range 데이터 타입의 메시지 만들준비 하기

msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02
msg.max_range = 2.0
msg.field_of_view = (30.0/180.0) * 3.14 # Range 메시지 내용 채우기

while not rospy.is_shutdown():	
	if lidar_points == None:
		continue # 아직 도착하지 않았을 때 기다리기
	
	h.frame_id = "front"
	msg.header = h
	msg.range = lidar_points[90]
	pub1.publish(msg)

	h.frame_id = "right"
	msg.header = h
	msg.range = lidar_points[180]
	pub2.publish(msg)

	h.frame_id = "back"
	msg.header = h
	msg.range = lidar_points[270]
	pub3.publish(msg)

	h.frame_id = "left"
	msg.header = h
	msg.range = lidar_points[0]
	pub4.publish(msg)
	
	time.sleep(0.5)

lidar_urdf.launch

<launch>
	<param name="robot_description" textfile="$(find rviz_lidar)/urdf/lidar_urdf.urdf" />
	<param name="use_gui" value="true" />
	
	<node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" args="-d $(find rviz_lidar)/rviz/lidar_urdf.rviz" />
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
	
	<node name="rosbag_play" pkg="rosbag" type="play" output="screen" required="true" args="$(find rviz_lidar)/src/lidar_topic.bag" />
	<node name="lidar" pkg="rviz_lidar" type="lidar_urdf.py" output="screen" />
</launch>

lidar_urdf.urdf

World의 중앙에 빨강 박스를 만들고 4 방향에 센서 프레임을 연결
- base_link에 가로, 세로 20cm 빨강 박스를 baseplate를 만들어 연결
- 센서는 x, y축을 기준으로 중심에서 10cm씩 이동시켜서 박스의 끝 부분에 배치

lidar_urdf.urdf

<?xml version="1.0" ?>

<robot name="xycar" xmlns:xacro="http://www.ros.org/wiki/xacro">
	
	<link name="base_link" />
		<link name="baseplate">
			<visual>
				<material name="red" />
				<origin rpy="0 0 0" xyz="0 0 0" />
				<geometry>
					<box size="0.2 0.2 0.07" />
				</geometry>
			</visual>
		</link>
			
	<joint name="base_link_to_baseplate" type="fixed">
		<parent link="base_link" />
		<child link="baseplate" />
		<origin rpy="0 0 0" xyz="0 0 0" />
	</joint>
	
	<link name="front" />
	
	<joint name="baseplate_to_front" type="fixed">
		<parent link="baseplate" />
		<child link="front" />
		<origin rpy="0 0 0" xyz="0.1 0 0" /> # rpy(롤,피치,요) 모두 0 이므로 회전이 없고 중심에서 x축으로 0.1미터 전진해서 위치
	</joint>
	
	<link name="back" />
	
	<joint name="baseplate_to_back" type="fixed">
		<parent link="baseplate" />
		<child link="back" />
		<origin rpy="0 0 3.14" xyz="-0.1 0 0" /> # y의 값이 3.14(파이) 이므로 180도 회전 및 중심에서 x 축으로 -0.1미터 전진해서 위치
	</joint>
	
	<link name="left" />
	
	<joint name="baseplate_to_left" type="fixed">
		<parent link="baseplate" />
		<child link="left" />
		<origin rpy="0 0 1.57" xyz="0 0.1 0" /> # y의 값이 1.57(절반파이) 이므로 90도 회전 및 중심에서 Y 축으로 0.1미터 전진해서 위치
	</joint>
	
	<link name="right" />
	
	<joint name="baseplate_to_right" type="fixed">
		<parent link="baseplate" />
		<child link="right" />
		<origin rpy="0 0 -1.57" xyz="0 -0.1 0" /> # y의 값이 -1.57(절반파이) 이므로 -90도 회전 및 중심에서 Y 축으로 -0.1미터 전진해서 위치
	</joint>
	
	<material name="black">
		<color rgba="0.0 0.0 0.0 1.0" />
	</material>
	
	<material name="blue">
		<color rgba="0.0 0.0 0.8 1.0" />
	</material>
	
	<material name="green">
		<color rgba="0.0 0.8 0.0 1.0" />
	</material>
	
	<material name="red">
		<color rgba="0.8 0.0 0.0 1.0" />
	</material>
	
	<material name="white">
		<color rgba="1.0 1.0 1.0 1.0" />
	</material>

	<material name="orange">
		<color rgba="1.0 0.423529411765 0.0392156862745 1.0" />
	</material>
</robot>