AMCL 기반의 로봇 초기 위치 추정

Global Localization은 사람이나 로봇 모두 로봇이 지도에서 어디에 있는지 모를 때 사용됩니다. 그러면 로봇이 스스로 현지화를 시도해야 합니다.
Global Localization을 사용해 로봇이 지도에서 자신의 위치를 파악하려고 시도
AMCL에서는 /reinitialize_global_localization이라는 서비스를 제공하며 필터의 모든 입자를 지도 전체에 배포하고, 아래와 같은 경우에 사용하면 효과적임

Q1. 터미널에서 Global Localization 수행

ros2 service call /reinitialize_global_localization std_srvs/srv/Empty

/reinitialize_global_localization 서비스 수행 직후

충분히 로봇을 움직여 준 후

위 결과에서 확인할 수 있듯이 Global Localization 수행 후 초기 위치 추정에 실패한 것을 확인할 수 있음

Q2. 프로그램 상에서 Global Localization 수행

localizer_server 패키지의 localizer_server 디렉터리에 global_localization.py를 만들고 아래 코드 작성

import rclpy
from rclpy.node import Node
from std_srvs.srv import Empty
from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType
from rcl_interfaces.srv import SetParameters
from time import sleep

class GlobalLocalization(Node):
    def __init__(self):
        super().__init__('global_localization_node')
        # create client
        self.relocalization_client = self.create_client(Empty, 'reinitialize_global_localization')
        self.param_client = self.create_client(SetParameters, '/amcl/set_parameters')

        # wait for service
        while not self.relocalization_client.wait_for_service(timeout_sec=1.0) \\
                and not self.param_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')

    def set_amcl_params(self, max_particles, min_particles):
        params = [
            Parameter(name='max_particles', value=ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=max_particles)),
            Parameter(name='min_particles', value=ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=min_particles))
        ]
        req = SetParameters.Request()

        for param in params:
            req.parameters.append(param)

        future = self.param_client.call_async(req)
        future.add_done_callback(self.set_amcl_params_callback)

        # print current max_particles and min_particles
        self.get_logger().info('max_particles: %d' % max_particles)
        self.get_logger().info('min_particles: %d' % min_particles)
        return future.result()

    def call_reinitialize_global_localization(self):
        req = Empty.Request()
        return self.relocalization_client.call_async(req)

    def set_amcl_params_callback(self, future):
        try:
            response = future.result()
        except Exception as e:
            self.get_logger().error('Service call failed %r' % (e,))
        else:
            self.get_logger().info('Service call succeeded %r' % (response,))

def main(args=None):
    rclpy.init(args=args)
    global_localization = GlobalLocalization()

    # adjust parameters for global localization
    global_localization.set_amcl_params(20000, 1000)
    rclpy.spin_once(global_localization)

    # call service reinitialize_global_localization
    future = global_localization.call_reinitialize_global_localization()
    rclpy.spin_until_future_complete(global_localization, future)
    if future.result() is not None:
        global_localization.get_logger().info('Global localization reinitialized successfully.')
    else:
        global_localization.get_logger().error('Failed to call service reinitialize_global_localization')

    # restore default parameters
    sleep(7)
    global_localization.set_amcl_params(2000, 500)

    rclpy.spin(global_localization)
    global_localization.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Global Localization을 수행하기 위해서는 지도 상에 많은 파티클을 뿌려야 유리하기 때문에 동적으로 두 개의 파리미터 수정

setup.py에 global_localization.py 스크립트의 실행 파일에 entry_points를 추가

from setuptools import find_packages, setup

package_name = 'localizer_server'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='spoons',
    maintainer_email='[email protected]',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'init_pose = localizer_server.init_robot:main',
            **'global_localization = localizer_server.global_localization:main'**
        ],
    },
)

패키지 빌드

cd ~/nav2_ws
colcon build --symlink-install --packages-select localizer_server
source ~/nav2_ws/install/local_setup.bash

노드를 실행시켜 파티클을 지도 전역에 뿌리기

ros2 run localizer_server global_localization

Global Localization을 수행하기 전에 최대, 최소 파티클 개수를 확 늘리고 수행 완료 후 원상태로 되돌려줌

# adjust parameters for global localization
**global_localization.set_amcl_params(20000, 1000) # !!!**
rclpy.spin_once(global_localization)

# call service reinitialize_global_localization
future = global_localization.call_reinitialize_global_localization()
rclpy.spin_until_future_complete(global_localization, future)
if future.result() is not None:
    global_localization.get_logger().info('Global localization reinitialized successfully.')
else:
    global_localization.get_logger().error('Failed to call service reinitialize_global_localization')

# restore default parameters
sleep(7)
**global_localization.set_amcl_params(2000, 500) # !!!**

로봇을 회전시키면서 Global Localization이 수행되는지 확인
```
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```