rviz上でLidarの光線を可視化するスクリプト

概要

VisualizationMarkerの練習のためにLidarの光線を可視化するスクリプトを作ったので自分用にメモ。

参考

実装

PointCloud2形式のトピックを受信して、Lidarの原点と各点群間を直線で描画する。

# !/usr/bin/python
# -*- coding: utf-8 -*-

from __future__ import print_function

import rospy
from visualization_msgs.msg import *
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
from geometry_msgs.msg import Point
import laser_geometry.laser_geometry as lg
import tf2_ros
import tf2_geometry_msgs
import tf2_sensor_msgs.tf2_sensor_msgs


class Raytrace:
    def __init__(self, pub_name, sub_name, sub_type):
        self.tfBuffer = tf2_ros.Buffer()
        self.listerner = tf2_ros.TransformListener(self.tfBuffer)
        self.lp = lg.LaserProjection()
        self.pub = rospy.Publisher(pub_name, Marker, queue_size=10)
        self.sub = rospy.Subscriber(
            sub_name, sub_type, self.pc2_callback)

    def pc2_callback(self, cloud):
        target_frame = "base_footprint"
        local_sensor_origin = tf2_geometry_msgs.PointStamped()
        local_sensor_origin.header.frame_id = cloud.header.frame_id
        local_sensor_origin.header.stamp = rospy.Time(0)
  
        while True:
            try:
                trans = self.tfBuffer.lookup_transform(
                    target_frame, cloud.header.frame_id, rospy.Time(0), rospy.Duration(1.0))
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
                continue
            marker = Marker()
            marker.header.frame_id = target_frame
            marker.ns = "ray"
            marker.id = 0
            marker.type = Marker.LINE_LIST
            # LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
            marker.scale.x = 0.001
            marker.color.b = 1.0
            marker.color.a = 1.0
            global_sensor_origin = tf2_geometry_msgs.do_transform_point(
                local_sensor_origin, trans)
            origin_point = Point()
            origin_point.x = global_sensor_origin.point.x
            origin_point.y = global_sensor_origin.point.y
            origin_point.z = global_sensor_origin.point.z
            global_cloud = tf2_sensor_msgs.tf2_sensor_msgs.do_transform_cloud(
                cloud, trans)
            for p in pc2.read_points(global_cloud, field_names=(
                    "x", "y", "z"), skip_nans=True):
                target_point = Point()
                target_point.x = p[0]
                target_point.y = p[1]
                target_point.z = p[2]
                marker.points.append(origin_point)
                marker.points.append(target_point)
            self.pub.publish(marker)
            break

if __name__ == "__main__":
    rospy.init_node("visualize_raytrace")
    pub_name = "/visualize_ray/maker"
    sub_name = "/velodyne_points"
    sub_type = PointCloud2
    raytrace = Raytrace(pub_name, sub_name, sub_type)
    rospy.spin()

使い方

• velodyne simulatorと可視化スクリプトを起動したら、rviz上で/visualize_ray/makerトピックを購読して以下の画像のように青い線が表示されたら成功。