Ubuntu 20,04 기반 ROS2 Foxy 로 TurtleBot3 Waffle Pi 구동 과정 정리
ROS2
ROS2는 Nav2에 사용되는 핵심 미들웨어임
Action Server
ROS서 Action 서버는 Navigation과 같이 장기간 실행되는 작업에서 제어하는 일반적인 방법
특정 목적까지 실행되는 장시간 실행 상황에서 Action 서버와 클라이언트는 다른 프로세스나 스레드에서 장기간 실행되는 테스크를 호출하고 실행이 완료되면 미래에 return 됨
작업이 완료될때까지 중간중간 확인하는 것이 안정적이여서 Action 서버는 클라이언트에게 feedback을 제공함
이 feedback은 ROS.action에 정의된 무엇이든 될 수 있음
불도저 로봇을 예로들면 Action 에 대한 요청은 각도이며, 피드백은 이동할 각도까지 남아있는 각도이며 결과는 최종 각도와 함께 성공 또는 실패의 boolean 값이 될 수 있음
네비게이션을 예로들면 요청은 최종 위치이고, 피드백은 네비게이션 시간과 목표까지 거리가 될 수 있으며 결과는 성공을 위한 boolean 값임
Action 클라이언트에 콜백을 등록하여 피드백과 결과를 얻을 수 있음
이런 구조에서 Action 서버는 상위레벌의 BT navigator와 NavigateToPose Actio 메시지를 주고받는데 사용됨
또한 BT Navigator가 차후 작은 action 서버들과 통신하여, plan을 계산하고, 동작제어 및 복구를 수행하는데도 사용됨
각 Action 서버는 각각의 nav2_msgs의 .action 타입을 가지고 상호작용함
Lifecycle Nodes and Bond
Lifecycle 노드는 ROS2에 만 있는 특별한 노드임
이 노드들은 ROS2 서버의 시작과 중지를 위한 state machine 전환이 포함된 노드임
노드가 시작하면 unconfigured 상태이며, ROS 네트워크 설정이나 parameter를 읽는 것을 포함하지 않는 단순 node 생성자만 처리함
launch 시스템에의해 노드들은 inactive 상태로 전환해야 함
이 후, activing 단계로 전환하면서 node가 activate 될 수 있음
이 상태에서 노드가 정보를 처리하고 전체 설정을 실행할 수 있음
configuration 단계에서 on_configure() 함수가 트리거 되며, 모든 parameter들을 설정함(ROS 네트워크 인터페이스, 동적메모리, 안전관련 시스템 등)
activation 단계에서 on_activate() 함수가 트리거 되며, ROS 네트워크 인터페이스가 활성화되고 프로세스 시작을 위한 정보들이 설정됨
Node 종료를 위해서는 deactivating, cleaning up, shutting down and end 로 각각 전환됨
네트워크 인터페이스가 deactivated 되고 프로세스 종료 및 메모리 해제, exit 등이 각각 실행됨
ROS 시스템의 모든 서버들이 이 Lifecycle 노드 프레임워크를 사용하는 것이 권장됨
Nav2에서 LifecycleNode들이 nav2_util LifecycleNode에 wrapper 되어 있음
이 wrapper는 LifecycleNode들의 일반적인 어플리케이션의 복잡성의 대부분을 wrap 하고 있음
또한 Lifecycle manager를 위한 server가 올라오는 것을 보장하기 위한 bond 연결도 포함하며 active 상태를 유지하게 됨
만일 서버가 crash 가 나면, lifecycle manager 에게 할리고 critical failure를 방지하기위해 서버를 내림
Nav2에는 map_server, Planner_server, controller_server가 있으며 이들 모드 lifecycle 관리가 가능함
Nav2의 lifecycle manager 가 위의 노드들의 상태를 변경가능하며 startup, shutdown, reset, pause, or resume 을 컨트롤 가능함
RVIZ 에서 lifecycle_manager/manage_nodes의 서비스를 사용하며 RVIZ panel 버튼에 따라 startup, reset, shutdown등의 상태를 변경함
자세한 사항은 아래 링크 참조
[nav2_lifecycle_manager]](https://github.com/ros-planning/navigation2/tree/main/nav2_lifecycle_manager)
Behavior Trees
Behavior trees 는 로봇의 복잡한 수행들에 점점 일반화가 되어가고 있음
이 트리구조는 많은 state의 어플리케이션에서 사람이 이해하기 쉽고 확장가능하게 만들 수 있으며, 수백 개의 transition을 가지는 Finite State Machine 과는 반대됨
축구하는 로봇을 예로 들면, 축구 경기의 logic을 FSM(Finite State Machine)에 임베딩하는 것은 많은 가능한 상태와 규칙으로 인해 오류가 발생하기 쉬우며 도전적임
또한 왼쪽, 오른쪽, 중앙에 목표를 향해 shoot을 하는 것과 같은 선택에 있어 특히 불분명함
BT를 사용하면 “kick”, “walk”, “go to ball” 과 같은 기본 primitive들을 만들어 많은 행동에서 재사용 가능함
FSM을 사용하면 규칙이 많아질수록 재사용이 어렵고 복잡해지는데 간단한 이유는 FSM은 goto문을 사용하여 프로그래밍하는 것이고 BT는 함수 형태의 call로 프로그래밍 한다고 비유할 수 있음
Nav2 에서는 BehaviorTree CPP V3 라이브러리를 사용하며 BT Navigator 안에 트리로 구성할 수 있는 노드 플러그인을 생성함
노드플러그인이 BT에 로드되고 트리의 XML 파일 형태의 파싱할 때, 등록된 이름이 연결됨
이 시점에 행동트리를 통해 navigate 할 수 있음
위의 CPP V3 라이브러리 사용되는 이유는 하위트리 로드 기능이 있음
즉, Nav2 동작 트리를 다른 상위 수준의 BT에 로드하여 node 플로그인으로서 이 프로젝트를 사용 가능함
일반적인 action 인터페이스를 통해 client가 call 할 수 있도록 BT용 NavToPoseAction 플러그인을 제공함
Navigation Servers
Planner와 Controller 서버는 navigation 업무에 핵심임
Recovery 서버는 로봇을 bad situation이나 다양한 형태의 문제를 해결하여 시스템을 결함에 강하도록 만드는데 사용됨
Planner, Controller, and Recovery Servers
Planner, Controller, Recovery 3가지 Action 서버들은 다양한 task를 수행하기 위한 맵 알고리즘 플러그의 host 로 사용되며, Output들의 계산을 위한 Environmental Representation의 호스트로도 사용됨
Planner와 Controller 서버들은 사용할 알고리즘의 유형에 따라 런타임에 구성됨
이러한 유형은 등록된 pluginlib 이름이고, 이 이름은 task의 별칭(aliases)임
예를들어 FollowPath라는 이름으로 사용되는 DWB 컨트롤러가 있으며, reference 된 경로에 따르게 됨
이 경우 DWB에 대한 모든 parameter가 해당 네임스페이스에 배치됨(ex, FollowPath.param)
이후 두 서버는 태스크에 해당하는 작업 인터페이스를 노출시킴
behavior tree가 일치하는 BT 노드를 체크하면 action 서버를 호출하여 태스크를 처리함
action 서버 콜백은 특정 알고리즘에 매핑되는 이름(ex, FollowPath)으로 선택한 알고리즘을 호출함
이를통해 behavior tree에 사용되는 알고리즘을 알고리즘 클래스로 추상화함
예를들어 경로를 주행하거나, 충전기와 도킹, 동적 장애물 피하기, tool들과 인터페이스 역할을 하는 N개의 플러그인 controller를 가질 수 있다고 해보자
이 모든 플러그인을 동일한 서버에 두는 것은 사용자가 하나의 Environmental Representation 객체를 사용하게 하며 이는 복제 비용이 많이듬
Recovery 서버는 각각 복구에 자체 이름들을 포함하지만, 각 플러그들은 자신의 특별한 action 서버로 노출시킬 수 있는데 이는 생성될 수 있는 recovery action들이 많기 때문에 공유할 단일 인터페이스를 가질 수 없기 때문임
또한 Recovery 서버는 로컬 costmap에 subscriber 에 대한 costmap을 포함하고 있으며, controller 서버로 부터 실시간으로 업데이트를 수신하여 작업을 계산함
복제하는데 계산적으로 많은 비용이 드는 로컬 costmap이 여러 instance를 가지는 것을 피하기 위해 위의 작업을 수행함
또는 BT 노드가 action이라 블리는 사소한 플러그인이므로 새로운 BT 노드를 작성하여 다른 작업 유형으로 다른 작업 서버를 호출할 수 있음
가능하다면 제공되는 서버들을 항상 사용하는 것이 좋으며 만약 플러그인 또는 action에 대한 인터페이스로 인해 새 서버가 필요한 경우 Nav2 프레임워크와 함께 유지할 수 있음
새로운 서버도 새로운 타입과 플로그인 인터페이스를 사용시 제공되는 서버와 비슷하게 사용해야함
새로운 BT 노드 플러그가 새로운 action 서버를 호출하도록 만들 필요가 있을 수 있음
그러나 nav2 자체의 fork나 수정이 필요하지는 않음
Planners
Planner 의 역할은 objective 함수를 완성하기 위해 경로를 계산하는 역할을 함
path는 선택된 알고리즘에 따라 하나의 노선으로 알수 있게 됨
두 가지 표준적인 예는 현재 position에서 goal까지의 계획을 계산하거나 모든 free space를 커버하는 계획을 수행함
Planner는 global environmental representation를 이용하며, 버퍼된 sensor data 에 이용함
- Compute shortest path
- Compute complete coverage path
- Compute paths along sparse or predefined routes
Planner를 위한 Nav2의 일반적인 역할은 current pose에서 goal pose 까지의 유효하고, 최선의 경로를 계산함
그러나 지원되는 많은 종류의 계획과 경로가 존재함
Controllers
Controller(ROS1에서 로컬 플래너로 알려진) 는 global하게 계산된 경로를 따르거나 로컬 작업을 완료하는 역할을 함
Controller는 로컬 environment representation에 접근하여 base가 따라야할 실행가능한 제어 활동 계산을 시도함
많은 controller 들이 robot을 공간에 투영하고 매번 update 시 마다 지역적으로 가능한 경로를 계산함
- Follow a path
- Dock with a charging station using detectors in the odometric frame
- Board an elevator
- interface with a tool
Nav2에서 Controller의 일반적인 역할은 global plan에 따르도록 유효한 Control 활동을 계산하는 것임
그러나 많은 종류의 controller들과 local planner 들이 존재함
따라서 nav2 프로젝트의 목표는 모든 controller 알고리즘이 공통 연구 및 산업 task들을 위해 이 서버에 플로그인이 될 수 있도록 하는 것임
Recoveries
Recovery 서버들은 내결함성 시스템의 주요 요소임
목표는 알려지지 않은 또는 고장 조건을 처리하고 자율적으로 에러들을 다루고자 함
예를 들어 로봇이 동적 장애물이나 제어 불량으로 stuck 상태에 빠지는 경우 백업을 하거나 제자리에서 회전하면서 로봇이 못가는 영역에서 벗어나 빈 공간으로 이동하여 경로를 성공적으로 탐색하고자 함
완전한 장애인 경우 recovery 에서 운영자에게 이메일, SMS 등으로 도움을 요청하도록 구현할 수 도 있음
Waypoint Following
Waypoint following 은 navigation 시스템의 기본특징이며, 많은 목적지에 어떻게 navigation을 사용해야할지 알려줌
nav2_waypoint_follower 에 specific task 실행을 위한 플러그 인터페이스로서 waypoint following 프로그램을 포함하고 있음
이는 지정된 위치로 이동하여 사진찍기, 상자들기, 사용자 입력 대기 등과 같은 특정 작업을 완료해야할 때 유용함
nav2에 샘플 데모들도 있지만 더 나아가 fleet manager 기능, dispatch 도 생각할 수 있음
우선 nav2_waypoint_follower 는 생선용 로봇 솔루션을 만들기에 충분함
외냐하면 autonomy 시스템에서 로봇의 자세, 배터리 수준, 현재 task 등을 고려하므로, 어플리케이션은 당변한 task 에 대해서만 신경 쓰고 시스템의 다른 복잡성은 작업이 필요 없음
이런 상황에서 waypointfollower 에 대한 요청을 1개의 작업단위(예로 들면 창고에서 1개 집기, 보안 패트롤 돌기, 통로 1개 등)로 생각하고 다음 작업을 위해 dispatcher 에게 return 시키거나 재충전을 요청해야함
학문적으로 응용프로그램을 따르는 waypoint 는 navigation 보다는 한 단계 위이고 시스템 자율 응용 프로그램 보다는 아래에 있음
두번째로 nav2_waypoint_follower 는 좋은 샘플 어플리케이션이지만 강력한 솔루션을 만들기 위해서는 waypoint following / autonomy system 이 필요함
이 경우 nav2_behavior_tree 패키지를 사용해 사용자 지정 응용프로그램 수준 behavior tree를 생성하여 navigation 작업에 활용할 수 있음
여기에는 도킹으로 돌아가거나 더 복잡한 작업에서 둘 이상의 작업 단위를 처리하기 위한 중간 작업 중 충전 상태 확인과 같은 하위 트리가 포함될 수 있음
곧 이 어플리케이션을 더 쉽게 만들 수 있는 nav2_bt_waypoint_follower 가 출시될 예정임
State Estimation
navigation 프로젝트에서 커뮤니티 standards 에 따라 2가지 transformation 이 필요함
map을 odom 으로 transform 하는 것은 positioning 시스템(SLAM)에서 제공되며 odometry 시스템에 의해 odom은 base_link로 변환됨
navigation system에서 LIDAR 사용을 요구하지는 않으며, vision이나 depth에 기반한 positioning 시스템이나 다른 센서를 충돌 회피에 사용 가능하지만 아래 표준을 따라 구현해야함
Standards & Global Positioning & Odometry
REP-105 에서 최소한으로 full map -> odom -> base_link -> [sensor frames] 를 포함하는 TF 트리를 작성해야함
TF2 는 time-variant transformation 라이브러리 이며 time sync된 transformation을 얻기위해 사용됨
최소한으로 map->odom 변환을 제공하는 것은 global positioning system(GPS, SLAM, Motion Capture) 의 작업임
그런다음 odometry 시스템은 odometry -> base_link 변환을 제공하는 것이 역할임 (odometry는 LIDAR, RADAR, wheel encoders, VIO, and IMU들을 통해 얻을 수 있음)
odometry는 smooth하고 continuous 하게 local frame을 제공하는 것을 목표로 함
모바일 로봇에서는 휠 인코더, IMU 및 vision으로 부터 odometry를 얻음
smmoth output은 정확한 모션을 위한 dead-reckoning 과 position 업데이트를 위해 사용하여 로봇의 위치를 정확히 업데이트할 수 있음
base_link와 연관된 나머지 변환은 static해야하며 URDF 에 정의되어 있어야함
Environmental Representation
Environmental Representation은 로봇이 환경을 인지하는 방식임
또한 다양한 알고리즘과 데이터 소스가 하나의 single space로 결합할수 있는 중앙 위치 지정 역할을 함
그런 다음 controller, planner, recovery 서버들이 이 space 를 사용하여 안전하고 효율적으로 서버들의 역할을 수행함
Costmaps and Layers
현재의 Environmental Representation 이 costmap임
costmap은 unknown, free, ocupied or inflated 비용을 포함하는 일반적인 2D 그리드 셀 임
이 costmap은 전역 plan을 계산하거나 local 제어 efforts를 계산하는데 샘플링됨
다양한 costmap 레이어는 costmap에 정보를 버퍼링하기위해 플러그인 형식으로 구현됨
이 정보에는 LIDAR, RADAR, 음파탐지, 깊이, 이미지 등의 정보가 포함됨
costmap 레이어에 입력하기 전에 센서데이터를 처리하는 것이 현명할 수 도 있지만 개발자에 달려있음
카메라 또는 깊이 센서를 사용하여 충돌 방지를 위해 장애물을 감지하고 추적하기 위한 costmap 레이어가 만들어 질 수 있음
또한 레이어는 기반이 되는 rule에 따라 costmap이 알고리즘적으로 바뀌도록 만들 수 있음
마지막으로 실시간 데이터를 2D 또는 3D 세계에서 이진 장애물 마킹(장애물 있는지 없는지 1,0)을 버퍼링 하는데 사용됨
Costmap Filters
annotated 된 map의 위치에 기반하여 특정 action이 수행되도록 지도 파일을 annotating 했다고 가정해보자
annotating의 예로 내부의 피해야할 구역을 비우거나, 특정영역에서 최대 속도를 지정할 수 있을 것임
위와 같이 표시한 map을 filter mask라 부름
이 filter mask의 주요 목표는 지도에 일부 추가 기능이나 행동 변화가 있는 영역을 표시하는 기능을 제공하는 것임
Costmap filter는 filter masks에 annotated 된 공간 의존적인 행동변화를 Nav2 스택에서 적용하는 costmap 계칭 기반한 방식임
Costmap filter들은 costmap 플로그인으로 구현됨
이러한 플로그인은 filter mask에 표시된 공간의 annotation으로 costmap을 필터링하기 때문에 filter라 불림
filter된 costmap을 만들고 해당 영역에서 로봇의 행동을 변화시키기 위해 filter 플러그인은 filter 마스크로 부터 가져온 데이터를 읽음
이 데이터는 filter 공간에서 feature map으로 linearly transform 됨
map/costmap과 함께 이 변형된 feature map을 사용하면, 센서 데이터와 현재 로봇 좌표 filter 가 costmap을 업데이트하고 위치에 따라 로봇의 동작을 변경할 수 있음
- Keep-out/safety zones where robots will never enter.
- Speed restriction areas. Maximum speed of robots going inside those areas will be limited.
- Preferred lanes for robots moving in industrial environments and warehouses.
Other Forms
Environmental Representation에 다른 다양한 형태들이 존재함
- gradient maps, which are similar to costmaps but represent surface gradients to check traversibility over
- 3D costmaps, which represent the space in 3D, but then also requires 3D planning and collision checking
- Mesh maps, which are similar to gradient maps but with surface meshes at many angles
- “Vector space”, taking in sensor information and using machine learning to detect individual items and locations to track rather than buffering discrete points.