Home [데브코스] 4주차 - ROS nodes and topics for lidar sensors
Post
Cancel
Preview Image

[데브코스] 4주차 - ROS nodes and topics for lidar sensors

Posted Mar 10, 2022 Updated Jul 29, 2022 33 min read


라이다 센서 ROS 패키지

라이다 센서 ROS 패키지를 xycar_lidar로 만든다. 토픽의 이름은 /scan으로 할 것이다.

/scan 토픽안에는

  • 타입 : sensor_msgs\/LaserScan
  • 구성
    • std_msgs/Header header # 시퀀스 번호, 시간, 아이디를 담는다. uid32 seq time stamp string frame_id # rviz에 사용되는 값 float32 angle_min float32 angle_max … float32[] ranges # 장애물까지의 거리 정보들을 담는 array float32[] intensities # 물체의 경도


라이다 거리 정보 화면에 출력

패키지 만들기 

1
2
3
4
5
6
7
8
9

xycar_ws
  ⊢ build
  ⊢ devel
  ∟ src
      ∟ my_lidar
          ⊢ launch
              ∟ lidar_scan.launch
          ∟ src
              ∟ lidar_scan.py

1

xycar_ws/src$ catkin_create_pkg my_lidar std_msgs rospy

서브 폴더 만들기

  • launch 파일 생성
1
2

$ mkdir launch
$ gedit lidar_scan.launch

소스파일 만들기

라이다로부터 주변 물체까지의 거리값을 받아 출력한다.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

#!/usr/bin/env python

# lidar_scan.py

import rospy
import time
from sensor_msgs.msg import LaserScan # LaserScan 메시지 사용 준비

lidar_points = None

# 라이다 토픽이 들어오면 실행되는 콜백 함수
def lidar_callback(data):
    global lidar_points 
    lidar_points = data.ranges

rospy.init_node('my_lidar', anonymous=True) # lidar 이름의 노드 생성
rospy.Subscriber('/scan', LaserScan, lidar_callback, queue_size=1) # laserscan 토픽이 오면 콜백함수가 호출되도록 셋팅

while not rospy.is_shutdown():
    if lidar_points == None:
        continue    # 토픽이 안왔으면 기다려라

    rtn = ''

    for i in range(12): # 30도씩 건너뛰면서 12개 거리값만 출력
        rtn += str(format(lidar_points[i*30],'.2f')) + ", "

    print(rtn[:2])
    time.sleep(1.0) # 천천히 출력
  • launch 파일 생성
1
2
3
4
5

<!-- lidar_scan.launch -->
<launch>
    <include file="$(find xycar_lidar)/launch/lidar_noviewer.launch" /> <!-- 라이다 장치를 구동시켜 토픽을 발행하는 파일 -->
    <node name="my lidar" pkg="my_liad" type="lidar_scan.py" output='screen' />
</launch>

실행 

1

$ roslaunch my_lidar lidar_scan.launch

출력에서 inf는 무한대로 너무 멀거나 너무 가깝다는 것을 의미한다. 출력된 정보는 거리로 m단위이다. 코드를 짤 때 inf와 같이 오류가 나는 것에 대해서 잘 처리해야 오류가 나지 않게 만들 수 있다.


출력 시각화하기

패키지 생성 

1
2
3
4
5
6
7
8
9
10

xycar_ws
  ⊢ build
  ⊢ devel
  ∟ src
      ∟ rviz_lidar
          ⊢ launch
              ∟ lidar_3d.launch
          ⊢ src
          ∟ rviz
              ∟ lidar_3d.rviz

rviz_lidar라는 이름의 ROS 패키지를 생성한다.

1

$ catkin_create_pkg rivz_lidar rospy tf geometry_msgs urdf rviz xacro

서브 폴더 생성

  • launch 폴더 만들기
    • lidar_3d.launch 생성
    1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

      <launch>
      <!-- rviz display -->
      <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true"
      args="-d $(find rviz_lidar)/rviz/lidar_3d.rviz"/>

      <!-- c++ 파일을 실행 -->
      <node name="xycar_lidar" pkg="xycar_lidar" type="xycar_lidar" output="screen" >
          <!-- 포트 -->
          <param name="serial_port"   xype="string"   value="/dev/ttyRPL"/>
          <!-- usb이지만 serial파일로 돌아간다. -->
          <param name="serial_baudrate"   xype="int"   value="115200"/>
          <param name="frame_id"   xype="string"   value="laser"/>
          <param name="inverterd"   xype="bool"   value="false"/>
          <param name="angle_compensate"   xype="bool"   value="true"/>
      </node>
  </launch>

