7. Publisher & Subscriber node 작성 - python

이전 예제의 launch file을 다시 살펴볼까요??

example_1.launch

<?xml version="1.0" encoding="UTF-8"?>

<launch>
    <!-- launch first exmaple -->
    <node pkg="topic_tutorial" type="cmdvel_pub.py" name="drive_forward"  output="screen">
    </node>
</launch>

이제 우리는, 패키지, node, topic, launch file에 대해 알고 있기 때문에 위 내용을 이해할 수 있게 되었습니다.

그럼, 드디어 코딩으로 넘어가서, example1_publisher.py를 파헤쳐볼까요?

publisher node 작성

cmdvel_pub.py

#! /usr/bin/env python

"""
Twist Publisher example

referenced from wiki.ros.org

url: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
"""

import time
import rospy
from geometry_msgs.msg import Twist

rospy.init_node("drive_forward")
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
r = rospy.Rate(1)  # 1 Hz

forward = Twist()
stop = Twist()

forward.linear.x = 0.5
stop.linear.x = 0.0

start_time = time.time()

rospy.loginfo("==== DriveForward node Started, move forward during 5 seconds ====\n")


while not rospy.is_shutdown():
    if time.time() - start_time < 5.0:
        pub.publish(forward)
    else:
        rospy.logwarn(" 5 seconds left, Stop!! ")
        pub.publish(stop)
        break
    r.sleep()

github 파일에 주석은 제외하였습니다.

shebang line - windows에서는 별 의미가 없습니다.

#! /usr/bin/env python

The #! magic, details about the shebang/hash-bang mechanism on various Unix flavours

파이썬 ros 프로그래밍을 위한 rospy , 이전 시간에 살펴봤던 message type인 Twist를 import 하고 있습니다.

import time
import rospy
from geometry_msgs.msg import Twist

message type과 import의 실제 구현 시에 어떻게 코드가 작성되는지 유의해서 보세요 🙂

$ rosnode info /drive_forward
--------------------------------------------------------------------------------
Node [/drive_forward]
Publications: 
 * /cmd_vel [geometry_msgs/Twist]
 * /rosout [rosgraph_msgs/Log]

publisher node 생성, queue_size에 대한 추가 설명

rospy.init_node('drive_forward')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1 )
r = rospy.Rate(1)

init_node API를 통해 node를 생성하며, 이 안에 들어가는 node 이름으로 구별되기 때문에, 반드시 유일해야 합니다!!

우리는 cmd_vel로 topic publish를 하는게 목적이죠? topic message 타입은 Twist 였구요.

rospy.Publisher('<topic-name>', <message-type>, <queue-size>)

이렇게 publisher 객체를 생성합니다.

publish할 Twist message에 적절한 속도 값을 채워줍니다.

forward = Twist()
stop = Twist()

forward.linear.x = 0.5
stop.linear.x = 0.0

출처 : http://docs.ros.org/

💡

linear의 단위는 m/s 이며, angular의 단위는 rad/s 입니다. pi = 3.14

어떤 부분을 수정해야 할 지 모르겠다면, 혹은 위와 같이 message type이 담고 있는 세부 내용들을 보고 싶다면 이전 강의에서 배웠듯이

앞선 rosmsg 커맨드를 사용하거나

검색을 통해 찾을 수 있습니다.

자, 이제 publish를 해봅니다.

start_time = time.time()

rospy.loginfo("==== DriveForward node Started, move forward during 5 seconds ====\n")

while not rospy.is_shutdown():
    if time.time() - start_time < 5.0:
        pub.publish(forward)
    else:
        rospy.logwarn(" 5 seconds left, Stop!! ")
        pub.publish(stop)
        break
    r.sleep()

pub.publish()를 통해 publish하며, 이 안에는 publish 할 message가 들어갑니다.

이 메세지는 우리가 원하는 정보를 담고 있습니다.

디버깅을 위해 로그를 찍은 부분이 보이시나요? rospy에는 다음과 같은 다양한 log type을 제공합니다.

rospy.loginfo

rospy.logdebug

rospy.logwarn

rospy.logerr

rospy.logfatal

이들의 구체적인 차이는 링크로 대체하도록 하고 간단한 예제를 살펴본 다음 넘어가도록 하겠습니다.

