[RIS] 1/10 (cv2를 통한 rviz map 생성)

 

- 평면도(Floor Plan)나 청사진(Blueprint)를 이용하여 rviz에서 활용할 수 있는 map을 생성하는 방법

• 이 과정은 robot이 자동적으로 navigate를 하도록 만들기 위해 중요한 과정
• 보통 Lidar를 통해 mapping을 수행하나, 미리 만들어놓은 평면도나 청사진이 있다면 더 정확한 map을 생성할 수 있음
• 이 과정을 수행하기 위해선 3.7 버전 이상의 Python이 필요함

 

• 우선 floor plan이나 blueprint가 .png format인지 확인한다

 

import cv2 # Import OpenCV
   
# read the image file
img = cv2.imread('test.png')
   
ret, bw_img = cv2.threshold(img, 220, 255, cv2.THRESH_BINARY)
   
# converting to its binary form
bw = cv2.threshold(img, 240, 255, cv2.THRESH_BINARY)
  
# Display and save image 
cv2.imshow("Binary", bw_img)
cv2.imwrite("test_binary.png", bw_img)
cv2.waitKey(0)
cv2.destroyAllWindows()

• Map image file을 OpenCV를 통해 binary format으로 변환해주는 convert_to_binary.py node
• Map image file인 'test.png'를 binary format file인 'test_binary.png'로 convert

 

• Rviz에 map을 display 하기 위해 필요한 .pgm와 .yaml map file 생성

$ gedit MakeROSMap.py

• .png file이 있는 directory에 MakeROSMap.py file을 생성 후 아래 코드 작성

import numpy as np
import cv2
import math
import os.path

# Start of map program
prompt = '> '

print("Name of floor plan you want to convert to ROS map : ")
file_name = input(prompt)
print("Need to choose the x coordinates horizontal with respect to each other")
print("Double Click the first x point to scale")

# Read in the image
image = cv2.imread(file_name)

# Some variables
ix, iy = -1, -1
x1 = [0, 0, 0, 0]
y1 = [0, 0, 0, 0]
font = cv2.FONT_HERSHEY_SIMPLEX

# mouse callback function 
# This allows me to point and it prompts me from the command line
def draw_point(event, x, y, flags, param) :
    global ix, iy, x1, y1n, sx, sy
    if event == cv2.EVENT_LBUTTONDBLCLK :
        ix, iy = x, y
        print(ix, iy)

# Draws the point with lines around it so you can see it
    image[iy, ix] = (0, 0, 255)
    cv2.line(image, (ix+2, iy), (ix+10, iy), 0, 0, 255)
    cv2.line(image, (ix-2, iy), (ix+10, iy), 0, 0, 255)
    cv2.line(image, (ix, iy+2), (ix+10, iy), 0, 0, 255)
    cv2.line(image, (ix, iy-2), (ix+10, iy), 0, 0, 255)

# This is for the 4 mouse clicks and the x and y lengths
    if x1[0] == 0 :
        x1[0] = ix
        y1[0] = iy
        print('Double click a second x point')
    elif (x1[0] != 0 and x1[1] == 0) :
        x1[1] = ix
        y1[1] = iy
        prompt = '> '
        print("What is the x distance in meters between the 2 points?")
        deltax = float(input(prompt))
        dx = math.sqrt((x1[1] - x1[0])**2 + (y1[1] - y1[0])**2)*.05
        sx - deltax / dx
        print("You will need to choose the y coordinates vertical with respect to each other")
        print("Double cilck a y point")
    elif (x1[1] != 0 and x1[2] == 0) :
        x1[2] = ix
        y1[2] = iy
        print("Double click a second y point")
    else :
        prompt = '> '
        print("What is the y distance in meters between the 2 points?")
        deltay = float(input(prompt))
        x1[3] = ix
        y1[3] = iy
        dy = math.sqrt((x1[1] - x1[0])**2 + (y1[1] - y1[0])**2)*.05
        sy = deltay / dy
        print (sx, sy)
        res = cv2.resize(image, None, fx = sx, fy = sy, interpolation = cv2.INTER_CUBIC)

