패스트캠퍼스 환급 챌린지 26일차 : 로봇 초기 위치 추정 전략

[ 본 포스팅은 패스트캠퍼스 환급 챌린지 참여를 위해 작성하였습니다. ]

https://fastcampus.info/4oKQD6b

올해 마지막 보너스 혜택! 1+1+1 쿠폰 이벤트 (~12/09 23:59) | 패스트캠퍼스

 

 

https://fastcampus.co.kr/data_online_selfdriving

자율주행 로봇을 위한 ROS 2 & SLAM & Nav2 한번에 끝내기 | 패스트캠퍼스

본 글은 위 강의를 참고하여 작성되었습니다

 

 

[강의시작 | 강의종료]

 

 

[완강 인증]

 

[학습 내용]

 

-Publish Point-

동영상 서비스가 종료되어 해당 콘텐츠를 재생할 수 없습니다.

 

 

-init_robot-

동영상 서비스가 종료되어 해당 콘텐츠를 재생할 수 없습니다.

 

[강의 후기]

AMCL기반의 로봇 초기 위치 추정 전략에 대해 실습했습니다. 

 

이전 강의에서 Localization을 수행할 때 초기 위치 선정이 중요하다는 것을 학습했습니다. Rviz2를 사용해 초기 위치를 확인할 수 있습니다.

 

Rviz2에서 publish point를 사용해 위치 좌표를 확인할 수 있습니다.

 

동영상 서비스가 종료되어 해당 콘텐츠를 재생할 수 없습니다.

 

좌표 확인 후 직접 초기 위치를 변경해줘야 합니다.

ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 5.0, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}}"

 

 

매번 로봇의 위치를 직접 확인하고 변경하는 것이 아닌 자동적으로 변경될 수 있게 속성을 추가합니다.

udo apt install ros-humble-tf-transformations

 

받은 속성을 사용해 서버 패키지를 추가합니다.

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PointStamped
from tf_transformations import quaternion_from_euler

# init_pose = x, y, theta
init_pose = [3.0, 4.0, 3.141592]

class InitRobot(Node):

    def __init__(self):
        super().__init__('initial_pose_pub_node')
        self.init_pose_pub_ = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1)
        self.clicked_point_sub_ = self.create_subscription(PointStamped, '/clicked_point', self.point_callback, 1)

        # Set initial pose after a short delay
        timer_period = 0.5  # seconds
        self.trial_count = 5
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        self.trial_count -= 1
        self.init_pose(init_pose[0], init_pose[1], init_pose[2])

        if self.trial_count == 0:
            self.timer.cancel()  # Cancel timer after firing once

    def point_callback(self, msg):
        self.get_logger().info('Recieved Data:\n X : %f \n Y : %f \n Z : %f' % (msg.point.x, msg.point.y, msg.point.z))
        self.init_pose(msg.point.x, msg.point.y)

    def init_pose(self, x, y, theta=0.0):
        # radian to quaternion
        quat = quaternion_from_euler(0.0, 0.0, theta)

        # Publish initial pose
        msg = PoseWithCovarianceStamped()
        msg.header.frame_id = '/map'
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        msg.pose.pose.position.z = 0.0
        msg.pose.pose.orientation.x = quat[0]
        msg.pose.pose.orientation.y = quat[1]
        msg.pose.pose.orientation.z = quat[2]
        msg.pose.pose.orientation.w = quat[3]

        self.get_logger().info('Publishing  Initial Position\n X= %f \n Y= %f '% (msg.pose.pose.position.x, msg.pose.pose.position.y))
        self.init_pose_pub_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    init_robot = InitRobot()

    rclpy.spin(init_robot)
    init_robot.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

 

빌드된 서버를 실행 시킵니다.

ros2 run localizer_server init_pose

 

이후 Rviz2에서 publish point를 실행시키면 로봇의 위치가 이동되는 것을 확인할 수 있습니다.

 

동영상 서비스가 종료되어 해당 콘텐츠를 재생할 수 없습니다.

 

 

- 생각해 볼 점-

이번 강의에서는 AMCL 기반 로봇의 초기 위치를 추정하고 변경하는 실습을 진행했습니다. 이전 강의에서 초기 위치 선정이 중요한 이유에 대해 학습했습니다. Localization 알고리즘은 반복 실행을 통해 확률적으로 위치를 추정하므로 초기 위치 선정이 잘못 되면 오류가 생길 수 있다는 내용이 있었습니다. 초기 위치는 Localization의 출발점으로, 오차 누적을 최소화하고 빠르고 안정적으로 위치를 추정하기 위해 필요하기 때문에 직접 지정해주는것이 안정적입니다. 일일이 cmd 창에서 코드를 입력하고 실습 환경을 껐다켰다하는 번거로운 작업을 해야 하므로 자동으로 로봇의 초기 위치를 변경하는 실습을 진행했습니다. 맵이 생성되고 로봇이 초기 위치에 소환 될 때 init에 들어있는 내용을 읽으며 소환되기 때문에 server 패키지를 만들고 속에 init_robot 노드를 만들어 생성되는 위치를 변경할 수 있도록 해줬습니다. 실습 영상에서 확인 할 수 있듯 시스템 내부에서 로봇의 x,y,z값을 상시 받고있고, 이를 이용해 로봇이 소환되는 x,y,z 좌표를 publish point에서 흭득한 좌표로 변경되었습니다. 이는 생성한 server 패키지 속의 init_robot 노드의 역할입니다. 이를 실제 프로젝트에 적용해 볼 수 있습니다. 저장된 지도를 로봇에 학습 시키고 이를 통해 Localization을 진행할 때 현재 로봇의 위치와 Localization의 초기 생성 위치가 잘못 되어있을 수 있습니다. 이때, Rviz2와 같은 디버깅 툴을 사용해 위치를 확인하고 init 속에 로봇 생성 데이터를 담고있는 파일을 확인합니다. 이후 init 속 로봇 생성 데이터를 원하는 좌표의 위치로 변경할 수 있도록 서버 패키지를 만들고 노드를 실행시켜준다면 이번 실습과 마찬가지의 결과를 얻을 수 있을것입니다. 내용 자체는 간단하지만 실제로 사용하면 어려울 것입니다. 스스로 활용할 수 있도록 반복 복습 하겠습니다.