1. Waypoint 보간
2. Waypoint 노드별 분리, 저장
w
x : 328540.00 y : 4161354.00
import math as m
float normalrad(float x):
if(x < - m.pi ) :
x = x + 2 * m.pi
elif x > m.pi:
x = x - 2*m.pi
return x
3. Dijkstra
코드
클래스
Dijkstra
다익스트라 알고리즘에 관련한 클래스
WaypointInfo
Waypoint 정보들에 관련한 리스트
waypoint txt파일들 읽어와서 노드들로 저장
std::vector<std::string> nodeList; // 각 노드의 알파벳 이름
std::vector<Pair> edgeList; // 어떤 노드들끼리 이어져있는지 (1,4)
std::vector<std::vector<geometry_msgs::PoseStamped>> edgeWayList; // edge의 waypoint
std::vector<Pairf> costList; // edge의 비용
C++
복사
노드, egde들이 저장된 “순서”가 index가 됨
두 클래스에서 노드들이 리스트로 저장되어 관리됨. 각 리스트에서 인덱스가 같은 것끼리 동일한 노드를 의미함
4. Waypoint 원점 수정 제안
•
원점 이동 전: rviz에서 경로가 이상하게 보임
•
원점 이동 후: 부드러움 
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