[ROS] Range 메시지 RVIZ 시각화 + LiDAR data 활용

RVIZ 시각화

1. lidar_range.py

#!/usr/bin/env python

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

rospy.init_node("lidar")
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 = "lidarXY"
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)

2. 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>

3. 권한 설정

$ chmod +x lidar_range.py

4. 실행

$ roslaunch rviz_lidar lidar_range.launch

아무것도 안나오기 때문에 설정필요

5. rviz 설정

• fixed frame을 lidarXY로 설정
• add -> by topic -> range 하나씩 추가
  
• range의 color 원하는 것으로 변경
  

6. 노드, 토픽 확인

$ rqt_graph

7. 저장

---

lidar 데이터 시각화

1. lidar_urdf.py

#!/usr/bin/env python

import rospy, time
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)

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()
#wrong answer
# h.frame_id = "lidarXY"
# msg.header = h
msg.radiation_type = Range().ULTRASOUND
msg.field_of_view = (15.0/180.0)*3.14
msg.min_range = 0.02
msg.max_range = 2

while not rospy.is_shutdown():
    if lidar_points == None :
        continue

    # msg.header.stamp = rospy.Time.now()

    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)

2. 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"/>

    <!--lidar topic publish-->
    <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>

3. 권한 설정

4. urdf 문법적 오류 확인

$ check_urdf lidar_urdf.urdf

5. 실행

• 설정
  add->robot model / topic에서 range들
  

• 다 같은 방향을 향하고 있다....
  

• msg의 header.frame_id를 front, left, back, right로 각각 설정해주니 된다. 왤까...? urdf의 link name과 같아서 연결되는 것 같다. (아마도 robot_state_publish가 연결해주는 것 같다. 왜냐면 얘가 tf를 수행하기 때문에?)