킴 테스트1 PointClouds로 RVIZ화면 구현

scout_odom > scripts

 

1. 기본 형태

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

import rospy
from sensor_msgs.msg import LaserScan, PointCloud
from geometry_msgs.msg import Point32
import math

## Rospy 실행 시 필요한 내용
## Node Name
## Subscriber 만드는 데 필요한 내용
## Data type, Topic Name, Callback function
## Data type --> 패키지 또는 모듈 불러오기에서 정의가 필요
## Data type : /Lidar2D [sensor_msgs/LaserScan]
## Publisher --> sensor_msgs/PointCloud

class lidar_sub():
	def __init__(self):
		rospy.init_node("lidar_subscriber")
		rospy.Subscriber("lidar2D", LaserScan, self.lidar_CB)
		self.point_pub = rospy.Publisher("lidar_xyz", PointCloud, queue_size=1)
		rospy.spin()

	def lidar_CB(self, data):
		publish_data = PointCloud()
		#publish_data.header.stamp = data.header.stamp
		publish_data.header.stamp = rospy.Time.now()
		publish_data.header.frame_id = data.header.frame_id

		"""
		rosmsg show PointCloud
		[sensor_msgs/PointCloud]:
		std_msgs/Header header
		  uint32 seq
		  time stamp
		  string frame_id
		geometry_msgs/Point32[] points
		  float32 x
		  float32 y
		  float32 z
		sensor_msgs/ChannelFloat32[] channels
		  string name
		  float32[] values

		[sensor_msgs/LaserScan]:
		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
		"""

		## ranges 에 대응하는 각도 계산(radian or degree)
		## ranges[0] ~ ranges[359]
		## ranges[0] = angle min
		## ranges[1] = angle_min + 1 * angle_increment
		## ranges[359] = angle_min + 359*angle_increment
		angles = []
		angles_degree = []
		#for i in range(360)
		## ranges = [3, 5, 7, 9]로 돌리면 i = 3, a = 9 식으로 나온다.
		for i, r in enumerate(data.ranges): # 인덱스와 값이 쌍으로 나오게 한다.
		#for a in data.ranges 와 for i in range(390)을 합쳤다.
			tmp_angle = data.angle_min + i * data.angle_increment
			angles.append(tmp_angle)
			angles_degree.append(tmp_angle * 180 / math.pi)
			x = r * math.cos(tmp_angle)
			y = r * math.sin(tmp_angle)
			tmp_point = Point32()
			tmp_point.x = x
			tmp_point.y = y
			tmp_point.z = 2.0
			publish_data.points.append(tmp_point)
			print("(x, y) = ({} {})".format(
				r * math.cos(tmp_angle), r * math.sin(tmp_angle)))
		self.point_pub.publish(publish_data)

		ranges = data.ranges
		## ranges, angles --> r, theta

		#print(angles_degree)
		#print(data.ranges)

		#print(data.header.frame_id)
if __name__ == '__main__':
	sub = lidar_sub()

 

 

2. CIRCLE로 구현

#! /usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan, PointCloud
from geometry_msgs.msg import Point32
import math

class Circle_sub():
	def __init__(self):
		rospy.init_node("lidar_subscriber")
		rospy.Subscriber("lidar2D", LaserScan, self.lidar_CB)
		self.point_pub = rospy.Publisher("lidar_xyz", PointCloud, queue_size=1)
		rospy.spin()

	def lidar_CB(self, data):
		publish_data = PointCloud()
		publish_data.header.stamp = rospy.Time.now()
		publish_data.header.frame_id = data.header.frame_id

		angles = []
		angles_degree = []
		for i,r in enumerate(data.ranges):
			tmp_angle = data.angle_min + i * data.angle_increment
			angles.append(tmp_angle)
			angles_degree.append(tmp_angle * 180 / math.pi)
			x = 5 * math.cos(tmp_angle)
			y = 5 * math.sin(tmp_angle)
			z = 25 - math.pow(x, 2) - math.pow(y, 2)
			tmp_point = Point32()
			tmp_point.x = x
			tmp_point.y = y
			tmp_point.z = math.pow(z, 1/2)
			publish_data.points.append(tmp_point)
		self.point_pub.publish(publish_data)
		ranges = data.ranges


if __name__ == "__main__":
	sub = Circle_sub()

 

 

 

3. SPHERE로 구현

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

import rospy
import math
from sensor_msgs.msg import PointCloud
from geometry_msgs.msg import Point32

class sphere_drawer():
    def __init__(self):
        rospy.init_node("sphere_drawer")
        self.pub = rospy.Publisher("sphere_pc", PointCloud, queue_size=1)
        rospy.Timer(rospy.Duration(1.0), self.timer_CB)
        rospy.spin()

        self.rate = rospy.Rate 0

    def timer_CB(self, event):
        tmp_data = PointCloud()
        tmp_data.header.stamp = rospy.Time.now()
        tmp_data.header.frame_id = "velodyne"

        num_points = 50
        radius = 5

        for i in range(num_points):
            for j in range(num_points):
                theta = i * 2 * math.pi / num_points ## z방향에 대한 각도
                phi = j * 2 * math.pi / num_points ## x, y 평면에서의 각
                tmp = Point32()
                tmp.x = radius * math.sin(theta) * math.cos(phi)
                tmp.y = radius * math.sin(theta) * math.sin(phi)
                tmp.z = radius * math.cos(theta)
                tmp_data.points.append(tmp)

        self.pub.publish(tmp_data)
        #rospy.loginfo("publish points")

if __name__ == '__main__':
    sphere_drawer()