dazi2_2.log

[OpenCV] OpenCV 기초 알아보기

Thu, 07 Mar 2024 10:17:04 GMT

0. 개요

앞서 OpenCV가 어떤방면에서 사용되는지 , 특징은 어떠한지 알아보았다. 직접 OpenCV를 활용하기 위해서 기초적인 사용법에 대해서 알아보자.

1. 도형그리기

OpenCV에서는 선, 사각형, 원, Text 와 같은 도형을 그리는 기능을 제공한다 아래의 사용법을 살펴보자.

선(line) 그리기

cv2.line(img, start, end , color, thickness)

사각형(rectangle) 그리기

cv2.rectangle(img, start, end , color, thickness)

원(circle) 그리기

cv2.circle(img, center, radius, color, thickness)

텍스트(Text) 그리기

cv2.text(img, text, org, font, fontScale, color)

OpenCV를 통해 여러가지 도형 및 텍스트를 그린 모습. color 같은경우에는 BGR 값으로 정의할 수 있다.

2.이미지 파일 다루기

컴퓨터 비전에서 이미지 파일을 다루는것은 당연한 일이기 때문에, 이미지 파일을 어떻게 다루는지 아는것은 필수적이다.

이미지 파일 읽기(imread)

cv2.imread(filename,flags)

여기서 flags 에는 이미지를 읽을 때 적용되는 옵션을 지정하는 매개변수이다. IMREAD_COLOR, IMREAD_GRAYSCALE, IMREAD_UNCHANGED 과 같은 옵션을 선택하여 이미지를 불러올 수 있다.

화면에 이미지를 표시하기

cv2.imshow(window_title,image)

window title 이름을 지정하고 image를 창으로 띄울 수 있다.

이미지를 파일로 저장하기

cv2.imwrite(filename, image)

filename으로 이미지를 저장 할 수 있다.

IMREAD_COLOR로 생성한 그림.

IMREAD_GRAYSCALE로 생성한 그림.

특정 색상의 픽셀 찾기

OpenCV 이미지는 픽셀로 구성되어 있고, 가로선 * 세로선 으로 읽을 수 있다. 아래의 예시코드를 통해 특정 픽셀을 찾는 방법을 알아보자.

import cv2
import numpy as np

#img = cv2.imread('src/ex_codes/opencv_ex/spot.png', cv2.IMREAD_GRAYSCALE)
img = cv2.imread('src/ex_codes/opencv_ex/grim.png', cv2.IMREAD_COLOR)

#height
h = img.shape[0]
w = img.shape[1]
print("The image dimension is %d x %d" % (w, h))

for i in range(0, h - 1):
   for j in range(0, w - 1):
       if np.array_equal(img[i, j],[255,255,255]):
          print(img[i, j])
          print(i, j)

cv2.imshow('spot', img)

cv2.waitKey(1000)

OpenCV에서 이미지를 불러오는 방식이 height, width 좌표의 픽셀의 BGR 값들로 구성된 array로 읽어들인다. img.shape 를 통해 해당 값을 알 수 있다. 그렇기 때문에 특정 색깔을 찾고자 할때 해당 픽셀의 좌표와 원하는 BRG 값을 대조하면 원하는 색깔의 위치를 찾을 수 있다. 위 코드에서는 검정색을 찾을 수 있다.

그리고 IMREAD_GRAYSCALE 로 불러온 파일은 무채색이기 때문에 색상의 밝기로만 픽셀을 구분 할 수 있다. 그래서 픽셀에 저장되는 값이 0~255 사이의 값이다.

ROI(Region of Interest) 설정하기

이미지를 처리할때 관심범위를 지정하여 작업을 할 수있다.

cv2.imshow(window_title,image[a:b, c:d])

잘라내는 범위는 당연히 원본이미지 값 보다 크면 안된다.

3. HSV 색상 모델

OpenCV에서는 BGR 색상 모델을 사용하기도 하지만, 때때로 HSV 라는 색상 표현 방식을 사용하기도 한다. H(Hue, 색상) , S(Saturation, 채도), V(Value, 명도) 로 표현하며 , 객체를 분할해 내기 용이한 장점이 있다. 아래의 코드를 통해 BGR을 HSV로 변환 할 수 있다.

cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

Hue 범위 : 0 ~ 179 (색상스펙트럼 각도) Saturation 범위 : 0 ~ 255 Value 범위 : 0~255

HSV 모델을 통한 차선 구분

도로의 색상은 주로 어두운색(검정색)이고, 차선은 밝은 색이기 때문에, 명도를 활용하면 구분해내기 쉽다. 아래의 코드와 입력하면 어떤 이미지에서 흰색으로 인식할 범위를 설정하여 inRange 함수를 통해 흰색 픽셀의 범위를 알아 낼 수 있다.

예시코드

import cv2
import numpy as np

img = cv2.imread('cars.png')
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

lower_white = np.array([0,0,150])
upper_white = np.array([179,255,255])

mask = cv2.inRange(hsv, lower_white, upper_white)

cv2.imshow('line', mask)

cv2.waitKey(10000)

normal image

value range 50~255

value range 100~255

value range 150~255

위와 같이 image를 HSV로 변한 한 뒤 범위내의 색상을 흰색 영역으로 추출하여 이미지를 변환 한 것을 확인 할 수 있다. value range 를 바꿔가면서 실행화면을 확인해보았을때 150~255 범위에서 차선을 잘 인식하는 것을 확인 할 수 있었다. 외부환경 및 상황을 고려하여 명도 값을 조절하여 차선을 인식하게 끔 바꾸는 것이 좋을 것 같다.

번외로 차선 외에 이미지의 특정 색상을 검출해내는 것도 시도해보았다.

응용하면 위와같이 파란부분의 영역만 골라서 확인 할 수도 있다.

cv2.bitwise_and(img, img, mask=blue_mask)

위 코드를 통해 원본이미지의 파란부분만 강조 표시 할수도 있다.

다른 이미지 예시.

4. 동영상 파일 다루기

openCV에서 이미지 뿐만아니라 동영상 및 실시간 화면을 처리해야하는 경우가 많다. 아래에서 동영상 파일을 핸들링 하는방법에 대해서 알아보자.

video.py

import cv2

vid = cv2.VideoCapture('src/ex_codes/opencv_ex/small.avi')

while True:
    ret, frame = vid.read()
    if not ret:
        #once
        break
        #if want roof
        #vid.set(cv2.CAP_PROP_POS_FRAMES,0)
        #continue

    frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    cv2.imshow('video', frame)

    if cv2.waitKey(1) > 0:
        break

vid.release()

# cv2.waitKey(20000)
cv2.destroyAllWindows()

[OpenCV] OpenCV 알아보기

Tue, 05 Mar 2024 11:59:06 GMT

😎1. 개요

OpenCV란 컴퓨터 비전 (Computer Vision)을 위해 만들어진 오픈소스로 이미지 및 비디오 처리를 위한 라이브러리이다. 컴퓨터 비전 및 머신러닝, 의료분야, 로봇 및 자율주행 차량, 인공지능, 딥러닝 등과 같이 다양한 방면에서 사용되고 있다. 예를들면 공장에서 카메라를 활용하여 어떠한 물품인지 식별하여 분류를 하기도 하고, 자율주행 자동차 방면에서는 차선을 인식하거나, 주변의 장애물이 어떤 물체인지, 앞선 차량을 인식하는 등과 같이 사용할 수 있다.

공식 사이트 : https://opencv.org/ OpenCV 문서 : https://docs.opencv.org/

위 이미지는 활용예시로 물체를 구분하는 기능을 가진다.

💻2. 특징

2.1 오픈소스

OpenCV의 특징으로는 우선 오픈소스로 누구나 사용할 수 있다.(수익 창출 가능) 하지만, OpenCV를 사용했다는 것을 반드시 명시해야한다.

2.2 다양한 플렛폼 및 언어 지원

Window , macOS, Linux 과 같은 다양한 플렛폼에서 사용하여, 파이썬 , C++로 주로 사용하며, 그 밖의 언어 환경에서도 사용 할 수 있다. 내부적으로는 C/C++로 실행되기 때문에 수행속도가 빠른 장점을 가지고 있다.

3. 🚌기능

3.1 이미지 및 비디오 처리

OpenCV는 이미지 및 비디오 데이터에 대한 다양한 처리 기능을 제공한다. 영상의 읽기, 쓰기, 변환, 필터링, 색상 공간 변환 등의 작업을 쉽게 수행할 수 있다.

3.2 컴퓨터 비전 및 알고리즘 제공

OpenCV는 다양한 컴퓨터 비전 알고리즘을 제공한다. 특징점 검출, 객체 검출, 얼굴 인식, 이미지 매칭 등 다양한 작업을 수행할 수 있는 함수와 클래스를 포함하고 있다.

4.마치며

이번에는 OpenCV에 대해서 가볍게 알아보는 시간을 가졌다. 아래의 영상을 보면 아두이노 및 OpenCV를 활용하여 손가락의 움직임을 인식하여 LED를 조작하는 것을 확인 할 수 있었다. 다음에는 활용하는 방법에 대해서 자세히 알아보도록 하자! https://www.youtube.com/watch?v=hKbtfto9trw&t=215s&pp=ygUGb3BlbkNW

Filter 코드 작성하고 그래프로 확인하기.

Thu, 01 Feb 2024 13:00:47 GMT

👀0.개요

~~## 💡0.1 데이터 만들기 두가지 데이터를 사용하였는데, 첫번째 데이터는 y = (x-10)(x-20) 두번째 데이터는 y = (x-10)(x-20)(x-30) 데이터를 x범위 0~~50 으로 세팅하여 csv파일을 각각 만들었다. 아래의 코드를 통해 쉽게 csv파일을 얻을 수 있었다.~~

알고보니 CSV 파일은 주어져 있었다.. 그냥 데이터를 좀더 다뤄본걸로 생각하자 ..

~~🐍makedata.py~~

#!/usr/bin/env python3

import pandas as pd
import numpy as np

a = 1
b = -60
c = 1100
d = -6000

x_values = np.arange(0, 50, 1)

y_values = a * x_values**3 + b * x_values**2 + c* x_values + d

df = pd.DataFrame({'time': x_values, 'y_measure': y_values})

df.to_csv('3rd_degree_equation_data_2.csv', index=False)

📁1.Average Filter

예측 결과값을 이전 결과의 평균값을 반영하여 추정값을 계산한다.

🐍1.1 파이썬 코드

🐍average_filter.py

#!/usr/bin/env python3

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

#input arraylike
class AverageFilter:
    def __init__(self, y_initial_measure):
        self.df = pd.DataFrame()
        self.df['initial'] = y_initial_measure

    def estimate(self):
        self.y_estimate = []
        initArray = self.df['initial'].values
        print(len(initArray))
        for i in range(len(initArray)):
            if (i==0) :
                self.y_estimate.append(initArray[i])
            else:
                value = round(self.y_estimate[i-1]*(i)/(i+1) + initArray[i]/(i+1),4)
                self.y_estimate.append(value)

        self.df['estimate'] =np.array(self.y_estimate)

    def getAverageValue(self):
        return self.df['estimate'].values

if __name__ == "__main__":
    #read csv file as date frame
    #signal = pd.read_csv("filters/data/example_Filter_1.csv")
    signal = pd.read_csv("filters/data/example_Filter_2.csv")

    #set values
    timeValues = signal['time'].values
    measureValues = signal['y_measure'].values

    #estimate value
    y_estimate = AverageFilter(measureValues)
    y_estimate.estimate()

    plt.figure()

    # use date for floating
    plt.plot(timeValues, measureValues,'k.', label = "Measure")
    plt.plot(timeValues, y_estimate.getAverageValue(),'r-',label = "Estimate")
    #set label
    plt.xlabel('time (s)')
    plt.ylabel('signal')

    #settings
    plt.legend(loc="best")
    #plt.axis("equal")
    plt.grid(True)
    plt.show()

📊1.2 차트 화면

📊first_data chart

📊second_data chart

실행결과를 보면 첫번째 데이터의 경우 추정치가 0에 거의 수렴하는 모습이 보였고, 두번째 데이터와 같이 어떤 양상이 있는 데이터에서는 뒤늦게 Data를 따라가는 모습을 보였다. 번외로 아래와 같이 sin(x)파를 한번 확인해보았다.

📊sin_data chart

그림과 같이 음수값에서도 추정값은 양수 값을 추정하는 모습을 보인다. 갈수록 추정값이 0에 수렴하는 걸로 보아 sin 파동이 무한정 반복하면 추정값은 한없이 0에 가까워 진다고 추정이된다.

