_ROS1_ultrasonic

이번에는 초음파 센서 ROS 패키지를 만들어보려고 한다.

필요한 것은 Linux computer, Arduino, HC-SR04 초음파센서이다.

 

소리의 속도는 초속 340m로 1cm 이동에 약 29us 소요가 된다면 다음과 같은 수식을 얻을 수 있다.

 

$$ x=\frac{t(us) \div 2}{29(us)}(cm) $$

x: 물체까지의 거리

t: 송신과 수신의 시간차

 

이제 아두이노와 초음파 센서를 연결하고 아두이노 IDE에 프로그램을 작성했다.

초음파 센서와의 연결은 다음과 같다. (D는 Digital pin을 의미한다.)

• Vcc: 5v pin
• Trig: D2 pin
• Echo: D3 pin
• Gnd: GND pin

소스코드는 다음과 같다.

Arduino

//핀 지정
#define trig 2
#define echo 3

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

void loop() {
  long duration, distance; // 거리 측정 변수 선언
  digitalWrite(trig, LOW); // Trig pin Low
  delayMicroseconds(2);    // 2us delay
  digitalWrite(trig, HIGH);// Trig pin High
  delayMicroseconds(10);   // 10us delay
  digitalWrite(trig, LOW); // Trig pin Low

  duration = pulseIn(echo, HIGH); // pulseIn()은 핀에서 펄스 신호를 읽어서 마이크로초 단위로 반환
  distance = duration * 170 / 1000;
  Serial.print("dis(mm): ");
  Serial.print(distance);
  Serial.print("\n");
  delay(100);
}

아두이노에 업로드를 하고 시리얼 모니터를 확인하면 다음의 결과를 볼 수 있다.

이제 초음파센서 데이터를 ROS토픽에 담아서 Publish를 하도록 해보자.

ultra_pub.py, ultra_sub.py를 만들어서 이를 확인해볼 것이다.

아두이노에서 데이터를 받아와야 하기 때문에 Serial 통신을 할 것이다.

Python

- ultra_pub.py

#!/usr/bin/env python

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

ser_front = serial.Serial(
    port='/dev/ttyACM0',
    baudrate=9600,
    )

def read_sensor():
    serial_data = ser_front.readline()
    ser_front.flushInput()
    ser_front.flushOutput() 
    ultrasonic_data = int(filter(str.isdigit, serial_data))
    msg.data = ultrasonic_data
  
if __name__ == '__main__':
    rospy.init_node('ultrasonic_pub', anonymous=False) # initialize node
    pub = rospy.Publisher('ultrasonic', Int32, queue_size=1)

    msg = Int32() # message type
    while not rospy.is_shutdown():
        read_sensor() 
        pub.publish(msg) # publish a message
        time.sleep(0.2)
    
    ser_front.close()

 

- ultra_sub.py

#!/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()

 

- 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>

 

일단 위의 launch파일을 실행하기 위해서 연결된 usb에 권한을 줘야 한다.

$ sudo chmod 666 /dev/ttyACM0

실행하면  다음의 결과를 볼 수 있다.