[simulator] publisher

# publishing_data_Poly.polygon.points = Point32() (X)
		publishing_data_Poly.polygon.points = [Point32(x=1.0, y=1.0, z=1.0),Point32(x=3.0, y=3.0, z=3.0), Point32(x=1.0, y=6.0, z=6.0)]

메세지 타입 확인하는 방법

kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

kw-cobot@kwcobot-HGDT-Series:~$ python
Python 2.7.17 (default, Feb 27 2021, 15:10:58) 
[GCC 7.5.0] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> import geometry_msgs
>>> geometry_msgs
<module 'geometry_msgs' from '/opt/ros/melodic/lib/python2.7/dist-packages/geometry_msgs/__init__.py'>
>>> 

roscore 한 상태로

ROS1 activated
kw-cobot@kwcobot-HGDT-Series:~$ rosrun scout_sim_launch publisher.py 
Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.
work
work
work
work
work
work
work
work
work
work
work
work
work
work
work
work
work

 

 

def timer_CB(self, event):
publishing_data = Twist()
#publising_data의 내부를 채우는 과정
"""
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z
"""

#publishing_data.linear.x
print(dir(Twist))
print(dir(publishing_data))

를 코드에 넣고 출력해보자

kw-cobot@kwcobot-HGDT-Series:~$ rosrun scout_sim_launch publisher.py 
['__class__', '__delattr__', '__doc__', '__eq__', '__format__', '__getattribute__', '__getstate__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__setstate__', '__sizeof__', '__slots__', '__str__', '__subclasshook__', '_check_types', '_connection_header', '_full_text', '_get_types', '_has_header', '_md5sum', '_slot_types', '_type', 'angular', 'deserialize', 'deserialize_numpy', 'linear', 'serialize', 'serialize_numpy']
['__class__', '__delattr__', '__doc__', '__eq__', '__format__', '__getattribute__', '__getstate__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__setstate__', '__sizeof__', '__slots__', '__str__', '__subclasshook__', '_check_types', '_connection_header', '_full_text', '_get_types', '_has_header', '_md5sum', '_slot_types', '_type', 'angular', 'deserialize', 'deserialize_numpy', 'linear', 'serialize', 'serialize_numpy']
Exception in thread Thread-4:
Traceback (most recent call last):
  File "/usr/lib/python2.7/threading.py", line 801, in __bootstrap_inner
    self.run()
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 234, in run
    self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
  File "/home/kw-cobot/catkin_ws/src/scout_sim_launch/scripts/publisher.py", line 38, in timer_CB
    self.publisher1.publish(publish_data)

publishing_data나 Twist 의 type는 같다.

_가 앞 뒤로 붙은 것들은 생성자 같은 역할을 한다.

_가 앞에 붙은 것은 클래스 내부에서 사용하는 변수(필드) 이다.

_가 붙지 않은 것들은  변수 또는 함수이다. linear와 angular가 내부 변수로 접근할 수 있다는 점을 알 수 있다.

 

 

 

rviz에 나오는 메세지 타입들: 그리거나 마커를 사용할 때 사용한다.

반드시 아래 변수들을 하나씩 채울 수 있어야 한다.

kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show visualization_msgs/Marker
uint8 ARROW=0
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8
uint8 TEXT_VIEW_FACING=9
uint8 MESH_RESOURCE=10
uint8 TRIANGLE_LIST=11
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string ns
int32 id
int32 type
int32 action
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
geometry_msgs/Vector3 scale
  float64 x
  float64 y
  float64 z
std_msgs/ColorRGBA color
  float32 r
  float32 g
  float32 b
  float32 a
duration lifetime
bool frame_locked
geometry_msgs/Point[] points
  float64 x
  float64 y
  float64 z
std_msgs/ColorRGBA[] colors
  float32 r
  float32 g
  float32 b
  float32 a
string text
string mesh_resource
bool mesh_use_embedded_materials

 

 

폴리컨 타입: rviz 상에서 stamp 타입을 이용하여 다각형을 그릴 수 있다.

kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show geometry_msgs/Polygon
geometry_msgs/Point32[] points
  float32 x
  float32 y
  float32 z

이거 말고

PolygonStamped

kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show geometry_msgs/PolygonStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Polygon polygon
  geometry_msgs/Point32[] points
    float32 x
    float32 y
    float32 z

Point32[] type 같이 포인트들을 다루는 경우

import Point32 를 따로 해주어야 한다.

 

# publishing_data_Poly.polygon.points = Point32() (X)
		publishing_data_Poly.polygon.points = [Point32(x=1.0, y=1.0, z=1.0),Point32(x=3.0, y=3.0, z=3.0), Point32(x=1.0, y=6.0, z=6.0)]

 

 

lst1 = [] #Int32[]와 같은 형식
lst1.append(0)
lst1.append(Point32())

tmp_data = Point32()
tmp_data.x = 1
tmp_data.y = 1
tmp_data.z = 3

lst.append(tmp_data) <-- Point32[]

Point32인 유형을 담는 리스트라고 생각하면 된다.

 

 

 

#!/usr/bin/env python
#-*- coding:utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PolygonStamped
from geometry_msgs.msg import Point32

NODE_NAME = "publisher_tester"
TOPIC_NAME = "cmd_vel"
#MSG_TYPE = Twist
MSG_TYPE = PolygonStamped
PUBLISH_HZ = 10.0