🤔1.3 고찰

Average filter의 확인결과 속도가 너무 느리고 최신데이터를 반영하는 힘이 약해서 굳이 사용하지는 않을 것 같다. 사용을 한다면 일정한 데이터를 수신 받는 도중 큰 노이즈가 가끔 발생하는 경우에 사용할 수 있을것 같다.

📁2.Moving Average Filter

평균값의 구간을 정해서 추정치를 정하는 필터.

🐍2.1 파이썬 코드

🐍moving_average_filter.py

#!/usr/bin/env python3

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

class MovingAverageFilter:
    def __init__(self, y_initial_measure, num_average=2.0):
        self.df = pd.DataFrame()
        self.df['initial'] = y_initial_measure
        self.mv = num_average

    def estimate(self):
        self.y_estimate = []
        initArray = self.df['initial'].values
        n = self.mv
        sum = 0
        for i in range(len(initArray)):
            if (i == 0 ) :
                value = initArray[i]
                sum+= value
            elif(i < n):
                sum += initArray[i]
                value = sum/(i+1)
            else:
                value = self.y_estimate[i-1] + (initArray[i]- initArray[i-n])/n

            self.y_estimate.append(value)

        self.df['estimate'] =np.array(self.y_estimate)

    def getMovingAverageValue(self):
        return self.df['estimate'].values

if __name__ == "__main__":
    #signal = pd.read_csv("filters/data/example_Filter_1.csv")
    #signal = pd.read_csv("filters/data/example_Filter_2.csv")
    signal = pd.read_csv("filters/data/sin_function_data.csv")

    #set values
    timeValues = signal['time'].values
    measureValues = signal['y_measure'].values

    #estimate value
    y_estimate = MovingAverageFilter(measureValues,5)
    y_estimate.estimate()

    plt.figure()
    plt.plot(timeValues, measureValues,'k.',label = "Measure")
    plt.plot(timeValues, y_estimate.getMovingAverageValue(),'r-',label = "Estimate")

    plt.xlabel('time (s)')
    plt.ylabel('signal')

    plt.legend(loc="best")
    #plt.axis("equal")
    plt.grid(True)
    plt.show()

📊2.2 차트 화면

2.2.1 MV = 5 일 때

📊first_data chart

📊second_data chart

📊sin_data chart

2.2.1 MV = 10 일 때

📊first_data chart

📊second_data chart

📊sin_data chart

🤔2.3 고찰

Average Filter 보다 새로운 데이터를 받아들이는 수치가 높은 것으로 보인다. 최신의 데이터를 훨씬 더 빠르게 반영하는 것을 확인 할 수 있었다. 그리고 mv 의 수가 높을수록 데이터를 반영하는 속도가 느리고, 진동폭이 좁아진다. sin그래프와 같이 진동하는 그래프에서 mv가 높을수록 0에 수렴하는 것을 확인하였다.

📁3.Low pass filter

저주파 통과 필터로 최근의 값에 가중치를 높게 줘서 최신데이터의 영향을 키우는 필터이다.

🐍3.1 파이썬 코드

🐍LowPassFilter.py

#!/usr/bin/env python3

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

class LowPassFilter:
    def __init__(self, y_initial_measure, alpha=0.9):
        self.df = pd.DataFrame()
        self.alpha = alpha
        self.df['initial'] = y_initial_measure

    def estimate(self):
        self.y_estimate = []
        initArray = self.df['initial'].values
        print(len(initArray))
        for i in range(len(initArray)):
            if (i==0) :
                self.y_estimate.append(initArray[i])
            else:
                value = self.y_estimate[i-1]*self.alpha + initArray[i]*(1-self.alpha)
                self.y_estimate.append(value)

        self.df['estimate'] =np.array(self.y_estimate)

    def getLowPassValue(self):
        return self.df['estimate'].values


if __name__ == "__main__":
    #read csv file as date frame
    #signal = pd.read_csv("filters/data/example_Filter_1.csv")
    signal = pd.read_csv("filters/data/example_Filter_2.csv")
    #signal = pd.read_csv("filters/data/sin_function_data.csv")

    #set values
    timeValues = signal['time'].values
    measureValues = signal['y_measure'].values

    #estimate value
    y_estimate = LowPassFilter(measureValues)
    y_estimate.estimate()


    plt.figure()
    plt.plot(timeValues, measureValues,'k.', label = "Measure")
    plt.plot(timeValues, y_estimate.getLowPassValue(),'r-',label = "Estimate")
    plt.xlabel('time (s)')
    plt.ylabel('signal')
    plt.legend(loc="best")
    #plt.axis("equal")
    plt.grid(True)
    plt.show()

📊3.2 차트 화면

3.2.1 alpha =0.2

📊first_data chart

📊second_data chart

📊sin_data chart

3.2.1 alpha =0.9

📊first_data chart

📊second_data chart

📊sin_data chart

🤔3.3 고찰

1,2차 데이터의 alpha =0.9 인 경우 MovingAverage 와 흡사한 모습을 보였다. alpha 값이 낮을 수록 최신데이터를 많이 반영하기 때문에 최신데이터와 거의 흡사한 것을 볼 수 있었다. sin과 같이 진동하는 그래프에서 명확한 차이를 볼수 있다.

4. Kalman Filter

5. Comparing Filter

6. Tuning Kalman Filter

7. Kalman Filter(State Space Equation,1)

8. Kalman Filter(State Space Equation,2)

To be Continue

PID Controller 알아보기

Thu, 01 Feb 2024 12:59:57 GMT

👀0.개요

이 세상에 존재하는 로봇을 컨트롤 하기 위해서는 컨트롤러가 필요하다. 로봇이 어떤 좌표에 도달하고자 할 때, 어떻게 제어해야 원하는 좌표를 찾아 갈 수 있을까? 이번시간에는 여러가지 컨트롤러들을 통해 로봇을 컨트롤 하는 값을 제어하여 원하는 좌표로 도달할 수 있게끔 하는 방법에 대해서 알아볼 예정이다.

💡1.P Controller

우선 P Controller 에 대해서 간략하게 설명하면, 만약 로봇의 현재 위치와 목적지의 위치의 크기에 따라 제어하는 값을 변경하는 기법이다. 아래에서 파이썬 코드를 작성하고 그래프를 확인하여 해당 컨트롤러의 동작 및 단점에 대해 알아보자.

🐍1.1 파이썬코드

🐍P_controller.py

#!/usr/bin/env python3

from vehicle_model import VehicleModel
import numpy as np
import matplotlib.pyplot as plt

class P_Controller(object):
    def __init__(self,P_Gain= 0.3):
        self.target_y = target_y
        self.P_Gain = P_Gain
    def ControllerInput(self, reference, measure):
        self.target_y = reference
        self.y_measure = measure
        self.error = target_y-measure
        self.u = self.P_Gain*(self.error)

if __name__ == "__main__":
    target_y = 0.0
    measure_y =[]

    time = []
    step_time = 0.1
    simulation_time = 30

    #vehicle
    plant = VehicleModel(step_time, 0.0, 0.99, 0.05) # R, force_ratio, force_bias

    controller = P_Controller()

    # 시뮬레이션
    for i in range(int(simulation_time/step_time)):
        #시간기록
        time.append(step_time*i)
        #y 좌표 기록
        measure_y.append(plant.y_measure[0][0])
        #u 값 측정
        controller.ControllerInput(target_y, plant.y_measure[0][0])
        plant.ControlInput(controller.u)

    #plt line
    plt.figure()
    plt.plot([0, time[-1]], [target_y, target_y], 'k-', label="reference")
    plt.plot(time, measure_y,'r-',label = "Vehicle Position")
    plt.xlabel('time (s)')
    plt.ylabel('signal')
    plt.legend(loc="best")
    plt.axis("equal")
    plt.grid(True)
    plt.show()

📊1.2 실행화면

1.2.1 Gain = 0.3

1.2.2 Gain = 0.5

1.2.3 Gain = 1.0

🚀1.3 Controller 평가

위 실행화면과 같이 로봇은 reference에 도달하기 힘든 것 같다. 그 이유는 컨트롤러가 제어 값을 변경하긴 하지만 레퍼런스에 도달하기 직전에 가장 속도가 빠르기 때문이다.(에러의 적분 값에 비례하는 속도) 이러한 현상을 overshot 이 발생한다고 한다. reference 에 도달하기 위해서는 overshot을 줄일 필요가 있을 것 같다. 그리고 Gain 값이 높을 수록 reference에 도달 하는 시간은 빠르지만, 에러가 점점 커지는 것을 볼 수 있었다.

💡2. PD Controller

앞서 알아본 컨트롤러에 D가 추가된 컨트롤러이다. 제어값에 미분값을 추가하여 좀더 reference에 도달할 수 있게 도와준다. 아래코드와 실행화면을 살펴보자.

🐍2.1 파이썬코드

🐍PD_controller.py

#!/usr/bin/env python3

from vehicle_model import VehicleModel
import numpy as np
import matplotlib.pyplot as plt

class PD_Controller(object):
    def __init__(self, reference, measure, step_time, P_Gain=0.6, D_Gain=1.2):
        self.target_y = reference
        self.y_measure = measure
        self.step_time = step_time
        self.error = reference-measure

        self.P_Gain = P_Gain
        self.D_Gain = D_Gain
    def ControllerInput(self, reference, measure):
        self.target_y = reference
        self.y_measure = measure
        #errorGap
        self.errorGap = (reference-measure) - self.error
        #update error
        self.error = reference-measure

        self.p_controller = self.P_Gain * self.error
        self.d_controller = self.D_Gain * self.errorGap
        self.u = self.p_controller + self.d_controller


if __name__ == "__main__":
    target_y = 0.0
    measure_y =[]
    time = []
    step_time = 0.1
    simulation_time = 30
    plant = VehicleModel(step_time, 0.0, 0.99, 0.1)
    controller = PD_Controller(target_y, plant.y_measure[0][0], step_time)

    for i in range(int(simulation_time/step_time)):
        time.append(step_time*i)
        measure_y.append(plant.y_measure[0][0])
        controller.ControllerInput(target_y, plant.y_measure[0][0])
        plant.ControlInput(controller.u)

    plt.figure()
    plt.plot([0, time[-1]], [target_y, target_y], 'k-', label="reference")
    plt.plot(time, measure_y,'r-',label = "Vehicle Position")
    plt.xlabel('time (s)')
    plt.ylabel('signal')
    plt.legend(loc="best")
    plt.axis("equal")
    plt.grid(True)
    plt.show()

📊2.2 실행화면

2.2.1 D_Gain = 1.2

2.2.2 Gain = 10.0

🚀2.3 Controller 평가

D컨트롤러를 추가하니 D_gain 값이 낮을때는 P Controller 와 비슷한 결과를 보였지만, D_gain 값을 10으로 설정하니 reference에 훨씬 가까워 지는 것을 확인 할 수 있었다. 하지만 reference와 일치되는 모습은 확인 할 수 없었는데, 여기에 I control을 추가하면 어떤 모습을 보일까?

💡3.PID Controller

I(적분) 이 추가된 컨트롤러. 에러의 값들을 적분하여 값을 제어한다. 코드에서는 error_sum 값을 추가하여 I_Gain 값에 곱하였다. 코드를 작성해보고, 실행화면을 보자.

🐍3.1 파이썬코드

🐍PID_controller.py

#!/usr/bin/env python3

from vehicle_model import VehicleModel
import numpy as np
import matplotlib.pyplot as plt

class PID_Controller(object):
    def __init__(self, reference, measure, step_time, P_Gain=10.0, D_Gain=50.0, I_Gain=0.02):
        self.target_y = reference
        self.y_measure = measure
        self.step_time = step_time
        self.error = reference-measure
        self.errorSum = 0
        self.P_Gain = P_Gain
        self.D_Gain = D_Gain
        self.I_Gain = I_Gain

    def ControllerInput(self, reference, measure):
        self.target_y = reference
        self.y_measure = measure

        #errorGap for D
        self.errorGap = (reference-measure) - self.error

        #update error
        self.error = reference-measure

        #integral for I
        self.errorSum+= self.error
        #print(self.errorSum)
        self.p_controller = self.P_Gain * self.error
        self.d_controller = self.D_Gain * self.errorGap
        self.i_controller = self.I_Gain * self.errorSum

        self.u = self.p_controller + self.d_controller +self.i_controller