+ 라이다 장치가 없을 경우

그러나 라이다 장치가 없을 경우에는 이를 사용할 수 없을 것이다. 하지만 실제 라이다 장치를 대신하여 /scan 토픽을 발행하는 프로그램을 이용할 수 있다.

ROS에서 제공하는 rosbag를 이용하면 된다. 이는 라이다에서 발행하는 scan 토픽을 저장해놓은 파일로, 그 당시의 시간 간격에 맞추어 scan 토픽을 발행할 수 있다.


이 rosbag을 사용할 경우 launch파일을 살짝 수정해야 한다.

1
2
3
4
5
6
7
8
9
10

<!-- lidar_3d_rosbag.launch -->
<launch>
    <!-- rviz display -->
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true"
    args="-d $(find rviz_lidar)/rviz/lidar_3d.rviz"/>

    <!-- c++ 파일을 실행 -->
    <node name="rosbag_play" pkg="rosbag" type="play" output="screen" 
        required="true" args="$(find rviz_lidar)/src/lidar_topic.bag" />
</launch>

더 자세한 건 아래에서 다루겠다.


실행

먼저 라이다가 있는 경우를 살펴보자.

1

roslaunch rviz_lidar lidar_3d.launch

실행을 해도 아직 아무것도 안나온다. 플러그인을 추가해야 한다.

  1. 플러그인 추가
    • 우측 displays 탭 하단에 add클릭
    • laserscan 선택 후 ok클릭
  2. topic 설정
    • laserscan 에서 topic을 /scan으로 작성하거나 화살표를 눌러 선택
  3. fixed frame
    • displays 탭에서 fixed frame을 laser로 수정
    • 이는 header - frame_id 에 해당하는 값임
  4. size 설정
    • laserscan에서 size를 설정해야 한다. 0.1 정도로 하면 화면에 잘 보일 것이다.
    • 너무 작으면 사이즈를 키우면 된다.



ROSBAG

ros 공식 문서 - rosbag

ROSBAG : ROS 명령어로 토픽을 구독하여 파일로 저장하거나파일에서 토픽을 꺼내 발행하는 기능이다.

토픽의 내용만 작성하는 것이 아니라 시간도 함께 담아서 꺼낼 때는 시간도 함께 받을 수 있다.

사용법

  • 터미널에서 실행
1
2
3
4
5

저장
$ rosbag record -O lidar_topic scan

불러오기
$ rosbag play lidar_topic.bag

lidar_topic은 저장할 파일의 이름이고, scan은 구독해서 저장할 토픽의 이름이다. 토픽은 여러개로 작성 가능하다.


  • 런치파일에서 실행

node를 선언할 것이다.

1
2
3
4

<launch>
    <node name="rosbag_play" pkg="rosbag" type="play" output="screen"
        required="true" args="$(find rviz_lidar)/src/liar_topic.bag" />
</launch>



range 데이터를 발행해서 뷰어에 표시해보기

range데이터를 담은 토픽을 발행하는 lidar_range.py를 만들어서 /scan1 이름의 토픽을 발행한다.

range타입의 데이터를 담은 /scan1 /scan2 /scan3 /scan4 총 4개 토픽을 발행한다. RVIZ에서는 원뿔 그림의 Range 거리정보를 시각화하여 표시한다.

1
2
3
4
5
6
7
8
9
10
11

xycar_ws
  ⊢ build
  ⊢ devel
  ∟ src
      ∟ rviz_lidar
          ⊢ launch
              ∟ lidar_range.launch
          ⊢ src
              ∟ lidar_range.py
          ∟ rviz
              ∟ lidar_range.rviz

기존의 rviz_lidar파일에서 진행할 것이고, 연결 관계는 다음과 같다.

  • 노드 이름: lindar_range
  • 토픽 이름: /scan1~4
  • 메시지 타입: range (from sensor_msgs.msg import Range)

