From 1e351acb333a834daebff847967905f5e34e01ab Mon Sep 17 00:00:00 2001 From: guillermogilg99 Date: Fri, 18 Oct 2024 11:02:01 +0000 Subject: [PATCH] Local almost functional. Reset of the occupation map is missing --- include/Grid3D/local_grid3d.hpp | 29 +++--- include/Planners/AlgorithmBase.hpp | 2 +- launch/local_planner.launch | 6 +- src/ROS/local_planner_ros_node.cpp | 140 ++++++++++++++++++----------- src/ROS/planner_ros_node.cpp | 2 + src/utils/ros/ROSInterfaces.cpp | 2 + 6 files changed, 111 insertions(+), 70 deletions(-) diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp index 184dd0a..550b3fa 100755 --- a/include/Grid3D/local_grid3d.hpp +++ b/include/Grid3D/local_grid3d.hpp @@ -39,6 +39,7 @@ #include //for std::setw, std::hex, and std::setfill #include //for all other OpenSSL function calls #include //for SHA512_DIGEST_LENGTH +#include // #include "utils/ros/ROSInterfaces.hpp" // #ifdef BUILD_VORONOI @@ -206,13 +207,13 @@ class Local_Grid3d void computeLocalGrid(torch::jit::script::Module& loaded_sdf, float drone_x, float drone_y, float drone_z) { - printf("-- Executing computeLocalGrid --\n"); + // printf("-- Executing computeLocalGrid --\n"); // Build global positions vector std::vector> coordinates_vector; coordinates_vector.reserve(m_gridSizeX*m_gridSizeY*m_gridSizeZ); - std::cout << "Drone position: " << "x = " << drone_x << " | y = " << drone_y << " | z = " << drone_z << std::endl; + // std::cout << "Drone position: " << "x = " << drone_x << " | y = " << drone_y << " | z = " << drone_z << std::endl; for(int iz=0; iz first_point = coordinates_vector.front(); // O coordinates_vector[0] - std::cout << "Primer punto: (" << first_point[0] << ", " << first_point[1] << ", " << first_point[2] << ")\n"; + // std::vector first_point = coordinates_vector.front(); // O coordinates_vector[0] + // std::cout << "Primer punto: (" << first_point[0] << ", " << first_point[1] << ", " << first_point[2] << ")\n"; - std::vector last_point = coordinates_vector.back(); // O coordinates_vector[coordinates_vector.size() - 1] - std::cout << "Último punto: (" << last_point[0] << ", " << last_point[1] << ", " << last_point[2] << ")\n"; + // std::vector last_point = coordinates_vector.back(); // O coordinates_vector[coordinates_vector.size() - 1] + // std::cout << "Último punto: (" << last_point[0] << ", " << last_point[1] << ", " << last_point[2] << ")\n"; - size_t middle_index = coordinates_vector.size() / 2; - std::vector middle_point = coordinates_vector[middle_index]; - std::cout << "Punto medio: (" << middle_point[0] << ", " << middle_point[1] << ", " << middle_point[2] << ")\n"; + // size_t middle_index = coordinates_vector.size() / 2; + // std::vector middle_point = coordinates_vector[middle_index]; + // std::cout << "Punto medio: (" << middle_point[0] << ", " << middle_point[1] << ", " << middle_point[2] << ")\n"; // Convert vector tu libtorch array and query the neural network auto num_points = coordinates_vector.size(); @@ -253,7 +254,7 @@ class Local_Grid3d torch::Tensor grid_output_tensor = loaded_sdf.forward({coordinates_tensor}).toTensor(); auto end = std::chrono::high_resolution_clock::now(); std::chrono::duration duration = end - start; - std::cout << "Points queried: " << num_points <<" | Time taken to query model: " << duration.count() << " ms" << std::endl; + //std::cout << "Points queried: " << num_points <<" | Time taken to query model: " << duration.count() << " ms" << std::endl; // Place the queried values into the m_grid distance data field for(int iz=0; iz - - - + + + diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp index 8ee4868..e66ec42 100755 --- a/src/ROS/local_planner_ros_node.cpp +++ b/src/ROS/local_planner_ros_node.cpp @@ -74,8 +74,8 @@ class HeuristicLocalPlannerROS // request_path_server_ = lnh_.advertiseService("request_path", &HeuristicLocalPlannerROS::requestPathService, this); // This is in planner_ros_node.cpp and the corresponding service defined. change_planner_server_ = lnh_.advertiseService("set_algorithm", &HeuristicLocalPlannerROS::setAlgorithm, this); - line_markers_pub_ = lnh_.advertise("local_path_line_markers", 1); - point_markers_pub_ = lnh_.advertise("local_path_points_markers", 1); + local_line_markers_pub_ = lnh_.advertise("local_path_line_markers", 1); + local_point_markers_pub_ = lnh_.advertise("local_path_points_markers", 1); cloud_test = lnh_.advertise >("/cloud_PCL", 1, true); // Defined by me to show the point cloud as pcl::PointCloud loop_duration = loop_end - loop_start; + printf("TIEMPO DE LOOP: %.2f ms\n", loop_duration.count()); + } } @@ -358,7 +366,7 @@ class HeuristicLocalPlannerROS // Agregar el punto desplazado al nuevo vector global_path_local.push_back(newpoint); } - std::cout << "Global path (local reference): " << global_path_local << std::endl; + // std::cout << "Global path (local reference): " << global_path_local << std::endl; // 2.2 - Find closest waypoint to the drone (which is supposed to be the "current" waypoint) double min_dist = std::numeric_limits::infinity(); @@ -380,7 +388,7 @@ class HeuristicLocalPlannerROS closest_index = i; } } - std::cout << "Closest waypoint: " << global_path_local[closest_index] << std::endl; + // std::cout << "Closest waypoint: " << global_path_local[closest_index] << std::endl; // 2.3 - Find furthest next waypoint that is still inside the local map (the drone will treat this waypoint as the local goal bool points_in_range = true; @@ -404,7 +412,7 @@ class HeuristicLocalPlannerROS } } - std::cout << "Local goal found: " << local_goal << std::endl; + // std::cout << "Local goal found: " << local_goal << std::endl; // 3 - Check if local goal accessible. If not, find closest point @@ -426,12 +434,40 @@ class HeuristicLocalPlannerROS std::cout << discrete_goal << ": Goal not valid" << std::endl; } + //std::cout << "LOCAL PATH CALCULATED SUCCESSFULLY" << std::endl; + //Planners::utils::CoordinateList local_path = std::get(local_path_data["path"]); + //std::cout << "Local path: " << local_path << std::endl; + auto local_path_data = algorithm_->findPath(discrete_start, discrete_goal, loaded_sdf_); - std::cout << "LOCAL PATH CALCULATED SUCCESSFULLY" << std::endl; - Planners::utils::CoordinateList local_path = std::get(local_path_data["path"]); - std::cout << "Local path: " << local_path << std::endl; - + if( std::get(local_path_data["solved"]) ){ + Planners::utils::CoordinateList local_path; + local_path = std::get(local_path_data["path"]); + + //Convert the local path to GLOBAL COORDINATES and push them into the markers + + Planners::utils::Vec3i local_origin = {origen_local_x, origen_local_y, origen_local_z}; + std::cout << "Local origin: " << local_origin; + + for(const auto &it: std::get(local_path_data["path"])){ + Planners::utils::Vec3i global_wp_point = it + local_origin; + local_path_line_markers_.points.push_back(Planners::utils::continousPoint(global_wp_point, resolution_)); + local_path_points_markers_.points.push_back(Planners::utils::continousPoint(global_wp_point, resolution_)); + } + + // std::cout << "Local_path_line_markers: " << local_path_line_markers_.points << std::endl; + // std::cout << "Local_path_points_markers: " << local_path_points_markers_.points << std::endl; + + publishMarker(local_path_line_markers_, local_line_markers_pub_); + publishMarker(local_path_points_markers_, local_point_markers_pub_); + + local_path_line_markers_.points.clear(); + local_path_points_markers_.points.clear(); + + ROS_INFO("Path calculated succesfully"); + } + else + ROS_INFO("Couldn't calculate path"); } @@ -536,8 +572,8 @@ class HeuristicLocalPlannerROS if(!lnh_.getParam("global_frame_id", globalFrameId)) globalFrameId = "map"; - // configMarkers(algorithm_name, globalFrameId, resolution_); // Not influence by baseFrameId - configMarkers(algorithm_name, baseFrameId, resolution_); + configMarkers(algorithm_name, globalFrameId, resolution_); // Not influence by baseFrameId + // configMarkers(algorithm_name, baseFrameId, resolution_); // From planner_ros_node // std::string frame_id; @@ -613,37 +649,37 @@ class HeuristicLocalPlannerROS void configMarkers(const std::string &_ns, const std::string &_frame, const double &_scale){ - path_line_markers_.ns = _ns; - path_line_markers_.header.frame_id = _frame; - path_line_markers_.header.stamp = ros::Time::now(); - path_line_markers_.id = rand(); - path_line_markers_.lifetime = ros::Duration(500); - path_line_markers_.type = visualization_msgs::Marker::LINE_STRIP; - path_line_markers_.action = visualization_msgs::Marker::ADD; - path_line_markers_.pose.orientation.w = 1; - - path_line_markers_.color.r = 0.0; - path_line_markers_.color.g = 1.0; - path_line_markers_.color.b = 0.0; - - path_line_markers_.color.a = 1.0; - path_line_markers_.scale.x = _scale; - - path_points_markers_.ns = _ns; - path_points_markers_.header.frame_id = _frame; - path_points_markers_.header.stamp = ros::Time::now(); - path_points_markers_.id = rand(); - path_points_markers_.lifetime = ros::Duration(500); - path_points_markers_.type = visualization_msgs::Marker::POINTS; - path_points_markers_.action = visualization_msgs::Marker::ADD; - path_points_markers_.pose.orientation.w = 1; - path_points_markers_.color.r = 0.0; - path_points_markers_.color.g = 1.0; - path_points_markers_.color.b = 1.0; - path_points_markers_.color.a = 1.0; - path_points_markers_.scale.x = _scale; - path_points_markers_.scale.y = _scale; - path_points_markers_.scale.z = _scale; + local_path_line_markers_.ns = _ns; + local_path_line_markers_.header.frame_id = _frame; + local_path_line_markers_.header.stamp = ros::Time::now(); + local_path_line_markers_.id = rand(); + local_path_line_markers_.lifetime = ros::Duration(500); + local_path_line_markers_.type = visualization_msgs::Marker::LINE_STRIP; + local_path_line_markers_.action = visualization_msgs::Marker::ADD; + local_path_line_markers_.pose.orientation.w = 1; + + local_path_line_markers_.color.r = 0.0; + local_path_line_markers_.color.g = 1.0; + local_path_line_markers_.color.b = 0.0; + + local_path_line_markers_.color.a = 1.0; + local_path_line_markers_.scale.x = _scale; + + local_path_points_markers_.ns = _ns; + local_path_points_markers_.header.frame_id = _frame; + local_path_points_markers_.header.stamp = ros::Time::now(); + local_path_points_markers_.id = rand(); + local_path_points_markers_.lifetime = ros::Duration(500); + local_path_points_markers_.type = visualization_msgs::Marker::POINTS; + local_path_points_markers_.action = visualization_msgs::Marker::ADD; + local_path_points_markers_.pose.orientation.w = 1; + local_path_points_markers_.color.r = 0.0; + local_path_points_markers_.color.g = 1.0; + local_path_points_markers_.color.b = 1.0; + local_path_points_markers_.color.a = 1.0; + local_path_points_markers_.scale.x = _scale; + local_path_points_markers_.scale.y = _scale; + local_path_points_markers_.scale.z = _scale; } @@ -654,13 +690,13 @@ class HeuristicLocalPlannerROS _marker.action = visualization_msgs::Marker::DELETEALL; _pub.publish(_marker); }else{ - path_points_markers_.id = rand(); - path_points_markers_.header.stamp = ros::Time::now(); - setRandomColor(path_points_markers_.color); + local_path_points_markers_.id = rand(); + local_path_points_markers_.header.stamp = ros::Time::now(); + setRandomColor(local_path_points_markers_.color); - path_line_markers_.id = rand(); - path_line_markers_.header.stamp = ros::Time::now(); - setRandomColor(path_line_markers_.color); + local_path_line_markers_.id = rand(); + local_path_line_markers_.header.stamp = ros::Time::now(); + setRandomColor(local_path_line_markers_.color); } _marker.action = visualization_msgs::Marker::ADD; _pub.publish(_marker); @@ -685,7 +721,7 @@ class HeuristicLocalPlannerROS ros::ServiceServer request_path_server_, change_planner_server_; ros::Subscriber pointcloud_local_sub_, occupancy_grid_local_sub_, path_local_sub_; //TODO Fix point markers - ros::Publisher line_markers_pub_, point_markers_pub_, cloud_test; + ros::Publisher local_line_markers_pub_, local_point_markers_pub_, cloud_test; tf::TransformListener m_tfListener; @@ -693,7 +729,7 @@ class HeuristicLocalPlannerROS std::unique_ptr algorithm_; - visualization_msgs::Marker path_line_markers_, path_points_markers_; + visualization_msgs::Marker local_path_line_markers_, local_path_points_markers_; //Parameters Planners::utils::Vec3i world_size_; // Discrete diff --git a/src/ROS/planner_ros_node.cpp b/src/ROS/planner_ros_node.cpp index 34122c0..365cb20 100755 --- a/src/ROS/planner_ros_node.cpp +++ b/src/ROS/planner_ros_node.cpp @@ -709,6 +709,8 @@ class HeuristicPlannerROS path_points_markers_.points.push_back(Planners::utils::continousPoint(it, resolution_)); } + std::cout << "Global path line markers: " << path_line_markers_.points << std::endl; + publishMarker(path_line_markers_, line_markers_pub_); publishMarker(path_points_markers_, point_markers_pub_); diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp index b7df4e3..b4a51fd 100755 --- a/src/utils/ros/ROSInterfaces.cpp +++ b/src/utils/ros/ROSInterfaces.cpp @@ -145,6 +145,8 @@ namespace Planners { float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); _algorithm.configureCellCost({i, j, k}, cost); + if(cost <= 0) + _algorithm.addCollision({i, j, k}); } } }