if __name__ == "__main__":
    target_y = 0.0
    measure_y =[]
    time = []
    step_time = 0.1
    simulation_time = 30
    plant = VehicleModel(step_time, 0.0, 0.99, 0.05)
    controller = PID_Controller(target_y, plant.y_measure[0][0], step_time)

    for i in range(int(simulation_time/step_time)):
        time.append(step_time*i)
        measure_y.append(plant.y_measure[0][0])
        controller.ControllerInput(target_y, plant.y_measure[0][0])
        plant.ControlInput(controller.u)

    plt.figure()
    plt.plot([0, time[-1]], [target_y, target_y], 'k-', label="reference")
    plt.plot(time, measure_y,'r-',label = "Vehicle Position")
    plt.xlabel('time (s)')
    plt.ylabel('signal')
    plt.legend(loc="best")
    #plt.axis("equal")
    plt.grid(True)
    plt.show()

📊3.2 실행화면

3.2.1 P_Gain=0.5, D_Gain=1.0, I_Gain=0.02

초기 Gain 설정으로는 reference 값에 다가가기 힘들었다.

3.2.2 P_Gain=10.0, D_Gain=50.0, I_Gain=0.02

💡3.3 Controller 평가

초기 설정으로 reference에 근접하기 쉽지가 않아 많은 튜닝을 거쳐 두번째 사진과 같이 거의 근접하는 것을 볼 수 있었다. 초기 화면과 같이 Gain 값을 튜닝하지 않으면 쉽게 맞출 수 없다는 것을 알 수 있었다.

🤔4. PID Controller 돌아보기

세 가지 컨트롤러를 알아보았다. 각 컨트롤러의 특징 및 장,단점을 정리해보자.

P Controller : 에러의 크기에 비례(P)한 컨트롤 값을 전달한다. 제어를 시작 하였을때 , 즉 에러가 가장 클때 많은 영향을 끼칠 수 있다. 미래를 보고 제어를 한다고 하기도 한다. #장점 : 빠르게 원하는 위치로 이동할 수 있다. #단점 : 단일로 사용 시 reference 근처에서 진동하는 모습을 보인다.

D Controller : 에러의 변화량,즉 에러를 미분한 값에 따른 제어를 제공한다. 현재를 제어한다고 한다. #장점 : P Controller만을 사용했을때의 Overshot을 막을 수 있다. #단점 : 에러의 변화량에 반응하기 때문에 정확한 목적지에 도달할 수 없는 점.

I Controller : 에러의 총합,즉 에러의 적분에 따른 제어를 제공한다. 과거를 보고 제어한다고 한다. #장점 : PD Controller만을 사용했을때의 reference에 도달 할 수 없는 문제를 해결 할 수 있다. #단점 : 아래의 사진과 같이 단일로 사용하였을 때 오류가 걷잡을 수 없게 커지는 점이 있다.

💡5.PID Controller 튜닝

PID 각각의 gain 값을 적절히 조절하였다.

🐍5.1 파이썬 코드

🐍Tuning_PID_Controller.py

#!/usr/bin/env python3

from vehicle_model import VehicleModel
import numpy as np
import matplotlib.pyplot as plt

class PID_Controller(object):
    def __init__(self, reference, measure, step_time, P_Gain=10.0, D_Gain=50.0, I_Gain=0.02):
        self.target_y = reference
        self.y_measure = measure
        self.step_time = step_time
        self.error = reference-measure
        self.errorSum = 0
        self.P_Gain = P_Gain
        self.D_Gain = D_Gain
        self.I_Gain = I_Gain

    def ControllerInput(self, reference, measure):
        self.target_y = reference
        self.y_measure = measure

        #errorGap for D
        self.errorGap = (reference-measure) - self.error

        #update error
        self.error = reference-measure

        #integral for I
        self.errorSum+= self.error
        #print(self.errorSum)
        self.p_controller = self.P_Gain * self.error
        self.d_controller = self.D_Gain * self.errorGap
        self.i_controller = self.I_Gain * self.errorSum

        self.u = self.p_controller + self.d_controller +self.i_controller

if __name__ == "__main__":
    target_y = 0.0
    measure_y =[]
    time = []
    step_time = 0.1
    simulation_time = 30
    plant = VehicleModel(step_time, 0.0, 0.99, 0.05)
    controller = PID_Controller(target_y, plant.y_measure[0][0], step_time)

    for i in range(int(simulation_time/step_time)):
        time.append(step_time*i)
        measure_y.append(plant.y_measure[0][0])
        controller.ControllerInput(target_y, plant.y_measure[0][0])
        plant.ControlInput(controller.u)

    plt.figure()
    plt.plot([0, time[-1]], [target_y, target_y], 'k-', label="reference")
    plt.plot(time, measure_y,'r-',label = "Vehicle Position")
    plt.xlabel('time (s)')
    plt.ylabel('signal')
    plt.legend(loc="best")
    #plt.axis("equal")
    plt.grid(True)
    plt.show()

5.2 실행화면

6.PID Controller with State Estimator : Low Pass Filter

🐍6.1 파이썬 코드

🐍PID_Controller_StateEstimator_LPF.py

#!/usr/bin/env python3

from vehicle_model import VehicleModel
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

class PID_Controller(object):
    def __init__(self, reference, measure, step_time, P_Gain=10.0, D_Gain=50.0, I_Gain=0.02):
        self.target_y = reference
        self.y_measure = measure
        self.step_time = step_time
        self.error = reference-measure
        self.errorSum = 0
        self.P_Gain = P_Gain
        self.D_Gain = D_Gain
        self.I_Gain = I_Gain

    def ControllerInput(self, reference, measure):
        self.target_y = reference
        self.y_measure = measure

        #errorGap for D
        self.errorGap = (reference-measure) - self.error

        #update error
        self.error = reference-measure

        #integral for I
        self.errorSum+= self.error
        #print(self.errorSum)
        self.p_controller = self.P_Gain * self.error
        self.d_controller = self.D_Gain * self.errorGap
        self.i_controller = self.I_Gain * self.errorSum

        self.u = self.p_controller + self.d_controller +self.i_controller

class LowPassFilter:
    def __init__(self, y_initial_measure, alpha=0.1):
        self.y_initial_measure = y_initial_measure
        self.y_estimate = y_initial_measure
        self.i = 0.0
        self.alpha = alpha

    def estimate(self, y_measure):
        if self.i == 0.0:
            value = self.y_initial_measure
            self.i+=1
        else:
            value = self.y_initial_measure * self.alpha + y_measure*(1.0-self.alpha)
        self.y_estimate = value
        return value


if __name__ == "__main__":
    target_y = 0.0
    measure_y =[]
    estimated_y = []

    time = []
    step_time = 0.1
    simulation_time = 30
    plant = VehicleModel(step_time, 0.25, 0.99, 0.05)
    estimator = LowPassFilter(plant.y_measure[0][0])
    controller = PID_Controller(target_y, plant.y_measure[0][0], step_time)

    for i in range(int(simulation_time/step_time)):
        #time
        time.append(step_time*i)

        #measure_y
        measure_y.append(plant.y_measure[0][0])

        # append
        estimated_y.append(estimator.estimate(plant.y_measure[0][0]))

        #estimator.estimate(plant.y_measure[0][0])

        controller.ControllerInput(target_y, estimator.y_estimate)
        plant.ControlInput(controller.u)

    plt.figure()
    plt.plot([0, time[-1]], [target_y, target_y], 'k-', label="reference")
    plt.plot(time, measure_y,'r-',label = "Vehicle Position(Measure)")
    plt.plot(time, estimated_y,'c-',label = "Vehicle Position(Estimator)")
    plt.xlabel('time (s)')
    plt.ylabel('signal')
    plt.legend(loc="best")
    #plt.axis("equal")
    plt.grid(True)
    plt.show()

6.2 실행화면

6.2.1 alpha = 0.2

6.2.2 alpha = 0.7

7.PID Controller with State Estimator : Kalman Filter

To be continue

[ROS]Path Planning

Sun, 21 Jan 2024 05:53:20 GMT

0. 개요

1. Rviz 에서 Path Planning 시각화하기

우선 rviz에 여러가지 토픽을 추가하자. 추가한 토픽에 대해서 설명하자면, 기존에 사용하였던 Map, LaserScan, RobotModel , PoseArray 을 제외하고 Global Cost Map, Local Cost Map, Global Plan, Local Plan 를 추가하였다. Global footprint 및 Local footprint 는 로봇의 위치를 나타낸다. 각 토픽의 역할을 rviz 화면을 통해 알아보자.

아래의 사진은 Local Cost Map 을 활성화 한 사진이다. 로봇의 주변의 환경을 인식하여 path planning에 효율적인 경로를 만드는데 사용된다. 아래의 사진은 Global Cost Map으로 map data를 통해 만들어진 전체의 Cost Map이다. 마찬가지로 path planning에 효율적인 경로를 만드는 정보를 제공한다.

Plan 요소의 경우 로봇을 움직여야지 확인 할 수 있는데, rviz 의 2D NavGaol 버튼을 통해 로봇의 목적지를 설정해주면 아래의 화면과 같이 로봇의 진행방향에 선이 나타나는 것을 확인 할 수 있다. 첫번째 사진과 같이 빨간 선이 Global Plan으로 전체 경로를 나타내고, 두번째 사진과 같이 초록색 짧은 선은 Local Plan으로 단기적인 경로를 나타낸다.

2. 오픈소스 Path Planning 패키지

Move_base 노드에는 다양한 토픽들이 존재하고, 해당 토픽을 발행하여 로봇을 컨트롤 할 수 있다. 파이썬 코드를 작성하여 토픽을 발행하여 로봇을 컨트롤 해보자.

2.1 실행 코드

send_goal_client.py

#! /usr/bin/env python

import rospy
import time
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult, MoveBaseFeedback

goal_list = [[1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0],
             [1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0],
             [-1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0]]

def feedback_callback(msg):
    """
    definition of the feedback callback. This will be called when feedback
    is received from the action server
    it just prints a message indicating a new message has been received
    """
    rospy.loginfo("[Feedback] Going to Goal Pose...")

# initializes the action client node
rospy.init_node('move_base_action_client')
# create the connection to the action server
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# waits until the action server is up and running
client.wait_for_server()

while not rospy.is_shutdown():
    i = 1
    for goal in goal_list:
        # creates a goal to send to the action server
        goal_msg = MoveBaseGoal()
        goal_msg.target_pose.header.frame_id = 'map'
        goal_msg.target_pose.pose.position.x = goal[0]
        goal_msg.target_pose.pose.position.y = goal[1]
        goal_msg.target_pose.pose.position.z = goal[2]
        goal_msg.target_pose.pose.orientation.x = goal[3]
        goal_msg.target_pose.pose.orientation.y = goal[4]
        goal_msg.target_pose.pose.orientation.z = goal[5]
        goal_msg.target_pose.pose.orientation.w = goal[6]

        rospy.loginfo("Goal Target " + str(i))

        # sends the goal to the action server, specifying which feedback function
        # to call when feedback received
        client.send_goal(goal=goal_msg, feedback_cb=feedback_callback)
        client.wait_for_result()

        rospy.loginfo("[Result] State: %d"%(client.get_state()))
        i += 1

세 가지 목적지를 부여하고, 순서대로 목적지에 도달하면 다음목적지를 목적지로 설정하고, 이 과정을 반복하는 코드이다.

2.2 실행영상

https://www.youtube.com/watch?v=QasvoEjDdjM

용량이 커서 유튜부 영상으로 업로드하였다.

3. Global planner & Global Costmap

4. Local Planner & Local Costmap

5. Commom Costmap Parameters

[ROS] Robot Localization

Sat, 20 Jan 2024 14:13:55 GMT

🐣0.개요

지난번에 로봇이 자율주행 하기 위해서 필요한 Mapping 이라는 요소를 알아보았다. 이번에는 Localization에 대해서 알아보고 어떤식으로 동작하는지 알아보자. 아래의 사진은 이번에 사용할 로봇 모델 husky.

🗺️1.Rviz에서 Localization 사용하기

rviz 에서 depth 카메라 및 AMCL을 사용해서 localization 을 어떤식으로 하는지, AMCL 노드의 경우 여러가지 파라미터를 수정 할 수 있는데, 각종 파라미터를 변경해보고 결과를 확인해보자.

📹2. depth 카메라

기존에 사용하던 cafe 맵에 husky 로봇을 소환하였다. rviz에서 depth 카메라를 확인 할 수 있다. 로봇 앞에 원통의 장애물을 두었을때 아래의 사진과 같이 기본 사진(좌측 하단)과, depth를 파악한 사진(좌측 상단)을 확인 할 수 있다.

⚙️3. AMCL (Adaptive Monte Carlo Localization)

