Rviz에서 Localization 시각화하기

Rviz Display

• map display
• laserscan display
• PoseArray display

실습

Husky 다운로드 & 설치

~/src 에 다운받은 husky 파일을 넣고

아래 명령어로 의존성 설치를 해준다.
ros와 ubuntu 버전에 맞게 설치해주면 된다.

rosdep install -i --from-path src/husky --rosdistro melodic -y --os=ubuntu:bionic

sudo apt install ros-melodic-ur-description
sudo apt install ros-melodic-lms1xx
catkin_make

husky launch 해보기

roslaunch husky_navigation_launch main.launch
roslaunch husky_navigation_launch keyboard_teleop.launch

• gazebo상에서 실외 네비게이션도 시뮬레이션 가능함 (가짜 topic - 경도 위도 값으로 가능)
  오호 다음에 한번 해보고 싶당😲

localization 해보기

roslaunch husky_navigation_launch main.launch
roslaunch husky_navigation amcl_demo.launch
rviz

rviz > add > Map > topic > /map
rviz > add > Laserscan
rviz > add > RobotModel
rviz > add > PoseArray

roslaunch husky_navigation_launch keyboard_teleop.launch

로봇을 점점 움직일수록 화살표들 (불확실성) 이 로봇 쪽으로 모이는 것을 확인할 수 있다.

.
.

Localization 다루기

Monte Carlo Localization (MCL)

로봇은 다음에 어디로 이동할지 무작위로 추정 = particle
Lidar와 같은 sensor 값을 바탕으로 가능성이 낮은 영역을 배제하면서 particle은 가장 로봇이 있을만한 위치로 수렴하게 된다.

로봇이 움직일수록 화살표가 로봇쪽으로 모였던 이유는
움직이면서 sensor를 통해 더 많은 값을 얻을 수 있으므로 위치파악이 더 정확해진 것이다.

AMCL (Adaptive Monte Carlo Localization) package

1. Launch 파일로 amcl 노드 켜주기

roslaunch husky_navigation amcl_demo.launch

2. 2D pose estimate를 통해 초기 로봇 위치 설정

3. 로봇을 움직이기 및 시각화

• Subscribing Topic: /scan, /map, /tf
• Publishing Topic: /amcl_pose, /particlecloud

mkdir ~/sim_ws/src/husky/husky_launch/scripts 
code ~/sim_ws/src/husky/husky_launch/scripts/get_pose_service.py

<get_pose_service.py>

#! /usr/bin/env python3

import rospy
from std_srvs.srv import Empty, EmptyResponse # Import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose

robot_pose = Pose()

def service_callback(request):
    rospy.loginfo("Robot Pose: " + str(robot_pose))

    return EmptyResponse() # the service Response class, in this case EmptyResponse


def sub_callback(msg):
    global robot_pose
    robot_pose = msg.pose.pose

rospy.init_node('get_pose_service')
my_service = rospy.Service('get_pose_service', Empty, service_callback) # create the Service called get_pose_service with the defined callback
sub_pose = rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, sub_callback)

rospy.spin() # mantain the service open.

rosrun husky_launch get_pose_service.py
rqt or rosservice call /get_pose_service "{}"

rqt > plugins > service > service caller > /get_pose_service 선택 후 call

rqt 상에서도 가능하고 terminal에서 명령어를 쳐도 가능하다.

map상에서 로봇의 좌표가 terminal 상에서 출력되는 것을 이야기한다.

Transforms

Localization을 위한 하드웨어 요구사항

• Good Laser data (/scan)
• Good odometry data (/odom/filtered)
• Good Map data (/map)

Localization을 위해 꼭 필요한 변환관계

1. base_laser => odom
   • 이 변환관계를 AMCL이 계산, 이를 통과하는 경로가 있어야 함
2. base_laser => base_link
   • LaserScan frame에 대한 로봇 base의 고정된 TF 관계가 필요

.
.

오픈소스 Localization 패키지

• 더 자세한 정보는 링크 참고 : amcl ros

amcl 노드의 일반 parameter

• odom_model_type (default: "diff")
  - 사용할 주행거리 측정 모델 설정
  • "diff", "omni", "diff-corrected", "omni-corrected"
• odom_frame_id (default: "odom")
  - odometry 시스템에 연결된 프레임의 이름
• base_frame_id (default: "base_link")
  - 로봇의 base와 연결된 프레임의 이름
• global_frame_id (default: "map")
  - localization 시스템에 의해 publising되는 기준좌표 프레임
• use_map_topic (default: false)
  - 노드가 topic에서 지도 데이터를 가져올지 아니면 서비스 호출에서 가져올지를 결정

.
.

amcl 노드의 filter 관련 파라미터

