Galactic sync 3 (July 13) (#2449)

* missing comma in bt_navigator plugin list (#2447)

* Reduce nodes for nav2_waypoint_follower  (#2441)

* Reduce node of waypoint_follower by using callback group and executor

Signed-off-by: zhenpeng ge <[email protected]>

* fix linting failures

Signed-off-by: zhenpeng ge <[email protected]>

* Add lazy_theta_star (#1839)

* Add lazy_theta_star

* Added license

* Added license

* Update lazy_theta_star_b.cpp

* restructured files

- separated planner part and the algorithm
- implemented all the changes as suggested

* restructured files

* Update CMakeLists.txt

* removed unnecessary comments

* removed comments

* Delete planner.cpp

* Delete lazy_theta_star_b.cpp

* Delete lazy_theta_star_b.h

* Delete planner.h

* Delete CMakeLists.txt

* Delete global_planner_plugin.xml

* Delete package.xml

* replaced the files

- refactored code
- improved reliability
- have to write a code similar to that in nav2_system_tests, to test it (working on it)

* removed comments

* removed comments

* Update lazy_theta_star2.cpp

* update files

- replaced manual management of priority queue with stl priority queue
- added the parameter ".lethal_cost"
- removed unnecessary parameters passed to the functions

* update files

- updated the header files in accordance to their .cpp counterparts

* Delete lazy_theta_star2.h

* Delete lazy_theta_star_planner.h

* Delete lazy_theta_star2.cpp

* Delete lazy_theta_star_planner.cpp

* Delete CMakeLists.txt

* Delete package.xml

* Delete global_planner_plugin.xml

* upload the changed code

Changes from last time are:
- the code has been changed to the Lazy Theta* P variant, in order to account for the costmap traversal costs
- parameters are available to change the weights of the costmap traversal cost (weight = 1.75, as of now) and the distance function (weight = 1.0, as of now
-

* Delete lazy_theta_star_p_planner directory

* Replace the old files

- the structure of code has been changed
- new functions have been added, namely : getTraversalCost, getEuclideanCost, getCellCost, isSafe[it is now an overloaded function]
- documentation added for variables and functions
- the parameters for the planner now consists of : how_many_corners, costmap_tolerance, euc_tolerance (documentation to added soon)
- fixed a bug where the incorrect traversal cost of the node was taken

* Delete lazy_theta_star_p_planner directory

* update the files

- renamed the project to nav2_theta_star_planner from lazy_theta_star_p_planner
- renamed files from lazy_theta_p_planner.hpp/.cpp to theta_star_planner.hpp/.cpp and lazy_theta_star.hpp/.cpp to theta_star.hpp/.cpp
- added a readme file outlining the parameters, usage notes and images to be added soon
- added parameters and renamed the parameters for the cost function (costmap_tolerance -> w_traversal_cost ; euc_tolerance -> w_euc_cost ; added a parameter for the heuristic)
- replaced the SharedPtr with a WeakPtr for node
- removed +1 and the pusher_ variable added to compensate for it

* Update README.md

* update the code

- linted the code 
- **updates to readme, are still pending**
- changed the type of message from INFO to DEBUG
- replaced the capital letters with the smaller ones

* update the readme file

* Update README.md

* Update README.md

* Update README.md

* Update README.md

* Delete global_planner_plugin.xml

* fix the linting issues

* remove the space on line 7

* change RCLCPP_INFO to RCLCPP_DEBUG

* Update README.md

* Add test coverage

- added tests to check the algorithm itself and its helper functions
- added smoke test to detect plugin level issues
- inlined some functions 
- shifted the functions `setStartAndGoal()` and `isSafeToPlan()` to the ThetaStar class
- removed the functions `dist()` and `getCellCost()` from the ThetaStar class

* update the test file

* update the test file

* Update README.md

* Update README.md

* Update README.md

* add test on the size of the output path

* Update README.md

* Update README.md

* change the function name from `isSafeToPlan()` to `isUnsafeToPlan()`

* update the files

- inlined the functions `getIndex` and `addIndex`
- removed typos from the comments

* fix typos

* Update README.md

* Update README.md

* Update README.md

* Update README.md

* Update README.md

* Update README.md

* Update README.md

* fix the typo

- the first isSafe calls output wasn't negated, it has been fixed

* Update theta_star.hpp

- added the `getCost()` function (again)
- replaced the use of indices with pointers to store the node's data, changes were made to the following functions - `addIndex()`, `getIndex()`, `initialisePosn()`.
- the priority queue now stores the pointer to node's data and accordingly changes were made to `comp` struct
- the global variable `curr_node` was renamed to `exp_node`
- removed the struct `pos`

* Update theta_star.cpp

* update default parameters

* update default parameters

* Update README.md

* update the test file

* fix linting issues

* Update README.md

* Update README.md

* Update README.md

* Update theta_star_planner.cpp

* update LETHAL_COST

* update README.md

* update dependency list

* Update package.xml

* Name bt_navigator action nodes uniquely (#2410)

Co-authored-by: Jonatan Olofsson <[email protected]>

* Hybrid-A* optimizations and completion without on-going State Lattice work (#2404)

* major smac planner collision checking speedups and improvements

* fix linting errors

* wireframe for state lattice node

* finishing boilerplate changes

* adding more context to each TODO and notes about path to completion

* prototype for the base plugin + refactor out common logic for all planners

* commiting speed up work in progress

* adding in improvements to overall user quality of life

* pushing updates to 2D for faster / smoother; hybrid for smoother and new heuristic function; a bit of rearchitecture; deprecating smoother to make room for a new one

* adding smoother prototype

* adding orientation guestimator

* correcing 2d

* done with smoother

* enabling collision checking full SE2 only when under inscribed cost according to current exponential decay function

* adding doxgyen on new function

* linting

* testing working

* fixing looping at end of paths + simplifying code

* fixed cost-based issue in smoother

* finishing touches on smoothing

* smoother recursion fix + crashing issue resolved

* incremental changes

* completed hybrid A* plannern

* adding images

* purge state lattice work to merge in just Hybrid-A* improvements

* linting

Co-authored-by: simutisernestas <[email protected]>
Co-authored-by: gezp <[email protected]>
Co-authored-by: Anshumaan Singh <[email protected]>
Co-authored-by: Jonatan Olofsson <[email protected]>
Co-authored-by: Jonatan Olofsson <[email protected]>

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -85,7 +85,7 @@ bool BtActionServer<ActionT>::on_configure()
   // use suffix '_rclcpp_node' to keep parameter file consistency #1773
   auto options = rclcpp::NodeOptions().arguments(
     {"--ros-args",
-      "-r", std::string("__node:=") + node->get_name() + "_rclcpp_node",
+      "-r", std::string("__node:=") + std::string(node->get_name()) + action_name_ + "_rclcpp_node",
       "--"});
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<rclcpp::Node>("_", options);

nav2_bt_navigator/src/bt_navigator.cpp

@@ -61,7 +61,7 @@ BtNavigator::BtNavigator()
     "nav2_is_battery_low_condition_bt_node",
     "nav2_navigate_through_poses_action_bt_node",
     "nav2_navigate_to_pose_action_bt_node",
-    "nav2_remove_passed_goals_action_bt_node"
+    "nav2_remove_passed_goals_action_bt_node",
     "nav2_planner_selector_bt_node",
     "nav2_controller_selector_bt_node",
     "nav2_goal_checker_selector_bt_node"

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -72,6 +72,13 @@ class FootprintCollisionChecker
   * @brief Set the current costmap object to use for collision detection
   */
   void setCostmap(CostmapT costmap);
+  /**
+  * @brief Get the current costmap object
+  */
+  CostmapT getCostmap()
+  {
+    return costmap_;
+  }
 
 protected:
   CostmapT costmap_;

nav2_costmap_2d/src/footprint_collision_checker.cpp

@@ -51,36 +51,40 @@ double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint footpr
   unsigned int x0, x1, y0, y1;
   double footprint_cost = 0.0;
 
+  // get the cell coord of the first point
+  if (!worldToMap(footprint[0].x, footprint[0].y, x0, y0)) {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  // cache the start to eliminate a worldToMap call
+  unsigned int xstart = x0;
+  unsigned int ystart = y0;
+
   // we need to rasterize each line in the footprint
   for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
-    // get the cell coord of the first point
-    if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
-      return static_cast<double>(LETHAL_OBSTACLE);
-    }
 
     // get the cell coord of the second point
     if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
       return static_cast<double>(LETHAL_OBSTACLE);
     }
 
     footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
-  }
 
-  // we also need to connect the first point in the footprint to the last point
-  // get the cell coord of the last point
-  if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
-    return static_cast<double>(LETHAL_OBSTACLE);
-  }
+    // the second point is next iteration's first point
+    x0 = x1;
+    y0 = y1;
 
-  // get the cell coord of the first point
-  if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
-    return static_cast<double>(LETHAL_OBSTACLE);
+    // if in collision, no need to continue
+    if (footprint_cost == static_cast<double>(INSCRIBED_INFLATED_OBSTACLE) ||
+      footprint_cost == static_cast<double>(LETHAL_OBSTACLE))
+    {
+      return footprint_cost;
+    }
   }
 
-  footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
-
-  // if all line costs are legal... then we can return that the footprint is legal
-  return footprint_cost;
+  // we also need to connect the first point in the footprint to the last point
+  // the last iteration's x1, y1 are the last footprint point's coordinates
+  return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost);
 }
 
 template<typename CostmapT>
@@ -95,6 +99,13 @@ double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int
     if (line_cost < point_cost) {
       line_cost = point_cost;
     }
+
+    // if in collision, no need to continue
+    if (line_cost == static_cast<double>(INSCRIBED_INFLATED_OBSTACLE) ||
+      line_cost == static_cast<double>(LETHAL_OBSTACLE))
+    {
+      return line_cost;
+    }
   }
 
   return line_cost;