AMCL은 적응형 몬테카를로 로컬라이제이션으로 센서의 값과 map data를 바탕으로 로봇의 위치를 추정하는 기법이다. 아래의 실행화면을 보면 로봇근처에 수많은 화살표를 볼 수 있다. 데이터를 취합해서 로봇이 위치할 수 있는 확률이 있는 곳에 표현하게 된다. 하나의 화살표를 파티클이라고도 한다. map의 데이터가 정확하고 로봇을 움직일 수록 화살표가 한점으로 모이게 된다.

3.1 AMCL 파라미터 변경해보기

.yaml 파일을 통해 amcl의 파라미터를 변경할 수 있다. 아래와 같이 런치 파일 및 .yaml 코드를 작성하였다.

🚀my_amcl.launch

🗒amcl_params.yaml

use_map_topic: true
odom_model_type: diff
odom_frame_id: odom

gui_publish_rate: 10.0
min_particles: 500
max_particles: 2000
kld_err: 0.05
update_min_d: 0.25
update_min_a: 0.2
resample_interval: 1
transform_tolerance: 1.0

laser_max_beams: 60
laser_max_range: 1.0
laser_z_hit: 0.5
laser_z_short: 0.05
laser_z_max: 0.05
laser_z_rand: 0.5

3.1.1 min_particle 및 max_particles를 각 1 ,5로 설정

기존의 설정 값보다 매우 적은 파티클을 확인 할 수 있다. 이렇게 되면 로봇의 위치파악이 훨씬 더 어려워 진다고 할 수 있다. 그러면 화살표가 많으면 좋을수 있지 않을까? 하지만 연산수가 늘어나기 때문에 로봇의 스펙에 맞춰서 결정하는것이 좋다.

3.1.2 laser_max_range 를 1로 변경하기

초기화면은 동일하나 로봇을 키보드를 통해 조작하였을 경우 파티클이 뭉치지 않고, 파티클이 좀더 분산돼어 정확한 위치를 확인 할 수 없었다. 레이저 센서를 사용하여 localization 에 얼마나 중요한 지 알 수 있었다.

3.2 global_localization

amcl 노드에서는 여러가지 서비스를 제공 사용할 수 있는데, 그중에는 global_localization 과 static_map 이 있다. global_localization은 모든 파티클이 Map의 빈공간에 무작위로 분산되는 기능이다. 무작위로 분산시키는 기능은 로봇의 위치를 제공해주지 않을 때, 센서로 주변환경의 데이터를 취합하여 amcl의 위치 추정 정보를 제공하게 된다. static_map 은 Laser 기반 위치 추정에 사용되는 Map을 가져오는 기능을 한다. 필요한 map data를 얻을 때 사용한다.

3.2.1 global_localization 호출코드

🐍init_particles_caller.py

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty, EmptyRequest
import sys

rospy.init_node("init_particles_caller")
rospy.wait_for_service('/global_localization')

disperse_particles_service = rospy.ServiceProxy('/global_localization', Empty)
msg = EmptyRequest()
result = disperse_particles_service(msg)

rospy.loginfo(result)

3.2.2 global_localization 실행화면

월드 및 로봇을 소환하고 아래의 명령어를 통해 파이썬코드를 실행하면 아래와 같이 파티클이 퍼지는 것을 확인 할 수 있다. 로봇을 계속움직일 수록 로봇의 실제 위치와 가까운 곳에 파티클이 모이는 것을 확인 할 수 있었다.

rosrun husky_launch init_particles_caller.py

[ROS] SLAM 알아보기

Sat, 20 Jan 2024 01:50:08 GMT

🐢0.개요

자율주행에서 mapping은 아주 중요한 요소이다. 로봇이 SLAM을 이용하여 Mapping을 어떻게 하여 Map 파일을 만드는지 아래의 과정을 통해 알아보자

Rviz에서 Mapping 과정 시각화 하기
Map 파일 다루기
Transforms
SLAM 오픈소스 패키지

⛰️1.Rviz에서 Mapping 과정 시각화

데모 gmapping 파일을 통해 mapping 과정을 시각적으로 알아보자. 카페 맵에 꼬부기 소환 및 gmapping_demo.launch 를 실행하였다.

1.1 터미널 명령어

roslaunch turtlebot_navigation_gazebo main.launch 
roslaunch turtlebot_navigation_gazebo gmapping_demo.launch

각각 다른 터미널에서 실행새주고 Rviz를 따로 실행시켜주자

1.2 실행화면

초기 mapping 화면은 위와 같다. Add를 통해 로봇 및 레이저 스캔을 추가해주었다.

키보드를 통해 꼬부기 로봇을 이동시켜 mapping 범위를 늘릴 수 있었다.

🗺️2.Map 다루기

gmaping 을 통해 저장된 map 은 map.yaml 과 map.pgm으로 저장 할 수 있다. 저장 터미널 명령어는 아래와 같다.

roslaunch rosrun map_server map_saver -f "저장할 map이름"

위와 같이 저장된 모습 확인 할 수 있다.

rosrun map_server map_server ~경로/파일.yaml

저장된 map은 rviz에서 로드 할 수 있다.

실습

2.1.1 launch 파일 작성하기

map_server 노드를 roslanch 를 통해 실행 할 수 있는 파일을 만들어 보자. 🚀map_server.launch

2.1.2 실행화면

2.2.1 /static_map 서비스를 처리 할 수 있는 서비스 클라이언트 노드 작성하기

🐍call_map_service.py

#! /usr/bin/env python

import rospy
from nav_msgs.srv import GetMap, GetMapRequest
import sys

rospy.init_node('service_client') # Initialise a ROS node with the name service_client
rospy.wait_for_service('static_map') # Wait for the service /static_map to be running
rospy.loginfo("init complete")

get_map_service = rospy.ServiceProxy('static_map', GetMap) # Create the connection to the service
get_map = GetMapRequest() # Create an object of type GetMapRequest

result = get_map_service(get_map) # Call the service
print(result) # Print the result given by the service called

2.1.2 실행화면

아래의 터미널과 같은 실행 결과를 확인 할 수 있었다.

🤖3.Transforms

mapping을 잘 하기 위해서는 제대로 된 map을 만들어야 한다. 그러기 위해서는 좋은 로봇을 사용해야한다. 로봇에서 인식하는 odom 과 laser 데이터의 퀄리티가 높아야 좋은 mapping을 할 수 있다.

3.1 TF란?

한 프레임에 표현된 데이터를 다른 프레임으로 변환하는 방법으로 Position, Orientation 정보를 포함한다. 특히 로봇의 base_link 와 laser_link 관계는 mapping에 반드시 필요하다.

$rqt 를 통해 알아본 꼬부기봇의 관계를 확인 할 수 있다.

매우 많지만 실행화면의 일부만을 가져왔다.

rviz에서 시각적으로도 확인 할 수 있다.

3.2 TF publishing

만약 어떤 로봇에 다른 TF를 추가하고 싶으면 아래와 같은 launch파일을 작성 하면 된다. 🚀pub_static_tf.launch

rviz 및 rqt 에서 button 이 추가 된 것을 확인 할 수 있다.

📦4.SLAM 오픈소스 패키지

SLAM 의 오픈소스인 gammping 에는 성능을 조절하기 위한 여러가지 파라미터가 존재한다. 아래의 ros 위키에서 파라미터를 확인 할 수 있다.

https://wiki.ros.org/gmapping

실습을 통해 어떤식으로 파라미터 값에 영향을 줄 수 있는지 , 각 파라미터가 어떤역할을 하는지 알아보자.

4.1 실습코드

.yaml 코드에서 파라미터를 설정하고, launch 파일을 통해 해당 설정을 불러 올 수 있다.

🗒gmapping_params.yaml

base_frame: base_footprint
odom_frame: odom
map_update_interval: 5.0
maxUrange: 6.0
maxRange: 8.0

minimumScore: 200

linearUpdate: 0.5
angularUpdate: 0.436
temporalUpdate: -1.0
resampleThreshold: 0.5
particles: 80
xmin: -1.0
ymin: -1.0
xmax: 1.0
ymax: 1.0

delta: 0.05
llsamplerange: 0.01
llsamplestep: 0.01
lasamplerange: 0.005
lasamplestep: 0.005

🚀my_gmapping.launch

4.2 실행화면

위 코드와 같이 설정하였을 때 아래와 같이 평소에 보던 실행화면을 확인 할 수 있었다. 아래에서 설정을 바꾼 실행화면을 확인해보자.

4.2.1 maxUrange 2로 변경

로봇의 mapping 범위가 매우 좁아 진 것을 확인 할 수 있다.

4.2.2 xmin, ymin = -100 xmax, ymax = 100 으로 설정 후 화면 확인

map의 크기가 매우 커진 것을 확인 할 수 있다.

[ROS] Mid project

Thu, 18 Jan 2024 15:39:32 GMT

🤔0.개요

지금까지 ROS의 시뮬레이션 기초에 대해서 강의를 들으면서 배운내용들을 확인하기 위한 과제를 수행해보자. 아래의 목차의 순서대로 진행하면서 배운내용들을 다시 돌아보자!

🗒목차

1.🚖모델 및 월드생성 2.🔌플러그인 생성 3.🤖gazebo 차량 수정 4.🎮회피 알고리즘 설계

🚖1.모델 및 월드 생성

🗿1.1 모델 생성

🗿model.sdf



    
        0 0 0.5 0 0 0
        false
        
            
                1.0
                
                    0.083
                    0.0
                    0.0
                    0.083
                    0.0
                    0.083
                
            

            
            
                0 2 0 0 0 0
                
                    
                
            
            
                0 2 0 0 0 0
                
                    
                        0.25
                        1
                    
                
            

            
                -1.732 -1 0 0 0 0 
                
                    
                        0.25
                        1
                    
                
            
            
                -1.732 -1 0 0 0 0 
                
                    
                        0.25
                        1
                    
                
            

            
                1.732 -1 0 0 0 0 
                
                    
                        0.25
                        1
                    
                
            
            
                1.732 -1 0 0 0 0 
                
                    
                        0.25
                        1
                    
                
            
            
            
                -1 0.366 0 0 0 1.047198 
                
                    
                        3.464 0.2 0.8
                    
                
            
            
                -1 0.366 0 0 0 1.047198 
                
                    
                        3.464 0.2 0.8
                    
                
            

            
                1 0.366 0 0 0 -1.047198 
                
                    
                        3.464 0.2 0.8
                    
                
            
            
                1 0.366 0 0 0 -1.047198 
                
                    
                        3.464 0.2 0.8
                    
                
            

            
                0 -1 0 0 0 0 
                
                    
                        3.464 0.2 0.8
                    
                
            
            
                0 -1 0 0 0 0 
                
                    
                        3.464 0.2 0.8

위와 같은 모델을 구성하는 sdf 파일이다. 기본모델인 box, cylinder를 통해 구성하였다. 모델을 생성하는 중 위와 같이 cylinder 의 형태를 인식 못하는 문제가 있었는데 아래와 같이 해결하였다.

수정 전 🗿model.sdf

...

...

수정 후

...

        0.25
          1

...

가제보에서 수정 전 코드의 tag를 인식못해 발생하는 문제 같다.

⚙️model.config




    Box Model
    1.0
    model.sdf

    
        Nodazi
        dk3146@naver.com
    

    
        A simple box model

모델의 설명을 config파일에 작성하였다. 가제보에서 해당 모델을 인식할 수 있게 .gazebo/models파일에 넣어두자!

🗺️1.2 월드 생성

20*20의 벽 안에 위에서 제작한 모델 4개를 10*10 안쪽에 생성하는 코드를 작성하였다. 🗺️mid_project_world.world



    

        
        
            model://sun
        

        
        
            model://ground_plane
        

        
        
            
                
                true
                model://custom_mid_obj
                
            
            0 0 0 0 0 0
            
                10 10 0.01
            
            4
            
                uniform
            
        

        
        
        
            
                
                    true
                    model://grey_wall
                
            
            3.333333 10.32 0.01 0 0 0
            
                20 20 0.01
            
            3
            
                linear-x
            
        

        
        
            
                
                    true
                    model://grey_wall
                
            
            3.333333 -10.32 0.01 0 0 0
            
                20 20 0.01
            
            3
            
                linear-x
            
        

        
        
            
                
                    true
                    model://grey_wall
                    0 0 0 0 0 1.57
                
            
            10.32 3.333333 0.01 0 0 0
            
                20 20 0.01
            
            3
            
                linear-y
            
        

        
        
            
                
                    true
                    model://grey_wall
                    0 0 0 0 0 1.57
                
            
            -10.32 3.333333 0.01 0 0 0
            
                20 20 0.01
            
            3
            
                linear-y