rospy.Rate() / r.sleep()

r = rospy.Rate(1) # 1 Hz

(...)
while (something):
    r.sleep()

💡

로봇을 포함한 임베디드 프로그래밍을 하다보면, 주기적으로 작업할 일이 많습니다. 지금과 같이 로봇에 제어 신호를 지속적으로 송신하는 경우도 있고, 센서 데이터를 수집하는 경우도 이에 해당합니다.

주기적으로 발생되는 일의 일정한 시간 간격을 보장해야 하는 경우 rospy.Rate()로 topic이 publish하는 주기를 설정할 수 있습니다.

지금까지 publisher를 생성하고, 원하는 topic에 publish하는 방법을 알아보았습니다.

예제로 돌아와서, 로봇이 주차 구역 안에 꼭 맞춰 들어오도록 전진하는 시간을 바꿔보거나, 속도를 바꿔보는 응용도 한 번 해보세요 😉

Subscriber node 작성

현재 우리가 사용중인 로봇에는 여러 센서들이 부착되어 있는데요. 이번 시간에는 그중에서 2D Lidar를 사용해 볼 것이며, 이 센서로부터 topic subscriber를 구현해 보도록 하겠습니다.

2D Lidar란?

출처: hokuyo-jp

추가 설명: What is LiDAR technology?

Laser Scan Subscriber 예제

이번에도 우선 예제를 실행해 보도록 하겠습니다. 다음의 커맨드 라인을 입력해주세요!

$ roslaunch gcamp_gazebo gazebo_world.launch
$ roslaunch topic_tutorial example2.launch

배열 안에 수많은 알 수 없는 숫자들이 보입니다... 이것들은 2D Lidar의 raw 데이터입니다.

지금은 이걸 어떻게 포착해서 읽을 수 있을지 감도 안잡힙니다 😕

이런 상황을 다루기 위해, ROS는 센서 데이터를 비롯한 로봇의 여러 상태를 3차원으로 시각화하여 볼 수 있는 멋있는 툴을 제공하고 있습니다.

rviz, 즉 ros의 3차원 visualization 툴입니다!!

https://www.youtube.com/watch?v=i--Sd4xH9ZE

지금은 우선 lidar data를 렌더링해주는 rviz라는 툴이 있다는 것 정도만 알아두고, topic subscribe에 집중하도록 하겠습니다.

laser_sub.py

#! /usr/bin/env python

"""
2D Lidar Subscriber example

referenced from wiki.ros.org

url: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
"""

import rospy
from sensor_msgs.msg import LaserScan


def callback(data):
    # print(data.ranges[360])
    print(data)


rospy.loginfo(
    "==== Laser Scan Subscriber node Started, move forward during 10 seconds ====\n"
)

rospy.init_node("laser_scan")
sub = rospy.Subscriber("/scan", LaserScan, callback)
rospy.spin()

앞선 topic publisher 코드와 중복되는 부분이 보이시나요?

우선, 이미 알고 있는 내용을 토대로 위 코드를 훑어봅시다.

import 부분을 살펴볼까요??

import rospy
from sensor_msgs.msg import LaserScan

이 node는 message type으로 sensor_msgs/LaserScan 형식을 사용하고 있음을 알 수 있습니다.

출처 : http://docs.ros.org/

rospy.init_node('laser_scan')
sub = rospy.Subscriber('/scan', LaserScan, callback)

node의 이름, 사용하는 topic의 이름을 알 수 있습니다. /scan topic에 대해서 더 자세히 알고 싶을때는 어떻게 했었죠? ⇒ rostopic info /scan

$ rostopic info /scan
Type: sensor_msgs/LaserScan

