basic information

sensor

• VCC: 센서에 전원공급 (DC 5V)
• GND: 그라운드
• Trig: 센서를 동작시키기 위한 트리거 신호 (입력)
• Echo: 거리 측정 결과를 전달하기 위한 시그널 (출력)

소리는 초당 340m를 이동하기 떄문에 1cm 이동할 때 29us 정도 걸린다. 즉 물체까지의 거리는

로 구할 수 있다. 하지만 물체의 크기가 너무 작거나 표면 질감에 따라 측정이 힘들 수 있다.

topic

/xycar_ultrasonic

message

sin@Ubuntu1804:~/xycar_ws$ rosmsg show std_msgs/Int16MultiArray
std_msgs/MultiArrayLayout layout
  std_msgs/MultiArrayDimension[] dim
    string label
    uint32 size
    uint32 stride
  uint32 data_offset
int16[] data

code

• xycar_ultrasonic 토픽을 구독하고 있다가 발생하는 메시지의 데이터를 출력하는 파이썬 코드이다.

#!/usr/bin/env python

import rospy, time
from std_msgs.msg import Int32MultiArray

multi_msg = None

def ultra_callback(msg):
	global ultra_msg
    ultra_msg = msg.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(.5)

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

• 센서를 연결하는 아두이노에 작성될 코드이다.

#define trig 2
#define echo 3

void setup(){
Serial.begin(9600); // 통신속도 설정
pinMode(trig, OUTPUT); // 트리거 핀을 출력으로 설정
pinMode(ehco, INPUT): // 에코핀을 입력으로 설정
}

void loop(){
  long duration, distance;
  digitalWrite(trig, LOW); // Trig 핀 low
  delaymicroseconds(2);    // 2us 딜레이
  digitalWrite(trig, HIGH);// Trig 핀 high
  delaymicroseconds(10);   // 10us 딜레이
  digitalWrite(trig, LOW); // Trig 핀 low

  duration = pulseIn(ehco, HIGH); // 펄스 신호를 읽어서 마이크로초 단위로 변환
  distance = duration * 170 / 1000
  Serial.print("Distance(mm): ");
  Serial.println(distance);
  delay(100);
}

• 아두이노가 수집한 데이터를 xycar_ultrasonic 토픽으로 발행하는 파이썬 코드이다.

#!/usr/bin/env python

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

ser_front = serial.Serial(port='/dev/ttyUSB0', baudrate=9600)
# Check board, processor port at arduino tool menu

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')
    pub = rospy.Publisher('ultrasonic', Int32, queue_size=1)
    
    msg = Int32()
    while not rospy.is_shutdown():
    	read_sensor()
        pub.publish(msg)
        time.sleep(.2)
ser_front.close()

#!/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('ultrasoinc', Int32, callback)
rospy.spin()

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

• 여러개의 센서를 이용하는 경우

int trig[4] = {2,4,6,8};
int echo[4] = {3,5,7,9};


void setup(){
Serial.begin(9600); // 통신속도 설정
for(int i=0; i<4; i++){
  pinMode(trig[i], OUTPUT); // 트리거 핀을 출력으로 설정
  pinMode(ehco[i], INPUT): // 에코핀을 입력으로 설정
}

void loop(){
  long duration[4] = {0.0,} 
  long distance[4] = {0.0,}

  for(int i=0; i<4; i++){
    digitalWrite(trig[i], LOW); // Trig 핀 low
    delaymicroseconds(2);    // 2us 딜레이
    digitalWrite(trig[i], HIGH);// Trig 핀 high
    delaymicroseconds(10);   // 10us 딜레이
    digitalWrite(trig[i], LOW); // Trig 핀 low

      duration[i] = pulseIn(ehco[i], HIGH); // 펄스 신호를 읽어서 마이크로초 단위로 변환
      distance[i] = duration[i] * 170 / 1000
      if(distance[i]>= 2000 || distance[i]<0{
          distance[i] = 0;
      }
  }
  Serial.print("Distance(mm): ");
  Serial.println(distance[0]);
  Serial.println("mm");
  Serial.println(distance[1]);
  Serial.println("mm");
  Serial.println(distance[2]);
  Serial.println("mm");
  Serial.println(distance[3]);
  Serial.println("mm");

  delay(50);
}

#!/usr/bin/env python

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

FRONT = [0,0,0,0]

ser_front = serial.Serial(port='/dev/ttyUSB0', baudrate=9600)
# Check board, processor port at arduino tool menu

def read_sensor():
	serial_data = ser_front.readline()
    ser_front.flushInput()
    ser_front.flushOutput()
    FRONT = read_Sdata(serial_data)
    msg.data = ultrasonic_data

def read_Sdata(data):
	data = data.replace(" ", "")
    data = data.split("mm")
    data.remove("\r\n")
    data = list(map(int, data))
    return data
    

if __name__ = "__main__":
	rospy.init_node('ultrasonic4_pub')
    pub = rospy.Publisher('ultrasonic4', Int32MultiArray, queue_size=1)
    
    msg = Int32MultiArray()
    while not rospy.is_shutdown():
    	read_sensor()
        pub.publish(msg)
        time.sleep(.2)
ser_front.close()

#!/usr/bin/env python

import rospy
from std_msgs.msg import Int32MulitArray

def callback(msg):
	print(msg.data)
   
rospy.init_node("ultrasonic4_sub")
sub=rospy.Subscriber("ultrasonic4", Int32MultiArray, callback)

rospy.spin()

<launch>
  <node pkg="ultrasonic" type="ultrasonic4_pub" name="ultrasonic4_pub"/>
  <node pkg="ultrasonic" type="ultrasonic4_sub" name="ultrasonic4_sub"/>
</launch>