라즈베리파이에서 Thread 구현하기

 

자율주행 플랫폼은 크게 '인지', '판단', '제어'의 중심 요소로 나누어집니다.

인지	차량에 장착된 센서(카메라, 레이더, 라이더, GPS 등), 차량 간/차량과 외부 간의 통신을 통해 주변 상황에 대한 정보를 수집하고, 인지
판단	인지된 정보를 해석하여 주행상황을 판단하고, 스스로 장애물, 교통신호 등을 반영한 적절한 주행 경로를 설정하여 운행
제어	제어시스템을 통해 실제 주행에 반영하기 위해 운전시스템을 제어

 

그렇다면 자율주행에서 수행되어야 할 프로세스들에는 무엇이 있을까요?

기본적으로 주행 영상 획득, 영상에 대한 전처리, 자동차의 모터제어, 센서(카메라, 레이더) 처리, 차량 간/인프라 간의 통신 등의 작업이 있을거에요.

이 process들을 순차적으로 진행한다고 해봅시다. 일단 수행 시간이 상당히 길어지겠죠.

만약 프로세스가 전환되는 시점에 장애물이 나타난다면? 탐지하지 못하고 사고가 발생할 수도 있게 될 겁니다.

이 때, multi-thread를 사용한다면, 주행의 안전성을 높이고 정확도를 향상시킬 수 있게 됩니다!

 

💡Process란 메모리에 적재되어 실행되고 있는 프로그램(program)이다

💡Thread란 프로세스(process) 내에서 작업을 수행하는 여러 흐름의 단위로, 하나의 프로세스에 여러 개의 thread가 생성되고, 또 동시에 독립적으로 실행될 수 있다

 

---

❓라즈베이파이에서 Thread를 구현하는 방법

병렬적으로 작동하는 소프트웨어의 작성을 위해서 제공되는 표준 API인 Pthread를 사용합니다.

1) pthread_create() - pthread를 생성

#include <pthread.h>

/*
  thread: thread를 구분하기 위하여 부여하는 ID
  attr: thread 생성 시 필요한 속성 값
  start_routine: thread로 수행할 일을 정의할 function
  arg: start_routine함수에 넘겨주는 parameter
    -> 함수가 성공적으로 호출되면, 현재의 thread에 대하여 설정한 ID를 첫 번째 인자(thread)에 설정하고, 0값을 반환한다
*/
int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void*), void *arg);

 

2) pthread_join() - 특정 thread가 종료하기를 기다렸다가, thread가 종료된 이후에 계속해서 진행

#include <pthread.h>

/*
  thread: pthread_create()함수에서 설정된 thread의 ID
  **retval: start_routine()함수에서 반환되는 값 저장
*/
int pthread_join(pthread_t thread, void **retval);

 

---

thread를 사용한 아주 간단한 예제 두 가지를 보도록 하겠습니다. 

thread를 생성하고, 병렬적으로 수행하는 첫 번째 예제입니다.

#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <string.h>
#include <unistd.h>
#include <iostream>

void* function_A(void* num){
  int i=0;
  while(1){
    printf(" thread A i=[%3d]\n",i);
    i++;
    i=i%100;
    //1초에 한 번씩 thread 실행
    sleep(1);
  }
}

void* function_B(void* num){
  int i=0;
  while(1){
    printf(" thread B i=[%3d]\n",i);
    i++;
    i=i%100;
    //3초에 한 번씩 thread 실행
    sleep(3);
	}
}
int main(){
	
  pthread_t pthread_A,pthread_B;
  int cnt=0;

  printf("Create Thread A \n");
  //thread A를 생성
  pthread_create(&pthread_A, NULL, function_A, NULL);

  printf("Create Thread B \n");
  //thread B를 생성
  pthread_create(&pthread_B, NULL, function_B, NULL);	
 
  while(1){
    printf("Thread test %3d\n ",cnt);
    cnt++;
    cnt = cnt%100;
    sleep(1);
  }
  return 1;	
}

작성한 코드대로, thread A는 1초에 한 번씩, thread B는 3초에 한 번씩 병렬적으로 수행되고 있음을 확인할 수 있네요.

 

다음은 pthread_join()을 사용한 예제입니다.

#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <string.h>
#include <unistd.h>
#include <iostream>

void* function_A(void* num){
  int i=0;
  for(i=0;i<50;i++){
    printf(" thread A i=[%3d]\n",i);
    
    //100msec동안 호출 thread의 실행을 중지
    usleep(100);
  }
  //thread 종료
  pthread_exit(NULL);
}

void* function_B(void* num){
  int i=0;
  while(1){
    printf(" thread B i=[%3d]\n",i);
    i++;
    i=i%100;
    sleep(3);
  }
}

int main(){
	
  pthread_t pthread_A,pthread_B;
  int cnt=0;

  printf("Create Thread A \n");
  pthread_create(&pthread_A, NULL, function_A, NULL);

  printf("Create Thread B \n");
  pthread_create(&pthread_B, NULL, function_B, NULL);	
  
  pthread_join(pthread_A,NULL);	//thread A의 수행이 종료될 때까지 기다린 후 return
  //pthread_join(pthread_B,NULL);	//thread B의 수행이 종료될 때까지, 기다린 후 return

  while(1){
    printf("Thread test %3d\n ",cnt);
    cnt++;
    cnt = cnt%100;
    sleep(1);
  }
  return 1;	
}