publish 노드

1
2
3
4

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)

하면 순서대로 날아갈 것이다.

range 타입을 확인하기 위해

1

$ rosmsg show sensor_msgs/Range

더 자세히 보려면 http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Range.html 로 가보면 자세히 나온다.

단순하게 얼만큼 떨어져 있는가에 대한 것으로 부채꼴 모양으로 측정함을 보여준다. 대체로 전파나 음파 등은 부채꼴 모양으로 날아가서 다시 돌아올 것이다.

  • header.timestamp: 송신부터 수신까지의 시간을 나타낸 것이다.
  • radiation_type: sound인지 적외선인지 등등의 타입을 나타낸다.
  • field.of_view: 중간을 0으로 기준잡아 반 부채꼴의 각도 (대체로 초음파 센서는 왼/오 각각 15도, 총 30도)
  • min_range/max_range: 이 장치가 감지할 거리의 경계값

  • range: 감지한 거리의 값 (-inf(거리가 너무 가까울 떄), +inf(거리가 너무 멀 때))

  • lidar_range.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

#!/usr/bin/env pythonm

# range, header import
import serial, time, rospy
from sensor_msgs.msg import Range
from std_msgs import Header

# mk node
rospy.init_node('lidar_range')

# mk 4 publisher
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()
# fill the range field - header information, required information about range expression of cone shape
header = Header()
header.frame_id = "sensorXY"
msg.header = header
msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02                    # 2cm
msg.max_range = 2.0                     # 2m
msg.field_of_view = (30.0/180.0)*3.14   # radian expression 1/6 pi

while not rospy.is_shutdown():
    msg.header.stamp = rospy.Time.now()

    # pushing the distance to the object at msg.range in meters and publishing topic
    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) # slow publish
  • lidar_range.launch
1
2
3
4
5
6
7
8

<launch>
    <!-- rviz display -->
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true"
        args="-d $(find rviz_lidar)/rviz/lidar_range.rviz"/>

    <!-- implement the python file we just made -->    
    <node name="lidar_range" pkg="rviz_lidar" type="lidar_range.py"/>
</launch>

실행 

1

$ roslaunch rviz_lidar lidar_range.launch

토픽 내용 확인

그림이 그려지기 전에 토픽이 잘 날아가고 있는지 봐야 한다.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

$ rostopic list
...
/scan1
/scan2
/scan3
/scan4

$ rostopic echo scan1
header:
    seq: 24
    stamp:
        secs: 
        nsecs:
    frame_id: "sensorXY"
radiation_type: 0
field_of_type: 0.52333
min_range: 0.019999
max_range: 2.0
range: 0.4000

다 잘 나오면 된 것이다.

RVIZ 설정

  1. fixed frame 지정
    • 원래는 map으로 되어 있기에 sensorXY로 바꿔야 한다.
  2. plugin 추가
    • display 하단에 add 누르고 by topic 탭에서 /scan - range가 있다. 이를 누르고 ok
  3. 토픽 이름 확인
    • 수신할 토픽이 잘 되어 있는지 확인
  4. 생상 확인
    • color 지정에서 자신이 원하는 색상 클릭

4개 모두 선택될 경우 겹쳐서 표시될 수 있어 확인이 어려울 수 있다.


라이다와 융합하여 시각화

rosbag파일에서 토픽(/scan)을 발행하여 lidar_urdf.py로 받아지고, 이를 정제하여 4개의 토픽을 발행함으로써 rviz로 전달되게 한다.

1.파이썬 파일(lidar_urdf.py)

LaserScan 타입의 데이터를 Range 타입으로 변경해야 한다. /scan 토픽을 받아 scan1, scan2, scan3, scan4, 4개의 토픽으로 발행


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

#!/usr/bin/env python

import serial
...

def lidar_callback(data):
    global lidar_points
    lidar_points = data.ranges

rospy.init_node("lidar")
...

while not rospy.is_shutdown():
    ...
    h.frame_id = "front"
    msg.range = lidar_points[90] # 북을 전방을 두었을 때 이쪽이 0도 이므로, front는 3시방향이르모 90
    ...

    h.frame_id = "back"
    msg.range = lidar_points[270] # 9시방향이므로 270

