Merge branch 'semantic' into neighbours

CMakeLists.txt

@@ -79,6 +79,7 @@ add_service_files(
   FILES
   GetPath.srv
   SetAlgorithm.srv
+  GetSemanticGrid.srv
 )
 
 ## Generate added messages and services with any dependencies listed here
@@ -193,6 +194,7 @@ if(BUILD_ROS_SUPPORT)
 
   catkin_install_python(PROGRAMS  scripts/test_algorithms.py
                                   scripts/test_algorithms_compare.py
+                                  scripts/grid3d_class.py
                                   scripts/test_algorithms_performance.py
                                   scripts/test_algorithms_pseudo_random_paths.py
                                   scripts/compare_trajectories_rviz.py

include/Grid3D/grid3d.hpp

@@ -13,6 +13,8 @@
 #include <octomap/OcTree.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <std_msgs/Float32.h>
+#include <std_msgs/Int32MultiArray.h>
+#include <IfcGrid/GetSemanticGrid.h>
 #include <stdio.h> 
 // PCL
 #include <pcl/point_cloud.h>
@@ -63,7 +65,12 @@ class Grid3d
 	sensor_msgs::PointCloud2 m_pcMsg;
 	ros::Publisher m_pcPub, percent_computed_pub_;
 	ros::Timer mapTimer;
+
+	// Semantic Grid
 
+	std_msgs::Int32MultiArray semantic_grid;
+	std::vector<int> semanticGrid;
+
 	// Visualization of a grid slice as 2D grid map msg
 	nav_msgs::OccupancyGrid m_gridSliceMsg;
 	ros::Publisher m_gridSlicePub;
@@ -105,6 +112,9 @@ class Grid3d
 		m_octomap = NULL;
 		m_grid = NULL;
 
+		if(loadSemanticGrid()){
+			std::cout<< "Grid Loaded";
+		}
 		if(loadOctomap(m_mapPath))
 		{
 			// Compute the point-cloud associated to the ocotmap
@@ -216,6 +226,16 @@ class Grid3d
 
 		return m_grid[index].prob;
 	}
+
+	float getCellSemantic(const float &_x, const float &_y, const float &_z){
+
+		if( !isIntoMap(_x, _y, _z) )
+			return 0;
+
+		int index = point2grid(_x, _y, _z);
+
+		return m_grid[index].semantic;
+	}
 
 	std::pair<Planners::utils::Vec3i, double>  getClosestObstacle(const Planners::utils::Vec3i& _coords){
 
@@ -246,6 +266,39 @@ class Grid3d
 
 protected:
 
+bool loadSemanticGrid() {
+    ros::NodeHandle n;
+
+    // Define a ROS service client
+    ros::ServiceClient client = n.serviceClient<IfcGrid::GetSemanticGrid>("get_semantic_grid");
+
+    IfcGrid::GetSemanticGrid srv;
+
+    // Wait for the service to become available
+    if (!client.waitForExistence(ros::Duration(40.0))) {
+        ROS_ERROR("Service get_semantic_grid unavailable");
+        return false;
+    }
+
+    // Call the service
+    if (client.call(srv)) {
+        // Process the response
+        // The shape and semantic grid are available in srv.response.shape and srv.response.semantic_grid
+        std::vector<short int> shape = srv.response.shape;
+        std::vector<int> int_shape(shape.begin(), shape.end());
+
+        semanticGrid = srv.response.semantic_grid;
+
+        return !semanticGrid.empty();
+    } else {
+        ROS_ERROR("Failed to call service get_semantic_grid");
+        return false;
+    }
+}
+
+
+
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-parameter"
 	void publishMapPointCloudTimer(const ros::TimerEvent& event)
@@ -530,16 +583,16 @@ class Grid3d
 								// 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
 					{
@@ -598,4 +651,4 @@ class Grid3d
 };
 
 
-#endif
+#endif

include/Planners/AlgorithmBase.hpp

@@ -80,7 +80,6 @@ namespace Planners
          * @param _resolution resolution to save inside the world object
          */
         void setWorldSize(const Vec3i &worldSize_,const double _resolution);
-
         /**
          * @brief Get the World Size, it simply call the getWorldSize method from the
          * discrete world internal object

include/utils/utils.hpp

@@ -61,6 +61,7 @@ namespace Planners
 	    {
 	    	float dist{0};
 	    	float prob{0};
+            int semantic{0};
 	    };
         /**
          * @brief 
@@ -221,12 +222,13 @@ namespace Planners
 
             Planners::utils::Vec3i coordinates;
 
-            unsigned int G{0}, H{0}, C{0};
+            unsigned int G{0}, H{0}, C{0}, S{0};
 
             unsigned int gplush{0};
             unsigned int world_index{0};
 
             double cost{0};
+            int semantic{0};
 
             bool occuppied{false};
             bool isInOpenList{false};

launch/grid3d.launch

@@ -0,0 +1,8 @@
+<launch>
+    <node name="semantic_grid" pkg="heuristic_planners" type="grid3d_class.py">
+        <param name="world_size_x" value="220" />
+        <param name="world_size_y" value="220" />
+        <param name="world_size_z" value="20" />
+        <param name="resolution" value="0.2" />
+    </node>
+</launch>