class Topic_publisher():
	def __init__(self):
		rospy.init_node(NODE_NAME)
		self.publisher1 = rospy.Publisher(TOPIC_NAME, MSG_TYPE, queue_size=1)
		rospy.Timer(rospy.Duration(1.0/PUBLISH_HZ), self.timer_CB2)
		rospy.spin()


	def timer_CB2(self, event):
		publishing_data_Poly = PolygonStamped()
		#publising_data의 내부를 채우는 과정
		#publishing_data_Poly.header.seq = 0
		publishing_data_Poly.header.stamp = rospy.Time.now()
		# publishing_data_Poly.header.stamp = rospy.Time.from_sec(0)
		publishing_data_Poly.header.frame_id = "triangle"
		# publishing_data_Poly.polygon.points = Point32() (X)
		publishing_data_Poly.polygon.points = [Point32(x=0.0, y=0.0, z=0.0),Point32(x=3.0, y=3.0, z=0.0), Point32(x=6.0, y=0.0, z=0.0)]
		self.publisher1.publish(publishing_data_Poly)

	def timer_CB(self, event):
		publishing_data = Twist()
		#publising_data의 내부를 채우는 과정
		publishing_data.linear = Vector3(1.0, 2.0, 3.0)
		# publishing_data.linear.x = 1.0 식으로 넣어줘도 된다.
		publishing_data.angular = Vector3(0, 0, 1.0)
		self.publisher1.publish(publish_data)
		"""
		geometry_msgs/Vector3 linear
			  float64 x
			  float64 y
			  float64 z
			geometry_msgs/Vector3 angular
			  float64 x
			  float64 y
			  float64 z
		"""
		#publishing_data.linear.x
		#print(dir(Twist))
		#print(dir(publishing_data))
		#print(publishing_data.deserialize)
		#print(publishing_data.linear)
		


if __name__=="__main__":
	Topic_publisher()

 

 

---

2. 3차원 도형 만들기

 

http://wiki.ros.org/rviz/DisplayTypes

rviz/DisplayTypes - ROS Wiki

여기에 적혀 있는 애들이 rviz에서 볼 수 있는 메세지 타입이다.

 

http://wiki.ros.org/rviz/DisplayTypes/Marker

rviz/DisplayTypes/Marker - ROS Wiki

 

 

 

kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show visualization_msgs/Marker
uint8 ARROW=0
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8
uint8 TEXT_VIEW_FACING=9
uint8 MESH_RESOURCE=10
uint8 TRIANGLE_LIST=11
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string ns
int32 id
int32 type
int32 action
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
geometry_msgs/Vector3 scale
  float64 x
  float64 y
  float64 z
std_msgs/ColorRGBA color
  float32 r
  float32 g
  float32 b
  float32 a
duration lifetime
bool frame_locked
geometry_msgs/Point[] points
  float64 x
  float64 y
  float64 z
std_msgs/ColorRGBA[] colors
  float32 r
  float32 g
  float32 b
  float32 a
string text
string mesh_resource
bool mesh_use_embedded_materials

 

stamp, fram_id, type, action (ADD = 0),

Position 위치를 정한다.

quarternion 회전 여부

 

scale 크기 결정

color, a는 투명도로 0.5로 바꿔야 보인다.

그 외는 신경 쓰지 않아도 된다.

 

 

원기둥 도형 하나만 보여주는 코드

#!/usr/bin/env python
#-*- coding:utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PolygonStamped
from geometry_msgs.msg import Point32
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray


NODE_NAME = "publisher_tester"
TOPIC_NAME = "cmd_vel1"
#MSG_TYPE = Twist
#MSG_TYPE = PolygonStamped
MSG_TYPE = Marker
#MSG_TYPE = MarkerArray
PUBLISH_HZ = 10.0

class Topic_publisher():
	def __init__(self):
		rospy.init_node(NODE_NAME)
		self.publisher1 = rospy.Publisher(TOPIC_NAME, MSG_TYPE, queue_size=1)
		rospy.Timer(rospy.Duration(1.0/PUBLISH_HZ), self.timer_CB3)
		rospy.spin()


	def timer_CB3(self, event):
		# visualization)msgs/Marker
		publishing_data_marker = Marker()
		publishing_data_marker.header.stamp = rospy.Time.now()
		publishing_data_marker.header.frame_id = "object"
		publishing_data_marker.type = 3
		publishing_data_marker.action = 0
		publishing_data_marker.pose.position.x = 1.0
		publishing_data_marker.pose.position.y = 1.0
		publishing_data_marker.pose.position.z = 3.0
		publishing_data_marker.pose.orientation.x = 1.0
		publishing_data_marker.pose.orientation.y = 1.0
		publishing_data_marker.pose.orientation.z = 0.0
		#publishing_data_marker.scale = Vector3(1.0, 1.0, 0.0)
		publishing_data_marker.scale.x = 1.0
		publishing_data_marker.scale.y = 1.0
		publishing_data_marker.scale.z = 1.0
		publishing_data_marker.color.r = 0
		publishing_data_marker.color.g = 141
		publishing_data_marker.color.b = 98
		publishing_data_marker.color.a = 0.8
		self.publisher1.publish(publishing_data_marker)

 

 

여러 개의 도형을 두려면 visualization_msgs/MarkerArray 를 import 하고

여러 마커를 list에 append 하면 된다.

 

 

http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Points%20and%20Lines

rviz/Tutorials/Markers: Points and Lines - ROS Wiki

이런식으로 튜토리얼 찾으면  좋다.

 

frame_id와 stamp는 꼭 찍자!!