🎈 중요한 것은 frame_id의 이름과 urdf의 블록의 이름을 일치시켜야 한다.


2.URDF 모델링 파일(lidar_urdf.urdf)

중앙에 빨간색 박스를 만들고, 4방향에서 센서 프레임을 연결한다.

  • base_link에 가로세로 20cm, red박스 baseplate를 만들어 연결
  • 센서는 x,y, 축을 기준으로 중심에서 10cm씩 이동시켜서 박스의 끝부분에 배치


중앙 박스의 크기는 가로x세로x높이 = 0.2m x 0.2m x 0.07m

1
2
3
4
5
6
7
8

<joint name="baseplate_to_front" type="fixed">
    <parent link="baseplate"/>
    <child link="front"/>
    <origin rpy="0 0 0" xyz="0.1 0 0"/> <!-- xyz에서 연결위치는 중심에서 x축으로 10cm이동 -->
<joint name="baseplate_to_back" type="fixed">
    <parent link="baseplate"/>
    <child link="back"/>
    <origin rpy="0 0 3.14" xyz="-0.1 0 0"/> <!-- xyz에서 연결위치는 중심에서 x축으로 -10cm이동, 방향은 z축을 기준으로 pi만큼 돌아야 한다. -->

1
2

link - base_link -\> joint - base_link_to_baseplate -\> link - baseplate  -\> joint - baseplate_to_front -\> link - front 
                                                                          -\> joint - baseplate_to_back -\> link - back


3.RVIZ 설정 파일(lidar_urdf.rviz)

rviz 프로그램에 들어가서 add -> 4개의 range타입을 추가


4.런치 파일(lidar_urdf.launch)

  • 박스 형상을 위한 모델링 파일(lidar_urdf.urdf)
  • RVIZ 설정 파일(lidar_urdf.rviz)
  • 라이다 토픽 발행(lidar_topic.bag)
  • rosbag파일 실행(<node pkg="rosbag" type="play" output="screen" ... args="$(find rviz_lidar)/src/lidar_topic.bag"/>)
  • 토픽 변환(lidar_urdf.py)
1
2
3
4

<!-- lidar_urdf.launch -->
<launch>

</launch>



1

$ roslaunch rviz_lidar lidar_urdf.launch

이를 실행하면 원뿔들이 물체와의 거리에 따라 짧아졌다 길어졌다 한다.



초음파 센서 ROS 패키지

초음파 센서 : 장애물까지의 거리를 초음파를 송수신해서 알려주는 센서다.


초음파센서는 아두이노를 거쳐서 프로세스와 연결된다. 노드로 이 둘 사이를 통신한다.

my_ultra이름의 패키지를 생성할 것이고, 토픽의 이름은 /ultra으로 생성할 것이다.

토픽의 내용

  • 타입: sensor_msgs/Int32MultiArray
  • 구성
    • std_msgs/MultiArrayLayout layout
      • std_msgs/MultiArrayDimension[] dim
      • string label
      • uint32 size
      • uint32 stride
      • unit32 data_offset
    • int32[] data
1
2
3
4
5
6
7
8
9

xycar_ws
  ⊢ build
  ⊢ devel
  ∟ src
      ∟ my_ultra
          ⊢ launch
              ∟ ultra_scan.launch
          ∟ src
              ∟ ultra_scan.py

패키지 생성

  • my_ultra 이름의 패키지 생성 / 서브 폴더 생성
1
2
3
4
5

xycar_ws/src$ caktin_create_pkg my_ultra std_msgs rospy
xycar_ws/src$ mkdir launch
xycar_ws/src$ cd launch
xycar_ws/src/launch$ gedit ultra_scan.launch
xycar_ws/src$ cd ../src
  • ultra_scan.py

초음파센서로부터 주변 물체까지의 거리값을 받아 출력하는 코드다.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

#!/usr/bin/env python

import rospy
import time

# prepare using Int32MultiArray msg
from std_msgs.msg import Int32MultiArray

# storage space
ultra_msg = None

# define the callback function to implement, if the topic of ultrasound come in
def ultra_callback(data):
    global ultra_msg
    ultra_msg = data.data

