From ea06d2fdfb4fde9924e5fa4ae3925583625be719 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 21 Aug 2024 13:34:37 -0700 Subject: [PATCH] Update Smac Planner for rounding to closest bin rather than flooring (#4636) --- nav2_smac_planner/src/node_hybrid.cpp | 4 ++-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 11 +++++------ nav2_smac_planner/test/test_nodehybrid.cpp | 6 +++--- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index d0a32bd3a6e..e377de7a698 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -250,8 +250,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(theta / static_cast(bin_size))) % - num_angle_quantization; + auto bin = static_cast(round(static_cast(theta) / bin_size)); + return bin < num_angle_quantization ? bin : 0u; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 482e0d27a73..fa6ae6951c3 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -294,7 +294,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { throw std::runtime_error("Start pose is out of costmap!"); } - double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; + + double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); } @@ -302,14 +303,13 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setStart(mx, my, orientation_bin_id); + _a_star->setStart(mx, my, static_cast(orientation_bin)); // Set goal point, in A* bin search coordinates if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { throw std::runtime_error("Goal pose is out of costmap!"); } - orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; + orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); } @@ -317,8 +317,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setGoal(mx, my, orientation_bin_id); + _a_star->setGoal(mx, my, static_cast(orientation_bin)); // Setup message nav_msgs::msg::Path plan; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 0050b0a0abd..d8f4149cbc5 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -323,7 +323,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; motion_table.num_angle_quantization = 2; - double test_theta = M_PI; + double test_theta = M_PI / 2.0 - 0.000001; unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); @@ -341,8 +341,8 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 0.0872664675; motion_table.num_angle_quantization = 72; - double test_theta = 6.28318526567925; - unsigned int expected_angular_bin = 71; + double test_theta = 6.28317530718; // 0.0001 less than 2 pi + unsigned int expected_angular_bin = 0; // should be closer to wrap around unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); }