Updated ThetaStarSIREN and LazyThetaStarSIREN

robotics-upo · Jul 1, 2024 · 8af9c40 · 8af9c40
1 parent 4e4be62
commit 8af9c40
Show file tree

Hide file tree

Showing 26 changed files with 640 additions and 134 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -107,7 +107,7 @@ generate_messages(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2
+ LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 AStarSIREN ThetaStar ThetaStarM1 ThetaStarM2 ThetaStarSIREN LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStarSIREN
  # 
  CATKIN_DEPENDS std_msgs visualization_msgs geometry_msgs nav_msgs rospy roscpp message_runtime costmap_2d sensor_msgs pcl_ros pcl_conversions
 #  DEPENDS system_lib
@@ -164,18 +164,21 @@ add_library(AStar                  src/Planners/AStar.cpp
                                             )
 
 add_library(AStarM1            src/Planners/AStarM1.cpp ) 
-add_library(AStarM2            src/Planners/AStarM2.cpp ) 
+add_library(AStarM2            src/Planners/AStarM2.cpp )
+add_library(AStarSIREN         src/Planners/AStarSIREN.cpp ) 
 add_library(ThetaStar          src/Planners/ThetaStar.cpp )
 add_library(ThetaStarM1        src/Planners/ThetaStarM1.cpp )
 add_library(ThetaStarM2        src/Planners/ThetaStarM2.cpp )
+add_library(ThetaStarSIREN     src/Planners/ThetaStarSIREN.cpp )
 add_library(LazyThetaStar      src/Planners/LazyThetaStar.cpp )
 add_library(LazyThetaStarM1    src/Planners/LazyThetaStarM1.cpp )
 add_library(LazyThetaStarM1Mod src/Planners/LazyThetaStarM1Mod.cpp )
 add_library(LazyThetaStarM2    src/Planners/LazyThetaStarM2.cpp )
+add_library(LazyThetaStarSIREN src/Planners/LazyThetaStarSIREN.cpp )
 
 
 
-list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2)  
+list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 AStarSIREN ThetaStar ThetaStarM1 ThetaStarM2 ThetaStarSIREN LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStarSIREN)  
 target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TORCH_LIBRARIES})
 add_dependencies( ${${PROJECT_NAME}_LIBRARIES} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 list(APPEND ${PROJECT_NAME}_TARGETS ${${PROJECT_NAME}_LIBRARIES})

diff --git a/include/Grid3D/grid3d.hpp b/include/Grid3D/grid3d.hpp
@@ -511,15 +511,17 @@ class Grid3d
 
 					if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
 					{
-						dist = pointNKNSquaredDistance[0];
+						dist = sqrt(pointNKNSquaredDistance[0]);
 						m_grid[index].dist = dist;
 						if(!use_costmap_function){
 							m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2);
+							m_grid[index].prob = dist;
 						}else{
-							double prob =  100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius)));
+							//double prob =  100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius)));
 							// ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob);
 							//JAC: Include the computation of prob considering the distance to the nearest voronoi edge.
-							m_grid[index].prob = prob;
+							//m_grid[index].prob = prob;
+							m_grid[index].prob = dist;
 						}
 					}
 					else

diff --git a/include/Planners/AStarSIREN.hpp b/include/Planners/AStarSIREN.hpp
@@ -0,0 +1,67 @@
+#ifndef ASTARSIREN_HPP
+#define ASTARSIREN_HPP
+/**
+ * @file AStarSIREN.hpp
+ * @author Guillermo Gil ([email protected])
+ * @author Jose Antonio Cobano ([email protected])
+ * 
+ * @brief This algorithm is a variation of the A*. The only                             TBD
+ * difference is that it reimplements the computeG method adding the
+ * following cost term to the resturned result:
+ * 
+ *  auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_);
+ * 
+ * @version 0.1
+ * @date 2021-06-29
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#include <Planners/AStar.hpp>
+
+namespace Planners{
+
+    /**
+     * @brief 
+     * 
+     */
+    class AStarSIREN : public AStar
+    {
+
+    public:
+        /**
+         * @brief Construct a new AStarM1 object
+         * @param _use_3d This parameter allows the user to choose between 
+         * planning on a plane (8 directions possibles) 
+         * or in the 3D full space (26 directions)
+         * @param _name Algorithm name stored internally
+         * 
+         */
+        AStarSIREN(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Cost Aware A Star M1 object
+         * 
+         * @param _use_3d 
+         */
+        AStarSIREN(bool _use_3d);
+
+    protected:
+
+        /**
+         * @brief Overrided ComputeG function. 
+         * 
+         * @param _current Pointer to the current node
+         * @param _suc Pointer to the successor node
+         * @param _n_i The index of the direction in the directions vector. 
+         * Depending on this index, the distance wi
+         * @param _dirs Number of directions used (to distinguish between 2D and 3D)
+         * @return unsigned int The G Value calculated by the function
+         */
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs, torch::jit::script::Module& loaded_sdf) override;
+
+    };
+
+}
+
+#endif 
diff --git a/include/Planners/LazyThetaStar.hpp b/include/Planners/LazyThetaStar.hpp
@@ -63,14 +63,14 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+        virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
 
         /**
          * @brief SetVertex function
          * Line of sight is checked inside this function
          * @param _s_aux 
          */