# mk node
rospy.init_node("ultra_node")
# define subcriber to receive msg type of Int32MultiArray
rospy.Subcriber("xycar_ultrasonic", Int32MultiArray, ultra_callback)

while not rospy.is_shutdown():
    if ultra_msg == None:
        continue
    
    # print ultrasound data    
    print(ultra_msg)

    # sleep 0.5s
    time.sleep(0.5)
  • ultra_scan.launch
1
2
3
4
5
6
7
8

<launch>
    <!-- make ultrasonic device run -->
    <node pkg="xycar_ultrasonic" type="xycar_ultrasonic.py"
        name="xycar_ultrasonic" output="screen" />
    
    <!-- implement file we just made -->
    <node pkg="my_ultra" type="ultra_scan.py" name="my_ultra" output="screen" />
</launch>

실행 

1

$ roslaunch my_ultra ultra_scan.launch

총 8개의 정보가 들어올 것이다. 이는 운전석을 0을 기준으로 시계방향으로 번호를 매긴다.


초음파 센서와 아두이노를 사용하여 ROS 패키지 제작

초음파센서 ROS 패키지

초음파센서를 제어하여 물체까지의 거리를 알아내고 그 정보를 ros 토픽으로 만들어 노드들에게 보낸다.

  1. 초음파 센서
    • 물체로 초음파를 쏘고, 반사된 초음파 신호를 감지
    • 처음 초음파를 쏜 시점과 반사파를 수신한 시점을 표시한 전기 신호인 펄스(pulse) 신호를 아두이노로 보냄
  2. 아두이노
    • 초음파센서가 보내준 펄스신호를 받아 분석
    • 초음파를 쏜 시점과 반사파를 받은 시점 사이의 시간차를 이용해서 물체까지의 거리를 계산하고 이를 ros에 알려준다.
    • serial 통신으로 ros에 보낸다. 간단하고 다른 소프트웨어 필요없이 보낼 수 있다.

이처럼 작은 보드에서 돌아가는 소프트웨어를 펌웨어라 한다. 대체로 c언어로 개발되어 있다.

  1. ros
    • 아두이노가 보내준 물체까지의 거리정보를 사용하기 좋은 형태로 적절히 가공
    • 그것을 ROS 토픽에 담아 그게 필요한 노드들에게 Publish
    • header나 1:n, n:n 등의 방법을 구현하면 된다.


제작을 위한 1단계 - 하드웨어를 이해하자

초음파 (ultrasonic wave)

  • 인간의 귀가 들을 수 있는 가청 주파수 대역보다 높은 진동수로 발생하는 파동
  • 가청 진동수는 사람마다 다르지만 약 20Hz~20kHz

  • 초음파 센서는 초음파를 이용하여 센서로부터 사물까지의 직선거리를 측정한다.

초음파센서

  • 물체로 초음파를 쏘고 반사된 초음파 신호를 감지
  • 처음 초음파를 쏜 시점부터 반사파를 수신한 시점을 표시한 pulse 신호를 아두이노에게 보냄


제작을 위한 2단계 - 아두이노를 이해하자

아두이노

  • 컴퓨터가 다 알아서 할만큼 좋은 성능이라면 사용하지 않아도 된다.
  • 초음파센서가 pulse 신호를 받아 분석
  • 초음파를 쏜 시점과 반사파를 받은 시점의 시간차이를 이용해서 물체까지의 거리를 계산하고 이를 ROS에 알린다.

각 센서의 Vcc,Trig,Echo,Gnd 를 각 핀에 연결한다.

  • Vcc : 5V
  • Trig : D2
    • 아두이노 입장에서 D2는 받는 것
  • Echo : D3
    • 아두이노 입장에서 D3는 내보내는 것
  • Gnd : GND

아두이노의 펌웨어를 개발하는 IDE가 존재한다. 아두이노 코드 작성할 때나 제작된 펌웨어를 아두이노에 적을 때 사용한다.

다운로드 링크

다운로드를 한 후 압축을 풀어 안에 있는 install.sh를 실행시키면 된다.

1
2

