Merge branch 'main' of github.com:dfki-ric/ugv_nav4d

CITATION.cff

@@ -0,0 +1,36 @@
+cff-version: "1.2.0"
+authors:
+- family-names: Böckmann
+  given-names: Arne
+- family-names: Machowinski
+  given-names: Janosch
+- family-names: Lodhi
+  given-names: Muhammad Haider Khan
+  orcid: "https://orcid.org/0009-0008-1199-3489"
+doi: 10.5281/zenodo.14236941
+message: If you use this software, please cite our article in the
+  Journal of Open Source Software.
+preferred-citation:
+  authors:
+  - family-names: Böckmann
+    given-names: Arne
+  - family-names: Machowinski
+    given-names: Janosch
+  - family-names: Lodhi
+    given-names: Muhammad Haider Khan
+    orcid: "https://orcid.org/0009-0008-1199-3489"
+  date-published: 2024-12-02
+  doi: 10.21105/joss.06983
+  issn: 2475-9066
+  issue: 104
+  journal: Journal of Open Source Software
+  publisher:
+    name: Open Journals
+  start: 6983
+  title: "ugv_nav4d: Advanced Multi-Surface Navigation for Unmanned
+    Ground Vehicles Using 4D Path Planning Techniques"
+  type: article
+  url: "https://joss.theoj.org/papers/10.21105/joss.06983"
+  volume: 9
+title: "ugv_nav4d: Advanced Multi-Surface Navigation for Unmanned Ground
+  Vehicles Using 4D Path Planning Techniques"

README.md

@@ -1,6 +1,7 @@
-CI build and test:
+CI build (main): ![Main](https://github.com/dfki-ric/ugv_nav4d/actions/workflows/c-cpp.yml/badge.svg) 
 
-Main: ![Main](https://github.com/dfki-ric/ugv_nav4d/actions/workflows/c-cpp.yml/badge.svg)
+JOSS Paper: 
+[![DOI](https://joss.theoj.org/papers/10.21105/joss.06983/status.svg)](https://doi.org/10.21105/joss.06983)
 
 ugv_nav4d: Advanced Multi-Surface Navigation for Unmanned Ground Vehicles Using 4D Path Planning Techniques
 =============
@@ -97,6 +98,14 @@ The button `Create PlannerDump` can be used to save the planner's state. The cre
 ugv_nav4d_replay ugv4d_dump_xxxx.bin
 ```
 
+An additional GUI is provided for tuning of parameters used in the generation of motion primitives. The generated spline motion primitives are also visualized. Furthermore, the `vizkit3d::SbplSplineVisualization` plugin under `Properties` offers further options for visualizing and analyzing the splines for various start and end angles.
+![MotionPrimitivesGui](doc/figures/motion_primitives_gui.png)
+
+Run the following executable in your terminal:
+```
+sbpl_spline_viz_bin
+``````
+
 #### Unit Tests
 
 Build the library again but this time enable the `-DTESTS_ENABLED=ON`

src/DebugDrawingDeclarations.cpp

@@ -1,3 +1,5 @@
+#ifdef ENABLE_DEBUG_DRAWINGS
+
 #include <vizkit3d_debug_drawings/DebugDrawing.hpp>
 
 // planner debug channels
@@ -19,4 +21,6 @@ V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_check_fail_start");
 V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_obs_check_fail_start");
 V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_goalBox");
 V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_startBox");
-V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_primitives");
+V3DD_DECLARE_DEBUG_DRAWING_CHANNEL("ugv_nav4d_primitives");
+
+#endif

src/EnvironmentXYZTheta.cpp

@@ -4,13 +4,16 @@
 #include <base/Pose.hpp>
 #include <base/Spline.hpp>
 #include <fstream>
-#include <vizkit3d_debug_drawings/DebugDrawing.hpp>
-#include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
 #include "PathStatistic.hpp"
 #include "Dijkstra.hpp"
 #include <limits>
 #include <base-logging/Logging.hpp>
 
+#ifdef ENABLE_DEBUG_DRAWINGS
+#include <vizkit3d_debug_drawings/DebugDrawing.hpp>
+#include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
+#endif
+
 using namespace std;
 using namespace sbpl_spline_primitives;
 using trajectory_follower::SubTrajectory;

src/ObstacleMapGenerator3D.cpp

@@ -1,6 +1,9 @@
 #include "ObstacleMapGenerator3D.hpp"
+
+#ifdef ENABLE_DEBUG_DRAWINGS
 #include <vizkit3d_debug_drawings/DebugDrawing.hpp>
 #include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
+#endif
 
 using namespace maps::grid;
 

src/PathStatistic.cpp

@@ -1,8 +1,11 @@
 #include "PathStatistic.hpp"
 #include <unordered_set>
 #include <deque>
+
+#ifdef ENABLE_DEBUG_DRAWINGS
 #include <vizkit3d_debug_drawings/DebugDrawing.hpp>
 #include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
+#endif
 
 ugv_nav4d::PathStatistic::Stats::Stats() :
     obstacles(0), 
@@ -290,4 +293,4 @@ bool ugv_nav4d::PathStatistic::isPathFeasible(const std::vector<const traversabi
             return false;
     }
     return true;
-}
+}

src/Planner.cpp

@@ -2,15 +2,18 @@
 #include <sbpl/planners/araplanner.h>
 #include <sbpl/utils/mdpconfig.h>
 #include <maps/grid/MultiLevelGridMap.hpp>
-#include <vizkit3d_debug_drawings/DebugDrawing.hpp>
-#include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
 #include <base/Eigen.hpp>
 #include "PlannerDump.hpp"
 #include <omp.h>
 #include <cmath>
 #include <base-logging/Logging.hpp>
 #include "Logger.hpp"
 
+#ifdef ENABLE_DEBUG_DRAWINGS
+#include <vizkit3d_debug_drawings/DebugDrawing.hpp>
+#include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
+#endif
+
 using namespace maps::grid;
 using trajectory_follower::SubTrajectory;
 

src/gui/PlannerGui.cpp

@@ -10,8 +10,6 @@
 #include <thread>
 #include <vizkit3d/Vizkit3DWidget.hpp>
 #include <ugv_nav4d/PreComputedMotions.hpp>
-#include <vizkit3d_debug_drawings/DebugDrawing.hpp>
-#include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
 #include <boost/filesystem.hpp>
 #include <boost/archive/binary_iarchive.hpp>
 #include <pcl/io/ply_io.h>
@@ -20,6 +18,11 @@
 #include <pcl/common/transforms.h>
 #include <base-logging/Logging.hpp>
 
+#ifdef ENABLE_DEBUG_DRAWINGS
+#include <vizkit3d_debug_drawings/DebugDrawing.hpp>
+#include <vizkit3d_debug_drawings/DebugDrawingColors.hpp>
+#endif
+
 using namespace ugv_nav4d;
 
 PlannerGui::PlannerGui(const std::string& dumpName): QObject()