diff --git a/CMakeLists.txt b/CMakeLists.txt index 54e266b..ec85f6d 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,7 @@ generate_messages( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF + LIBRARIES AlgorithmBase AStar AStarSemantic AStarSemanticCost AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarSemantic ThetaStarSemanticCost ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarSemantic LazyThetaStarSemanticCost LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF # CATKIN_DEPENDS std_msgs visualization_msgs geometry_msgs nav_msgs roscpp message_runtime costmap_2d grid_message # DEPENDS system_lib @@ -156,12 +156,18 @@ add_library(AStar src/Planners/AStar.cpp add_library(AStar_Gradient src/Planners/AStar_Gradient.cpp ) add_library(AStar_EDF src/Planners/AStar_EDF.cpp ) +add_library(AStarSemantic src/Planners/AStarSemantic.cpp ) +add_library(AStarSemanticCost src/Planners/AStarSemanticCost.cpp ) add_library(AStarM1 src/Planners/AStarM1.cpp ) add_library(AStarM2 src/Planners/AStarM2.cpp ) add_library(ThetaStar src/Planners/ThetaStar.cpp ) +add_library(ThetaStarSemantic src/Planners/ThetaStarSemantic.cpp ) +add_library(ThetaStarSemanticCost src/Planners/ThetaStarSemanticCost.cpp ) add_library(ThetaStarM1 src/Planners/ThetaStarM1.cpp ) add_library(ThetaStarM2 src/Planners/ThetaStarM2.cpp ) add_library(LazyThetaStar src/Planners/LazyThetaStar.cpp ) +add_library(LazyThetaStarSemantic src/Planners/LazyThetaStarSemantic.cpp ) +add_library(LazyThetaStarSemanticCost src/Planners/LazyThetaStarSemanticCost.cpp ) add_library(LazyThetaStarM1 src/Planners/LazyThetaStarM1.cpp ) add_library(LazyThetaStarM1Mod src/Planners/LazyThetaStarM1Mod.cpp ) add_library(LazyThetaStarM2 src/Planners/LazyThetaStarM2.cpp ) @@ -170,7 +176,7 @@ add_library(LazyThetaStar_EDF src/Planners/LazyThetaStar_EDF.cpp ) -list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF) +list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarSemantic AStarSemanticCost AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarSemantic ThetaStarSemanticCost ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarSemantic LazyThetaStarSemanticCost LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF) target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies( ${${PROJECT_NAME}_LIBRARIES} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) list(APPEND ${PROJECT_NAME}_TARGETS ${${PROJECT_NAME}_LIBRARIES}) @@ -232,4 +238,4 @@ else() ) endif() -unset(BUILD_ROS_SUPPORT CACHE) +unset(BUILD_ROS_SUPPORT CACHE) \ No newline at end of file diff --git a/include/Planners/AStarSemantic.hpp b/include/Planners/AStarSemantic.hpp new file mode 100755 index 0000000..689a18e --- /dev/null +++ b/include/Planners/AStarSemantic.hpp @@ -0,0 +1,66 @@ +#ifndef ASTARSemantic_HPP +#define ASTARSemantic_HPP +/** + * @file AStarSemantic.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @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(cost_weight_ * _suc * dist_scale_factor_reduced_); + * + * @version 0.1 + * @date 2024-01-23 + * + * @copyright Copyright (c) 2021 + * + */ +#include + +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 diff --git a/include/Planners/AStarSemanticCost.hpp b/include/Planners/AStarSemanticCost.hpp new file mode 100755 index 0000000..48b3be1 --- /dev/null +++ b/include/Planners/AStarSemanticCost.hpp @@ -0,0 +1,66 @@ +#ifndef ASTARSemanticCost_HPP +#define ASTARSemanticCost_HPP +/** + * @file AStarSemantic.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @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(cost_weight_ * _suc * dist_scale_factor_reduced_); + * + * @version 0.1 + * @date 2024-01-23 + * + * @copyright Copyright (c) 2021 + * + */ +#include + +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 diff --git a/include/Planners/LazyThetaStarSemantic.hpp b/include/Planners/LazyThetaStarSemantic.hpp new file mode 100755 index 0000000..bd4202d --- /dev/null +++ b/include/Planners/LazyThetaStarSemantic.hpp @@ -0,0 +1,89 @@ +#ifndef LAZYTHETASTARSEMANTIC_HPP +#define LAZYTHETASTARSEMANTIC_HPP +/** + * @file LazyThetaStarSemantic.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * @brief + * @version 0.1 + * @date 2024-01-23 + * + * @copyright Copyright (c) 2024 + * + */ +#include + +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=3; + + + }; + +} + +#endif diff --git a/include/Planners/LazyThetaStarSemanticCost.hpp b/include/Planners/LazyThetaStarSemanticCost.hpp new file mode 100755 index 0000000..64e7778 --- /dev/null +++ b/include/Planners/LazyThetaStarSemanticCost.hpp @@ -0,0 +1,91 @@ +#ifndef LAZYTHETASTARSEMANTICCOST_HPP +#define LAZYTHETASTARSEMANTICCOST_HPP +/** + * @file LazyThetaStarSemantic.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * @brief + * @version 0.1 + * @date 2024-01-23 + * + * @copyright Copyright (c) 2024 + * + */ +#include +// #include + +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=3; + + + }; + +} + +#endif diff --git a/include/Planners/ThetaStarSemantic.hpp b/include/Planners/ThetaStarSemantic.hpp new file mode 100755 index 0000000..6e12c1f --- /dev/null +++ b/include/Planners/ThetaStarSemantic.hpp @@ -0,0 +1,95 @@ +#ifndef THETASTARSEMANTIC_HPP +#define THETASTARSEMANTIC_HPP +/** + * @file ThetaStarSemantic.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This header declares the functions and class + * associated to the Semantic Theta* Algorithm. It inherits + * from the original Theta* algorithm and override two functions: + * 1. ComputeCost + * 2. ComputeG + * + * @version 0.1 + * @date 2024-01-23 + * + * @copyright Copyright (c) 2021 + * + */ +#include + +namespace Planners +{ + + /** + * @brief Theta* Algorithm Class + * + */ + class ThetaStarSemantic : public ThetaStar + { + + public: + + /** + * @brief Construct a new Semantic 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 + * + */ + ThetaStarSemantic(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Semantic Theta Star object + * + * @param _use_3d + */ + ThetaStarSemantic(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) override; + + /** + * @brief Compute edge distance + * + * @param _checked_nodes + * @param _s + * @param _s2 + * @return unsigned int + */ + virtual unsigned int ComputeSemanticEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Node* _s, const Node* _s2); + + + /** + * @brief This functions implements the algorithm G function. The difference + * with the original A* function is that the returned G value here is composed of three terms: + * 1. Dist between the current and successor (1, sqrt(2) or sqrt(3)) + * 2. The G value of the current node (How does it cost to reach to the current) + * 3. The edge_neighbour cost which is calculated as the averaged between the _current and _suc cost + * scaled with the value dist_scale_factor_reduced_ + * + * + * @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) override; + int coef=10; + + }; + +} + +#endif diff --git a/include/Planners/ThetaStarSemanticCost.hpp b/include/Planners/ThetaStarSemanticCost.hpp new file mode 100755 index 0000000..21b2cdf --- /dev/null +++ b/include/Planners/ThetaStarSemanticCost.hpp @@ -0,0 +1,95 @@ +#ifndef THETASTARSEMANTICCOST_HPP +#define THETASTARSEMANTICCOST_HPP +/** + * @file ThetaStarSemantic.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This header declares the functions and class + * associated to the Semantic Theta* Algorithm. It inherits + * from the original Theta* algorithm and override two functions: + * 1. ComputeCost + * 2. ComputeG + * + * @version 0.1 + * @date 2024-01-23 + * + * @copyright Copyright (c) 2021 + * + */ +#include + +namespace Planners +{ + + /** + * @brief Theta* Algorithm Class + * + */ + class ThetaStarSemanticCost : public ThetaStar + { + + public: + + /** + * @brief Construct a new Semantic 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 + * + */ + ThetaStarSemanticCost(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Semantic Theta Star object + * + * @param _use_3d + */ + ThetaStarSemanticCost(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) override; + + /** + * @brief Compute edge distance + * + * @param _checked_nodes + * @param _s + * @param _s2 + * @return unsigned int + */ + virtual unsigned int ComputeSemanticEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Node* _s, const Node* _s2); + + + /** + * @brief This functions implements the algorithm G function. The difference + * with the original A* function is that the returned G value here is composed of three terms: + * 1. Dist between the current and successor (1, sqrt(2) or sqrt(3)) + * 2. The G value of the current node (How does it cost to reach to the current) + * 3. The edge_neighbour cost which is calculated as the averaged between the _current and _suc cost + * scaled with the value dist_scale_factor_reduced_ + * + * + * @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) override; + int coef=10; + + }; + +} + +#endif diff --git a/launch/simulator_atlas.launch b/launch/simulator_atlas.launch index 5339339..2dc8792 100755 --- a/launch/simulator_atlas.launch +++ b/launch/simulator_atlas.launch @@ -49,7 +49,7 @@ - + diff --git a/resources/3dmaps/Atlas_8_puertas_02.bt b/resources/3dmaps/Atlas_8_puertas_02.bt new file mode 100644 index 0000000..b323c08 Binary files /dev/null and b/resources/3dmaps/Atlas_8_puertas_02.bt differ diff --git a/src/Planners/AStarSemantic.cpp b/src/Planners/AStarSemantic.cpp new file mode 100644 index 0000000..d7ff729 --- /dev/null +++ b/src/Planners/AStarSemantic.cpp @@ -0,0 +1,148 @@ +#include "Planners/AStarSemantic.hpp" + +namespace Planners{ + +AStarSemantic::AStarSemantic(bool _use_3d):AStar(_use_3d, "astarsemantic") {} +AStarSemantic::AStarSemantic(bool _use_3d, std::string _name = "astarsemantic" ):AStar(_use_3d, _name) {} + +// // Cost Aware AStar +// inline unsigned int AStarSemantic::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + +// unsigned int cost = _current->G; + +// if(_dirs == 8){ +// cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient +// }else{ +// cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient +// } + +// // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); +// // IROS +// // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc +// // RA-L +// auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); +// cost += cost_term; +// _suc->C = cost_term; + +// return cost; +// } + +inline unsigned int AStarSemantic::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + unsigned int cost = _current->G; + unsigned int cost2 = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + c_wall=1; + c_door=3; + c_colum=2.5; + c_furnish=1; + c_stair=1.5; + c_panel=1.5; + c_lamp=2; + c_glass=2; + + // c_wall=1; + // c_door=1; + // c_colum=1; + // c_furnish=1; + // c_stair=1; + // c_panel=1; + // c_lamp=1; + // c_glass=1; + + + if(_dirs == 8){ + cost2 = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost2 = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // if (_suc->semantic != 1){ + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + // } + + // SEMANTIC COST + // Wall + if (_suc->semantic == 1){ + cost2=c_wall*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (_suc->semantic == 2){ + cost2=c_door*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + + // Colum + else if (_suc->semantic == 3){ + cost2=c_colum*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Furnishing + else if (_suc->semantic == 4){ + cost2=c_furnish*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Stairs + else if (_suc->semantic == 5){ + cost2=c_stair*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Panels --> Barandilla + else if (_suc->semantic == 6){ + cost2=c_panel*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Lamp + else if (_suc->semantic == 7){ + cost2=c_lamp*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Glass wall + else if (_suc->semantic == 8){ + cost2=c_glass*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + + cost = cost + cost2; + + _suc->C = _suc->cost; + return cost; +} +} diff --git a/src/Planners/AStarSemanticCost.cpp b/src/Planners/AStarSemanticCost.cpp new file mode 100644 index 0000000..376fee5 --- /dev/null +++ b/src/Planners/AStarSemanticCost.cpp @@ -0,0 +1,148 @@ +#include "Planners/AStarSemanticCost.hpp" + +namespace Planners{ + +AStarSemanticCost::AStarSemanticCost(bool _use_3d):AStar(_use_3d, "astarsemantic_cost") {} +AStarSemanticCost::AStarSemanticCost(bool _use_3d, std::string _name = "astarsemantic_cost" ):AStar(_use_3d, _name) {} + +// // Cost Aware AStar +// inline unsigned int AStarSemantic::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + +// unsigned int cost = _current->G; + +// if(_dirs == 8){ +// cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient +// }else{ +// cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient +// } + +// // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); +// // IROS +// // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc +// // RA-L +// auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); +// cost += cost_term; +// _suc->C = cost_term; + +// return cost; +// } + +inline unsigned int AStarSemanticCost::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + unsigned int cost = _current->G; + unsigned int cost2 = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + c_wall=1; + c_door=3; + c_colum=2.5; + c_furnish=1; + c_stair=1.5; + c_panel=1.5; + c_lamp=2; + c_glass=2; + + // c_wall=1; + // c_door=1; + // c_colum=1; + // c_furnish=1; + // c_stair=1; + // c_panel=1; + // c_lamp=1; + // c_glass=1; + + + if(_dirs == 8){ + cost2 = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost2 = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // if (_suc->semantic != 1){ + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + // } + + // SEMANTIC COST + // Wall + if (_suc->semantic == 1){ + cost2=c_wall*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (_suc->semantic == 2){ + cost2=c_door*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + + // Colum + else if (_suc->semantic == 3){ + cost2=c_colum*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Furnishing + else if (_suc->semantic == 4){ + cost2=c_furnish*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Stairs + else if (_suc->semantic == 5){ + cost2=c_stair*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Panels --> Barandilla + else if (_suc->semantic == 6){ + cost2=c_panel*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Lamp + else if (_suc->semantic == 7){ + cost2=c_lamp*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Glass wall + else if (_suc->semantic == 8){ + cost2=c_glass*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + + cost = cost + cost2; + + _suc->C = _suc->cost; + return cost; +} +} diff --git a/src/Planners/LazyThetaStarSemantic.cpp b/src/Planners/LazyThetaStarSemantic.cpp new file mode 100644 index 0000000..d6ef0a5 --- /dev/null +++ b/src/Planners/LazyThetaStarSemantic.cpp @@ -0,0 +1,297 @@ +#include "Planners/LazyThetaStarSemantic.hpp" + +namespace Planners +{ + LazyThetaStarSemantic::LazyThetaStarSemantic(bool _use_3d):ThetaStarSemantic(_use_3d, "lazythetastar_semantic") {} + LazyThetaStarSemantic::LazyThetaStarSemantic(bool _use_3d, std::string _name = "lazythetastar_semantic" ):ThetaStarSemantic(_use_3d, _name) {} + + void LazyThetaStarSemantic::SetVertex(Node *_s_aux) + { + + // line_of_sight_checks_++; + // if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + if( !los_neighbour_ ){ + + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + // Semantic cost of the succesor + + unsigned int cost_semantic = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef, c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=1; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + // c_stair=1; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + auto distance = geometry::distanceBetween2Nodes(successor2, _s_aux); + + // SEMANTIC COST + // Wall + if (successor2->semantic == 1){ + cost_semantic=c_wall*distance; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (successor2->semantic == 2){ + cost_semantic=c_door*distance; + } + + // Colum + else if (successor2->semantic == 3){ + cost_semantic=c_colum*distance; + } + // Furnishing + else if (successor2->semantic == 4){ + cost_semantic=c_furnish*distance; + } + // Stairs + else if (successor2->semantic == 5){ + cost_semantic=c_stair*distance; + } + // Panels --> Barandilla + else if (successor2->semantic == 6){ + cost_semantic=c_panel*distance; + } + // Lamp + else if (successor2->semantic == 7){ + cost_semantic=c_lamp*distance; + } + // Glass wall + else if (successor2->semantic == 8){ + cost_semantic=c_glass*distance; + } + else + cost_semantic=distance; + + G_new = successor2->G + cost_semantic; + if (G_new < G_max) + { + G_max = G_new; + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_semantic; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + los_neighbour_ = false; + } + + inline void LazyThetaStarSemantic::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + + // // WITHOUT CONSIDERING A MAXIMUM LINE OF SIGHT + // line_of_sight_checks_++; + // if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) { + + // los_neighbour_ = true; + + // auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // auto edge2 = ComputeSemanticEdgeCost(checked_nodes, _s_aux->parent, _s2_aux); + // if (edge2 == 0){ + // edge2 = 1; + // } + + // if ( ( _s_aux->parent->G + (dist2 * edge2) ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + (dist2 * edge2); + // _s2_aux->C = dist2 * edge2; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // } + // } + // checked_nodes->clear(); + + // CONSIDERIN A MAXIMUM LINE OF SIGHT --> CORRECT + line_of_sight_checks_++; + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + // Line of sight + else{ + if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) { + + los_neighbour_ = true; + + auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + auto edge2 = ComputeSemanticEdgeCost(checked_nodes, _s_aux->parent, _s2_aux); + if (edge2 == 0){ + edge2 = 1; + } + + if ( ( _s_aux->parent->G + (dist2 * edge2) ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s2_aux->parent->G + (dist2 * edge2); + _s2_aux->C = dist2 * edge2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + checked_nodes->clear(); + } + + inline unsigned int LazyThetaStarSemantic::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = 0; + unsigned int cost = _current->G; + unsigned int cost2 = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef, c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=1; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + // c_stair=1; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + if(_dirs == 8){ + cost2 = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost2 = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // double cc = ( _current->cost + _suc->cost ) / 2; + // auto edge_neighbour = static_cast( cc * cost_weight_ * dist_scale_factor_reduced_); + + // cost += ( _current->G + edge_neighbour ); + + // _suc->C = edge_neighbour; + + // SEMANTIC COST + // Wall + if (_suc->semantic == 1){ + cost2=c_wall*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (_suc->semantic == 2){ + cost2=c_door*cost2; + } + + // Colum + else if (_suc->semantic == 3){ + cost2=c_colum*cost2; + } + // Furnishing + else if (_suc->semantic == 4){ + cost2=c_furnish*cost2; + } + // Stairs + else if (_suc->semantic == 5){ + cost2=c_stair*cost2; + } + // Panels --> Barandilla + else if (_suc->semantic == 6){ + cost2=c_panel*cost2; + } + // Lamp + else if (_suc->semantic == 7){ + cost2=c_lamp*cost2; + } + // Glass wall + else if (_suc->semantic == 8){ + cost2=c_glass*cost2; + } + + cost = cost + cost2; + + _suc->C = _suc->cost; + + return cost; + } + + PathData LazyThetaStarSemantic::findPath(const Vec3i &_source, const Vec3i &_target) + { + utils::Clock main_timer; + main_timer.tic(); + + MagicalMultiSet openSet; + + Node *current = nullptr; + line_of_sight_checks_ = 0; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + + node_by_cost& indexByCost = openSet.get(); + node_by_position& indexByWorldPosition = openSet.get(); + + indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) + { + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) + { + solved = true; + break; + } + closedSet_.push_back(current); + + current->isInOpenList = false; + current->isInClosedList = true; + + SetVertex(current); +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + exploreNeighbours(current, _target, indexByWorldPosition); + } + main_timer.toc(); + PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), + solved, _source, line_of_sight_checks_); + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; + } +} diff --git a/src/Planners/LazyThetaStarSemanticCost.cpp b/src/Planners/LazyThetaStarSemanticCost.cpp new file mode 100644 index 0000000..2675a65 --- /dev/null +++ b/src/Planners/LazyThetaStarSemanticCost.cpp @@ -0,0 +1,332 @@ +#include "Planners/LazyThetaStarSemanticCost.hpp" + +namespace Planners +{ + LazyThetaStarSemanticCost::LazyThetaStarSemanticCost(bool _use_3d):ThetaStarSemanticCost(_use_3d, "lazythetastar_semantic_cost") {} + // LazyThetaStarSemanticCost::LazyThetaStarSemanticCost(bool _use_3d):ThetaStarSemantic(_use_3d, "lazythetastar_semantic_cost") {} + LazyThetaStarSemanticCost::LazyThetaStarSemanticCost(bool _use_3d, std::string _name = "lazythetastar_semantic_cost" ):ThetaStarSemanticCost(_use_3d, _name) {} + // LazyThetaStarSemanticCost::LazyThetaStarSemanticCost(bool _use_3d, std::string _name = "lazythetastar_semantic_cost" ):ThetaStarSemantic(_use_3d, _name) {} + + void LazyThetaStarSemanticCost::SetVertex(Node *_s_aux) + { + + // line_of_sight_checks_++; + // if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + if( !los_neighbour_ ){ + + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + // Semantic cost of the succesor + + unsigned int cost_semantic = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + float coste; + // float coef,c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=1; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + // coste=1+static_cast(successor2->cost); + coste=static_cast(successor2->cost); + + auto distance = geometry::distanceBetween2Nodes(successor2, _s_aux); + + // Wall + if (successor2->semantic == 1){ + // cost_semantic=(c_wall/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_wall/coste)*distance; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (successor2->semantic == 2){ + // cost_semantic=(c_door/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_door/coste)*distance; + } + + // Colum + else if (successor2->semantic == 3){ + // cost_semantic=(c_colum/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_colum/coste)*distance; + } + // Furnishing + else if (successor2->semantic == 4){ + // cost_semantic=(c_furnish/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_furnish/coste)*distance; + } + // Stairs + else if (successor2->semantic == 5){ + // cost_semantic=(c_stair/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_stair/coste)*distance; + } + // Panels --> Barandilla + else if (successor2->semantic == 6){ + // cost_semantic=(c_panel/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_panel/coste)*distance; + } + // Lamp + else if (successor2->semantic == 7){ + // cost_semantic=(c_lamp/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_lamp/coste)*distance; + } + // Glass wall + else if (successor2->semantic == 8){ + // cost_semantic=(c_glass/(1+static_cast(successor2->cost)))*distance; // + distance; + cost_semantic=(c_glass/coste)*distance; + } + else + cost_semantic=distance; + + G_new = successor2->G + cost_semantic; + if (G_new < G_max) + { + G_max = G_new; + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_semantic; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + los_neighbour_ = false; + } + + inline void LazyThetaStarSemanticCost::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // // WITHOUT CONSIDERING A MAXIMUM LINE OF SIGHT + // line_of_sight_checks_++; + // if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) { + + // los_neighbour_ = true; + + // auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // auto edge2 = ComputeSemanticEdgeCost(checked_nodes, _s_aux->parent, _s2_aux); + // if (edge2 == 0){ + // edge2 = 1; + // } + + // if ( ( _s_aux->parent->G + (dist2 * edge2) ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + (dist2 * edge2); + // _s2_aux->C = dist2 * edge2; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // } + // } + // checked_nodes->clear(); + + // CONSIDERIN A MAXIMUM LINE OF SIGHT --> CORRECT + line_of_sight_checks_++; + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + // Line of sight + else{ + if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) { + + los_neighbour_ = true; + + auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + auto edge2 = ComputeSemanticEdgeCost(checked_nodes, _s_aux->parent, _s2_aux); + if (edge2 == 0){ + edge2 = 1; + } + + if ( ( _s_aux->parent->G + (dist2 * edge2) ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s2_aux->parent->G + (dist2 * edge2); + _s2_aux->C = dist2 * edge2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + checked_nodes->clear(); + } + + inline unsigned int LazyThetaStarSemanticCost::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = 0; + unsigned int cost = _current->G; + unsigned int cost2 = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef,c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=1; + // std::cout << "coef: " << coef << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + if(_dirs == 8){ + cost2 = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost2 = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + float coste; + // coste=1+static_cast(_suc->cost); + coste=static_cast(_suc->cost); + + // double cc = ( _current->cost + _suc->cost ) / 2; + // auto edge_neighbour = static_cast( cc * cost_weight_ * dist_scale_factor_reduced_); + + // cost += ( _current->G + edge_neighbour ); + + // _suc->C = edge_neighbour; + + // SEMANTIC COST + // std::cout << "c_colum: " << c_colum << std::endl; + // std::cout << "cost: " << static_cast(_suc->cost) << std::endl; + // std::cout << "divisin: " << (c_colum/static_cast(_suc->cost)) << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + // Wall + if (_suc->semantic == 1){ + // cost2=(c_wall/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_wall/coste)*cost2; + + // cost2=(c_wall/static_cast(_suc->cost))*cost2 + cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (_suc->semantic == 2){ + // cost2=(c_door/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_door/coste)*cost2; + } + + // Colum + else if (_suc->semantic == 3){ + // cost2=(c_colum/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_colum/coste)*cost2; + } + // Furnishing + else if (_suc->semantic == 4){ + // cost2=(c_furnish/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_furnish/coste)*cost2; + } + // Stairs + else if (_suc->semantic == 5){ + // cost2=(c_stair/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_stair/coste)*cost2; + } + // Panels --> Barandilla + else if (_suc->semantic == 6){ + // cost2=(c_panel/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_panel/coste)*cost2; + } + // Lamp + else if (_suc->semantic == 7){ + // cost2=(c_lamp/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_lamp/coste)*cost2; + } + // Glass wall + else if (_suc->semantic == 8){ + // cost2=(c_glass/(1+static_cast(_suc->cost)))*cost2; // + cost2; + cost2=(c_glass/coste)*cost2; + } + + cost = cost + cost2; + + _suc->C = _suc->cost; + + return cost; + } + + PathData LazyThetaStarSemanticCost::findPath(const Vec3i &_source, const Vec3i &_target) + { + utils::Clock main_timer; + main_timer.tic(); + + MagicalMultiSet openSet; + + Node *current = nullptr; + line_of_sight_checks_ = 0; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + + node_by_cost& indexByCost = openSet.get(); + node_by_position& indexByWorldPosition = openSet.get(); + + indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) + { + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) + { + solved = true; + break; + } + closedSet_.push_back(current); + + current->isInOpenList = false; + current->isInClosedList = true; + + SetVertex(current); +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + exploreNeighbours(current, _target, indexByWorldPosition); + } + main_timer.toc(); + PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), + solved, _source, line_of_sight_checks_); + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; + } +} diff --git a/src/Planners/ThetaStarSemantic.cpp b/src/Planners/ThetaStarSemantic.cpp new file mode 100644 index 0000000..f6201e2 --- /dev/null +++ b/src/Planners/ThetaStarSemantic.cpp @@ -0,0 +1,276 @@ +#include "Planners/ThetaStarSemantic.hpp" + +namespace Planners +{ + ThetaStarSemantic::ThetaStarSemantic(bool _use_3d):ThetaStar(_use_3d, "thetastar_semantic") {} + ThetaStarSemantic::ThetaStarSemantic(bool _use_3d, std::string _name = "thetastar_semantic" ):ThetaStar(_use_3d, _name) {} + + inline void ThetaStarSemantic::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + line_of_sight_checks_++; + if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) + { + auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + auto edge2 = ComputeSemanticEdgeCost(checked_nodes, _s_aux->parent, _s2_aux); + if (edge2 == 0) + edge2 = 1; + + line_of_sight_checks_++; + LineOfSight::bresenham3D(_s_aux, _s2_aux, discrete_world_, checked_nodes_current); + + auto dist1 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + auto edge1 = ComputeSemanticEdgeCost(checked_nodes_current, _s_aux, _s2_aux); + if (edge1 == 0) + edge1 = 1; + + if ( ( _s_aux->parent->G + (dist2 * edge2) ) < ( _s_aux->G + (dist1 * edge1))) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + (dist2 * edge2); // This is the same than A* + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + _s2_aux->C = edge2 * dist2; + } + else{ + _s2_aux->parent =_s_aux; + _s2_aux->G = _s_aux->G + (dist1 * edge1); // This is the same than A* + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + _s2_aux->C = edge1 * dist1; + } + } else { + + _s2_aux->parent=_s_aux; + + line_of_sight_checks_++; + LineOfSight::bresenham3D(_s_aux, _s2_aux, discrete_world_, checked_nodes); + + auto dist1 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + auto edge1 = ComputeSemanticEdgeCost(checked_nodes, _s_aux, _s2_aux); + if (edge1 == 0) + edge1 = 1; + + _s2_aux->G = _s_aux->G + (dist1 * edge1); // This is the same than A* + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + _s2_aux->C = edge1 * dist1; + } + checked_nodes->clear(); + checked_nodes_current->clear(); + } + + inline unsigned int ThetaStarSemantic::ComputeSemanticEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Node* _s, const Node* _s2){ + + double cost_semantic{0}; + double mean_cost_semantic{0}; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef, c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=1; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + // c_stair=1; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + auto n_checked_nodes = _checked_nodes->size(); + + if( n_checked_nodes >= 1 ){ + // std::cout << "Nodes: " << n_checked_nodes << std::endl; + for(auto &it: *_checked_nodes){ + // if (discrete_world_.getNodePtr(it)->semantic !=0){ + // std::cout << "COSTE: " << discrete_world_.getNodePtr(it)->cost << std::endl; + // std::cout << "COSTE SEMANTICO: " << discrete_world_.getNodePtr(it)->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + // } + + if (discrete_world_.getNodePtr(it)->semantic == 1) + cost_semantic += c_wall; + else if (discrete_world_.getNodePtr(it)->semantic == 2) + cost_semantic += c_door; + else if (discrete_world_.getNodePtr(it)->semantic == 3) + cost_semantic += c_colum; + else if (discrete_world_.getNodePtr(it)->semantic == 4) + cost_semantic += c_furnish; + else if (discrete_world_.getNodePtr(it)->semantic == 5) + cost_semantic += c_stair; + else if (discrete_world_.getNodePtr(it)->semantic == 6) + cost_semantic += c_panel; + else if (discrete_world_.getNodePtr(it)->semantic == 7) + cost_semantic += c_lamp; + else if (discrete_world_.getNodePtr(it)->semantic == 8) + cost_semantic += c_glass; + + } + } + // if (cost_semantic != 0){ + // std::cout << "cost_semantic: " << cost_semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + // } + + if( n_checked_nodes >= 1){ + if (_s->semantic == 1) + cost_semantic += c_wall; + else if (_s->semantic == 2) + cost_semantic += c_door; + else if (_s->semantic == 3) + cost_semantic += c_colum; + else if (_s->semantic == 4) + cost_semantic += c_furnish; + else if (_s->semantic == 5) + cost_semantic += c_stair; + else if (_s->semantic == 6) + cost_semantic += c_panel; + else if (_s->semantic == 7) + cost_semantic += c_lamp; + else if (_s->semantic == 8) + cost_semantic += c_glass; + + if (_s2->semantic == 1) + cost_semantic += c_wall; + else if (_s2->semantic == 2) + cost_semantic += c_door; + else if (_s2->semantic == 3) + cost_semantic += c_colum; + else if (_s2->semantic == 4) + cost_semantic += c_furnish; + else if (_s2->semantic == 5) + cost_semantic += c_stair; + else if (_s2->semantic == 6) + cost_semantic += c_panel; + else if (_s2->semantic == 7) + cost_semantic += c_lamp; + else if (_s2->semantic == 8) + cost_semantic += c_glass; + mean_cost_semantic = cost_semantic/(n_checked_nodes+1); // Dividir entre el numero de nodos + // mean_cost_semantic = cost_semantic/(n_checked_nodes+1); } + } + // checked_nodes == 0 + else{ + cost_semantic = 0; + if (_s->semantic == 1) + cost_semantic += c_wall; + else if (_s->semantic == 2) + cost_semantic += c_door; + else if (_s->semantic == 3) + cost_semantic += c_colum; + else if (_s->semantic == 4) + cost_semantic += c_furnish; + else if (_s->semantic == 5) + cost_semantic += c_stair; + else if (_s->semantic == 6) + cost_semantic += c_panel; + else if (_s->semantic == 7) + cost_semantic += c_lamp; + else if (_s->semantic == 8) + cost_semantic += c_glass; + else + cost_semantic=0; + + if (_s2->semantic == 1) + cost_semantic += c_wall; + else if (_s2->semantic == 2) + cost_semantic += c_door; + else if (_s2->semantic == 3) + cost_semantic += c_colum; + else if (_s2->semantic == 4) + cost_semantic += c_furnish; + else if (_s2->semantic == 5) + cost_semantic += c_stair; + else if (_s2->semantic == 6) + cost_semantic += c_panel; + else if (_s2->semantic == 7) + cost_semantic += c_lamp; + else if (_s2->semantic == 8) + cost_semantic += c_glass; + else + cost_semantic=0; + mean_cost_semantic = cost_semantic/2; // Dividir entre el numero de nodos + // mean_cost_semantic = cost_semantic; + } + // return static_cast( mean_cost_semantic * dist_scale_factor_reduced_); + // std::cout << "edge: " << mean_cost_semantic << std::endl; + return static_cast( mean_cost_semantic); + } + + inline unsigned int ThetaStarSemantic::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + unsigned int cost2 = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef, c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=1; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + // c_stair=1; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + if(_dirs == 8){ + cost2 += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost2 += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // EDF COST --> ThetarStarM1 + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + // cost += cost_term; + // _suc->C = cost_term; + + // SEMANTIC COST + // Wall + if (_suc->semantic == 1){ + cost2=c_wall*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (_suc->semantic == 2){ + cost2=c_door*cost2; + } + + // Colum + else if (_suc->semantic == 3){ + cost2=c_colum*cost2; + } + // Furnishing + else if (_suc->semantic == 4){ + cost2=c_furnish*cost2; + } + // Stairs + else if (_suc->semantic == 5){ + cost2=c_stair*cost2; + } + // Panels --> Barandilla + else if (_suc->semantic == 6){ + cost2=c_panel*cost2; + } + // Lamp + else if (_suc->semantic == 7){ + cost2=c_lamp*cost2; + } + // Glass wall + else if (_suc->semantic == 8){ + cost2=c_glass*cost2; + } + + cost = cost + cost2; + + _suc->C = _suc->cost; + + return cost; + } +} diff --git a/src/Planners/ThetaStarSemanticCost.cpp b/src/Planners/ThetaStarSemanticCost.cpp new file mode 100644 index 0000000..2abea31 --- /dev/null +++ b/src/Planners/ThetaStarSemanticCost.cpp @@ -0,0 +1,345 @@ +#include "Planners/ThetaStarSemanticCost.hpp" + +namespace Planners +{ + ThetaStarSemanticCost::ThetaStarSemanticCost(bool _use_3d):ThetaStar(_use_3d, "thetastar_semantic_cost") {} + ThetaStarSemanticCost::ThetaStarSemanticCost(bool _use_3d, std::string _name = "thetastar_semantic_cost" ):ThetaStar(_use_3d, _name) {} + + inline void ThetaStarSemanticCost::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + line_of_sight_checks_++; + if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) + { + auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + auto edge2 = ComputeSemanticEdgeCost(checked_nodes, _s_aux->parent, _s2_aux); + if (edge2 == 0) + edge2 = 1; + + line_of_sight_checks_++; + LineOfSight::bresenham3D(_s_aux, _s2_aux, discrete_world_, checked_nodes_current); + + auto dist1 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + auto edge1 = ComputeSemanticEdgeCost(checked_nodes_current, _s_aux, _s2_aux); + if (edge1 == 0) + edge1 = 1; + + if ( ( _s_aux->parent->G + (dist2 * edge2) ) < ( _s_aux->G + (dist1 * edge1))) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + (dist2 * edge2);// + dist2; // This is the same than A* + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + _s2_aux->C = edge2 * dist2; + } + else{ + _s2_aux->parent =_s_aux; + _s2_aux->G = _s_aux->G + (dist1 * edge1);// + dist1; // This is the same than A* + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + _s2_aux->C = edge1 * dist1; + } + } else { + + _s2_aux->parent=_s_aux; + + line_of_sight_checks_++; + LineOfSight::bresenham3D(_s_aux, _s2_aux, discrete_world_, checked_nodes); + + auto dist1 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + auto edge1 = ComputeSemanticEdgeCost(checked_nodes, _s_aux, _s2_aux); + if (edge1 == 0) + edge1 = 1; + + _s2_aux->G = _s_aux->G + (dist1 * edge1);// + dist1; // This is the same than A* + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + _s2_aux->C = edge1 * dist1; + } + checked_nodes->clear(); + checked_nodes_current->clear(); + } + + inline unsigned int ThetaStarSemanticCost::ComputeSemanticEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Node* _s, const Node* _s2){ + + double cost_semantic{0}; + double mean_cost_semantic{0}; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef, c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=10; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + // Compute semantic cost depending on the distance along of the segment considering all the crossed nodes + auto n_checked_nodes = _checked_nodes->size(); + // std::cout << "Nodes: " << n_checked_nodes << std::endl; + if( n_checked_nodes >= 1 ){ + // std::cout << "Nodes: " << n_checked_nodes << std::endl; + for(auto &it: *_checked_nodes){ + // if (discrete_world_.getNodePtr(it)->semantic !=0){ + // std::cout << "COSTE: " << discrete_world_.getNodePtr(it)->cost << std::endl; + // std::cout << "COSTE SEMANTICO: " << discrete_world_.getNodePtr(it)->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + // } + if (discrete_world_.getNodePtr(it)->semantic == 1) + cost_semantic += c_wall/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_wall/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 2) + cost_semantic += c_door/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_door/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 3) + cost_semantic += c_colum/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_colum/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 4) + cost_semantic += c_furnish/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_furnish/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 5) + cost_semantic += c_stair/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_stair/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 6) + cost_semantic += c_panel/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_panel/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 7) + cost_semantic += c_lamp/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_lamp/(1+discrete_world_.getNodePtr(it)->cost); + else if (discrete_world_.getNodePtr(it)->semantic == 8) + cost_semantic += c_glass/discrete_world_.getNodePtr(it)->cost; + // cost_semantic += c_glass/(1+discrete_world_.getNodePtr(it)->cost); + // std::cout << "cost_semantic: " << cost_semantic << std::endl; + } + } + + float coste1, coste2; + // coste1=1+_s->cost; + coste1=_s->cost; + // coste2=1+_s2->cost; + coste2=_s2->cost; + + if( n_checked_nodes >= 1){ + if (_s->semantic == 1) + // cost_semantic += c_wall/_s->cost; + // cost_semantic += c_wall/(1+_s->cost); + cost_semantic += c_wall/(coste1); + else if (_s->semantic == 2) + // cost_semantic += c_door/_s->cost; + // cost_semantic += c_door/(1+_s->cost); + cost_semantic += c_door/(coste1); + else if (_s->semantic == 3) + // cost_semantic += c_colum/_s->cost; + // cost_semantic += c_colum/(1+_s->cost); + cost_semantic += c_colum/(coste1); + else if (_s->semantic == 4) + // cost_semantic += c_furnish/_s->cost; + // cost_semantic += c_furnish/(1+_s->cost); + cost_semantic += c_furnish/(coste1); + else if (_s->semantic == 5) + // cost_semantic += c_stair/_s->cost; + // cost_semantic += c_stair/(1+_s->cost); + cost_semantic += c_stair/(coste1); + else if (_s->semantic == 6) + // cost_semantic += c_panel/_s->cost; + // cost_semantic += c_panel/(1+_s->cost); + cost_semantic += c_panel/(coste1); + else if (_s->semantic == 7) + // cost_semantic += c_lamp/_s->cost; + // cost_semantic += c_lamp/(1+_s->cost); + cost_semantic += c_lamp/(coste1); + else if (_s->semantic == 8) + // cost_semantic += c_glass/_s->cost; + // cost_semantic += c_glass/(1+_s->cost); + cost_semantic += c_glass/(coste1); + + if (_s2->semantic == 1) + // cost_semantic += c_wall/_s2->cost; + // cost_semantic += c_wall/(1+_s2->cost); + cost_semantic += c_wall/(coste2); + else if (_s2->semantic == 2) + // cost_semantic += c_door/_s2->cost; + // cost_semantic += c_door/(1+_s2->cost); + cost_semantic += c_door/(coste2); + else if (_s2->semantic == 3) + // cost_semantic += c_colum/_s2->cost; + // cost_semantic += c_colum/(1+_s2->cost); + cost_semantic += c_colum/(coste2); + else if (_s2->semantic == 4) + // cost_semantic += c_furnish/_s2->cost; + // cost_semantic += c_furnish/(1+_s2->cost); + cost_semantic += c_furnish/(coste2); + else if (_s2->semantic == 5) + // cost_semantic += c_stair/_s2->cost; + // cost_semantic += c_stair/(1+_s2->cost); + cost_semantic += c_stair/(coste2); + else if (_s2->semantic == 6) + // cost_semantic += c_panel/_s2->cost; + // cost_semantic += c_panel/(1+_s2->cost); + cost_semantic += c_panel/(coste2); + else if (_s2->semantic == 7) + // cost_semantic += c_lamp/_s2->cost; + // cost_semantic += c_lamp/(1+_s2->cost); + cost_semantic += c_lamp/(coste2); + else if (_s2->semantic == 8) + // cost_semantic += c_glass/_s2->cost; + // cost_semantic += c_glass/(1+_s2->cost); + cost_semantic += c_glass/(coste2); + + mean_cost_semantic = cost_semantic/(n_checked_nodes+1); // Dividir entre el numero de nodos + // mean_cost_semantic = cost_semantic; + } + else{ + cost_semantic = 0; + if (_s->semantic == 1) + // cost_semantic += c_wall/_s->cost; + // cost_semantic += c_wall/(1+_s->cost); + cost_semantic += c_wall/(coste1); + else if (_s->semantic == 2) + // cost_semantic += c_door/_s->cost; + // cost_semantic += c_door/(1+_s->cost); + cost_semantic += c_door/(coste1); + else if (_s->semantic == 3) + // cost_semantic += c_colum/_s->cost; + // cost_semantic += c_colum/(1+_s->cost); + cost_semantic += c_colum/(coste1); + else if (_s->semantic == 4) + // cost_semantic += c_furnish/_s->cost; + // cost_semantic += c_furnish/(1+_s->cost); + cost_semantic += c_furnish/(coste1); + else if (_s->semantic == 5) + // cost_semantic += c_stair/_s->cost; + // cost_semantic += c_stair/(1+_s->cost); + cost_semantic += c_stair/(coste1); + else if (_s->semantic == 6) + // cost_semantic += c_panel/_s->cost; + // cost_semantic += c_panel/(1+_s->cost); + cost_semantic += c_panel/(coste1); + else if (_s->semantic == 7) + // cost_semantic += c_lamp/_s->cost; + // cost_semantic += c_lamp/(1+_s->cost); + cost_semantic += c_lamp/(coste1); + else if (_s->semantic == 8) + // cost_semantic += c_glass/_s->cost; + // cost_semantic += c_glass/(1+_s->cost); + cost_semantic += c_glass/(coste1); + else + cost_semantic=0; + + if (_s2->semantic == 1) + // cost_semantic += c_wall/_s2->cost; + // cost_semantic += c_wall/(1+_s2->cost); + cost_semantic += c_wall/(coste2); + else if (_s2->semantic == 2) + // cost_semantic += c_door/_s2->cost; + // cost_semantic += c_door/(1+_s2->cost); + cost_semantic += c_door/(coste2); + else if (_s2->semantic == 3) + // cost_semantic += c_colum/_s2->cost; + // cost_semantic += c_colum/(1+_s2->cost); + cost_semantic += c_colum/(coste2); + else if (_s2->semantic == 4) + // cost_semantic += c_furnish/_s2->cost; + // cost_semantic += c_furnish/(1+_s2->cost); + cost_semantic += c_furnish/(coste2); + else if (_s2->semantic == 5) + // cost_semantic += c_stair/_s2->cost; + // cost_semantic += c_stair/(1+_s2->cost); + cost_semantic += c_stair/(coste2); + else if (_s2->semantic == 6) + // cost_semantic += c_panel/_s2->cost; + // cost_semantic += c_panel/(1+_s2->cost); + cost_semantic += c_panel/(coste2); + else if (_s2->semantic == 7) + // cost_semantic += c_lamp/_s2->cost; + // cost_semantic += c_lamp/(1+_s2->cost); + cost_semantic += c_lamp/(coste2); + else if (_s2->semantic == 8) + // cost_semantic += c_glass/_s2->cost; + // cost_semantic += c_glass/(1+_s2->cost); + cost_semantic += c_glass/(coste2); + else + cost_semantic=0; + mean_cost_semantic = cost_semantic/2; // Dividir entre el numero de nodos + // mean_cost_semantic = cost_semantic; + } + // return static_cast( mean_cost_semantic * dist_scale_factor_reduced_); + return static_cast( mean_cost_semantic); + } + + inline unsigned int ThetaStarSemanticCost::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + unsigned int cost2 = 0; + + float c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // float coef, c_wall, c_door, c_colum, c_furnish, c_stair, c_panel, c_lamp, c_glass; + // coef=10; + c_wall=coef; + c_door=3*coef; + c_colum=2.5*coef; + c_furnish=1*coef; + c_stair=1.5*coef; + c_panel=1.5*coef; + c_lamp=2*coef; + c_glass=2*coef; + + if(_dirs == 8){ + cost2 += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost2 += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // EDF COST --> ThetarStarM1 + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + // cost += cost_term; + // _suc->C = cost_term; + + // SEMANTIC COST + // Wall + if (_suc->semantic == 1){ + cost2=c_wall*cost2; + // std::cout << "cost: " << _suc->cost << std::endl; + // std::cout << "semantic: " << _suc->semantic << std::endl; + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. + } + // Door + else if (_suc->semantic == 2){ + cost2=c_door*cost2; + } + + // Colum + else if (_suc->semantic == 3){ + cost2=c_colum*cost2; + } + // Furnishing + else if (_suc->semantic == 4){ + cost2=c_furnish*cost2; + } + // Stairs + else if (_suc->semantic == 5){ + cost2=c_stair*cost2; + } + // Panels --> Barandilla + else if (_suc->semantic == 6){ + cost2=c_panel*cost2; + } + // Lamp + else if (_suc->semantic == 7){ + cost2=c_lamp*cost2; + } + // Glass wall + else if (_suc->semantic == 8){ + cost2=c_glass*cost2; + } + + cost = cost + cost2; + + _suc->C = _suc->cost; + + return cost; + } +} diff --git a/src/ROS/planner_ros_node.cpp b/src/ROS/planner_ros_node.cpp index 177b6ec..6f16f28 100755 --- a/src/ROS/planner_ros_node.cpp +++ b/src/ROS/planner_ros_node.cpp @@ -1,14 +1,20 @@ #include #include "Planners/AStar.hpp" +#include "Planners/AStarSemantic.hpp" +#include "Planners/AStarSemanticCost.hpp" #include "Planners/AStarM2.hpp" #include "Planners/AStarM1.hpp" #include "Planners/AStar_Gradient.hpp" #include "Planners/AStar_EDF.hpp" #include "Planners/ThetaStar.hpp" +#include "Planners/ThetaStarSemantic.hpp" +#include "Planners/ThetaStarSemanticCost.hpp" #include "Planners/ThetaStarM1.hpp" #include "Planners/ThetaStarM2.hpp" #include "Planners/LazyThetaStar.hpp" +#include "Planners/LazyThetaStarSemantic.hpp" +#include "Planners/LazyThetaStarSemanticCost.hpp" #include "Planners/LazyThetaStar_Gradient.hpp" #include "Planners/LazyThetaStar_EDF.hpp" #include "Planners/LazyThetaStarM1.hpp" @@ -254,6 +260,12 @@ class HeuristicPlannerROS }else if( algorithm_name == "costastar" ){ ROS_INFO("Using Cost Aware A*"); algorithm_.reset(new Planners::AStarM1(use3d_)); + }else if( algorithm_name == "astar_semantic" ){ + ROS_INFO("Using Semantic A*"); + algorithm_.reset(new Planners::AStarSemantic(use3d_)); + }else if( algorithm_name == "astar_semantic_cost" ){ + ROS_INFO("Using Semantic Cost A*"); + algorithm_.reset(new Planners::AStarSemanticCost(use3d_)); }else if( algorithm_name == "astar_gradient" ){ ROS_INFO("Using A* Gradient"); algorithm_.reset(new Planners::AStarGradient(use3d_)); @@ -269,6 +281,12 @@ class HeuristicPlannerROS }else if ( algorithm_name == "costhetastar" ){ ROS_INFO("Using Cost Aware Theta* "); algorithm_.reset(new Planners::ThetaStarM1(use3d_)); + }else if ( algorithm_name == "thetastar_semantic" ){ + ROS_INFO("Using Semantic Theta* "); + algorithm_.reset(new Planners::ThetaStarSemantic(use3d_)); + }else if ( algorithm_name == "thetastar_semantic_cost" ){ + ROS_INFO("Using Semantic Cost Theta* "); + algorithm_.reset(new Planners::ThetaStarSemanticCost(use3d_)); }else if ( algorithm_name == "thetastarsafetycost" ){ ROS_INFO("Using Theta* Safety Cost"); algorithm_.reset(new Planners::ThetaStarM2(use3d_)); @@ -278,6 +296,12 @@ class HeuristicPlannerROS }else if( algorithm_name == "costlazythetastar"){ ROS_INFO("Using Cost Aware LazyTheta*"); algorithm_.reset(new Planners::LazyThetaStarM1(use3d_)); + }else if( algorithm_name == "lazythetastar_semantic"){ + ROS_INFO("Using Semantic LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStarSemantic(use3d_)); + }else if( algorithm_name == "lazythetastar_semantic_cost"){ + ROS_INFO("Using Semantic Cost LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStarSemanticCost(use3d_)); }else if( algorithm_name == "costlazythetastarmodified"){ ROS_INFO("Using Cost Aware LazyTheta*"); algorithm_.reset(new Planners::LazyThetaStarM1Mod(use3d_)); @@ -481,6 +505,7 @@ class HeuristicPlannerROS std::string heuristic_; }; + int main(int argc, char **argv) { ros::init(argc, argv, "heuristic_planner_ros_node");