Minor changes

robotics-upo · Sep 30, 2024 · 0ccbbff · 0ccbbff
1 parent 2bd5dca
commit 0ccbbff
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 7 deletions.
diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp
@@ -388,7 +388,7 @@ class Local_Grid3d
 			}
 		}
 
-		//std::cout << "---!!!--- Completed coordinates_vector ---!!!---" << std::endl;
+		std::cout << "---!!!--- Completed coordinates_vector ---!!!---" << std::endl;
 
 		// Convertir el vector de puntos a un tensor de libtorch
         auto num_points = coordinates_vector.size();
@@ -399,7 +399,7 @@ class Local_Grid3d
             coordinates_tensor[i][2] = coordinates_vector[i][2];
         }
 
-		//std::cout << "---!!!--- Completed coordinates_tensor ---!!!---" << std::endl;
+		std::cout << "---!!!--- Completed coordinates_tensor ---!!!---" << std::endl;
 		//std::cout << coordinates_tensor << std::endl;
 
         // Pasar el tensor por la red neuronal
@@ -414,7 +414,7 @@ class Local_Grid3d
         // getchar(); // Comentar para no usar tecla.
         std::cout << "Points queried: " << num_points <<" |  Time taken to query model: " << duration.count() << " ms" << std::endl;
 
-		//std::cout << "---!!!--- Completed query ---!!!---" << std::endl;
+		std::cout << "---!!!--- Completed query ---!!!---" << std::endl;
 
 		int index;
 		//Asignar cada valor a la posición correcta del grid
@@ -429,6 +429,7 @@ class Local_Grid3d
 				}
 			}
 		}
+		std::cout << "---!!!--- Exiting computeLocalGrid ---!!!---" << std::endl;
 	}
 
 	std::pair<Planners::utils::Vec3i, double>  getClosestObstacle(const Planners::utils::Vec3i& _coords){

diff --git a/launch/local_planner.launch b/launch/local_planner.launch
@@ -71,13 +71,14 @@
     </node>
 
     <!-- x y z yaw pitch roll frame_id child_frame_id period_in_ms -->
-    <node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0 0 0 0 0 0 map world 100" />
+   <!-- <node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0 0 0 0 0 0 map world 100" /> -->
     <!-- occupancy_map is to show the occupancy markers in Rviz  -->
     <!-- TODO: I have to put the arguments world_size_x world_size_y world_size_z -->
     <!-- <node pkg="tf" type="static_transform_publisher" name="occupancy_world_broadcaster" args="5 5 2 0 0 0 occupancy_map base_link 100" /> -->
-    <node pkg="tf" type="static_transform_publisher" name="occupancy_world_broadcaster" args="$(arg local_world_size_x) $(arg local_world_size_y) $(arg local_world_size_z) 0 0 0 occupancy_map base_link 100" />
+
+   <!-- <node pkg="tf" type="static_transform_publisher" name="occupancy_world_broadcaster" args="$(arg local_world_size_x) $(arg local_world_size_y) $(arg local_world_size_z) 0 0 0 occupancy_map base_link 100" /> -->
 
-    <node pkg="rviz" name="rviz" type="rviz" args="-d $(find heuristic_planners)/rviz/local_planner.rviz"/>
+   <!--  <node pkg="rviz" name="rviz" type="rviz" args="-d $(find heuristic_planners)/rviz/local_planner.rviz"/> -->
 
     <!-- <node pkg="rqt_service_caller" type="rqt_service_caller" name="rqt_service_caller"/> -->
 

diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp
@@ -122,6 +122,7 @@ class HeuristicLocalPlannerROS
     }
 
     void networkUpdateCallback(const std_msgs::Empty::ConstPtr& msg){
+        ROS_INFO("Received new network callback");
         networkReceivedFlag_ = 1;
     }
     // From lazy_theta_star_planners
@@ -325,7 +326,7 @@ class HeuristicLocalPlannerROS
 
     void calculatePath3D()
 {
-
+    std::cout << "---Entered calculatePath3D--- " << std::endl;
     // PASO 1 - Revisar si el punto de inicio está libre
     if(m_local_grid3d_->m_grid[(m_local_grid3d_->m_gridSize-1)/2].dist > 0)
     {