깔끔하게 생성된 모습

🔌2.플러그인 생성

위에서 벽안에 둘러쌓인 모델 4개를 균일하게 배치하였다. 플러그인을 생성하여 나머지 공간에 움직이는 모델 4개를 배치해보자.

🔌2.1 플러그인 작성

🔌moving_plugin.cc

#include 
#include 
#include 
#include 
#include 

namespace gazebo {
class MovingPlugin : public ModelPlugin {
public:
  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
    // Store the pointer to the model
    this->model = _parent;
    this->iterations = 10 * 1000;
    if (_sdf->HasElement("iterations")) {
      this->iterations = _sdf->Get("iterations");
    }
    this->linear_vel = 0.1;
    if (_sdf->HasElement("linear_vel")) {
      this->linear_vel = _sdf->Get("linear_vel");
    }

    this->direction_x = 1.0;
    if (_sdf->HasElement("direction_x")) {
      this->direction_x = _sdf->Get("direction_x");
    }

    this->direction_y = 0.0;
    if (_sdf->HasElement("direction_y")) {
      this->direction_y = _sdf->Get("direction_y");
    }

    // this->direction = true;
    // if (_sdf->HasElement("direction")) {
    //   this->direction = _sdf->Get("direction");
    // }

    // Listen to the update event. This event is broadcast every simulation
    // iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        std::bind(&MovingPlugin::OnUpdate, this));
  }

  // Called by the world update start event
public:
  void OnUpdate() {
    // Apply a small linear velocity to the model.
    if ((counter / iterations) % 2 == 0) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(this->linear_vel * this->direction_x,
                                   this->linear_vel * this->direction_y, 0));
    } else if ((counter / iterations) % 2 == 1) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(-this->linear_vel * this->direction_x,
                                   -this->linear_vel * this->direction_y, 0));
    }
    counter++;
  }
  // Pointer to the model
private:
  physics::ModelPtr model;

private:
  double linear_vel;
  int iterations;
  int counter;
  double direction_x;
  double direction_y;
  // Pointer to the update event connection
private:
  event::ConnectionPtr updateConnection;
};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(MovingPlugin)
} // namespace gazebo

코드를 간략히 설명하면 아래의 파라미터 값을 받아서 물체를 움직인다. linear_vel : 물체가 움직이는 속도. iterations : 물체가 방향 전환을 하기 까지의 시간. counter : 물체의 방향 전환을 인식하기 위한 값. direction_x : x방향으로 이동하는 값의 가중치. direction_y : y방향으로 이동하는 값의 가중치.

x축 및 y축의 움직임을 유연하게 구현 하기위해 direction_x와 같은 변수를 추가하였다. CMakeList.txt

...
add_library(moving_plugin SHARED src/moving_plugin.cc)
target_link_libraries(moving_plugin ${GAZEBO_LIBRARIES})
...

플러그인을 인식하기 위해 위의 코드를 반드시 CMakeLists.txt파일에 넣어주자. 코드를 작성하고 catkin_make를 통해 빌드를 하면 플러그인이 생성된 것을 확인 할 수 있다.(위치 sim_ws/devel/lib)

🗺️2.2 월드 파일 수정

🗺️mid_project_world.world

...
        
            -7.5 7.5 0.5 0 0 0
            false
            
                model://custom_mid_obj
            

            
                5.0
                3000
                1.0
                0.0
            
        

        
            7.5 7.5 0.5 0 0 0
            false
            
                model://custom_mid_obj
            

            
                5.0
                3000
                0.0
                -1.0
            
        

        
            7.5 -7.5 0.5 0 0 0
            false
            
                model://custom_mid_obj
            

            
                5.0
                3000
                -1.0
                0.0
            
        

        
            -7.5 -7.5 0.5 0 0 0
            false
            
                model://custom_mid_obj
            

            
                5.0
                3000
                0.0
                1.0
            
        
...

지난번에 생성한 모델에 플러그인을 설치하여, 움직임을 구현하였다.

🖥️2.3 실행화면

위와 같이 잘 움직이는 것을 확인 할 수 있었다. 나중에는 모델의 현제 위치를 받아서 방향을 바꾸는 방법도 구상하면 좋을 것 같다.

🤖3. gazebo 차량 수정

기존에 만든 로봇은 2개의 바퀴와 1개의 caster wheel로 구성돼있었다. 바퀴를 2개 더 추가하여 4개의 바퀴달린 차량을 만들고, skid_steer_drive_controller 플러그인을 추가해 사륜차량을 제어해보자. 로봇에 바퀴를 추가한 코드는 생략하겠다.

3.1 차량 추가 코드

🤖robot.gazebo

    
    
    
        100.0
        /
        joint_chassis_left_front_wheel
        joint_chassis_right_front_wheel
        joint_chassis_left_back_wheel
        joint_chassis_right_back_wheel
        1.16
        0.8
        link_chassis
        20
        cmd_vel
        true

기존의 이륜차량에는 libgazebo_ros_diff_drive 플러그인을 사용했었는데 실습중 그대로 작성하였을때는, 차량이 뭔가 제어가 잘 안되는 느낌이 가득 들었었다. 알고보니 skid_steer_drive_controller 이라는 스키드 방식의 조향 플러그인을 사용해야 한다.

참고 : https://classic.gazebosim.org/tutorials?tut=ros_gzplugins#JointPoseTrajectory

＊참고로 스키드 조향은 여러가지 바퀴의 회전을 달리하여 조향을 만들어 내는 시스템이다. 해당 플러그인을 넣어주고, 바퀴를 추가한뒤 차체 크기를 줄였다.

3.2 차량 동작 영상

전에 생성한 world에 spawn하여 키보드를 통해 조작을 해보았다. 움직이는 장애물의 크기가 크고 빠르게 움직여서 마구마구 두둘겨 맞는 우리의 로봇을 확인 할 수 있었다. 장애물 크기 및 속도를 개선해야겠다. 그리고 뭔가 조향이 마음대로 잘 안되는데 이에 대해서는 로봇의 설계를 좀 바꿔야 하는걸까..라는 생각을 하게 되었다.

차후 개선해볼 예정!

🎮4. 장애물 회피 알고리즘 설계

우선 장애물 크기를 줄이고 레이저를 통해 상황만을 인식하는 sample 코드를 입력해서 동작 시켜보았다. 상황에 대한 브리핑이 위와 같이 터미널에서 나타나는데 코드를 개선하여 움직임을 줘봐야겠다. ..to be continue

[ROS] World 만들기

Wed, 17 Jan 2024 12:42:10 GMT

🗺️과제 : World 만들기

population을 이용하여 World를 제작해 보았다.

population의 다양한 패턴을 사용하여 물체를 배치해보았다. random , grid , linear 과 같은 패턴이 존재하는데, 각각의 tag의 사용법을 좀더 익힐 수 있었다.

만든 world에 robot을 소환시켜 camera 토픽에 어떤식으로 보이는지 확인하였다.

🚗실행화면

아래의 링크에서 카메라 토픽의 화면을 확인 할 수 있다.

실습 영상 -> https://www.youtube.com/watch?v=zjq92hpUuFU&ab_channel=Nodazi

🌍world 코드

🌍test_world.world



    

        
        
            model://sun
        

        
        
            model://ground_plane
        

        
        
            
                
                true
                model://asphalt_plane
                
            
            0 0 0.01 0 0 0
            
                grid
                2
                2
                20 20 0.0
            

        

        
        
        
            
                
                    true
                    model://brick_box_3x1x3
                
            
            1.5 12.5 0.3 0 0 0
            
                30 30 0.01
            
            10
            
                linear-x
            
        

        
        
            
                
                    true
                    model://brick_box_3x1x3
                
            
            1.5 -12.5 0.3 0 0 0
            
                30 30 0.01
            
            10
            
                linear-x
            
        

        
        
            
                
                    true
                    model://brick_box_3x1x3
                    0 0 0 0 0 1.57
                
            
            14.5 1.5 0.3 0 0 0
            
                24 24 0.01
            
            8
            
                linear-y
            
        

        
        
            
                
                    true
                    model://brick_box_3x1x3
                    0 0 0 0 0 1.57
                
            
            -14.5 1.5 0.3 0 0 0
            
                24 24 0.01
            
            8
            
                linear-y
            
        

        
        
            
                
                false
                model://construction_barrel
                
            
            8.5 5 0.2 0 0 0
            
                10 5 0.01
            
            15
            
                uniform
            
        

        
        
            
                
                false
                model://coke_can
                
            
            -8.5 -5 0.2 0 0 0
            
                5 5 0.01
            
            100
            
                random
            
        

        
        
            
                
                    false
                    model://dumpster
                    0 0 0 0 0 1.57
                
            
            -11.8 -4 0.1 0 0 0
            
                10 12 0.01
            
            4
            
                linear-y
            
        

        
        
            
                
                false
                model://double_pendulum_with_base
                
            
            8.5 -5 0.2 0 0 0
            
                10 10 0.01
            
            10
            
                random

[ROS] 시뮬레이터 심화편

Tue, 16 Jan 2024 09:21:17 GMT

🐢0.개요

앞에서 시뮬레이션 환경의 기본편을 알아보았다. 이번시간에는 자율주행을 위한 네비게이션 요소에 대해서 알아보고 실용적인 실습도 진행해본다. 예제코드를 통해 자율주행에 무엇이 필요한지 간단하게 알아보고, 자세한 활용방법에 대한 것은 차후 알아보도록 하겠다.

🤔1.자율주행에 필요한 요소

위 모델링 파일이 이번시간에서 사용될 시뮬레이션 Map이다. 중간에 서빙로봇 kobugi가 위치한 것을 확인 할 수 있다.

🗺️1.1 Mapping

가장 먼저해야 할 일으로 자율주행 하고자 하는 맵에 대한 정보가 필요하다. 이는 로봇의 카메라 정보로 하거나 기존에 가지고 있는 정보를 통해 설정 할 수 있다. 위 화면은 gmapping_demo를 실행한 모습이다. rviz상에서 로봇이 인식한 맵을 확인 할 수 있고, 로봇을 움직여서 맵을 만들 수 있다. 로봇 조작은 아래의 명령어로 키보드로 조작할 수 있다.

roslaunch turtlebot_teleop keyboard_teleop.launch

로봇을 조작하여 위와 같이 mapping을 할 수 있었다.

🛣️1.2 Localization

mapping이 끝났다고 해서 자율주행을 할 수 있을까? 정답은 그렇지 않다. 로봇의 위치나 어디를 향하는 지를 알아야 제대로 된 자율주행을 만들 수 있다. amcl이라는 알고리즘을 통해 로봇의 위치를 알 수 있다. 아래의 실행화면을 보자. 화면을 보면 초록색 화살표가 보이는데 초기에는 퍼져서 분포되어 있다가 로봇을 조작하자 로봇의 위치와 거의 흡사한 위치를 추정하는 것을 알 수 있다. amcl알고리즘에 대해서는 차후 알아보자.

🤖1.3 Path Planning

앞서 두가지 요소를 알아봤는데, 여기에 경로계획이라는 시스템이 있어야 자율주행이 가능하다. 그 중에는 장애물을 피하고, 목적지에 도달하는 알고리즘이 필요하다. 로봇의 목적지를 아래의 rviz 실행화면의 상단에 있는 2D Nav Goal을 통해 설정이 가능하다. 목적지를 설정해두고 장애물을 추가하였을 때 어떤식으로 로봇이 동작하는 지 확인해보자.

로봇이 지나가는 경로에 여러가지 장애물을 추가해보았다. 장애물이 나타나는 순간 자연스럽게 경로를 꺾어 피해서 가는 모습을 확인 할 수 있었다.

아래의 실행화면은 장애물을 로봇이 어떻게 인식하여 회피하는 지 확인해보자.

로봇이 장애물을 확인하여 회피경로를 통해 이동하는 것을 확인할 수 있다. 실행화면을 자세히보면 초록색 경로와 빨간색 경로가 나타나는데, 초록색 경로는 전체경로, 빨간색 경로 장애물 회피경로이다. 로봇의 아래쪽에 깔린 여러가지 색상을 확인 할 수 있는데, 로봇이 앞으로 가야하는 경로의 우선순위(빨간색 우선)를 나타낸다고 볼 수 있다.

아래의 rqt_graph 실행 화면으로 각 노드가 주고받는 토픽에 대해서 알 수 있다.