-        virtual void SetVertex(Node *_s_aux);
+        virtual void SetVertex(Node *_s_aux, torch::jit::script::Module& loaded_sdf);
 
     };
 

diff --git a/include/Planners/LazyThetaStarM1.hpp b/include/Planners/LazyThetaStarM1.hpp
@@ -49,14 +49,14 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
 
         /**
          * @brief SetVertex function
          * Line of sight is checked inside this function
          * @param _s_aux 
          */
-        virtual void SetVertex(Node *_s_aux) override;
+        virtual void SetVertex(Node *_s_aux, torch::jit::script::Module& loaded_sdf) override;
 
 
         /**

diff --git a/include/Planners/LazyThetaStarM1Mod.hpp b/include/Planners/LazyThetaStarM1Mod.hpp
@@ -49,7 +49,7 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+        virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
 
     };
 

diff --git a/include/Planners/LazyThetaStarM2.hpp b/include/Planners/LazyThetaStarM2.hpp
@@ -58,14 +58,14 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
 
         /**
          * @brief SetVertex function
          * Line of sight is checked inside this function
          * @param _s_aux 
          */
-        virtual void SetVertex(Node *_s_aux);
+        virtual void SetVertex(Node *_s_aux, torch::jit::script::Module& loaded_sdf);
 
         /**
          * @brief 

diff --git a/include/Planners/LazyThetaStarSIREN.hpp b/include/Planners/LazyThetaStarSIREN.hpp
@@ -0,0 +1,79 @@
+#ifndef LAZYTHETASTARSIREN_HPP
+#define LAZYTHETASTARSIREN_HPP
+/**
+ * @file LazyThetaStarSIREN.hpp
+ * @author Guillermo Gil ([email protected])
+ * @author Jose Antonio Cobano ([email protected])
+ * @brief 
+ * @version 0.1
+ * @date 2024-06-27
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#include <Planners/LazyThetaStar.hpp>
+
+namespace Planners
+{
+    /**
+     * @brief Lazy Theta* SIREN Algorithm Class
+     * 
+     */
+    class LazyThetaStarSIREN : public LazyThetaStar
+    {
+
+    public:
+
+        /**
+         * @brief Construct a new Lazy Theta Star SIREN object
+         * 
+         * @param _use_3d This parameter allows the user to choose between planning on a plane 
+         * (8 directions possibles) or in the 3D full space (26 directions)
+         * @param _name Algorithm name stored internally
+         * 
+         */
+        LazyThetaStarSIREN(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Lazy Theta Star SIREN object
+         * 
+         * @param _use_3d 
+         */
+        LazyThetaStarSIREN(bool _use_3d);
+
+    protected:
+
+        /**
+         * @brief Compute cost function of the Lazy version of the algorithm
+         * 
+         * @param _s_aux Pointer to first node
+         * @param _s2_aux Pointer to second node
+         */
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
+
+        /**
+         * @brief SetVertex function
+         * Line of sight is checked inside this function
+         * @param _s_aux 
+         */
+        virtual void SetVertex(Node *_s_aux, torch::jit::script::Module& loaded_sdf) override;
+
+
+        /**
+         * @brief This functions implements the algorithm G function. 
+         * 
+         * @param _current Pointer to the current node
+         * @param _suc Pointer to the successor node
+         * @param _n_i The index of the direction in the directions vector. 
+         * Depending on this index, the distance wi
+         * @param _dirs Number of directions used (to distinguish between 2D and 3D)
+         * @return unsigned int The G Value calculated by the function
+         */
+        virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs, torch::jit::script::Module& loaded_sdf) override;
+
+
+    };
+
+}
+
+#endif
diff --git a/include/Planners/ThetaStar.hpp b/include/Planners/ThetaStar.hpp
@@ -53,15 +53,15 @@ namespace Planners
          * @param _s2 Pointer to node s2
          * @param _index_by_pos reference to openset to erase and insert the nodes in some cases
          */
