Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ft tree base #24

Merged
merged 2 commits into from
Oct 31, 2023
Merged

Ft tree base #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a91b81b
Apply same method for update a node
bilalkah Oct 31, 2023
03eba0f
improve functionality
bilalkah Oct 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 1 addition & 6 deletions planning/tree_base/rrt/rrt.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,12 +55,7 @@ Path RRT::FindPath(const Node &start_node, const Node &goal_node,
std::lock_guard<std::mutex> 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_)
Expand Down
22 changes: 14 additions & 8 deletions planning/tree_base/rrt_star/rrt_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -185,19 +186,24 @@ void RRTStar::IterativelyCostUpdate(const std::shared_ptr<NodeParent> &node)
std::queue<std::shared_ptr<NodeParent>> 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<NodeParent> 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);
}
}

Expand Down
123 changes: 52 additions & 71 deletions planning/tree_base/src/common_tree_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,15 +50,16 @@ Node RandomNode(const std::shared_ptr<Map> map)

std::vector<Node> Get2DRayBetweenNodes(const Node &src, const Node &dst)
{
if (src == dst)
{
return std::vector<Node>{};
}
std::pair<double, double> 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<Node> ray;
if (src == dst)
{
return ray;
}

// Check ray vector corresponds which past of cartesian coordinate system.
std::pair<double, double> sign_vector{0.5, 0.5};
if (ray_vector.first < 0)
Expand Down Expand Up @@ -119,57 +120,41 @@ std::shared_ptr<NodeParent>
GetNearestNodeParent(const Node &node,
const std::vector<std::shared_ptr<NodeParent>> &nodes)
{
std::shared_ptr<NodeParent> nearest_node;
double min_distance{std::numeric_limits<double>::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<NodeParent> &node1,
const std::shared_ptr<NodeParent> &node2) {
return EuclideanDistance(node1->node, node) <
EuclideanDistance(node2->node, node);
});

return *nearest_node;
}

std::vector<std::shared_ptr<NodeParent>> GetNearestNodeParentVector(
const int neighbor_radius, const Node &node,
const std::vector<std::shared_ptr<NodeParent>> &nodes)
{
std::vector<std::shared_ptr<NodeParent>> nearest_nodes;
std::shared_ptr<NodeParent> nearest_node;
double min_distance{std::numeric_limits<double>::max()};
auto nearest_nodes = std::vector<std::shared_ptr<NodeParent>>{};

// copy_if and sort
std::copy_if(nodes.begin(), nodes.end(), std::back_inserter(nearest_nodes),
[&node, &neighbor_radius](const std::shared_ptr<NodeParent> &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<NodeParent> &node1,
const std::shared_ptr<NodeParent> &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<NodeParent> &node1,
const std::shared_ptr<NodeParent> &node2) {
return node1->cost.f < node2->cost.f;
});

return nearest_nodes;
}

Expand All @@ -179,48 +164,44 @@ WireNewNode(const int max_branch_length, const int min_branch_length,
const std::shared_ptr<NodeParent> &nearest_node,
const std::shared_ptr<Map> 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)
{
new_node.x_ = nearest_node->node.x_ + max_branch_length * unit_vector_x;
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<NodeParent>(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<NodeParent>(*reverse_iterator_ray, nearest_node,
Cost{});
return std::make_shared<NodeParent>(ray[index - 1], nearest_node, Cost{});
}

return std::nullptr_t();
Expand Down