diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 86a083b3a29..2dc5f0517b4 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -103,7 +103,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::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, diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 5897cf142b0..4650514efdf 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -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++; diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index ac53503c2b1..b881cbf484c 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -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++;