[Ros] 초음파 센서 ROS 패키지 제작

arduino ide 설치

• 아두이노 펌웨어 프로그래밍 도구

1. https://www.arduino.cc/en/software 에서 1.8.19 linux 64bit 다운

2. 압축 풀고 install.sh 실행

$ sudo ./install.sh

아두이노 펌웨어 sw 작성

-ultrasonic_1_fw.ino

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

#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);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

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

패키지 만들기

$ cs
$ catkin_create_pkg ultrasonic std_msgs rospy
$ cd ultrasonic
$ mkdir launch

• ultrasonic_pub.py
• ultrasonoc_sub.py
• ultra.launch

4개 연결

• 펌웨어(ultrasonic_4_fw.ino)

/*
 * HC-SR04 초음파 센서 아두이노 펌웨어
 */
int trig[4] = {2,4,6,8};
int echo[4] = {3,5,7,9};
int i = 0;

void setup() {
  Serial.begin(9600); //통신속도 9600bps로 시리얼 통신 시작
  for(i=0;i<4;i++){
    pinMode(trig[i],OUTPUT);
    pinMode(echo[i],INPUT);
  }
}

void loop() {
  long dur[4] = {0,};
  long dist[4] = {0,};
  
  for(i=0;i<4;i++){
    digitalWrite(trig[i], LOW);
    delayMicroseconds(2);
    digitalWrite(trig[i], HIGH);
    delayMicroseconds(10);
    digitalWrite(trig[i], LOW);

    dur[i] = pulseIn(echo[i], HIGH);
    dist[i] = dur[i] * 170 / 1000; 
    if(dist[i]>=2000 || dist[i]<0){
      dist[i] = 0;
    }
  }
 
  Serial.print(dist[0]);
  Serial.print("mm ");
  Serial.print(dist[1]);
  Serial.print("mm ");
  Serial.print(dist[2]);
  Serial.print("mm ");
  Serial.print(dist[3]);
  Serial.println("mm ");
  
  delay(50);
}

• ultra4_pub.py
• ultra4_sub.py