$ sudo ~/Downloads/Arduino-1.8.19-linux64/arduino-1.8.19/install.sh
Adding desktop shortcut, menu item and file associations for Arduino IDE ...

그 다음 실행

1

$ sudo arduino

실행이 되면 ultrasonic_1_fw.ino 파일을 작성한다.

이 파일의 내용은 구글링하면 나온다.

소스 코드 (초음파센서가 보내는 신호로부터 거리정보 추출)

/*
HL-340 초음파 센서 아두이노 펌웨어
*/

#define trig 2 // 트리거 핀 선언
#define echo 3 // 에코 핀 선언

void setup()
{
    Serial.begin(9600);     // 통신속도 9600bps로 시리얼 통신 시작
    pinMode(trig, OUTPUT);  // 트리거 핀을 출력으로 선언
    pinMode(echo, INPUT);   // 에코핀을 입력으로 선선
}

void loop() {
    long duration, distance;    // 거리 측정을 위한 변수 선언
    // 트리거 핀으로 10us 동안 펄스 출력
    digitalWrite(trig, LOW);    // Trig 핀 Low
    delayMicroseconds(2);       // 2us 딜레이
    digitalWrite(trig, HIGH);   // Trig 핀 High
    delayMicroseconds(10);      // 2us 딜레이
    digitalWrite(trig, LOW);    // Trig 핀 Low

    // pulseln() 함수는 핀에서 펄스 신호를 읽어서 마이크로초 단위로 반환
    duration = pulseIn(echo, HIGH);
    distance = duration * 170 / 1000;   // 왕복시간이므로 340/2=170 곱하는걸로 계산, 마이크로초를 mm로 변환하기 위해 1000나눔
    Serial.print("Distance(mm): ");
    Serial.println(distance);           // 거리정보를 시리얼 모니터에 출력
    delay(100);
}


아두이노가 pc에 연결되었는지를 확인해봐야 한다. 이를 위해

1
2
3
4
5
6
7

$ lsusb
Bus 004 Device 002: ID 0bc2:231a Seagate RSS LLC 
Bus 004 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
Bus 003 Device 006: ID 2b7e:0134  
Bus 003 Device 005: ID 04e8:730b Samsung Electronics Co., Ltd 
Bus 003 Device 011: ID 04d9:1836 Holtek Semiconductor, Inc. 
Bus 003 Device 016: ID 1a86:7523 QinHeng Electronics HL-340 USB-Serial adapter

리스트 중에 자신의 장치 번호를 찾는다. 나의 경우 HL-340 이다.

아두이노와 pc가 물리적으로 USB케이블로 연결되어 있으나 내부적으로는 serial통신이 이루어지고 있다. 따라서 이에 맞게 serial통신으로 맞춰서 짜야 한다. 이를 Serial over USB라 한다.


출력이 되었다면 하드웨어적으로는 통신이 되고 있다는 것이다. 그렇다면 아두이노에서 연결이 되고 있는지도 확인해봐야 한다.

tool 메뉴에서 board/ processor / port 확인

  1. board : arduion Nano
  2. processor : ATmega328P
  3. Port : /dev/ttyUSB0 또는 /dev/ttyACM0

이것을 잘 선택하면 상단에 v(체크)를 눌러 디버깅이 가능해진다. 그 후 잘 동작되면 옆에 화살표를 클릭하면 코드를 펌웨어에 업로드 할 수 있다.

그 후 값이 어떤지 보려면 tools -> serial monitor에 들아가면 출력이 될 것이다.


제작을 위한 3단계 - 리눅스 프로그래밍이 필요하다.

리눅스 ROS

  • 아두이노가 보내주는 물체까지의 거리정보를 사용하기 좋은 형태로 적절히 가공
  • ROS토픽에 담아 필요한 노드들에게 Publish


  • 패키지 생성
1
2
3
4
5
6
7
8
9
10

xycar_ws
  ⊢ build
  ⊢ devel
  ∟ src
      ∟ ultrasonic
          ⊢ launch
              ∟ ultra.launch
          ∟ src
              ⊢ ultrasonic_sub.py
              ∟ ultrasonic_pub.py



1
2
3
4
5
6
7
8

패키지 생성
$ catkin_create_pkg ultrasonic std_msgs rospy