# Convert to grey
        res = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
        cv2.imwirte("KEC_BuildingCorrected.pgm", res);
        cv2.imshow("Image2", res)
        
        # for i in range(0, res.shape[1], 20) :
        #     for j in range(0, res.shape[0], 20) : 
        #         res[j][i][0] = 0
        #         res[j][i][1] = 0
        #         res[j][i][2] = 0
        #     cv2.imwrite("KEC_BuildingCorrectedDots.pgm", res)

# Show the image in a new window
# Opon a file            
        prompt = '> '
        print("What is the name of the new map?")
        mapName = input(prompt)

        prompt = '> '
        print("Where is the desired location of the map and yaml file?")
        print("NOTE: if this program is not run on the TurtleBot, Please input the file location of where the map should be saved on TurtleBot. The file will be saved at that location on this computer. Please then tranfer the files to TurtleBot.")
        mapLocation = input(prompt)
        completeFileNameMap = os.path.join(mapLocation, mapName +".pgm")
        completeFileNameYaml = os.path.join(mapLocation, mapName +".yaml")
        yaml = open(completeFileNameYaml, "w")
        cv2.imwrite(completeFileNameYaml, res);

# Write some information into the file
        yaml.write("image : " + mapLocation + "/" + mapName + ".pgm\n")
        yaml.write("resolution : 0.050000\n")
        yaml.write("origin : [" + str(-1) + "," + str(-1) + ", 0.000000]\n")
        yaml.write("negate : 0\noccupied_thresh : 0.65\nfree_thresh : 0.196")
        yaml.close()
        exit()

cv2.nameWindow('image', cv2.WINDOW_NOLMAL)
cv2.setMouseCallback('image', draw_point)

# Waiting for a Esc hit to quit and close everything
while(1) : 
    cv2.imshow('image', image)
    k = cv2.waitKey(20) & 0xFF
    if k == 27 :
        break
    elif k == ord('a') : 
        print('Done')
cv2.destroyAllWindows()

 

$ cd ~/catkin_ws/maps
$ python MakeROSMap.py
$ python3 MakeROSMap.py

• 위 node를 실행시키기 위해선 위의 두 명령어 중 하나를 사용

 

$ pwd
$ gedit ris_map.yaml

• pwd 명령어를 통해 map directory의 pull path를 확인하고 map의 yaml file에 아래와 같이 해당 내용을 입력

