/
/
/
1#include "fast_slam/PathList.hpp"
2
3namespace fastslam{
4
5Path::Path(StateVector new_pose, uint32_t k){
6 std::shared_ptr<NodePath> first_path_node = std::make_shared<NodePath>();
7
8 first_path_node->pose = new_pose;
9 first_path_node->k = k;
10 first_path_node->delta_time = std::chrono::nanoseconds(0);
11 first_path_node->next_node = nullptr;
12
13 path_root_ = std::make_shared<NodePath>();
14
15 first_path_node->referenced = 1;
16 path_root_->next_node = std::move(first_path_node);
17
18
19 path_length_ = 1;
20}
21
22Path::Path(const Path &path_to_copy){
23 // create shared past from copy
24 path_root_ = std::make_shared<NodePath>();
25 path_root_->next_node = path_to_copy.path_root_->next_node;
26 path_to_copy.path_root_->next_node->referenced++;
27
28 path_length_ = path_to_copy.getLength();
29}
30
31Path::~Path(){}
32
33void Path::addPose(StateVector pose, uint32_t k, std::chrono::nanoseconds delta_time){
34
35 std::shared_ptr<NodePath> new_node = std::make_shared<NodePath>();
36 new_node->pose = pose;
37 new_node->k = k;
38 new_node->delta_time = delta_time + path_root_->next_node->delta_time;
39 new_node->referenced = 0;
40 new_node->next_node = std::move(path_root_->next_node);
41
42
43 path_root_->next_node = std::move(new_node);
44 path_root_->next_node->referenced++;
45
46 path_length_++;
47}
48
49uint32_t Path::countLengthOfPath(){
50
51 if (path_root_ == nullptr){
52 path_length_ = 0;
53 return path_length_;
54 }
55 path_length_ = 1;
56 return countLengthRecurse(path_root_->next_node);
57}
58
59uint32_t Path::countLengthRecurse(std::shared_ptr<NodePath> & next){
60 path_length_++;
61 if(next->next_node == nullptr) return path_length_;
62 else return countLengthRecurse(next->next_node);
63}
64
65StateVector Path::getPose(uint32_t n_pose){
66 // returns specific pose!
67 if (path_root_ == nullptr)
68 return StateVector::Zero();
69
70 return getPoseNode(n_pose, path_root_)->pose;
71}
72
73std::shared_ptr<NodePath> Path::getPoseNode(uint32_t n_pose, std::shared_ptr<NodePath> &next){
74 if(next == nullptr) return nullptr;
75 else if(next->k == n_pose) return next;
76 else return getPoseNode(n_pose, next->next_node);
77}
78
79void Path::printAllPathPoses(std::ostream &out){
80 for(size_t i = 1 ; i<=path_length_ ; i++){
81 auto pose_node = getPoseNode(i, path_root_);
82
83 if (pose_node != nullptr){
84 out <<" Pose_" << i << ": " << pose_node->pose.transpose() << " at address: " << std::hex << pose_node << std::dec << std::endl;
85 }
86 else{
87 out <<"Error: NULL pointer!";
88 }
89 }
90}
91
92} //namespace fastslam
93