localize 문제

[yeodonghyeon1]

문제점

1. localize 0,0 에 가깝게 설정되는 문제 -> localize가 똑바로 작동안하는 것 같다
2. pepper navigationTo 할 때마다 방향이 달라짐 -> localize 문제일 수도 있고 매번 킬 때마다 xy가 달라졌을 수 있다.

진행 상황

1. pepper가 움직일 때마다 getMetricMap()을 매번 새롭게 뽑았는데 그때마다 맵은 그대로다. getMeticMap()으로 반환 받는 loadmap 했을 때 맵으로 고정인 것 같다.
2. pepper가 현재 읽고 있는 맵 상황을 출력하는 거 찾는 중이다. -> 아직 못찾음.

[yeodonghyeon1]

localization을 꽤 오래 했는데도 위치를 못잡는 거 보면 실제 지형과 맵이 좀 다른 문제, 만들어진 맵이 분명하지 않은 문제가 영향이 있는 것 같다.

좌표계가 매번 킬 때마다 달라지는 점이 localize가 잘 안 되서인지 애초에 설계가 그렇게된 건지 모르겠다.

[yeodonghyeon1]

한 바퀴 돌리고 시작해도 정확하게 로컬라이즈 되지 않음
self.motion_service.move(0,0,1)

[yeodonghyeon1]

지도가 계속 바뀌는 것 같아서 theta 값 맞춰 줌 (theta값과 회전 중심 x,y 값은 최초 localize에 맞춤. 초기 localize에 많이 의존하는 것 같아서)

        center_x = (self.localization_first[0] - offset_x) / resolution
        center_y = (self.localization_first[1] - offset_x) / resolution
        import math
        angle = self.localization_first[2]
        angle = math.degrees(angle)
        center = (center_x , center_y)
        M = cv2.getRotationMatrix2D(center, angle, 1)
        rotated = cv2.warpAffine(img, M, (map_width, map_height))
        img = rotated

frist localization은 맨 처음에 relocalizeinMap을 [0., 0.]으로 맞춰주고 시작 일단 시작하는 건 웬만하면 0,0 근처에서 시작해야
얼추 비슷하게 되는듯

self.navigation_service.navigateToInMap([2., 0., 0.])는 2일 때 제일 활발하게 움직여서 2로 해놨음

    def first_localization(self):
        try:
            self.navigation_service.relocalizeInMap([0., 0.])    
            self.navigation_service.startLocalization()
            a =time.sleep(3)
            print(a)

            self.navigation_service.navigateToInMap([2., 0., 0.])
            # self.motion_service.move(0,0,1)

            localization = self.navigation_service.getRobotPositionInMap()
            # exploration_path = self.navigation_service.getExplorationPath()
            self.localization_first = localization[0]
            print("localization", self.localization_first)

            print("[INFO]: Localization complete")

        except Exception as error:
            print(error)
            print("[ERROR]: Localization failed")

이렇게 했을 시 어느 정도 비슷하게 움직임. 그래도 초기 localize가 똑바로 되야 어느 정도 비슷하게 움직임.

잘 되는 경우가 매우 적다.

localization이 제대로 작동하는 경우가 드물고 slam 맵도 깔끔하게 나오는 편이 아니라 생기는 문제 같다.

일단 맵 이미지를 클릭으로 동작하는 방식이 아예 잘못 작동하고 있었으나 그 부분은 어느 정도 나아짐