01화이노15

IMU로 Encoder를 보정할 때 칼만필터를 사용해야하는데, 이때 각속도가 필요하다고 한다!

그래서 IMU에서 받아온 yaw 값으로 각속도를 계산했다.

중심 식은 

                                         change_yaw=float((now_yaw-pre_yaw)/(now_sec-pre_sec))

간단하게 "yaw값의 변화량/시간의 변화량" 이다ㅋㅋ

time 라이브러리를 사용해서 현재 시간을 받아왔다. 

time 라이브러리는 epoch time(Unix time)을 다룰 때 사용한다고 한다. epoch time은 UTC 기준으로 1970년 1월 1일 0시 0분 0초 부터의 경과시간을 나타낸다고 한다! (신기행)

datetime 라이브러리를 사용해서 정규화된 시간을 사용할까도 했지만, 그냥 간단하게 변화량, 즉 빼기만 하면 되기 때문에 그냥 time 라이브러리를 사용했다ㅋㅋ

처음 코드가 실행될 때는 pre_sec, pre_yaw 즉 이전의 시간과 yaw 값이 없기 때문에 if 문으로 i=0일 때를 따로 나눠줬다.

간단한 코드지만 은근히 기분이 좋다ㅎㅎ

이제 대회가 얼마 안남았기 때문에 경기대에서 테스트 주행을 해봐야 한다!! 테스트 주행을 하기위해서는 경기대 waypoint 뿐만 아니라  시뮬레이터에서 전해주는 센서값이 아닌 실제 플랫폼의 센서값을 erp42_planner.py로 보내줘야 한다.

이 코드를 짜기 전까지는 "시뮬레이터에서 오는 값은 뭐고, 플랫폼에서 오는 값은 뭐지!??" , "파일 구조들이 대체 어떻게 되먹은거지!!"

싶었는데 선배랑 같이 그림을 그린 후 체계가 잘 잡혔다ㅎㅅㅎ

초기 계획도는 저게 아니었는데, 코딩을 하다보니 파일 구조가 많이 개편됐다.

원래는 serial_node.py에 gps, encoder, imu 정보를 한번에 publish하려고 했는데, 하나의 쓰레드 안에서 시리얼 통신을 하고 Publish 하는것은 불가능했다..

센서마다 데이터를 전하는 주기가 다른데, 세개를 하나의 쓰레드 안에서 Publish 하면 가장 느린 센서를 기준으로 Publish 하게돼서 세 파일로 나누기로 결정했다.

세개를 각각 Publish 해서 바로 erp42_planner.py에 전해주면 되지 왜 또 serial_node.py가 있나 싶을거다.

erp42_planner.py에서는 /Ego_topic 을 EgoVehicleStatus 자료형으로 받고 statusCB함수로 넘겨준다.

이 /Ego_topic에 gps, encoder, imu값이 들어가야 한다.

statusCB에 들어가보면 status_msg를 EgoVehicleStatus 형으로 만들어준 후 바로 data를 넣어준다.

그러나 gps.py , imu.py에서 publish되는 토픽은 Float32MultiArray이고,

encoder.py에서 publish되는 토픽은 Int16()이기 때문에 세개의 토픽을 erp42_planner.py에서 바로 합쳐주는 것이 많이 번거로웠다.

(선배가 뼈대가 되는 코드는 많은 사람들이 여러곳에서 접근하고, 수정할 수도 있기 때문에 왠만하면 바꾸지 않는것이 좋다고 하셨다.)

(그림에 Int16인데 Int36으로 적었네ㅋㅋㅋ)

그래서 gps, encoder, imu 정보를 serial_node.py에서 받아서 EgoVehicleStatus 형태로 합쳐준 다음에 다시 Publish해서 erp42_planner.py에서 sub 하기로 했다.

코드 짜고, 형태를 수정하고 하면서 진짜 이부분은 완전히 잘 알게된것같다ㅏ....ㄷㄷ

erp42_GPS.py

다행히 gps 정보를 받아오는 코드는 K-city에 가서 gps 값을 얻어올때 사용한 코드가 있었다. 이 코드를 가져와서 USB시리얼포트를 열어서 정보를 읽고 보정해서 publish 하는 코드를 만들었다. 

