diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp index e6be01d..5326d9b 100644 --- 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 getClosestObstacle(const Planners::utils::Vec3i& _coords){ diff --git a/launch/local_planner.launch b/launch/local_planner.launch index d4b1c2a..ae114c7 100644 --- a/launch/local_planner.launch +++ b/launch/local_planner.launch @@ -71,13 +71,14 @@ - + - + + - + diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp index a037dcb..4d1f5e3 100644 --- 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) {