cmd_vel -> 네이베이션 스텍에 출력을 받아서 모터의 속도로 변환하는 역할 LaserScan -> 스캐너의 스캔값을 네이게이션에 전달하는 역할 odom -> 네이게이션 스텍에 오도메트리 값 전달 tf -> 네이게이션 스텍에 map을 기준으로 로봇의 위치나 센서의 tf 정보를 전달하는 역할. move_base -> 모든 정보를 모아서 로봇을 목적지까지 도달 할 수 있는 명령을 전달.

[ROS] Gazebo Plugins

Mon, 15 Jan 2024 17:19:13 GMT

🐢0.개요

Gazebo Plugin이란? Gazebo 시뮬레이션 환경에서 다양한 사물의 움직임을 구현하거나, 기능을 구현하는데 사용된다. 로봇의 센서,엑추에이터, 컨트롤러를 구현하는데 활용할 수 있다. Python이나 C++로 작성 될 수 있으며, 이번시간에서 C++로 Plugin을 직접 작성해보면서 Plugin의 역할을 알아보자.

🔌1.Plugin 작성 과정

이번 실습에는 Gazebo의 사물을 정사각형 모양을 그리면서 움직이는 모형을 만들어 볼 예정이다. 아래의 순서대로 진행해보자.

1.1 CMakeList.txt 작성하기

프로젝트의 의존성을 추가하기 위한 CMakeList를 작성한다. gazebo_plugins,gazebo_ros,roscpp package를 추가해주고 아래와 같이 코드를 작성한다. ⛰️ CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(writing_plugins)

find_package(gazebo REQUIRED)

find_package(catkin REQUIRED COMPONENTS
  gazebo_plugins
  gazebo_ros
  roscpp
)

catkin_package()

include_directories(
    # include
  ${catkin_INCLUDE_DIRS}
)

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

add_library(a_model_plugin SHARED src/a_model_plugin.cc)
target_link_libraries(my_gazebo_plugin ${GAZEBO_LIBRARIES})

add_library 라인에 본인이 작성한 플러그인 파일 (a_model_plugin.cc)을 넣어준다. 아래에서 .cc파일을 확인해보자

1.2 .CC 파일 작성

.cc파일에서는 Gazebo의 모델을 컨트롤할 코드를 작성해준다. Load 함수는 플러그인이 로드 될 때 실행되는 함수이다. 클래스의 변수를 초기화 하여 Gazebo에 알려주는 역할을 한다. 여기에서는 물체의 속도 및 움직이는 시간을 입력하였다. 입력되는 변수가 없다면 초기에 선언한 값을 사용 할 수 있다.

OnUpdate 함수는 Gazebo 시뮬레이션에서 각 타임스탭에서 호출되는 함수이다. 아래와 같이 정사각형으로 움직이게 하기 위해 +x -> +y -> -x -> -y 순으로 동작하게 작성하였다. 🗒a_model_plugin.cc

#include 
#include 
#include 
#include 
#include 

namespace gazebo {
class AModelPlugin : public ModelPlugin {
public:
  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
    // Store the pointer to the model
    this->model = _parent;
    this->iterations = 10 * 1000;
    if (_sdf->HasElement("iterations")) {
      this->iterations = _sdf->Get("iterations");
    }
    this->linear_vel = 0.1;
    if (_sdf->HasElement("linear_vel")) {
      this->linear_vel = _sdf->Get("linear_vel");
    }

    // Listen to the update event. This event is broadcast every simulation
    // iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        std::bind(&AModelPlugin::OnUpdate, this));
  }

  // Called by the world update start event
public:
  void OnUpdate() {
    // Apply a small linear velocity to the model.
    if ((counter / iterations) % 4 == 0) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(this->linear_vel, 0, 0));
    } else if ((counter / iterations) % 4 == 1) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(0, this->linear_vel, 0));
    } else if ((counter / iterations) % 4 == 2) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(-this->linear_vel, 0, 0));
    } else if ((counter / iterations) % 4 == 3) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(0, -this->linear_vel, 0));
    }

    counter++;
  }
  // Pointer to the model
private:
  physics::ModelPtr model;

private:
  double linear_vel;
  int iterations;
  int counter;
  // Pointer to the update event connection
private:
  event::ConnectionPtr updateConnection;
};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(AModelPlugin)
} // namespace gazebo

1.3 .world

world 에서 물체의 속성과 적용할 플러그인을 선언하였다. .cc의 변수를 plugin tag 내에 삽입하여 해당 값을 사용 할 수 있게 한다. 🌍model.world

...
        
            2 2 0.5 0 0 0
            false
            
                model://person_walking
            

            
            
                0.2
                2500
            
        
...

1.4 Plugin 경로 설정

Gazebo에 사용할 Plugin의 경로를 알려주기 위해 터미널에 아래와 같이 입력한다.

export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/sim_ws/devel/lib

🚗2.실행화면

launch파일을 작성하여 실행화면을 확인해보자. 조금 속도가 느리지만 정사각형 형태를 그리면서 잘 동작하는 것을 확인 할 수 있다.

속도가 많이 느려서 아래와 같이 조정하였다. linear_vel = 5.0 값 iterations = 1000

🌍model.world

...
5.0
2500
...

아래와 같이 코드를 수정하여 물체를 사람으로 변경하여 움직이게 할 수 있다.

🌍model.world

...
        
            2 2 0.5 0 0 0
            false
            
                model://person_walking
            

            
            
                5.0
                1000
            
        
...

box객체를 밀어내는 사람의 모습을 확인 할 수 있었다.

[ROS] 잡학사전

Mon, 15 Jan 2024 15:32:23 GMT

0.개요

ROS 개발을 하면서 자주 깜빡하거나 알아두면 좋은 내용들을 순서 상관없이 정리하는 글이다.

1.Gazebo Plugin 생성시 아래와 같이 CMake파일을 추가해야한다.

2.`.bashrc` 파일에서 단축 터미널 명령어를 alias를 통해 정의 할 수 있다.

자주쓰는 명령어가 있다면 추가해서 활용해보자!

3. .py 파일에 권한 부여하기.

rosrun에 쓸 .py파일은 아래의 명령어로 반드시 권한을 부여해줘야한다.

chmod +x *.py

4. 새로운 워크 스페이스 추가시 해야할 것

5. rqt quaternions 값

https://eater.net/quaternions/video/intro

용어집

odomety : 거래주행계, 로봇이 기준점으로 부터 얼마나 이동했는지를 나타내는 지표
TF : Transform 으로 한 프레임에 표현된 데이터를 다른 프레임으로 변환하는 방법

[ROS] 카메라 플러그인 적용하기

Sat, 13 Jan 2024 04:12:54 GMT

🐣0.개요

지난 시간에 Gazebo에서 사용할 수 있는 Tag 및 Plugin, 적용방법에 대해서 학습하였다. 그리고 world 파일 tag population에 대해서 학습하면서 규칙을 생성하여 월드를 만드는 방법에 대해서 학습하였다. 이번 시간에는 카메라 모듈을 적용한 로봇 모델을 지난 시간에 만든 world에 배치하여 Camera Plugin의 동작 화면을 확인하는게 목표이다. 아래의 링크에서 Camera 플러그인 적용법을 참조 할 수 있었다.

[Gazebo plugins in ROS] - https://classic.gazebosim.org/tutorials?tut=ros_gzplugins#Camera

🗒1.코드 작성

📒1.1 .xacro 파일 작성

카메라의 형태를 정의해준다. 카메라의 위치는 이전에 만든 Ladar 위에 위치시키기 위해 parent link를 해당 링크로 정의하였다. joint의 위치를 parent link의 z축으로 0.1m 위치한 곳으로 선언하였다. xacro:property를 통해 정육면체 모양의 카메라의 길이를 선언하였다. 🤖robot.xacro

카메라의 색상은 robot.gazebo에서 적용하였다. 🏛️robot.gazebo

...
    
    Gazebo/Red
    
...

🔌1.2 .gazebo 파일 Camera plugin 적용

.gazebo 파일에 Camera plugin을 사용하기 위한 코드를 아래와 같이 추가하였다.

🏛️robot.gazebo

...

        
        30.0
        
            1.3962634
            
                800
                800
                R8G8B8
            
            
                0.02
                300
            
            
                gaussian
                
                0.0
                0.007
            
        
        
            true
            0.0
            rrbot/camera1
            image_raw
            camera_info
            camera_link
            0.07
            0.0
            0.0
            0.0
            0.0
            0.0
        
        
    
...

.gazebo에서 선언한 camera_link에 camera 플러그인을 적용하는 코드이다. 각각의 Tag의 역할에 대해서 알아보자. sensor : 센서를 정의한다. 위코드에서는 camera 센서의 속성 값 및 플러그인이 포함되어 있는 Tag이다. updata_rate : 카메라 센서의 프레임을 선언 (FPS: Frame per Second). horizontal_fov : 카메라의 시야각을 나타낸다. 위 코드의 선언값은 1.3962634 약 80도의 시야각을 나타낸다. image : 카메라의 해상도 및 형식을 지정한다. (800 * 800) , R8G8B8 은 RGB의 각각의 값들이 8비트로 표현된다는 뜻이다. clip : 카메라의 클리핑을 설정한다. 카메라의 최대 거리 및 최소 거리를 지정 할 수 있다. noise : 사진의 노이즈를 추가한다. 노이즈를 추가하는 이유는 현실의 카메라는 노이즈가 존재하기 때문에 시뮬레이션 환경에서 불확실성을 추가하여 현실과 비슷한 시뮬레이션을 진행 할 수 있게 한다. gaussian 노이즈를 추가하였고 표준편차 및 평균 값을 지정할 수 있다.

❓gaussian noise 란 - 가우스 분포를 나타내는 잡음으로 이미지 처리를 할 때 주로 나타나는 노이즈. 차후 자세히 다뤄보겠다.

plugin : 플러그인 내용으로 libgazebo_ros_camera.so 플러그인을 사용한다. 내부의 내용은 카메라의 이름, 주기, 발행토픽이름, 왜곡 등을 설정 할 수 있다.

✈️2.실행화면

전에 작업한 world 에 모형 로봇을 소환하여 카메라 시점을 확인 할 수 있었다. 로봇이 움직이면서 카메라의 시점이 바뀌는 데, 카메라 시야각에 보이는 물체들을 잡아내는 것을 확인 할 수 있다.

실행과정은 아래와 같다. 1.world 실행

$roslaunch robot_description empty_world.launch

2.spawn robot

$roslaunch robot_description spawn.launch

3.동작 명령

$rosrun move_with_laser.py

4.rviz 에서 카메라 토픽 기반으로 만들어진 이미지 확인

$roslaunch robot_description rviz.launch

rqt_graph 를 통해 토픽의 흐름을 확인 할 수 있다.

[ROS] Gazebo population 알아보기

Fri, 12 Jan 2024 15:33:31 GMT

👀0.개요

world를 생성할때 population tag를 사용하면 오브젝트를 하나씩 배치하지 않고 일정한 규칙을 지정하여 생성하거나 랜덤으로 배치 할 수 있는 기능을 가진다. 실습을 통해 population의 사용법을 알아보자.

🚙1.실습 진행

population tag를 이용하여 world 파일을 작성 한 뒤 gazebo에서 어떤 식으로 나타나는지 확인하고 각각의 코드가 어떤식으로 동작하는지 알아보겠다.

🗒1.1 코드

🌍population.world



    

        
        
            model://sun
        

        
        
            model://ground_plane
        

        
        
            
                
                    true
                    model://jersey_barrier
                
            
            0 19 0.3 0 0 0
            
                40 40 0.01
            
            10
            
                linear-x
            
        

        
        
            
                
                    true
                    model://jersey_barrier
                
            
            0 -21.5 0.3 0 0 0
            
                40 40 0.01
            
            10
            
                linear-x
            
        

        
        
            
                
                    true
                    model://jersey_barrier
                    0 0 0 0 0 1.57
                
            
            19 0 0.3 0 0 0
            
                40 40 0.01
            
            10
            
                linear-y
            
        

        
        
            
                
                    true
                    model://jersey_barrier
                    0 0 0 0 0 1.57
                
            
            -21.5 0 0.3 0 0 0
            
                40 40 0.01
            
            10
            
                linear-y
            
        


        
        
            
                
                false
                model://construction_barrel
                
            
            0 0 0.2 0 0 0
            
                30 30 0.01
            
            100
            
                random

💻1.2 Gazebo 화면

1️⃣1.2.1 첫번째 실행

2️⃣1.2.2 두번재 실행

🤔2. 코드 분석

실행화면을 보면 사각형 형태로 둘러싼 바리케이드와 내부에 꼬깔이 여러개 배치되어 있는 것을 확인 할 수 있다. 아래에 population에서 사용가능한 tag를 설명하였고, 바리케이트 및 꼬깔이 어떤식으로 배치되었는지 살펴보자. 🗒테그 목록