thread A가 종료된 후, thread B가 실행되고 있습니다.

 

위의 예제들을 응용하여 초음파 센서를 thread형태로 구현해 볼 수도 있습니다!

#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include <wiringPi.h>
#include <signal.h>
#include <softPwm.h>

//GPIO Motor Control
#define ENA 1	
#define IN1 4
#define IN2 5

#define ENB 0
#define IN3 2
#define IN4 3

#define MAX_PWM_DUTY 100

//Ultrasonic Sensor
#define TRIG 21					//ultrasonic sensor output
#define ECHO 22					//ultrasonic sensor input

#define OBSTACLE_DISTANCE 20

int flag_obstacle_data = 0;
float u_sensor_data = 0;
int pwm_r,pwm_l;

void sig_Handler(int sig);

//오른쪽 바퀴 제어
void motor_control_r(int pwm){
  if(pwm>0){
    digitalWrite(IN1,HIGH);
    digitalWrite(IN2,LOW);
    softPwmWrite(ENA,pwm);
  }
  else if(pwm==0){
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,LOW);
    softPwmWrite(ENA,0);		
  }
  else{
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,HIGH);
    softPwmWrite(ENA,-pwm);
  }
}

//왼쪽 바퀴 제어
void motor_control_l(int pwm){
  if(pwm>0){
    digitalWrite(IN3,LOW);
    digitalWrite(IN4,HIGH);
    softPwmWrite(ENB,pwm);
  }
  else if(pwm==0){
    digitalWrite(IN3,LOW);
    digitalWrite(IN4,LOW);
    softPwmWrite(ENB,0);		
  }
  else{
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,LOW);
    softPwmWrite(ENB,-pwm);
  }
}

int GPIO_control_setup(void){
  if(wiringPiSetup() == -1){
    printf("WiringPi Setup error!\n");
    return -1;
  }
  
  //오른쪽 바퀴 제어
  pinMode(ENA,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  
  //왼쪽 바퀴 제어
  pinMode(ENB,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);

  pinMode(TRIG,OUTPUT);
  pinMode(ECHO,INPUT);

  softPwmCreate(ENA,1,MAX_PWM_DUTY);
  softPwmCreate(ENB,1,MAX_PWM_DUTY);

  softPwmWrite(ENA,0);
  softPwmWrite(ENB,0);

  //pwm값 초기화
  pwm_r=pwm_l=0;
  
  return 0;
}
float ultrasonic_sensor(){
  long start_time, end_time;
  long temp_time1, temp_time2;

  int duration=-1;
  float distance=0;
	
  //input trigger
  digitalWrite(TRIG,LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);

  //print sonic burst
  delayMicroseconds(200);			//wait for burst signal(40kH*8= 8*25us=200)

  temp_time1 = micros();
  
  //wait until ECHO pin is HIGH
  while(digitalRead(ECHO) == LOW)
  {
	temp_time2 = micros();
    duration = temp_time2 - temp_time1;

    if(duration > 1000) return -1;
  }
	
  start_time=micros();
  //wait until ECHO pin is LOW
  while(digitalRead(ECHO) == HIGH)
  {
    temp_time2 = micros();
    duration = temp_time2 - start_time;
    if(duration > 2500) return -1;
  }
  end_time=micros();

  duration = end_time - start_time;
  distance = duration / 58 ;

  return distance;
		
}
void* ultrasonic_sensor_thread(void* num){

  while(1){
    u_sensor_data = ultrasonic_sensor();
    if(u_sensor_data <= OBSTACLE_DISTANCE) {
      printf("Obstacle detected\n\n");
      flag_obstacle_data = 1;
    }			
  }
	
	//pthread_exit(NULL);
}

void* motor_control_thread(void* num){
  while(1){
    motor_control_l(pwm_l);
    motor_control_r(pwm_r);
  }
}
int main(){
	
  pthread_t pthread_A,pthread_B;
  int cnt=0;

  if (GPIO_control_setup()==-1){
    return -1;
  }
  //signal(SIGINT,sig_Handler);

  printf("Create Thread A \n");
  pthread_create(&pthread_A, NULL, ultrasonic_sensor_thread, NULL);

  printf("Create Thread B \n");
  pthread_create(&pthread_B, NULL, motor_control_thread, NULL);	

  //pthread_join(ultrasonic_sensor_thread,NULL);
  //pthread_join(motor_control_thread,NULL);

  while(1){
    printf("UltraSonic sensor : %6.3lf [cm]\n",u_sensor_data);
    printf("Thread test %3d\n ",cnt);
    cnt++;
    cnt = cnt%100;
    pwm_r = pwm_l = cnt;
    delay(100);
  }
	return 1;	
}

void sig_Handler(int sig){
  printf("\n\n\n\nStop Program and Motor!\n\n\n");
  motor_control_l(0);
  motor_control_r(0);
  exit(0);
}

초음파 센서의 input에 따라 모터가 제어되고 있네요!

초음파 센서