launch 파일 생성
ultrasonic$ mkdir launch

새로만든 패키지 빌드
$ cm


  • 소스코드 생성 (ultrasonic_pub.py)

초음파센서가 보낸 거리정보를 토픽에 담아 publishing

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

#!/usr/bin/env python

import serial, time, rospy
from std_msgs.msg import Int32

ser_front = serial.Serial( 
    port='/dev/ttyUSB0', # 아두이노가 연결된 포트
    baudrate=9600,       # 아두이노에서 선언한 통신 속도
)

def read_sensor():
    serial_data = ser_front.readline() # 시리얼 포트로 들어온 데이터를 받아옴
    ser_front.flushInput()  # 중간에 버퍼들이 있어서 그를 삭제해주는 flush
    ser_front.flushOutput()
    ultrasonic_data = int(filter(str.isdigit, serial_data)) # string을 숫자로 변환
    msg.data = ultrasonic_data

if __name__ == '__main__':
    rospy.init_node('ultrasonic_pub', anonymous=False)
    pub = rospy.Publisher('ultrasonic', Int32, queue_size= 1)

    msg = Int32()
    
    while not rospy.is_shutdown():
        read_sensor() # 시리얼포트에서 센서가 보내준 문자열 읽엇거 거리 정보 추출
        pub.publish(msg)    
        time.sleep(0.2) # 토픽에 담아서 publish

    ser_front.close() # 끝나면 시리얼포트 닫기
  • 추가로 검증용으로 사용할 subscriber 노드도 생성 (ultrasonic_sub.py)

publish의 메시지를 받아 출력한다.

1
2
3
4
5
6
7
8
9
10
11
12

#!/usr/bin/env python

import rospy
from std_msgs.msg import Int32

def callback(msg):
    print(msg.data)

rospy.init_node('ultrasonic_sub')
sub = rospy.Subscriber('ultrasonic', Int32, callback)

rospy.spin()
  • 실행을 위한 launch 파일 만들기
1
2
3
4
5

<!-- ultra.launch -->
<launch>
    <node pkg="ultrasonic" type="ultrasonic_pub.py" name="ultrasonic_pub"/>
    <node pkg="ultrasonic" type="ultrasonic_sub.py" name="ultrasonic_sub" output="screen"/>
</launch>


  • 실행
1

$ roslaunch ultrasonic ultra.launch

실행하면 초음파센서의 데이터가 숫자만 출력하게 될 것이다.


초음파 센서 4개를 지원하는 ROS 패키지 제작하기

초음파센서 4개를 프로세서보드로 연결해야 한다. 그러려면 펌웨어(아두이노)를 조정하고, ROS관점에서는 4개의 방향이 있으므로 4개를 발행해줘야 한다.

노드는 /ultra4_pub로 하고, 토픽은 /ultra4, 메시지 타입은 Int32MultiArray로 한다.


ultrasoinc_fw.ino를 수정하여 ultrasonic_4_fw.ino 파일 작성한다. 각 센서의 Vcc,Trig,Echo,Gnd를 각 핀에 연결한다. 각 센서를 연결하여 5V, Gnd를 아두이노 보드에 연결하고, 나머지는 각각 아두이노 보드에 연결한다.

거리정보는 300mm 121mm 186mm 67mm 의 형태로 4개를 전송한다.


기존에 사용했던 ultrasonic 패키지를 사용하여 ultra4.launch, ultra4_pub.py, ultra4_sub.py , 3개의 파일을 생성한다.

  • ultra4_pub.py

초음파 센서가 보낸 거리정보를 토픽에 담아 publishing한다

  1. 아두이노가 연결된 포트 지정 (usb 포트는 리눅스가 관리하기에 리눅스에게 요청)
  2. def read_sensor() 생성(시리얼 데이터 한번에 문자열로 받아오고 문자열에서 숫자 4개를 추출하여 리스트에 담는다.)
  3. main함수 (ultra4_pub 노드 생성, ultra4 토픽 발행, while문(아두이노에게 정보를 받아와서 토픽안에 잘 채워넣어 토픽을 발행), 시리얼을 닫고 정리)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

import Serial
...

FRONT = [0,0,0,0]

