Ubuntu16.04 ROS Kinetic - 초음파센서(HC-SR04) 4개 연결 실습

1. 하드웨어 연결

각 초음파센서들의 Vcc, Gnd를 하나로 연결하고 그것을 아두이노의 5V, GND에 연결하였다.

또 각 초음파센서들의 Trig, Echo핀들을 2,3 / 4,5 / 6,7 / 8,9에 각각 연결함.

빵판은 없고 전선들의 피복을 벗겨서 연결하였다.

아두이노를 PC에 연결하고 가상머신(리눅스)에 연결한다.

연결 확인 명령어: 

lsusb

 

2. 아두이노

ultrasonic_4_fw.ino 파일 작성

4개의 초음파센서 거리 정보를 아래와 같이 출력하기. 가운데 띄어쓰기 한칸씩.

300mm 121mm 186mm 67mm

 

1) 아두이노 실행 명령어

sudo arduino

 

2) 아두이노 코드

/*
HC-SR04 초음파 센서 4개 아두이노 펌웨어

1번 초음파 센서 trig=2, echo=3
2번 초음파 센서 trig=4, echo=5
3번 초음파 센서 trig=6, echo=7
4번 초음파 센서 trig=8, echo=9
*/

void setup()
{
  Serial.begin(9600);
  for(int i=1;i<=4;i++)
  {
    pinMode(2*i, OUTPUT);  // trig
    pinMode(2*i+1, INPUT);  // echo
  }
}

int calcdist(int num)
{
  long duration, distance;
  int trig = 2 * num;
  int echo = 2 * num + 1;
  
  // 트리거 핀으로 10us동안 펄스 출력
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  
  // pulseIn 함수는 핀에서 펄스 신호를 읽어서 마이크로초 단위로 반환
  duration = pulseIn(echo, HIGH);
  distance = duration * 170 / 1000;
  Serial.print(distance);
  Serial.print("mm ");
  if(num==4){
    Serial.println();
  }
  delay(100);
}

void loop(){
  calcdist(1);
  calcdist(2);
  calcdist(3);
  calcdist(4);
  delay(100);

}

 

3) 실행 결과

 

3. ROS

1) ultrasonic 패키지

아래 실습때 만들었던 패키지를 사용한다.

2021/01/14 - [ROS] - Ubuntu16.04 ROS Kinetic - 초음파센서(HC-SR04) 실습

Ubuntu16.04 ROS Kinetic - 초음파센서(HC-SR04) 실습

 

2) 실행파일

ultra4_pub.py

#!/usr/bin/env python

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

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 = serial_data.split()
	for i, x in enumerate(ultrasonic_data):
		ultrasonic_data[i] = int(x[:-2])
	msg.data = ultrasonic_data

if __name__ == '__main__':
	rospy.init_node('ultrasonic_pub', anonymous=False)
	pub = rospy.Publisher('ultra4', Int32MultiArray, queue_size=1)
	
	msg = Int32MultiArray()
	while not rospy.is_shutdown():
		read_sensor()
		pub.publish(msg)
		time.sleep(0.2)
	
	ser_front.close()

 

ultra4_sub.py

#!/usr/bin/env python

import rospy
from std_msgs.msg import Int32MultiArray

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

rospy.init_node('ultrasonic_sub')
sub = rospy.Subscriber('ultra4', Int32MultiArray, callback)

rospy.spin()

 

3) launch 파일

ultra4.launch

<launch>
	<node pkg="ultrasonic" type="ultra4_pub.py" name="ultra4_pub"/>
	<node pkg="ultrasonic" type="ultra4_sub.py" name="ultra4_sub" output="screen"/>
</launch>

 

4) 빌드 및 실행

빌드

cm

 

실행

roslaunch ultrasonic ultra4.launch

 

실행 결과

 

참고

위 화면과 같이 에러가 발생하면서 실행이 안되는 오류가 종종 있는데, 무시하고 다시 실행하였다.

 

rqt_graph