wip

stevedanomodolor · Feb 10, 2024 · 279ed93 · 279ed93
1 parent 67873f9
commit 279ed93
Show file tree

Hide file tree

Showing 189 changed files with 98 additions and 20,793 deletions.
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
@@ -236,7 +236,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(

diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt
@@ -142,7 +142,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py
@@ -154,6 +154,7 @@ def generate_launch_description():
                 respawn_delay=2.0,
                 parameters=[configured_params],
                 arguments=['--ros-args', '--log-level', log_level],
+                prefix=['xterm -e gdb -ex run --args'],
                 remappings=remappings,
             ),
             Node(

diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
@@ -131,7 +131,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ### Ament stuff ###

diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
@@ -53,7 +53,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 install(

diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
@@ -101,7 +101,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
@@ -179,7 +179,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
   pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml)
 endif()
 

diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt
@@ -69,7 +69,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt
@@ -73,7 +73,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
 
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt
@@ -42,7 +42,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 install(TARGETS standard_traj_generator

diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt
@@ -72,7 +72,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_graceful_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt
@@ -68,7 +68,7 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt
@@ -85,7 +85,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
 
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
@@ -152,7 +152,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
@@ -114,7 +114,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
   # add_subdirectory(benchmark)
 endif()
 

diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
@@ -72,7 +72,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(tf2_ros REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(nav2_core REQUIRED)
+add_compile_options(-g)
 
 nav2_package()
 
@@ -84,7 +85,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt
@@ -59,7 +59,7 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt
@@ -56,7 +56,7 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_smac_planner)
 
 set(CMAKE_BUILD_TYPE Release) #Debug, Release
+add_compile_options(-g)
 
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
@@ -155,7 +156,7 @@ if(BUILD_TESTING)
   set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
-  # add_subdirectory(test)
+  # #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include ${OMPL_INCLUDE_DIRS})

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -41,15 +41,15 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   _x_size(0),
   _y_size(0),
   _search_info(search_info),
-  _goals_coordinates(CoordinateVector),
+  _goals_coordinates(CoordinateVector()),
   _start(nullptr),
-  _goals(NodeVector),
+  _goals(NodeVector()),
   _motion_model(motion_model)
 {
   _graph.reserve(100000);
   // TODO remove 
-  _goals.reserve(1);
-  _goals_coordinates.reserve(1);
+  _goals.resize(1);
+  _goals_coordinates.resize(1);
 }
 
 template<typename NodeT>
@@ -197,7 +197,8 @@ template<typename NodeT>
 void AStarAlgorithm<NodeT>::setGoal(
   const unsigned int & mx,
   const unsigned int & my,
-  const unsigned int & dim_3)
+  const unsigned int & dim_3,
+  const GoalHeading & goal_heading)
 {
   _goals[0] = addToGraph(NodeT::getIndex(mx, my, dim_3));
 
@@ -206,7 +207,7 @@ void AStarAlgorithm<NodeT>::setGoal(
     static_cast<float>(my),
     static_cast<float>(dim_3));
 
-  if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates[0]) {
+  if (!_search_info.cache_obstacle_heuristic || goal_coords != _goals_coordinates[0]) {
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");
     }
@@ -319,8 +320,14 @@ bool AStarAlgorithm<NodeT>::createPath(
 
     // 2.1) Use an analytic expansion (if available) to generate a path
     expansion_result = nullptr;
+    NodePtr goal = _goals[0];
+    if (goal == nullptr) {
+      std::cout << "-----------------------------------------------------------------------------------------------------------------------------------------" << std::endl;
+      std::cout << "AStarAlgorithm::createPath: goal == nullptr" << std::endl;
+      return false;
+    }
     expansion_result = _expander->tryAnalyticExpansion(
-      current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance);
+      current_node, goal, neighborGetter, analytic_iterations, closest_distance);
     if (expansion_result != nullptr) {
       current_node = expansion_result;
     }

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -80,6 +80,21 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     if (analytic_iterations <= 0) {
       // Reset the counter and try the analytic path expansion
       analytic_iterations = desired_iterations;
+      // check if goal_node is not a nullptr
+      std::cout << "------------------------------------------------------" << std::endl;
+      if (!goal_node) {
+        std::cerr << "Goal node is nullptr" << std::endl;
+      }
+      if (!current_node) {
+        std::cerr << "Current node is nullptr" << std::endl;
+      }
+      if(!getter) {
+        std::cerr << "Getter is nullptr" << std::endl;
+      }
+      if(current_node->motion_table.state_space) {
+        std::cerr << "State space is nullptr" << std::endl;
+      }
+
       AnalyticExpansionNodes analytic_nodes =
         getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space);
       if (!analytic_nodes.empty()) {

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -643,6 +643,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "Valid options are DEFAULT, BIDIRECTIONAL, ANY_HEADING. ",
             goal_heading_type.c_str());
         }
+      }
     }
   }
 

diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt
@@ -110,7 +110,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
-  add_subdirectory(test)
+  #add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)
-Original file line number
+Diff line change
@@ Expand Up @@
                 "Valid options are DEFAULT, BIDIRECTIONAL, ANY_HEADING. ",
                 goal_heading_type.c_str());
             }
+          }
         }
       }
@@ Expand Down @@