Publishers: 
 * /gazebo (http://localhost:41665/)

Subscribers: 
 * /rviz (http://localhost:45421/)

message type으로 앞서 말하였듯이 sensor_msgs/LaserScan를 사용하고 있음을 알 수 있습니다.

LaserScan에 대한 정보를 터미널에서 알고 싶다면? ⇒ rosmsg info

$ rosmsg info 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

이들 중에서 실질적으로 lidar data를 담고 있는 부분은 ranges입니다.

ranges는 길이 720의 list로, 다음과 같이 0.25도의 분해능으로 scan된 값을 저장합니다.

Subscriber를 생성하는 부분을 좀 더 유심히 살펴볼까요?

sub = rospy.Subscriber('/scan', LaserScan, callback)

rospy.Subscriber('<topic-name>', <message_type>, callback)

callback이라는 새로운 함수가 보입니다!!!

callback

구글 환율을 사용해 본 적 있으신가요??

숫자를 바꿀 때마다 변환된 값이 계산되어 표시됩니다. 그리고 callback이 바로 이렇게 동작하죠

이를 파이썬으로 이를 구현한다면 이와 같을 것입니다.

>>> def exchange(val):
...     """The callback."""
...     return val * 1092.92
...
>>> def value_in(func, val):
...     return func(val)
...
>>> value_in(exchange, 5)
5464.60

그리고 지금 우리가 보고 있는 Subscriber도 이런 식으로 동작합니다.

rospy.spin()이 순환될 때마다 callback이 실행되는 것이죠!!

def callback(data):
    # data.ranges
    print(data)

(...)
sub = rospy.Subscriber('/scan', LaserScan, callback)
rospy.spin()

subscribe 받은 data에서 ranges만 선택하여 작업하고 싶다면 다음과 같이 접근 가능합니다.

def callback(data):
    laser_ranges =  data.ranges
		print(laser_ranges)

rospy.sleep() rospy.spin()

publish 예제에서는 rospy.Rate()를 사용했는데, 이번 subscriber 예제에서는 rospy.spin()을 사용했죠?

일정한 시간 간격을 두고 작업해야 할 프로세스에는 시간을 지정할 수 있는 rospy.Rate()를 주로 사용한다고 합니다.

일정 시간마다 센싱된 데이터를 처리하거나, 수집하는 로봇을 예로 들 수 있겠네요 🙂

손쉬운 subscribe

publish 되고 있는 데이터를 단순히 보고 싶은 경우, 굳이 파이썬 코드를 짜지 않아도 앞선 rviz를 통해 시각화가 가능했죠?? 그런데, rviz가 아닌 터미널을 통해서도 topic을 다룰 수 있는 방법이 있답니다.

rostopic 커맨드 사용

$ rostopic echo /scan
(...)

publish되는 값을 보고 싶다면, echo를 사용하면 됩니다.

우리는 주어진 시간 제약이 있어 모두 살피지 못했지만, rostopic pub와 같이 publish도 가능하는 등, rostopic에는 미리 편의를 위한 여러 커맨드들이 준비되어 있답니다.

모두 ROS의 기여자분들이 미리 개발해두신 것이겠지요? 감사합니다 🙂🙂

출처 : ROS Wiki

[Assignment] Parking!!

지금까지 배운 내용들을 통해 다음과 같이 로봇을 주차 구역 안에 알맞게 이동시키는 프로젝트입니다!!

#! /usr/bin/env python

"""
Parking Assignment Answer

Try it out!!
"""

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist


def callback(data):
    laser_range = data.ranges
    cmd_vel = Twist()
	
		# 이 부분을 채워보세요!!

    pub.publish(cmd_vel)


rospy.init_node("parking", disable_signals=True)
rospy.loginfo("==== parking node Started ====\n")

pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
sub = rospy.Subscriber("/scan", LaserScan, callback)

rospy.spin()

여러 방식이 있을 수 있습니다. 제가 구현한 코드의 링크를 남길 테니, 복습도 해보고, 이리저리 분석해보시면서 진행해 보세요!!

저작자표시 비영리 동일조건

Swiming_Lab

7. Publisher & Subscriber node 작성 - python

7. Publisher & Subscriber node 작성 - python

publisher node 작성

cmdvel_pub.py

Subscriber node 작성

Laser Scan Subscriber 예제

laser_sub.py

callback

[Assignment] Parking!!

티스토리툴바

7. Publisher &amp; Subscriber node 작성 - python

publisher node 작성

cmdvel_pub.py

Subscriber node 작성

Laser Scan Subscriber 예제

laser_sub.py

callback

[Assignment] Parking!!

티스토리툴바

7. Publisher & Subscriber node 작성 - python