1.population - population 객체의 이름을 지정할 수 있다. 2. model - 객체의 모델을 정의하는데 사용된다. 3. include - 모델에 필요한 소스를 불러오거나 ,속성을 정의 하는데 사용된다. 4. pose - 시뮬레이션에서 오브젝트의 초기위치 또는 방향을 설정한다. 5. box - 시뮬레이션에서 객체가 위치할 영역을 설정한다. 6. model_count - 시뮬레이션에서 만들 모델의 수를 정의한다. 7. distribution - 개체를 배포할 방법을 지정한다 (ex : random , uniform, grid, linear-x,linear-y,linear-z)

*🚧바리케이스 생성코드 *

        
            
                
                    true
                    model://jersey_barrier
                
            
            0 19 0.3 0 0 0
            
                40 40 0.01
            
            10
            
                linear-x

우선 population의 name은 barriers_population이고 , model name 은 jersey_barrier_1이다. static 이 true이므로 고정된 성질을 가진다. uri에 해당되는 모델 jersey_barrier을 불러온다. pose모델의 초기 위치는 xyz = 0 19 0.3 pry = 0 0 0 으로 회전 값은 따로 부여하지 않았다. box 테그는 객체가 위치할 영역으로 40*40 으로 설정하였다. model_count 값이 10이므로 10개의 바리케이트가 설치될 것이다. distribution == linear-x 이므로 x축으로 일렬로 배열한다.

해당 내용으로 x축 및 y축으로 총 4개의 population의 pose, distribution 값을 바꾸면서 구성할 수 있었고 , y축 바리케이트는 include 의 pose 중 pry = 0 0 1.57 을 부여해 모델이 Y축으로 향하게 배치할 수 있었다.

위 실행화면을 보면 첫번째 실행화면과 두번째 실행화면의 바리케이트 배치는 같으나 꼬깔의 배치가 다른 것을 확인 할 수 있다. 그이유는 아래의 코드를 보면서 확인해보자.

*🚧꼬깔 생성코드 *

        
        
            
                
                false
                model://construction_barrel
                
            
            0 0 0.2 0 0 0
            
                30 30 0.01
            
            100
            
                random

차이점은 distribution 이부분의 값이 random으로 설정되어있기 때문이다. random 값을 부여하면 지정된 범위 내에 랜덤으로 해당 객체를 생성하게 된다.

[ROS] Gazebo model 제작하기

Thu, 11 Jan 2024 12:18:21 GMT

🚙1.Model 구성 살펴보기.

Gazebo에서 사용할 3D모델을 직접 구성하여 여러가지 속성의 동작에 대해서 알아보자. 박스형태의 간단한 모델을 제작해보자.

🚖1.1 SDF 파일

🚖model.sdf



    
        0 0 0.5 0 0 0
        false
        
            
                1.0
                
                    0.083
                    0.0
                    0.0
                    0.083
                    0.0
                    0.083
                
            
            
                
                    
                        1 1 1
                    
                
            
            
                
                    
                        1 1 1
                    
                
            
            
                0 0 1 0 0 0

sdf파일에는 urdf와 비슷하게 물체의 속성 (무게, 크기, 재질 등)을 결정하는 코드를 작성 할 수 있다. 위 코드 같은 경우 아래의 사진과 같은 모델을 gazebo에서 확인 할 수 있다.

sdf 파일은 collision속성과 visual속성이 존재한다. collision은 말그대로 충돌 범위를 설정 할 수 있고, visual은 시각적인 정보만 담고 있다. 그렇기 때문에 어떤 객체에 visual만 설정하고 collision속성을 같게하여 보이는 것과 충돌하게 할 수도 있고 visual만 설정하여 보이지만 충돌하지 않는 객체를 생성 할 수 있다. 이러한 점을 위에서 만든 모델로 확인해보자. 코드의 collision이라는 이름을 가지는 충돌범위와 visual를 동일하게 정의하였고, visual2의 경우 visual 만 정의하였다.

그렇기 때문에 해당 model 위에 자동차 모형을 떨어뜨리면 실린더 모양의 구조물을 통과하는 모습을 확인 할 수 있다.

⚙️1.2 Config 파일

⚙️model.config




    Box Model
    1.0
    model.sdf

    
        Nodazi
        dk3146@naver.com
    

    
        A simple box model

Config 파일은 해당 모델의 name, version ,author등 의 요소를 기입할 수 있다.

[ROS] ROS 연동하기

Wed, 10 Jan 2024 11:58:22 GMT

🐣1.Gazebo의 Ros 노드

Gazebo에서 여러가지동작을 하는 명령어를 터미널을 이용 할 수 있다. 아래의 코드에 추가로 덧붙혀서 사용할 수 있으며 각각의 기능에 대해서 알아보자.

$rosservice call gazebo/

pause_phsics

gazebo의 모든 객체들이 멈추게 된다.

unpause_phsics

일시정지가 해제된다.

reset_simulator

모든 오브젝트의 위치를 초기 설정으로 되돌린다.

reset_world

모든 오브젝트의 위치를 초기 설정으로 되돌린다.(시뮬레이션 시간 포함)

👀2.Gazebo Tag

urdf 파일에서 gazebo 속성을 추가하여 gazebo 시뮬레이션에 적용될 속성을 추가 할 수 있다.

2.1 Tag로 색상 부여하기

지난 시간에 urdf 파일의 내용을 수정하여 로봇의 구성 및 생상을 변경하였었다. 하지만 Rviz상에서 잘 적용되던 색상같은경우 Gazebo 환경에서 로봇을 소환하였을 때 색이 없는 것을 확인 할 수 있었다.

아래의 사진과 같이gazebo 상에서 색상이 없는 모습

이를 해결하기 위해서는 urdf파일에 gazebo tag를 입력하여 원하는 색상 및 해당 객체의 여러가지 값들을 변경 할 수 있다. 코드를 입력하여 물체의 색상을 넣어보고, 마찰계수를 넣는 코드를 작성해보자.

*🤖robot.urdf *

...
    
         Gazebo/Purple 
    
    
         Gazebo/Grey 
    
    
         Gazebo/Black 
    
    
         Gazebo/Black 
    
...

색상 테그를 추가한 화면

gazebo material 확인

$vim /usr/share/gazebo-9/media/materials/scripts/gazebo.material

위 명령어를 통해 위 코드의 색상에 사용된 Gazebo/Black 과 같이 gazebo tag에서 사용할 수 있는 material 을 확인 할 수 있다.

2.2 Tag로 물체 마찰계수 부여하기

추가로 로봇 모델에 tag를 추가하여 마찰 설정을 해보자. *🤖robot.urdf *

...
    
         Gazebo/Purple 
    
    
         Gazebo/Grey 
        0
        0
    
    
         Gazebo/Black 
        1
        1
    
    
         Gazebo/Black 
        1
        1
    
...

Force를 가하여 움직이는 모습

🔌3. ROS Plugin

플러그인을 사용하면 Gazebo에서 유용한 기능을 추가할 수 있다.

3.1 Robot Differential Driver

3.1.1 코드 작성

🐍move_simple.py

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

def odom_callback(msg):
        rospy.loginfo('X: %s / Y: %s' % (msg.pose.pose.position.x,msg.pose.pose.position.y))

def main():
    odom_sub = rospy.Subscriber('odom',Odometry,odom_callback)
    twist_pub = rospy.Publisher('cmd_vel', Twist,queue_size=10)

    rospy.init_node('move_simple_node', anonymous=True)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        vel_msg = Twist()
        vel_msg.linear.x = 0.3
        vel_msg.linear.z = 0.3

        twist_pub.publish(vel_msg)
        rate.sleep()

if __name__=='__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

3.1.2 실행화면

위 파이썬 코드를 실행시키면 아래와 같이 로봇이 움직이게 된다.

X,Y 좌표정보가 갱신되는 것도 확인 할 수 있다.

3.2 Robot Laser Sensor

3.2.1 코드 작성

*🤖robot.urdf *

...

    
        true
    
    
        Gazebo/DarkGrey
    

    
    
        
        
        
        
    
    
        
            
            
            
        
        
            
            
                
            
            
                
            
        
        
            
            
                
            
        
    

    
        
            0 0 0 0 0 0
            true
            20
            
                
                    
                        720
                        1
                        -1.570796
                        1.570796
                    
                
                
                    0.20
                    10.0
                    0.01
                
                
                    gaussian
                    0.0
                    0.01
                
            
            
                /laser/scan
                sensor_laser
            
        
    
...

urdf 파일에 Laser sensor Plugin에 대한 정보를 입력하고 , Ladar 센서 형태를 로봇의 전면에 생성한다. 아래의 사진과 같이 센서의 범위를 눈으로 확인 할 수 있다. visualize 설정을 통해 레이저를 보이지 않게 할 수도 있다.

🐍move_with_laser.py

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

def laser_callback(msg):
    rospy.loginfo('Minimum distance is: %s' % min(msg.ranges))

def main():
    laser_sub = rospy.Subscriber('laser/scan', LaserScan, callback=laser_callback)
    twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)

    rospy.init_node('move_with_laser_node', anonymous=True)
    rate = rospy.Rate(10) # 10hz

    while not rospy.is_shutdown():
        twist_msg = Twist()
        twist_msg.linear.x = 0.3
        twist_msg.angular.z = 0.3
        twist_pub.publish(twist_msg)
        rate.sleep()

if __name__=='__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

로봇의 동작 및 Laser 센서 값을 통해 최단 거리를 계산하는 코드이다.

3.2.2 실행화면

*1.gazebo 실행화면 *

*2.터미널 실행화면 *

위 사진과 같이 로봇이 회전운동하면서 아래의 터미널과 같이 Ladar에서 가장 가까운 거리를 계산하여 출력되는 것을 확인 할 수 있었다.

3.3 XACRO

로봇 XML 파일을 생성할 때 여러가지 요소의 값을 동시에 바꾸고 싶었던 적이 있는가? 그런 행위를 가능하게 하는 것이 XACRO이다. XACRO란 XML + Macro의 합성어로 로봇모델을 간단하게 메크로를 사용하여 XML 파일을 생성 할 수 있는 프로그램이다. Parameter를 사용하여 더욱더 빠르고 쉽게 urdf파일을 생성 할 수 있다. 실습을 통해 XACRO 사용법을 알아보자.

3.3.1 코드작성

🤖robot.xacro

Parameters 항목에서 변수에 값을 선언하여 사용할 수 있다. chassis_mass 값을 10으로 선언하였고 chassis_mass 변수를 중간에 사용하는 모습을 확인 할 수 있다. 그리고 pi과 같은 상수도 선언하여 사용하면 편리하다.

3.4 Joint Control

Joint Control은 말그대로 관절의 움직임과 제어를 처리하는데 사용되는 도구이다. 로봇 모델의 관절에 대한 토픽을 통해 명령을 보내거나 프로그래밍을 통해 관절을 제어 할 수 있다.

3.4.1 동작화면

코드는 생략하고 동작화면은 아래와 같다. Joint_control_gui 를 통해 아래와 같이 로봇의 꼬리를 제어할 수 있었다.

다음시간에는 Camera Plugin을 설치하여 로봇 시점에서 카메라의 동작을 확인해보겠다.

[ROS] Gazebo에 Robot 불러오기

Tue, 09 Jan 2024 12:17:14 GMT

🐣0.개요

지난 시간에 로봇을 만드는 실습을 진행하였다. 이번시간에는 Gazebo에서 로봇을 스폰하는 방법에 대해서 알아보겠다.

🗒1.Launch 파일 만들기

🚀1.1 Gazebo World 생성 launch 파일

🚀empty_world.launch

월드를 생성하는 launch 파일. 월드의 속성을 정의할 수 있다.

1.2 spawn launch 파일

🚀spawn_robot.launch

로봇을 스폰할 위치를 정의해준다.

그리고 empty_world를 먼저 launch 하고 spawn_robot를 launch 하면 된다.

🌍2.실행화면

위와 같이 모델링한 자동차 모형이 Gazebo에 등장 하는 것을 확인 할 수 있다.

🤖2.1 같은 로봇 여러번 불러오기

같은 로봇을 월드에서 여러번 불러올려면 어떻게 해야할까? 위 과정으로 한번더 spawn 을 launch 하면 에러가 나타나면서 로봇이 월드에 스폰할 수 없다고 나온다. 이유는 중복되는 model_name을 가지는 객체를 소환 할 수 없기 때문이다. 위 문제를 해결하기 위해 아래와 같이 모델네임을 정의 할 수 있게 런치파일을 수정하면 된다.

