icuas2024

include/Planners/AStarSemantic.hpp

@@ -0,0 +1,66 @@
+#ifndef ASTARSemantic_HPP
+#define ASTARSemantic_HPP
+/**
+ * @file AStarSemantic.hpp
+ * @author Jose Antonio Cobano ([email protected])
+ * 
+ * @brief This algorithm is a variation of the A*. The only
+ * 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 * dist_scale_factor_reduced_);
+ * 
+ * @version 0.1
+ * @date 2024-01-23
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#include <Planners/AStar.hpp>
+
+namespace Planners{
+
+    /**
+     * @brief 
+     * 
+     */
+    class AStarSemantic : public AStar
+    {
+
+    public:
+        /**
+         * @brief Construct a new AStarSemantic 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
+         * 
+         */
+        AStarSemantic(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Cost Aware A Star M1 object
+         * 
+         * @param _use_3d 
+         */
+        AStarSemantic(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) override;
+
+    };
+
+}
+
+#endif 

include/Planners/AStarSemanticCost.hpp

@@ -0,0 +1,66 @@
+#ifndef ASTARSemanticCost_HPP
+#define ASTARSemanticCost_HPP
+/**
+ * @file AStarSemantic.hpp
+ * @author Jose Antonio Cobano ([email protected])
+ * 
+ * @brief This algorithm is a variation of the A*. The only
+ * 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 * dist_scale_factor_reduced_);
+ * 
+ * @version 0.1
+ * @date 2024-01-23
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#include <Planners/AStar.hpp>
+
+namespace Planners{
+
+    /**
+     * @brief 
+     * 
+     */
+    class AStarSemanticCost : public AStar
+    {
+
+    public:
+        /**
+         * @brief Construct a new AStarSemantic 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
+         * 
+         */
+        AStarSemanticCost(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Cost Aware A Star M1 object
+         * 
+         * @param _use_3d 
+         */
+        AStarSemanticCost(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) override;
+
+    };
+
+}
+
+#endif 

include/Planners/LazyThetaStarSemantic.hpp

@@ -0,0 +1,89 @@
+#ifndef LAZYTHETASTARSEMANTIC_HPP
+#define LAZYTHETASTARSEMANTIC_HPP
+/**
+ * @file LazyThetaStarSemantic.hpp
+ * @author Jose Antonio Cobano ([email protected])
+ * @brief 
+ * @version 0.1
+ * @date 2024-01-23
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#include <Planners/ThetaStarSemantic.hpp>
+
+namespace Planners
+{
+    /**
+     * @brief Lazy Theta* Algorithm Class
+     * 
+     */
+    class LazyThetaStarSemantic : public ThetaStarSemantic
+    {
+
+    public:
+
+        /**
+         * @brief Construct a new Semantic Lazy Theta Star 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
+         * 
+         */
+        LazyThetaStarSemantic(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Semantic Lazy Theta Star object
+         * 
+         * @param _use_3d 
+         */
+        LazyThetaStarSemantic(bool _use_3d);
+
+        /**
+         * @brief 
+         * 
+         * @param _source 
+         * @param _target 
+         * @return PathData 
+         */
+        virtual PathData findPath(const Vec3i &_source, const Vec3i &_target) override;
+
+    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) override;
+
+        /**
+         * @brief SetVertex function
+         * Line of sight is checked inside this function
+         * @param _s_aux 
+         */
+        virtual void SetVertex(Node *_s_aux);
+
+        /**
+         * @brief 
+         * 
+         * @param _current 
+         * @param _suc 
+         * @param _n_i 
+         * @param _dirs 
+         * @return unsigned int 
+         */
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+
+        // Variable to ensure that the los is true between the parent of the current and one neighbour, so SetVertex function should not be executed
+        bool los_neighbour_{false}; /*!< TODO Comment */
+        int coef=10;
+
+
+    };
+
+}
+
+#endif

include/Planners/LazyThetaStarSemanticCost.hpp

@@ -0,0 +1,91 @@
+#ifndef LAZYTHETASTARSEMANTICCOST_HPP
+#define LAZYTHETASTARSEMANTICCOST_HPP
+/**
+ * @file LazyThetaStarSemantic.hpp
+ * @author Jose Antonio Cobano ([email protected])
+ * @brief 
+ * @version 0.1
+ * @date 2024-01-23
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#include <Planners/ThetaStarSemanticCost.hpp>
+// #include <Planners/ThetaStarSemantic.hpp>
+
+namespace Planners
+{
+    /**
+     * @brief Lazy Theta* Algorithm Class
+     * 
+     */
+    class LazyThetaStarSemanticCost : public ThetaStarSemanticCost
+    // class LazyThetaStarSemanticCost : public ThetaStarSemantic
+    {
+
+    public:
+
+        /**
+         * @brief Construct a new Semantic Lazy Theta Star 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
+         * 
+         */
+        LazyThetaStarSemanticCost(bool _use_3d, std::string _name );
+
+        /**
+         * @brief Construct a new Semantic Lazy Theta Star object
+         * 
+         * @param _use_3d 
+         */
+        LazyThetaStarSemanticCost(bool _use_3d);
+
+        /**
+         * @brief 
+         * 
+         * @param _source 
+         * @param _target 
+         * @return PathData 
+         */
+        virtual PathData findPath(const Vec3i &_source, const Vec3i &_target) override;
+
+    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) override;
+
+        /**
+         * @brief SetVertex function
+         * Line of sight is checked inside this function
+         * @param _s_aux 
+         */
+        virtual void SetVertex(Node *_s_aux);
+
+        /**
+         * @brief 
+         * 
+         * @param _current 
+         * @param _suc 
+         * @param _n_i 
+         * @param _dirs 
+         * @return unsigned int 
+         */
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+
+        // Variable to ensure that the los is true between the parent of the current and one neighbour, so SetVertex function should not be executed
+        bool los_neighbour_{false}; /*!< TODO Comment */
+        int coef=10;
+
+
+    };
+
+}
+
+#endif