nav2_smac_planner/CMakeLists.txt

@@ -19,7 +19,6 @@ find_package(builtin_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(ompl REQUIRED)
@@ -35,12 +34,16 @@ add_compile_options(-O3 -Wextra -Wdeprecated -fPIC)
 
 include_directories(
   include
-  ${CERES_INCLUDES}
   ${OMPL_INCLUDE_DIRS}
   ${OpenMP_INCLUDE_DIRS}
 )
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+
+find_package(OpenMP)
+if(OPENMP_FOUND)
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
 
 set(library_name nav2_smac_planner)
 
@@ -62,16 +65,16 @@ set(dependencies
   eigen3_cmake_module
 )
 
-# SE2 plugin
+# Hybrid plugin
 add_library(${library_name} SHARED
-  src/smac_planner.cpp
+  src/smac_planner_hybrid.cpp
   src/a_star.cpp
-  src/node_se2.cpp
+  src/node_hybrid.cpp
   src/costmap_downsampler.cpp
   src/node_2d.cpp
 )
 
-target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
+target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
 target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS})
 
 ament_target_dependencies(${library_name}
@@ -82,12 +85,12 @@ ament_target_dependencies(${library_name}
 add_library(${library_name}_2d SHARED
   src/smac_planner_2d.cpp
   src/a_star.cpp
-  src/node_se2.cpp
+  src/node_hybrid.cpp
   src/costmap_downsampler.cpp
   src/node_2d.cpp
 )
 
-target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES})
+target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES})
 target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS})
 
 ament_target_dependencies(${library_name}_2d