fix bidirectional goal

Signed-off-by: stevedan <[email protected]>
ros-navigation · Oct 10, 2024 · 05262f5 · 05262f5
1 parent 232a1a7
commit 05262f5
Show file tree

Hide file tree

Showing 13 changed files with 1,210 additions and 23 deletions.
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
diff --git a/nav2_bringup/params/nav2_params.yaml.backup b/nav2_bringup/params/nav2_params.yaml.backup
diff --git a/nav2_bringup/params/nav2_params_default.yaml b/nav2_bringup/params/nav2_params_default.yaml
diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz
@@ -25,8 +25,8 @@ Panels:
     Name: Navigation 2
   - Class: nav2_rviz_plugins/Selector
     Name: Selector
-  - Class: nav2_rviz_plugins/Docking
-    Name: Docking
+  # - Class: nav2_rviz_plugins/Docking
+  #  Name: Docking
 Visualization Manager:
   Class: ""
   Displays:

diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
@@ -21,6 +21,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(backward_ros REQUIRED)
 
 nav2_package()
 

diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml
@@ -27,6 +27,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>backward_ros</depend>
 
   <exec_depend>libqt5-core</exec_depend>
   <exec_depend>libqt5-gui</exec_depend>

diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
@@ -138,4 +138,4 @@ void CostmapCostTool::handleGlobalCostResponse(
 }  // namespace nav2_rviz_plugins
 
 #include <pluginlib/class_list_macros.hpp>
-PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool)
+PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool)
diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
@@ -94,11 +94,11 @@ inline GoalHeadingMode fromStringToGH(const std::string & n)
   }
 }
 
-const float UNKNOWN = 255.0;
-const float OCCUPIED = 254.0;
-const float INSCRIBED = 253.0;
-const float MAX_NON_OBSTACLE = 252.0;
-const float FREE = 0;
+const float UNKNOWN_COST = 255.0;
+const float OCCUPIED_COST = 254.0;
+const float INSCRIBED_COST = 253.0;
+const float MAX_NON_OBSTACLE_COST = 252.0;
+const float FREE_COST = 0;
 
 }  // namespace nav2_smac_planner
 

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -195,6 +195,7 @@ void AStarAlgorithm<Node2D>::setGoal(
   const unsigned int & dim_3,
   const GoalHeadingMode & goal_heading_mode)
 {
+  (void) goal_heading_mode;
   if (dim_3 != 0) {
     throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
   }
@@ -232,7 +233,7 @@ void AStarAlgorithm<NodeT>::setGoal(
       goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3)));
       // 180 degrees
       dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins;
-      _goalsSet.insert(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin)));
+      goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin)));
       goals_coordinates.push_back(
         typename NodeT::Coordinates(
           static_cast<float>(mx),
@@ -550,13 +551,6 @@ typename AStarAlgorithm<NodeT>::CoordinateVector & AStarAlgorithm<NodeT>::getGoa
   return _goals_coordinates;
 }
 
-template<typename NodeT>
-void AStarAlgorithm<NodeT>::clearStart()
-{
-  auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3());
-  _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE);
-}
-
 // Instantiate algorithm for the supported template types
 template class AStarAlgorithm<Node2D>;
 template class AStarAlgorithm<NodeHybrid>;

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -375,9 +375,9 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
 
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
-  const NodePtr & current_node, const NodeSet & goals_node, const CoordinateVector & goals_coords,
-  const NodeGetter & getter, int & analytic_iterations,
-  int & closest_distance)
+  const NodePtr &, const NodeSet &, const CoordinateVector &,
+  const NodeGetter &, int &,
+  int &)
 {
   return NodePtr(nullptr);
 }

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -403,8 +403,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
-  _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading_mode);
+  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin), _goal_heading_mode);
 
   // Setup message
   nav_msgs::msg::Path plan;

diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py
@@ -106,7 +106,7 @@ def main():
     costmap = np.asarray(costmap_msg.data)
     costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x)
 
-    planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice']
+    planners = ["DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"] 
     max_cost = 210
     side_buffer = 100
     time_stamp = navigator.get_clock().now().to_msg()

diff --git a/tools/planner_benchmarking/planners.pickle b/tools/planner_benchmarking/planners.pickle