From a91b81b853a02896f76a35980f3e295a7d0dc4fe Mon Sep 17 00:00:00 2001 From: Bilal Kahraman Date: Tue, 31 Oct 2023 21:33:47 +0300 Subject: [PATCH 1/2] Apply same method for update a node --- planning/tree_base/rrt/rrt.cpp | 7 +------ planning/tree_base/rrt_star/rrt_star.cpp | 22 ++++++++++++++-------- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/planning/tree_base/rrt/rrt.cpp b/planning/tree_base/rrt/rrt.cpp index f5d12a4..7ab1e06 100644 --- a/planning/tree_base/rrt/rrt.cpp +++ b/planning/tree_base/rrt/rrt.cpp @@ -55,12 +55,7 @@ Path RRT::FindPath(const Node &start_node, const Node &goal_node, std::lock_guard lock(log_mutex_); log_.first.emplace_back(new_node); } - - auto ray{Get2DRayBetweenNodes(nearest_node->node, new_node->node)}; - for (const auto &node : ray) - { - map_copy->SetNodeState(node, NodeState::kVisited); - } + map_copy->SetNodeState(new_node->node, NodeState::kVisited); // Check if goal node is in radius. if (EuclideanDistance(new_node->node, goal_node) <= goal_radius_) diff --git a/planning/tree_base/rrt_star/rrt_star.cpp b/planning/tree_base/rrt_star/rrt_star.cpp index 993a9b4..9b1cfe1 100644 --- a/planning/tree_base/rrt_star/rrt_star.cpp +++ b/planning/tree_base/rrt_star/rrt_star.cpp @@ -68,6 +68,7 @@ Path RRTStar::FindPath(const Node &start_node, const Node &goal_node, log_.first.push_back(new_node); } + map_copy->SetNodeState(new_node->node, NodeState::kVisited); parent_child_map_[new_node->parent].push_back(new_node); CheckIfGoalReached(new_node, final, goal_node); @@ -185,19 +186,24 @@ void RRTStar::IterativelyCostUpdate(const std::shared_ptr &node) std::queue> queue; queue.push(node); - while (!queue.empty()) - { - auto current_node{queue.front()}; - queue.pop(); - - for (const auto &child : parent_child_map_[current_node]) - { + auto update_cost = [&](std::shared_ptr current_node) { + std::for_each( + parent_child_map_[current_node].begin(), + parent_child_map_[current_node].end(), [&](const auto &child) { child->cost = Cost(child->parent->cost.g + 1, child->parent->cost.h + EuclideanDistance(child->node, child->parent->node)); queue.push(child); - } + }); + }; + + while (!queue.empty()) + { + auto current_node{queue.front()}; + queue.pop(); + + update_cost(current_node); } } From 03eba0f4b4b6011d59946a63971f926992a69cd2 Mon Sep 17 00:00:00 2001 From: Bilal Kahraman Date: Tue, 31 Oct 2023 21:34:06 +0300 Subject: [PATCH 2/2] improve functionality --- planning/tree_base/src/common_tree_base.cpp | 123 +++++++++----------- 1 file changed, 52 insertions(+), 71 deletions(-) diff --git a/planning/tree_base/src/common_tree_base.cpp b/planning/tree_base/src/common_tree_base.cpp index 7e4f10d..86c77cc 100644 --- a/planning/tree_base/src/common_tree_base.cpp +++ b/planning/tree_base/src/common_tree_base.cpp @@ -50,15 +50,16 @@ Node RandomNode(const std::shared_ptr map) std::vector Get2DRayBetweenNodes(const Node &src, const Node &dst) { + if (src == dst) + { + return std::vector{}; + } std::pair ray_vector{dst.x_ - src.x_, dst.y_ - src.y_}; auto ray_length{std::hypot(ray_vector.first, ray_vector.second)}; auto unit_vector{std::make_pair(ray_vector.first / ray_length, ray_vector.second / ray_length)}; std::vector ray; - if (src == dst) - { - return ray; - } + // Check ray vector corresponds which past of cartesian coordinate system. std::pair sign_vector{0.5, 0.5}; if (ray_vector.first < 0) @@ -119,57 +120,41 @@ std::shared_ptr GetNearestNodeParent(const Node &node, const std::vector> &nodes) { - std::shared_ptr nearest_node; - double min_distance{std::numeric_limits::max()}; - - for (const auto &node_parent : nodes) - { - double distance{EuclideanDistance(node_parent->node, node)}; - if (distance < min_distance) - { - min_distance = distance; - nearest_node = node_parent; - } - } - - return nearest_node; + // Get nearest node. + auto nearest_node = + std::min_element(nodes.begin(), nodes.end(), + [&node](const std::shared_ptr &node1, + const std::shared_ptr &node2) { + return EuclideanDistance(node1->node, node) < + EuclideanDistance(node2->node, node); + }); + + return *nearest_node; } std::vector> GetNearestNodeParentVector( const int neighbor_radius, const Node &node, const std::vector> &nodes) { - std::vector> nearest_nodes; - std::shared_ptr nearest_node; - double min_distance{std::numeric_limits::max()}; + auto nearest_nodes = std::vector>{}; + + // copy_if and sort + std::copy_if(nodes.begin(), nodes.end(), std::back_inserter(nearest_nodes), + [&node, &neighbor_radius](const std::shared_ptr &n) { + return EuclideanDistance(n->node, node) < neighbor_radius; + }); - // Search in the neighbor_radius - for (const auto &node_parent : nodes) - { - double distance{EuclideanDistance(node_parent->node, node)}; - if (distance < neighbor_radius) - { - nearest_nodes.emplace_back(node_parent); - } - if (distance < min_distance) - { - min_distance = distance; - nearest_node = node_parent; - } - } if (nearest_nodes.empty()) { - nearest_nodes.emplace_back(nearest_node); - } - if (nearest_nodes.size() > 1) - { - // Sort nearest nodes according to their cost from low to high. - std::sort(nearest_nodes.begin(), nearest_nodes.end(), - [](const std::shared_ptr &node1, - const std::shared_ptr &node2) { - return node1->cost.f < node2->cost.f; - }); + nearest_nodes.emplace_back(GetNearestNodeParent(node, nodes)); } + + std::sort(nearest_nodes.begin(), nearest_nodes.end(), + [](const std::shared_ptr &node1, + const std::shared_ptr &node2) { + return node1->cost.f < node2->cost.f; + }); + return nearest_nodes; } @@ -179,13 +164,10 @@ WireNewNode(const int max_branch_length, const int min_branch_length, const std::shared_ptr &nearest_node, const std::shared_ptr map) { - // Calculate distance between random node and nearest node. double distance{EuclideanDistance(random_node, nearest_node->node)}; - // Calculate unit vector between random node and nearest node. double unit_vector_x{(random_node.x_ - nearest_node->node.x_) / distance}; double unit_vector_y{(random_node.y_ - nearest_node->node.y_) / distance}; - // Calculate new node. Node new_node{random_node}; if (distance > max_branch_length) { @@ -193,34 +175,33 @@ WireNewNode(const int max_branch_length, const int min_branch_length, new_node.y_ = nearest_node->node.y_ + max_branch_length * unit_vector_y; distance = EuclideanDistance(new_node, nearest_node->node); } + else if (distance < min_branch_length) + { + return std::nullptr_t(); + } auto ray{Get2DRayBetweenNodes(nearest_node->node, new_node)}; - - auto check_collision = [&ray, &map](auto reverse_iterator_ray) { - while (reverse_iterator_ray != ray.rend()) - { - if (map->GetNodeState(*reverse_iterator_ray) == NodeState::kOccupied) - { - return true; - } - reverse_iterator_ray++; - } - return false; - }; - - auto reverse_iterator_ray{ray.rbegin()}; - while (check_collision(reverse_iterator_ray) && distance > min_branch_length) + if (ray.empty()) { - reverse_iterator_ray++; - distance = EuclideanDistance(nearest_node->node, *reverse_iterator_ray); + return std::nullptr_t(); } - - if (reverse_iterator_ray != ray.rend() && - *reverse_iterator_ray != nearest_node->node && - !check_collision(reverse_iterator_ray)) + auto index{0u}; + while (index < ray.size() && + map->GetNodeState(ray[index]) != NodeState::kOccupied) + { + index++; + } + if (index == ray.size()) + { + return std::make_shared(new_node, nearest_node, Cost{}); + } + auto isLengthValid{EuclideanDistance(ray[index - 1], nearest_node->node) > + min_branch_length}; + auto isNodeValid{map->GetNodeState(ray[index]) != NodeState::kOccupied && + ray[index] != new_node}; + if (isLengthValid && isNodeValid) { - return std::make_shared(*reverse_iterator_ray, nearest_node, - Cost{}); + return std::make_shared(ray[index - 1], nearest_node, Cost{}); } return std::nullptr_t();