ser_front = serial.Serial(  # 아두이노가 연결된 포트 지정
    port='/dev/ttyUSB0',
    baudrate=9600,
)

def read_sensor():
    sersor_data = ser_front.readline()
    ser_front.flushInput()
    ser_front.flushOutput()
    FRONT = read_Sdata(sensor_data)
    msg.data = FRONT

def read_Ssdata(s):
    s = s.replace(" ", "")
    s_data = s.split("mm")
    s_data.remove('\r\n')
    s_data = list(map(int,s_data))
    return s_data
if __name__ == '__main__':
    ...
    ser_front.close()


  • ultra4_sub.py

publisher의 메시지를 받아 출력한다.

  1. 토픽에서 데이터를 꺼내서 출력
  2. ultra4_sub 노드 생성
  3. ultra4 토픽 구독 준비
  4. ultra4 토픽을 받으면 callback 호출
  5. 무한 루프
1
2
3
4
5
6
7
8
9
10
11
12

#!/usr/bin/env python

import rospy
from std_msgs.msg import Int32

def callback(msg):
    print(msg.data)

rospy.init_node('ultrasonic_sub')
sub = rospy.Subscriber('ultrasonic', Int32, callback)

rospy.spin()


  • ultra4.launch
  1. 토픽의 발행자 실행
  2. 토픽의 구독자 실행
1
2
3
4

<launch>
    <node pkg="ultrasonic" type="ultrasonic_pub.py" name="ultrasonic_pub"/>
    <node pkg="ultrasonic" type="ultrasonic_sub.py" name="ultrasonic_sub" output="screen"/>
</launch>


실행

  1. 연결 확인
1
2

$ lsusb
~~~~~~ HL-340 USB~~~
  1. 펌웨어 프로그래밍
1

$ sudo arduino
  1. 아두이노가 연결된 리눅스 포트 확인

tools메뉴에서 Board/Processor Port를 체크해야 한다.

  • board : Arduino Nano
  • Processor : Atmega328P
  • Port : /dev/ttyUSB0 or /dev/ttyACM0
  1. 아두이노 펌웨어 소스코드

초음파센서가 보내는 신호로부터 거리정보를 추출하는 소스코드다.

int trig[4] = {2,4,6,8}; // 센서를 꼽았던 포트 번호
int echo[4] = {3,5,7,9}; //
int i=0;

void setup() {
    Serial.begin(9600);
    for(i=0;i<4;i++) {
        pinMode(trig[i],OUTPUT);    // TRIG 핀을 출력모드로 세팅
        pinMode(echo[i], INPUT);    // ECHO 핀을 입력모드로 세팅
    }
}

void loop() {
    long dur[4]={0.0,};
    long dist[4]={0.0,};

    for(i=0; i<4; i++) {
        digitalWrite(trig[i],LOW);
        delayMicroseconds(2);
        digitalWrite(trig[i],HIGH);         // trig 신호를 high 상태로 변경
        delayMicroseconds(10);              // 10마이크로초 대기
        digitalWrite(trig[i],LOW);          // Trig 신호를 low 상태로 변경

        dur[i] = pulseIn(echo[i],HIGH);     // Echo 펄스에서 왕복시간을 계산해서 저장
        dist[i] = dur[i]*170/1000;          // 시간을 이동거리로 환산해서 저장
        if(dist[i]>=2000 || dist[i]<0) {    // 거리가 200cm이상이거나 0보다 작으면 0으로 설정
            dist[i]=0;
        }
    }

    Serial.print(dist[0]); //dist[0] 출력
    Serial.print("mm ");
    Serial.print(dist[1]);
    Serial.print("mm ");
    Serial.print(dist[2]);
    Serial.print("mm ");
    Serial.print(dist[3]);
    Serial.print("mm ");

    delay(50);
}
  1. 컴파일 & 업로드

컴파일 및 업로드를 진행한다.

  1. 결과 확인

시리얼 모니터를 이용해서 아두이노의 출력값을 확인할 수 있다. tools -> serial Monitor

  1. 실행
1

$ roslaunch ultrasonic ultra4.launch
Classlog, devcourse
ros urdf rviz devcourse
This post is licensed under CC BY 4.0 by the author.
Recently Updated
Contents