-        virtual void UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos);
+        virtual void UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos, torch::jit::script::Module& loaded_sdf);
 
         /**
          * @brief Compute cost algorithm function
          * 
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to the second node
          */
-        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux);
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf);
 
         /**
          * @brief This function is the secondary inside the main loop of findPath 

diff --git a/include/Planners/ThetaStarM1.hpp b/include/Planners/ThetaStarM1.hpp
@@ -57,7 +57,7 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to the second node
          */
-        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
 
 
         /**

diff --git a/include/Planners/ThetaStarM2.hpp b/include/Planners/ThetaStarM2.hpp
@@ -57,7 +57,7 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to the second node
          */
-        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
 
         /**
          * @brief Compute edge distance

diff --git a/include/Planners/ThetaStarSIREN.hpp b/include/Planners/ThetaStarSIREN.hpp
@@ -0,0 +1,76 @@
+#ifndef THETASTARSIREN_HPP
+#define THETASTARSIREN_HPP
+/**
+ * @file ThetaStarSIREN.hpp
+ * @author Guillermo Gil ([email protected])
+ * @author Jose Antonio Cobano ([email protected])
+ * 
+ * @brief TBD
+ * 
+ * @version 0.1
+ * @date 2024-06-27
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#include <Planners/ThetaStar.hpp>
+
+namespace Planners
+{
+
+    /**
+     * @brief Theta* SIREN Algorithm Class
+     * 
+     */
+    class ThetaStarSIREN : public ThetaStar
+    {
+
+    public:
+
+        /**
+         * @brief Construct a new Theta Star SIREN object
+         * 
+         * @param _use_3d This parameter allows the user to choose between planning on 
+         * a plane (8 directions possibles) or in the 3D full space (26 directions)
+         * @param _name Algorithm name stored internally
+         * 
+         */
+        ThetaStarSIREN(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Theta Star SIREN object
+         * 
+         * @param _use_3d 
+         */
+        ThetaStarSIREN(bool _use_3d);
+
+
+    protected:
+
+        /**
+         * @brief Compute cost algorithm function
+         * 
+         * @param _s_aux Pointer to first node
+         * @param _s2_aux Pointer to the second node
+         */
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux, torch::jit::script::Module& loaded_sdf) override;
+
+
+        /**
+         * @brief TBD
+         * 
+         *
+         * @param _current Pointer to the current node
+         * @param _suc Pointer to the successor node
+         * @param _n_i The index of the direction in the directions vector. 
+         * Depending on this index, the distance wi
+         * @param _dirs Number of directions used (to distinguish between 2D and 3D)
+         * @return unsigned int The G Value calculated by the function
+         */
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs, torch::jit::script::Module& loaded_sdf) override;
+
+    };
+
+}
+
+#endif
diff --git a/include/utils/SaveDataVariantToFile.hpp b/include/utils/SaveDataVariantToFile.hpp
@@ -64,7 +64,7 @@ namespace Planners
                              "explored_nodes", "path_length", "total_cost1", "total_cost2", "h_cost", "g_cost1", "g_cost2", "c_cost", "grid_cost1", "grid_cost2", "g_final_node",
                              "line_of_sight_checks", "min_dist", "max_dist", "mean_dist", "std_dev",
                              "solved", "cost_weight","max_line_of_sight_cells", "av_curv", "std_dev_curv", "min_curv", "max_curv", 
-                             "av_angles", "std_dev_angles", "min_angle", "max_angle", "angle_changes" }): fields_(_fields)
+                             "av_angles", "std_dev_angles", "min_angle", "max_angle", "angle_changes", "path" }): fields_(_fields)
             {
 
             }

diff --git a/src/Planners/AStar.cpp b/src/Planners/AStar.cpp
@@ -198,7 +198,7 @@ PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target, torch::jit:
     main_timer.tic();
 
     line_of_sight_checks_ = 0;
-    
+
     node_by_cost&     indexByCost          = openSet_.get<IndexByCost>();
     node_by_position& indexByWorldPosition = openSet_.get<IndexByWorldPosition>();