이 코드를 짜면서 처음으로 Float32MultiArray형의 토픽을 만들어봤다. GPS 값이 x, y가 나오는데 두개나 publish 하기 귀찮아서 배열에 담아서 publish 했다.

차선인식, 신호판별 등 자율주행에 쓰이는 카메라는 Logitech 제품으로 윈도우에서 Logitech Capture 프로그램으로 녹화를 하고있었다.

근데 팀장 선배가 혹시 Logitech Capture 프로그램에서 자체적으로 보정을 하고 있지는 않을까?!라는 의문을 제기해서 Ubuntu에서

카메라에 찍히는 영상을 바로 저장하는 코드를 만들어봤다. (이러다가 Opencv 고수될듯..아직 멀었지만ㅋㅋㅋ)

찾아본 블로그들에서는 맨날 gray로 바꾸고 색 반전을 해서 그냥 비디오만 캡쳐하는 코드만 가져와서 돌려봤는데, 파일은 저장되지만 영상이 열리지 않았다ㅜㅅㅜ

VideoWriter() 함수의 파라미터를 좀 더 살펴볼걸 그랬다.

코드와 문제점을 Github Issue에 올렸더니, 선배가 조금 손봐주셨다ㅏ

바뀐점은 VideoWriter의 파라미터를 get함수를 사용해서 우리가 사용하는 캠에 맞는 값으로 변경 후 

if cv2.waitKey(1) == ord('q')로 바꿔줬다. 

&0xFF 부분에서 오류가 났다고 한다ㅜㅜ

#오류난 이유#

cv2.waitKey(1) 은 바로 전에 입력된 key를 반환한다. 입력된 key가 없으면 -1을 반환

&0xFF 는 키의 단일바이트 표시를 유지하기 위한 이진 AND 작업이라고 한다.

그니까 q키가 입력되기 전까지 계속 while문을 돌려야하는건데, 처음 짰던 코드에는 1이 아닌 0이 들어가있다...대체 왜 들어갔지?ㅋㅋㅋㅋ

저 코드들은 여러 코드들 조합 보면서 끄적끄적 적었던거여서 출처가 정확히 기억이 안난다ㅜㅅㅜ

결국 저 코드로 USB CAM 영상을 촬영했는데 결과는!!(뚜둥!!)

선배가 카메라 영상을 RViz에 토픽 구독으로 띄우는걸 해보라고 하셔서 진행한 작업이다. 

이 블로그를 보고 작업했다.

http://chofukutomi.blogspot.com/2017/01/usb-camera-ros-kinetic-ubuntu-1604.html

그런데 선배가!!  "카메라에 직접 연결하는 형태 말고, image 전달하면 rviz에 띄워주는 함수로 구현되면 좋을 것 같습니다. 추가로, 카메라 여려개 사용할 예정인데, 여러개 띄우는것도 가능한지 궁금하네요" 라고 하셨다!! (뚜둥!)

이전까지는 ros의 pub, sub 개념을 직접 코드화 해보지 않아서 약간 멘붕이 왔었다.

다행히 선배가 이해하기 쉽게 잘 알려주셔서 ros의 빨리 개념이 잡히고 image를 publish 하는 함수를 짤 수 있었던 것 같다!

시뮬레이터의 플랫폼에 달려있는 카메라에서 송출되는 이미지가 RViz에 뜨는 것이 신기했다ㅋㅋㅋ

코드 출처!!

정말 여러 블로그를 보면서 시도해봤는데 이 코드가 젤 잘돌아간다ㅋㅋㅋ 

https://www.programmersought.com/article/61544639460/

자율주행은 정말 정말 많은 기술들이 합쳐져서 돌아간다...카메라 인식, 센서 인식(LiDAR, Encoder, GPS, IMU), 딥러닝 등ㅜㅅㅜ

그 모든 작업들에 참여하지는 못했지만 내가 참여한 작업들은 기록을 남겨놓으려고 한당

팀에 들어온 이후, 처음으로 이뤄낸 성과같다ㅋㅋ

