자이카 - 초음파 센서

초음파 센서란?

• 장애물까지의 거리를 알려주는 센서
• 자이카에선 전방 3개, 후방 3개, 좌우 1개씩 총 8개가 장착되어 있다.
  • 아두이노를 거쳐 프로세서와 연결된다.

ROS 초음파 센서 패키지

패키지 위치

• xycar_ws
  • src
    • xycar_ultrasonic

초음파 센서 관련 노드와 토픽

• 노드 - /xycar_ultrasonic
• 토픽 - /xycar_ultrasonic

초음파 센서 테스트

패키지 생성

catkin_create_pkg my_ultra std_msgs rospy

• 런치 파일 - ultra_scan.launch
• 소스 파일 - ultra_scan.py

# ultra_scan.launch
<launch>
	<node name="xycar_ultrasonic" pkg="xycar_ultrasonic"  type="xycar_ultrasonic.py" output="screen"/>

	<node name="my_ultra" pkg="my_ultra" type="ultra_scan.py" output="screen"/>
</launch>

# ultra_scan.py
#!/usr/bin/env python

import rospy
import time
from sensor_msgs.msg import Int32MultiArray

ultra_msg = None

def ultra_callback(data):
    global ultra_msg
    ultra_msg = data.data

rospy.init_node('ultra_node')
rospy.Subscriber("xycar_ultrasonic", Int32MultiArray, ultra_callback)

while not rospy.is_shutdown():
    if ultra_msg == None:
        continue

    print(ultra_msg)
    time.sleep(0.5)

실행

roslaunch my_ultra ultra_scan.launch

실제 자이카가 없어 실행해 보지는 못했다.

초음파 센서 활용을 위한 준비

자이카의 초음파 센서 동작 구조

초음파 센서로 거리 측정하기

• 소리의 속도는 340m/s,
  • 1cm당 29us소요

• 위 공식을 아두이노IDE를 통해 펌웨어를 작성하면 된다.

#define trig 2  // 트리거 핀 선언
#define echo 3  // 에코 핀 선언
// 센서 여러 개를 사용하려면 trig[4] = { p1, p2, p3, p4 }; 이렇게 선언하자.

void setup()
{
  Serial.begin(9600);     // 통신속도 9600bps로 시리얼 통신 시작
  // Serial.println("Start... Ultrasonic Sensor");
  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);    // 10us 딜레이
  digitalWrite(trig, LOW);  // Trig 핀 Low

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

아두이노→ROS

• Serial통신을 통해 접근할 수 있다.
• python에서 Serial모듈을 import해서 코딩
• 예시

  # ultrasonic_pub.py
  #!/usr/bin/env python
  
  import serial, time, rospy, re
  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()
      ser_front.flushOutput() 
      ultrasonic_data = int(filter(str.isdigit, serial_data))
      msg.data = ultrasonic_data
      
  # 여러 개 받을 때 사용하는 함수
  def read_Sdata(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__':
  
      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()

  set_front부분이 시리얼 통신을 연결하는 설정 코드