From eafcb13b371eee59d7854ee4186006e207a61393 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 21 Aug 2024 13:11:12 -0700 Subject: [PATCH] Update Smac Planner for rounding to closest bin rather than flooring --- nav2_smac_planner/src/node_hybrid.cpp | 4 ++-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 10 ++++------ nav2_smac_planner/test/test_nodehybrid.cpp | 6 +++--- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 5314cd68c11..1d3d36d520b 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -330,8 +330,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 5ba10d7d9a0..70806aae201 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -368,7 +368,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( std::to_string(start.pose.position.y) + ") was outside bounds"); } - 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); } @@ -376,8 +376,7 @@ 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->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { @@ -385,7 +384,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); } - 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); } @@ -393,8 +392,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 fb17dad5201..d98c8336f62 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -396,7 +396,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); @@ -414,8 +414,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); }