• min_particles (default: 100)
  - 필터에 허용되는 최소 입자수 | CPU 사용량에 영향을 미침
• max_particles (default: 5000)
  - 필터에 허용되는 최대 파티클 수 | CPU 사용량에 영향을 미침
• kid_err (default: 0.01)
  - 실제 분포와 추정 분포 사이에 허용되는 최대오차
• update_min_d (default: 0.2)
• update_min_a (default: pi/6.0)
• resample_interval (default: 2)

실습

mission

1. min_particle 및 max_particle를 각각 1과 5로 설정하고 결과 확인
2. laser_max_range를 1로 변경하고 결과 확인
3. 모든 파라미터 변경은 yaml 파일에서 하도록 함 (amcl_params.yaml)
   (launch 파일도 새로 만들기 - my_amcl.launch)

code /home/yujin/sim_ws/src/husky/husky_launch/my_amcl.launch

<my_amcl.launch>

<?xml version="1.0"?>
<launch>
    <arg name="scan_topic" default="scan" />
    <arg name="map_file" default="$(find husky_navigation)/maps/my_map.yaml" />

    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

    <!-- AMCL Node -->
    <node pkg="amcl" type="amcl" name="amcl">
        <rosparam file="$(find husky_navigation)/config/amcl_params.yaml" command="load" />
        <remap from="scan" to="$(arg scan_topic)" />
    </node>

    <!-- Visulization -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find husky_navigation)/rviz/amcl.rviz"/>
</launch>

code ~/sim_ws/src/husky/husky/husky_navigation/config/amcl_params.yaml

<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

roslaunch husky_navigation_launch main.launch
roslaunch husky_launch my_amcl.launch

나는 이렇게 해도 particle이 보이지 않았다... 아직 해결하지 못함

amcl 노드 관련 service

• /global_localization (std_srvs/Empty)
  - 모든 파티클이 map 의 빈 공간에 무작위로 분산되는 global localization을 시작
• /static_map (nav_msgs/GetMap)
  - Laser 기반 위치 추정에 사용되는 map을 가져옴

로봇의 위치를 알려줄 수 없을 때 (2D pose estimate로 지정 불가)
global_localization을 통해서 할 수 있다.

실습

• 목표 : /global_localization 서비슬르 호출하는 노드 만들기

Mission

1. AMCL의 파티클 초기 위치를 랜덤하게 설정하기 위해 /global_localization 서비스를 사용
2. 파일 안에 서비스 클라이언트를 위한 코드 작성
3. 이 클라이언트를 통해 파티클을 분산

code ~/sim_ws/src/husky/husky_launch/scripts/init_particles_caller.py

<init_particles_caller.py>

#! /usr/bin/env python3

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)

rosrun husky_launch init_particles_caller.py

흩어진 불확실성 particle들이 로봇을 움직여주면 robot의 위치로 모두 모이는 것을 확인할 수 있다.

map과 scan 데이터가 어느정도 matching되면 로봇의 위치를 잘 추정했다고 생각하는 방식이다.
문제는 만약 환경이 우리가 사용하는 환경처럼 직사각형 모양이라면 위아래가 뒤집혀도
map과 scan 데이터가 matching 되므로 로봇의 위치를 잘 추정했다고 생각할 수 있다는 것이다.

따라서 환경의 특징이 매우 뚜렷하다면 global localization을 사용하기 좋지만
그렇지 않다면 위치 추정에 실패할수도 있음을 기억해두어야 한다.

.
.

과제 - Localization

목표

• 로봇이 모르는, 알 수 없는 환경에서 위치를 추정한다고 가정
• 위치에 대한 공분상(불확실성, covariance) 값이 일정 수준으로 떨어지도록 로봇을 정사각형 형태로 이동시키기

Mission

1. Square_move.py 파일 생성
2. Particle을 맵 전체에 분산시키는 서비스 호출
3. 로봇은 정사각형 형태로 이동을 수행
4. 주행 중 공분산(covariance)이 0.65 미만이면 로봇이 올바르게 위치를 파악했다는 뜻이므로 프로그램 종료
5. 공분산이 0.65보다 클 경우, 파티클 분산, 이동수행, 공분산 확인 과정을 계속 반복
6. 프로그램 종료 전에, 최종 파티클 필터의 공분산을 터미널에 출력

hint

• amcl과 관련된 서비스를 호출하려면 amcl 노드가 실행중이어야 함
• 파티클의 공분산은 /amcl_pose 토픽을 통해 확인할 수 있음
• 파티클 공분산은 행렬형태로 publish
  (첫번째 값은 x의 공분산, 8번째 값은 y의 공분산, 마지막 값은 z의 공분산