🚀spawn_robot.launch

$roslaunch spawn.launch x:=6 model_name:=second

터미널 명령어로 model_name을 선언하여 launch하면 이상없이 아래의 사진처럼 로봇을 스폰 할 수 있다.

[ROS] Rviz 로봇 모델 만들기

Tue, 09 Jan 2024 11:46:32 GMT

🤔0.개요

Rviz 및 urdf를 활용하여 자동차 모델링을 해보자.

🗒1.예제코드

🤖robot.urdf

launch 파일은 생략한다. Rviz를 통해 위 코드를 동작시켜보면 아래의 그림과 같은 모형이 나타난다.

각각의 코드가 어떤역할을 하길래 위와 같은 동작을 수행 할 수 있는 것일까?

위 코드는 link_chassis를 parent로 선언하여 기준으로 삼는다는 뜻이고 기준점에서 origin속성만큼의 값만큼 떨어진 곳에 link_left_wheel를 연결한다는 뜻이다.origin에서 rpy는 기울어짐 정도이고, xyz는 위상차이로 상대적인 좌표를 나타내는 것이다. axis 는 어떤 축을 기준으로 link를 작동시킬 것인지 나타내는 것이고, 위코드에는 y축을 기준으로 움직이기 때문에 위와같이 선언하였다.

위 내용은 link_left_wheel에 대한 내용을 담고 있다. 무게와 좌표 등을 담고 있다. 충돌 정보를 담고 있는 collision 그리고 보여지는 요소 visual로 구성된다.

💻2.실습

위 내용을 토대로 반대쪽 바퀴를 추가하고, 아래쪽에 캐스터 휠을 추가한 로봇을 제작해보자.

🤖robot.urdf

학습내용을 토대로 위 로봇을 모델링 하였다. TF축도 확인하였다.

🗿3.Meshes 설정하기

urdf를 통해 로봇을 디자인할때 .stl과 같은 파일을 이용하여 물체의 외형을 변경 할 수 있다. 앞에서 만든 자동차 모형에 mesh속성을 추가해보자.

mesh 파일을 다운로드 하여 src/robot_description/meshes 파일에 .stl파일을 추가하여 위의 코드와 같이 geometry 속성에서 불러와서 사용하였다. 위 코드는 몸통에 Mesh를 적용 한 것이며 바퀴에도 따로 적용시켰다.

부드러워진 자동차의 형태. Cad를 다룰줄 알면 모델링을 할때 유용 할 것 같다.

[ROS] URDF 알아보기

Mon, 08 Jan 2024 14:36:04 GMT

🤔0.URDF란?

URDF (Unified Robot Description Format)은 직역하면 통합 로봇 설명 형식으로 로봇의 3D 형상 및 외관, 관성 등 물리적 특성 등을 XML 언어로 정의한 로봇 모델링 언어이다. URDF로 정의된 로봇은 RVIZ 및 Gazebo에서 물리 시뮬레이션을 할 수 있다.

🏁1.URDF 좌표계와 단위

URDF 는 아래의 그림과 같이 x, y, z 축을 사용한다. 회전을 표현하기위해 오일러 각도 roll, pitch, yaw 를 사용한다.

단위는 아래의 표와 같이 사용된다.

길이 : 미터 m

각도 : 라디안 radian

질량 : kg

속도 : m/s

각속도 : radian /s

🐣2.URDF 형상과 색상 표현

2.1형상표현

cylinder : 원통 box : 상자 sphere : 공

2.2색상 표현

RGB 3원색과 투과율(0~1) 을 정의하여 사용한다.

🐤.URDF 기구 표현

기구의 속성을 부여하여 동작을 제어할 수 있다.

Base: 고정부분(grounded) Link: 관절에 연결되는 로봇 팔의 부분 Joint: 링크를 연결하는 부위로 보통 모터의 회전을 통해 움직임을 만듦

Joint 의 경우 아래의 속성을 부여하여 제어 할 수 있다.

fixed: 고정 revolution : 작동 범위 제한 continuous : 연속회전

🐢4.예제를 통해 URDF 알아보기

🤖pan_tilt.urdf




     ## link : base_link
        
            
                 ## 형태 (길이, 각도)
            
             ## x,y,z 좌표 및 영점조절
            
                  ## 색상값
            
        

        
            
                
            
            
        

        
            
            
        
    

     ## joint : pan_joint 
        
        
        
        
        
        
    

      ## link : pan_link
        
            
                
            
            
            
                
            
        

        
            
                
            
            
        

        
            
            
        
    

     ## joint : tilt_joint
        
        
        
        
        
        
    

       ## link : tilt_link

여기서 아래의 명령어를 터미널 창에 입력하면 Link와 Joint 관계도를 PDF파일로 만들어준다.

🖥️terminal

$urdf_to_graphiz pan_tilt.urdf

🖥️terminal

$sudo apt-get install liburdfdom-tools

만약 찾을수 없는 명령이라고 나오면 위의 명령어를 터미널에 입력해줘야 한다.

🚀pan_tilt_urdf.launch

Joint state 변경 gui 설치 🖥️terminal

$sudo apt install ros-melodic-joint-state-publisher-gui

해당 명령어를 터미널에 입력하여 Joint state 변경 gui를 설치해준다. 아래에서 어떤식으로 동작하는지 알아보자.

이제 코드를 모드 작성하였으니 launch 파일을 실행시켜 나타나는 화면을 확인하자.

🚗5.실행화면

위와 같은 기둥형태의 로봇을 확인 할 수 있다.

아까 터미널을 통해 ros-melodic-joint-state-publisher-gui를 설치하였다면 위 화면을 확인 할 수 있다. 이 창은 Joint의 각도를 조절하는 gui이다. 아래 영상에서 버튼을 이용하여 Joint를 조절하는 모습을 볼 수 있다.

Randomize 버튼을 통해 랜덤으로 조절할 수도 있다.

[ROS]RViz 3D자동차 주행프로그래밍

Sun, 07 Jan 2024 08:18:43 GMT

🤔0.개요

지난시간에는 RViz에 3D모델링 된 자동차를 띄우고, 바퀴를 제어하는 프로그래밍을 진행하였다. 이번 시간에는 odom이라는 토픽을 만들어, RViz 오도메트리 프로그램에 전송하여 자동차를 가상공간에서 직접 움직여보고, 궤적을 남기는것을 추가하여 주행하는 프로그램을 작성하고 실제로 어떤식으로 움직이는지 확인해보자. (아래 사진은 지난시간 제어프로그래밍 사진이다.)

🚗1.동작 방식

동작 방식은 위와 같다. driver노드에서 xycar_motor라는 모터 제어 토픽을 convertor노드에 전송하고, 전송 받은 토픽을 가공하여, joint_states토픽으로 RViz viewer 및 rviz_odom이라는 노드에 전송한다. RViz viewer는 가상 자동차 모델의 바퀴의 움직임을 제어하는 역할을 하고, rviz_odom노드는 가상 자동차 모델을 실제로 움직이는 odom토픽으로 전송한다.

📬1-1.Msg 구성

각 토픽이 어떤 메세지를 주고받는지 터미널 명령어를 통해 알아보자. xycar_motor 에는 header 및 angle, speed 로 구성되어있다. JointState는 header 및 name, position,effort 로 구성되어있으며, xycar_motor 으로 부터 받은 토픽을 해석하여 다시 토픽에 담아서 다음 토픽으로 전달한다. odem 은 position,orientation 및 선속도, 각속도 정보를 담고 있다. 코드를 통해 해당 메세지들이 어떻게 동작하는지 알아보자.

💻1-2.코드 구성

🚀rviz_odom.launch

🐍odom_8_drive.py

import rospy
import time
from xycar_motor.msg import xycar_motor

motor_control = xycar_motor()

rospy.init_node('driver')
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size=1)

def motor_pub(angle, speed): 
    global pub
    global motor_control

    motor_control.angle = angle
    motor_control.speed = speed

    pub.publish(motor_control)

speed = 3

while not rospy.is_shutdown():
    angle = -50
    for i in range(70): 
        motor_pub(angle, speed) 
        time.sleep(0.1)

    angle = 0
    for i in range(20):
        motor_pub(angle, speed)
        time.sleep(0.1)

    angle = 50
    for i in range(70):
        motor_pub(angle, speed) 
        time.sleep(0.1)

    angle = 0
    for i in range(20):
        motor_pub(angle, speed) 
        time.sleep(0.1)

8자 주행을 할 코드를 작성한다. angle , speed 값을 xycar_motor 토픽으로 전달한다. 🐍convertor.py

import rospy, rospkg
import math
from xycar_motor.msg import xycar_motor
from sensor_msgs.msg import JointState
from std_msgs.msg import Header

global pub
global msg_joint_states
global l_wheel, r_wheel

def callback(data):
    global msg_joint_states,l_wheel, r_wheel, pub
    Angle = data.angle
    msg_joint_states.header.stamp = rospy.Time.now()

    steering = math.radians(Angle * -0.4)

    if l_wheel > 3.14:
        l_wheel = -3.14
        r_wheel = -3.14
    else:
        l_wheel += 0.01
        r_wheel += 0.01

    msg_joint_states.position = [steering, steering, r_wheel, l_wheel, 
                                    r_wheel, l_wheel]    
    pub.publish(msg_joint_states)

def start():
    global msg_joint_states,l_wheel, r_wheel, pub    
    rospy.init_node('converter')
    pub = rospy.Publisher('joint_states', JointState, queue_size=10)

    msg_joint_states = JointState()
    msg_joint_states.header = Header()
    msg_joint_states.name = ['front_right_hinge_joint',
                'front_left_hinge_joint',                       
                'front_right_wheel_joint', 
                'front_left_wheel_joint',                               
                'rear_right_wheel_joint',
                'rear_left_wheel_joint']
    msg_joint_states.velocity = []
    msg_joint_states.effort = []
    l_wheel = -3.14
    r_wheel = -3.14
    rospy.Subscriber("xycar_motor", xycar_motor, callback)
    rospy.spin()

if __name__ == '__main__':
    start()

xycar_motor의 토픽을 받아서 joint_states 토픽 형태로 가공하여 토픽을 보낸다.

🐍rviz_odom.py

import math
from math import sin, cos, pi
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from sensor_msgs.msg import JointState

global Angle

def callback(msg):  
       global Angle 
       Angle = msg.position[msg.name.index("front_left_hinge_joint")]

rospy.Subscriber('joint_states', JointState, callback)

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)

odom_broadcaster = tf.TransformBroadcaster()

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(30.0)

current_speed = 0.4
wheel_base = 0.2
x_ = 0
y_ = 0
yaw_ = 0
Angle = 0

while not rospy.is_shutdown():
    current_time = rospy.Time.now()

    # compute odometry in a typical way given the velocities of the robot
    dt = (current_time - last_time).to_sec()

    current_steering_angle = Angle
    current_angular_velocity = current_speed * math.tan(current_steering_angle) / wheel_base

    x_dot = current_speed * cos(yaw_)
    y_dot = current_speed * sin(yaw_)   
    x_ += x_dot * dt;
    y_ += y_dot * dt;
    yaw_ += current_angular_velocity * dt 

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, yaw_)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x_, y_, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    #odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, yaw_))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()

joint_states 토픽에서 받은 데이터를 가공하여 odom 토픽을 오도메트리 프로그램으로 전달한다. 메세지에는 속도를 기반으로 계산한 x,y 좌표 및 오일러값을 쿼터니언 값으로 변환한 yaw_ 을 담고 있고, Odometry의 헤더를 만든다.

🐜2.실행화면

위 사진과 같이 8자 모양을 그리면서 운행되는 모습을 확인 할 수 있다. 만약 주행을 변경하고 싶으면 odom_8_drive.py에서 값을 일부 수정하면 된다. rqt_graph 로 노드간 통신을 확인.

🚖2-1. (번외) Rviz 궤적 추가하기

RViz를 실행하면 위 화면과 같이 시뮬레이션 차가 이동해도 궤적이 남지 않는다. 궤적을 확인하기 위해서는 RViz의 Odmoetry 설정을 추가해야한다.

RViz의 화면에서 순서대로 버튼을 눌러 Odometry를 추가해준다.

붉은 화살표가 추가된 모습. 왼쪽의 설정을 통해 궤적의 옵션을 조절 할 수 있다. 이중 Shaft Length항목을 조절하여 화살표의 크기를 작게 만들어 준다. 적절한 크기로 조절된 모습 . 입맛에 맞춰 조절하면 된다.