자이카 - 라이다 센서

ROS 라이다 센서의 노드와 토픽

• 패키지 위치 - xycar_wa/src/xycar_device/xycar_lidar
• 토픽 이름 - /scan

rosmsg show sensor_msgs/LaserScan

# Output
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

센서를 활용해 보자

• 패키지 만들기

catkin_create_pkg my_lidar std_msgs rospy

• 패키지 구조
  • 소스 파일 - lidar_scan.py
  • 런치 파일 - lidar_scan.launch

# lidar_scan.py
#!/usr/bin/env python

import rospy
import time
from sensor_msgs.msg import LaserScan

lidar_points = None

def lidar_callback(data):
    global lidar_points
    lidar_points = data.ranges

rospy.init_node('my_lidar', anonymous=True)
rospy.Subscriber("/scan", LaserScan, lidar_callback, queue_size=1)

while not rospy.is_shutdown():
    if lidar_points == None:
        continue
    
    rtn = ""
    for i in range(12):
        rtn += str(format(lidar_points[i*30],'.2f')) + ", "

    print(rtn[:-2])
    time.sleep(1.0)

# lidar_scan.launch
<launch>
  <node name="xycar_lidar" pkg="xycar_lidar"  type="xycar_lidar" output="screen">
  <node name="my_lidar" pkg="my_lidar" type="lidar_scan.py" output="screen"/>
</launch>

• 실제 라이다 센서에서 받아오는 값을 보면

• 주변을 30도씩 12개로 거리 정보를 표현하는 것을 볼 수 있다.
  • inf는 감지 범위를 벗어난 것(혹은 빈 공간)

Lidar 센서 데이터 시각화

패키지 생성

• rviz_lidar패키지 생성

# 위치는 ~/xycar_ws/src
catkin_create_pkg rviz_lidar rospy tf geometry_msgs urdf rviz xacro

• 패키지 구성
  • rviz 파일 - lidar_3d.rviz(자동 생성)
  • 런치 파일 - lidar_3d.launch

# lidar_3d.launch
<launch>
  <node name="xycar_lidar" pkg="xycar_lidar"  type="xycar_lidar" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyRPL"/>
    <param name="serial_baudrate"     type="int"    value="115200"/>
    <param name="frame_id"            type="string" value="laser"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
    <param name="scan_mode"           type="string" value="Express"/>
  </node>

  <node name="my_lidar" pkg="my_lidar" type="lidar_scan.py" output="screen"/>
</launch>

rosbag

• 라이다 장치가 없는 경우, rosbag프로그램을 통해 /scan토픽을 대신 발행해 사용할 수 있다.
  • ROS에서 제공하는 기능으로,
    • 라이다에서 발행하는 /scan토픽을 저장해 놓은 파일(lidar_topic.bag)을 사용해
      • 시간 간격에 맞추어 /scan토픽을 발행할 수 있다.
• 런치 파일 역시 바뀐다.

# lidar_3d_rosbag.launch
<launch>
    <!-- rviz display -->
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
          args="-d $(find rviz_lidar)/rviz/lidar_3d.rviz"/>

    <node name="rosbag_play" pkg="rosbag" type="play" output="screen" 
          required="true" args="$(find rviz_lidar)/src/lidar_topic.bag"/>
</launch>

실행

roslaunch rviz_lidar lidar_3d.launch
# 혹은
roslaunch rviz_lidar lidar_3d_rosbag.launch

• RVIZ 설정
  • Add→LaserScan→Topic = /scan
  • Global Options→Fixed Frame = map→laser
  • LaserScan→Size(m) = 0.1 (빨간 점이 커진다)

뭔가 움직인다.

Range데이터를 발행해 뷰어에 표시하기

패키지 구성

• 패키지는 기존 rviz_lidar사용
  • 런치 파일 - lidar_range.launch
  • 소스 파일 - lidar_range.py
  • rviz 파일 - lidar_range.vriz

# lidar_range.launch
<launch>
    <!-- rviz display -->
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
          args="-d $(find rviz_lidar)/rviz/lidar_range.rviz"/>

    <node name="lidar_range" pkg="rviz_lidar" type="lidar_range.py"/>
</launch>

# lidar_range.py
#!/usr/bin/env python

import serial, time, rospy
from sensor_msgs.msg import Range
from std_msgs.msg import Header

rospy.init_node('lidar_range')

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)

msg = Range()
h = Header()
h.frame_id = "sensorXY"
msg.header = h
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

while not rospy.is_shutdown():
	msg.header.stamp = rospy.Time.now()

	msg.range = 0.4
	pub1.publish(msg)

	msg.range = 0.8
	pub2.publish(msg)

	msg.range = 1.2
	pub3.publish(msg)

	msg.range = 1.6
	pub4.publish(msg)

	time.sleep(0.2)

노드와 토픽 관계

• 노드 이름 - lidar_range
• 토픽 이름 - /scan1, /scan2, /scan3, /scan4
  • 메시지 이름 - Range

실행

roslaunch rviz_lidar lidar_range.launch

이런 식으로 표시할 수 있다.

Lidar데이터를 Range로 표시하기

이런걸 만들어 볼 것.

파일 구성

• 패키지는 기존 rviz_lidar
• 파일 목록
  • 런치 파일 - lidar_urdf.launch
  • 소스 파일 - lidar_urdf.py, lidar_topic.bag
  • urdf 파일 - lidar_urdf.urdf
  • rviz 파일 - lidar_urdf.rviz

# 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="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.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

lidar_points = None

def lidar_callback(data):
	global lidar_points
	lidar_points = data.ranges

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)

msg = Range()
h = Header()

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

while not rospy.is_shutdown():
	#msg.header.stamp = rospy.Time.now()
	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.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"/>
	</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"/>
	</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"/>
	</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"/>
	</joint>

	<material name="black">
		<color rgba="0.0 0.0 0.0 1.0"/>
	</material>
	<material name="blue">
		<color rgba="0.0 0.0 1.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>

# 실행
roslaunch rviz_lidar lidar_urdf.launch

라이다 센서의 감지 범위를 Range로 표현할 수 있었다.