처음에 카메라 캘리브레이션 미션을 맡았을 때는 "이게 뭐지ㅣ...이런것까지 해야하는건가!!" 싶었는데, 카메라 캘리브레이션에 대해 공부하면서 정확히 장애물을 인식하고, 차선을 인식하기 위해서는 꼭 필요한 기술이라는 것을 알았다.

캘리브레이션 이론은 증말루 복잡해서 내가 설명을 하지는 못할 것 같다.. 구글에 치면 많이 나온다!!

다행히 Python Opencv를 사용하면 코드 몇줄로도 카메라 캘리브레이션을 할 수 있다.

현대 과학, 컴퓨터 공학 너무 사랑해😍

wayp0int/workspace/src/wayp0int/scripts/lib/cam_util.py 에 추가한 코드의 완성본은 이렇다!

이제 어떻게 해서 이 코드가 된건지 같이 알아보자.

코드는 블로그에서 참고했다! http://www.gisdeveloper.co.kr/?p=6868 

증말루 증말루 감사드린다ㅜㅅㅜ

이론을 설명하자면 정말 길고 이미 많은 전문 블로그에 잘 설명이 되어있기 때문에 나는 야매로 빨리 설명해보겠다!!

코드를 두 부분으로 나눠서 설명하겠다!

5~37번째 줄은 ./data/*안의 모든 사진들을 for문으로 분석하면서 카메라 메트릭스를 알아낸다. 

카메라 메트릭스 계수는 현재 내가 사용하는 카메라의 휘어짐(?쉽게말해서) 특성이다. 그래서 카메라 메트릭스(=카메라 내부 파라미터)만 알면 나중에 어떤 이미지가 들어와도 이전에 알아낸 카메라 메트릭스 계수만 대입해서 왜곡을 보정하는 과정만 거치면 된다.

(너무 야매여서 이론적으로 틀릴수도 있습니다ㅜㅅㅜ일단 처음하는 사람들은 대충 맥락만 이해하세요ㅎㅎ...)

37번째 줄을 보면 

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

라고 cv2.calibrateCamera() 함수가 ret, mtx, dist, rvecs, tvecs 를 반환하는 것을 볼 수 있다. 

cv2.calibrateCamera() 는 카메라 메트릭스, 왜곡계수, 회전/이동 벡터들을 반환한다. 나중에 이 값들에 어떤 수가 들어있는지 한번 살펴보자!!

40~58번째 줄은 내가 보정하고 싶은 사진을 불러와서 아까 찾았던 계수들로 보정을 해서 calibresult.png로 저장한다. 이미지를 저장하고 나서 calib.npz로도 보정 계수를 저장해준다.

그 아래 mean_error가 나오는 이유는 왜곡 제거 시 수행된 작업에 발생된 오차가 얼마인지 알기위한 기능이다. 결과값이 0에 가까울수록 정확하다.

위의 코드에서 ret, mtx, dist, Total error 값만 뽑아낸 것이다. 

처음엔 print하고 나서 이게 뭐지..? 하면서 보다가 두번째 mtx의 배열 모양이 

camera matrix의 배열 모양과 같은걸 발견했다. 

바로 이게 이거였구나!! 이해가 되고 찾아낸 저값을 코드에 박아줘서 계속 재활용할 수 있게 만들어줬다ㅋㅋㅋㅋ

다시 코드 완성본을 보자! (지금보니 ret 값은 필요 없는 것 같다..)

mtx, dist에 np.array를 사용해서 똑같은 배열을 만들어준 후 파라미터로 받은 image를 캘리브레이션 하는 코드다! 당연히 return 도 dst(=image)다.

최종적인 ret, mtx, dist 계수는 직접 저렇게 사진을 100장 이상 찍어서 한번에 돌려서 나온 숫자다ㅋㅋㅋ큐ㅠㅠㅠ

정말 노가다였지만 결과물을 보니 흡족하다. (사진을 보면 색깔 점이 찍힌 것을 볼 수 있다!)

오른쪽이 보정 전, 왼쪽이 보정 후

솔직히 그렇게 많은 차이가 있지는 않다...그래도 보정후 오른쪽 코너를 보면 조금 펴진걸 볼수는 있다ㅋㅋㅋ