image: /home/lim/catkin_ws/maps/ris_map.pgm
resolution: 0.050000
origin: [-51.224998, -51.224998, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

 

$ roscore

$ cd ~/catkin_ws/maps
$ rosrun map_server map_server ris_map.yaml

• ROS Master를 실행시키고 다른 terminal 창에서 map file을 실행

 

$ rviz

• 또 다른 terminal 창에서 rviz 실행 후 좌측 하단의 Add 버튼을 통해 Topic에 있는 /map을 추가

 

 

- Coordinate Frame과 Transform

• ROS에서의 mobile robot의 standard coordinate frame (x, y, z axes)에 관한 내용
• Two-wheeled differential drive mobile robot의 coordinate frame 설정법
• 아래 link는 Differential wheeled robot에 관한 설명

https://en.wikipedia.org/wiki/Differential_wheeled_robot

Differential wheeled robot - Wikipedia

 

• tf (transformation)이 중요한 이유에 대한 설명

https://youngseong.tistory.com/104

tf란?

 

• 정확한 autonomous navigatoin을 위해 각 coordinate frame의 data를 tranform 해야 함

 

• Basic two wheeled diffrenetial drive robot의 standard Coordinate Frame에 관한 설명

https://youngseong.tistory.com/111

[KUSMO] 11/23

• Coordinate Frames
• base_link : Robot의 base에 rigidly하게 붙는 coordinate frame으로, 임의의 위치나 방향으로 부착할 수 있으며, 모든 hardware platform에 대하여 명확한 기준점을 제공하는 다른 위치가 있다
• laser_link : Lidar의 중심에 origin을 가지는 frame. base_link에 상대적으로 static 하다
• base_stabilized frame과 비교하여  roll, pitch angle 정보를 제공한다. (roll/pitch motion을 나타내지 않는 platform에서는 base_link와 base_stabilized frame이 동일하다)
• odom : World-fixed frame으로, 이 frame 내에서 robot의 pose는 시간이 지남에 따라 drift되어 따라서 장기적으로 사용하기에는 어려움이 있으나 연속적인 frame이므로 frame 내의 robot의 pose가 discrete한 jump없이 부드럽게 회전한다.
• 보통 바퀴나 visual, 혹은 inertial 측정 unit으로 부터 받은 odometry source를 통해 odom frame이 계산된다
  • Odometry : 주행기록계라는 뜻으로 시간에 따른 위치 변화를 추정하는 개념으로, GPS와 같은 절대적 위치가 아닌 출발 지점으로부터의 상대적인 위치를 추정한다.
  • Motor의 Encoder의 회전수를 통해 거리를 측정하고, IMU센서를 통해 기울기를 측정하여 로봇의 위치 추정한다
• map : odom과 마찬가지로 world-fixed frame으로, Z축이 위쪽을 향한다. 이 frame에서의 robot의 pose는 시간이 지나도 drift되어서는 안된다. 불연속적인 frame이라 frame 내의 robot의 pose는 어느때나 discrete하게 jump될 수 있다.
• 보통 localization component가 계속적으로 sensor 측정 값을 토대로 robot의 pose를 계산하므로 drift를 없애나 새로운 sensor의 정보가 입력되면 discrete한 jump가 발생한다
• 장기적으로 사용하기 유용하나 position estimator에서 discrete한 jump로 인해 local sensing과 acting의 성능을 저하시킨다
• base_footprint : 높이 정보를 제외한 robot의 2D pose를 나타낸다 (위치, 방향)
• base_stabilized : map/odom layer와 관련된 robot의 높이 정보를 나타낸다

 

- tf 예시

• map coordinate frame에 x=3.7, y=1.23, z = 0.0 에 위치해있는 object가 있다고 가정
• 해당 object로 robot을 navigate 하고자 함 : base_link frame와 비교했을 때 obect의 position은 어떻게 되는가?
• tf package를 통해 각 coordinate를 조절할 수 있다

 

• 시간이 지나도 변하지 않는 coordinate frame (ex) Robot에 고정되어 있는 Lidar에 대한 laser_link는 base_link에 대해 static함)에 대해 Static Transform Publisher를 사용함
• 시간이 지남에 따라 변하는 coordinate frame (ex) base_link 에 대한 map)에 대해 tf broadcasters and listeners를 사용함
• 다수의 ROS package들은 이러한 움직이는 coordinate frame에 대한 transform을 제공하므로 일일히 작성할 필요가 없음

 

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms

• Static Transform Publisher에 대한 syntax
• 보통 package의 launch file에 포함되어 있음
• Meter 단위의 x/y/z offset과 radian 단위의 yaw/pitch/roll을 사용하여 static coordinate transform을 tf에 publish함
• yaw는 Z에 대한 회전, pitch는 Y에 대한 회전, roll은 X에 대한 회전
• 주기는 millisecond 단위로, transform을 보내는 빈도를 지정. 10hz정도가 적당한 값

 

<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 30" />

• 'map'과 'odom' frame을 동일하게 설정하는 launch file 내의 코드
• map -> odom transform은 map coodinate frame 내에서 로봇의 시작점에 대한 위치와 방향을 알려준다
  • map이 parent, odom이 child

 

 

 

 

 

 

---

참고 자료 :

https://automaticaddison.com/how-to-create-a-map-for-ros-from-a-floor-plan-or-blueprint/#google_vignette

How to Create a Map for ROS From a Floor Plan or Blueprint – Automatic Addison

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

https://automaticaddison.com/coordinate-frames-and-transforms-for-ros-based-mobile-robots/

Coordinate Frames and Transforms for ROS-based Mobile Robots – Automatic Addison

https://en.wikipedia.org/wiki/Differential_wheeled_robot

Differential wheeled robot - Wikipedia