Skip to content

Commit

Permalink
fixing code format
Browse files Browse the repository at this point in the history
  • Loading branch information
stevedanomodolor committed Mar 10, 2024
1 parent 8245dd6 commit ec32f39
Show file tree
Hide file tree
Showing 3 changed files with 5 additions and 5 deletions.
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/analytic_expansion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
test_node->parent->parent->parent->parent->parent)
{
test_node = test_node->parent->parent->parent->parent->parent;
// print the goals pose
// print the goals pose
refined_analytic_nodes =
getAnalyticPath(
test_node, goals_node[g], getter,
Expand Down
4 changes: 2 additions & 2 deletions nav2_smac_planner/src/node_hybrid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -828,9 +828,9 @@ void NodeHybrid::precomputeDistanceHeuristic(
// we need to check for bidirectional goal heading and take the minimum
if (current_goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) {
unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int;
from[2] = heading_180_offset * angular_bin_size;
from[2] = heading_180_offset * angular_bin_size;
float motion_heuristic_180 = motion_table.state_space->distance(from(), to());
motion_heuristic = std::min(motion_heuristic,motion_heuristic_180);
motion_heuristic = std::min(motion_heuristic, motion_heuristic_180);
}
dist_heuristic_lookup_table[index] = motion_heuristic;
index++;
Expand Down
4 changes: 2 additions & 2 deletions nav2_smac_planner/src/node_lattice.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -473,9 +473,9 @@ void NodeLattice::precomputeDistanceHeuristic(
// we need to check for bidirectional goal heading and take the minimum
if (current_goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) {
unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int;
from[2] = motion_table.getAngleFromBin(heading_180_offset);
from[2] = motion_table.getAngleFromBin(heading_180_offset);
float motion_heuristic_180 = motion_table.state_space->distance(from(), to());
motion_heuristic = std::min(motion_heuristic,motion_heuristic_180);
motion_heuristic = std::min(motion_heuristic, motion_heuristic_180);
}
dist_heuristic_lookup_table[index] = motion_heuristic;
index++;
Expand Down

0 comments on commit ec32f39

Please sign in to comment.