Merged with local_planner implementation without changes. It compiles…

… and global planner works

CMakeLists.txt

@@ -189,6 +189,10 @@ if(BUILD_ROS_SUPPORT)
   add_dependencies(planner_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_LIBRARIES})
   target_link_libraries(planner_ros_node ${catkin_LIBRARIES} ${TORCH_LIBRARIES} ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
   list(APPEND ${PROJECT_NAME}_TARGETS planner_ros_node)
+  add_executable(local_planner_ros_node src/ROS/local_planner_ros_node.cpp )
+  add_dependencies(local_planner_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_LIBRARIES})
+  target_link_libraries(local_planner_ros_node ${catkin_LIBRARIES} ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
+  list(APPEND ${PROJECT_NAME}_TARGETS local_planner_ros_node)
 #############
   ## Install ##
 #############

include/Grid3D/grid3d.hpp

@@ -56,8 +56,8 @@ class Grid3d
 	int m_gridStepY, m_gridStepZ;
 
 	// 3D point clound representation of the map
-	pcl::PointCloud<pcl::PointXYZI>::Ptr m_cloud;
-	pcl::KdTreeFLANN<pcl::PointXYZI> m_kdtree;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
+	pcl::KdTreeFLANN<pcl::PointXYZ> m_kdtree;
 
 	// Visualization of the map as pointcloud
 	sensor_msgs::PointCloud2 m_pcMsg;
@@ -74,7 +74,7 @@ class Grid3d
 	bool use_costmap_function;
 
 public:
-	Grid3d(): m_cloud(new pcl::PointCloud<pcl::PointXYZI>)
+	Grid3d(): m_cloud(new pcl::PointCloud<pcl::PointXYZ>)
 	{
 		// Load paraeters
 		double value;
@@ -167,14 +167,14 @@ class Grid3d
 			buildGridSliceMsg(m_gridSlice);
 		}
 	}
-	float computeCloudWeight(std::vector<pcl::PointXYZI> &points)
+	float computeCloudWeight(std::vector<pcl::PointXYZ> &points)
 	{
 		float weight = 0.;
 		int n = 0;
 
 		for(long unsigned int i=0; i<points.size(); i++)
 		{
-			const pcl::PointXYZI& p = points[i];
+			const pcl::PointXYZ& p = points[i];
 			if(p.x >= 0.0 && p.y >= 0.0 && p.z >= 0.0 && p.x < m_maxX && p.y < m_maxY && p.z < m_maxZ)
 			{
 				int index = point2grid(p.x, p.y, p.z);
@@ -219,7 +219,7 @@ class Grid3d
 
 	std::pair<Planners::utils::Vec3i, double>  getClosestObstacle(const Planners::utils::Vec3i& _coords){
 
-		pcl::PointXYZI searchPoint;
+		pcl::PointXYZ searchPoint;
 
 		searchPoint.x = _coords.x * m_resolution;
 		searchPoint.y = _coords.y * m_resolution;
@@ -313,7 +313,7 @@ class Grid3d
 		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 );
 
@@ -485,7 +485,7 @@ class Grid3d
 		float dist;
 		float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI));
 		float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev);
-		pcl::PointXYZI searchPoint;
+		pcl::PointXYZ searchPoint;
 		std::vector<int> pointIdxNKNSearch(1);
 		std::vector<float> pointNKNSquaredDistance(1);
 		double count=0;