Merge pull request #9 from robotics-upo/neighbours

first version ral2023

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 AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2
+ LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF
  # 
  CATKIN_DEPENDS std_msgs visualization_msgs geometry_msgs nav_msgs roscpp message_runtime costmap_2d
 #  DEPENDS system_lib
@@ -154,6 +154,8 @@ add_library(AStar                  src/Planners/AStar.cpp
                                             ${${PROJECT_NAME}_UTILS_SOURCES}
                                             )
 
+add_library(AStar_Gradient            src/Planners/AStar_Gradient.cpp ) 
+add_library(AStar_EDF            src/Planners/AStar_EDF.cpp ) 
 add_library(AStarM1            src/Planners/AStarM1.cpp ) 
 add_library(AStarM2            src/Planners/AStarM2.cpp ) 
 add_library(ThetaStar          src/Planners/ThetaStar.cpp )
@@ -163,10 +165,12 @@ 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(LazyThetaStar_Gradient    src/Planners/LazyThetaStar_Gradient.cpp )
+add_library(LazyThetaStar_EDF    src/Planners/LazyThetaStar_EDF.cpp )
 
 
 
-list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2)  
+list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar 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})

config/costmap_for_rand_maps.yaml

@@ -18,7 +18,7 @@ costmap:
       cost_scaling_factor: 2
       enabled: true
       inflate_unknown: false
-      inflation_radius: 3.0
+      _radius: 3.0
 
    static_layer:
       unknown_cost_value: -1

include/Grid3D/grid3d.hpp

@@ -366,7 +366,7 @@ bool loadSemanticGrid() {
 		m_oneDivRes = 1.0/m_resolution;
 		ROS_INFO("Map size:\n");
 		ROS_INFO("\tx: %.2f to %.2f", minX, maxX);
-		ROS_INFO("\ty: %.2f to %.2f", minZ, maxZ);
+		ROS_INFO("\ty: %.2f to %.2f", minY, maxY);
 		ROS_INFO("\tz: %.2f to %.2f", minZ, maxZ);
 		ROS_INFO("\tRes: %.2f" , m_resolution );
 
@@ -490,13 +490,15 @@ bool loadSemanticGrid() {
 		// Get map parameters
 		double minX, minY, minZ;
 		m_octomap->getMetricMin(minX, minY, minZ);
-
+				
 		// Load the octomap in PCL for easy nearest neighborhood computation
 		// The point-cloud is shifted to have (0,0,0) as min values
 		int i = 0;
 		m_cloud->width = m_octomap->size();
+		// std::cout << "dim1: " << m_cloud->width << std::endl;
 		m_cloud->height = 1;
 		m_cloud->points.resize(static_cast<long>(m_cloud->width) * m_cloud->height);
+		std::cout << "dim1: " << m_cloud->width << std::endl;
 		for(octomap::OcTree::leaf_iterator it = m_octomap->begin_leafs(), end = m_octomap->end_leafs(); it != end; ++it)
 		{
 			if(it != NULL && m_octomap->isNodeOccupied(*it))
@@ -510,6 +512,7 @@ bool loadSemanticGrid() {
 		}
 		m_cloud->width = i;
 		m_cloud->points.resize(i);
+		// std::cout << "dim2: " << m_cloud->width << std::endl;
 
 		// Create the point cloud msg for publication
 		pcl::toROSMsg(*m_cloud, m_pcMsg);
@@ -536,8 +539,8 @@ bool loadSemanticGrid() {
 		// Compute the distance to the closest point of the grid
 		int index;
 		float dist;
-		float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI));
-		float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev);
+		// float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI));
+		// float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev);
 		pcl::PointXYZI searchPoint;
 		std::vector<int> pointIdxNKNSearch(1);
 		std::vector<float> pointNKNSquaredDistance(1);
@@ -561,21 +564,34 @@ bool loadSemanticGrid() {
 						percent_msg.data = percent;
 						// percent_computed_pub_.publish(percent_msg);
 					}
-
 					if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
 					{
-						dist = pointNKNSquaredDistance[0];
-						m_grid[index].dist = dist;
-						if(!use_costmap_function){
-							m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2);
-						}else{
-							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;
-
-							//En sí esto no va aquí, pero no entiendo muy bien donde meterlo
-							m_grid[index].semantic = semanticGrid[index];
+						// JAC: Always square distance has been considered!!!!!!
+						// dist = pointNKNSquaredDistance[0];
+						dist = sqrt(pointNKNSquaredDistance[0]);
+
+						// std::cout << "dist: " << dist << std::endl;
+						if (dist < 200){
+							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)));
+								// 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 = 100-dist;
+								m_grid[index].prob = dist;
+                m_grid[index].semantic = semanticGrid[index];
+							}
+						}
+						else{
+							// std::cout << "dist: " << dist << std::endl;
+							// m_grid[index].dist = -1.0;
+							m_grid[index].dist = 90.0;
+							m_grid[index].prob =  0.0;
+
 						}
 					}
 					else