Skip to content

Commit

Permalink
Local almost functional. Reset of the occupation map is missing
Browse files Browse the repository at this point in the history
  • Loading branch information
@guillermogilg99
guillermogilg99 committed Oct 18, 2024
1 parent eecbf7f commit 1e351ac
Show file tree
Hide file tree
Showing 6 changed files with 111 additions and 70 deletions.
29 changes: 15 additions & 14 deletions include/Grid3D/local_grid3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <iomanip> //for std::setw, std::hex, and std::setfill
#include <openssl/evp.h> //for all other OpenSSL function calls
#include <openssl/sha.h> //for SHA512_DIGEST_LENGTH
#include <chrono>
// #include "utils/ros/ROSInterfaces.hpp"

// #ifdef BUILD_VORONOI
Expand Down Expand Up @@ -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<std::vector<float>> 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<m_gridSizeZ; iz++)
{
for(int iy=0; iy<m_gridSizeY; iy++)
Expand All @@ -230,15 +231,15 @@ class Local_Grid3d

// DEBUGGING: PRINT FIRST, MIDDLE AND LAST POINT

std::vector<float> 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<float> 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<float> 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<float> 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<float> 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<float> 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();
Expand All @@ -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<double, std::milli> 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<m_gridSizeZ; iz++)
Expand Down Expand Up @@ -281,7 +282,7 @@ class Local_Grid3d

// std::cout << "Stored grid: " << gridSlice << std::endl;

std::cout << "---!!!--- Exiting computeLocalGrid ---!!!---" << std::endl;
//std::cout << "---!!!--- Exiting computeLocalGrid ---!!!---" << std::endl;


}
Expand Down Expand Up @@ -452,9 +453,9 @@ class Local_Grid3d
// Get map parameters: They have to take from local_world_size_x, local_world_size_y , local_world_size_z of the launch
double maxX, maxY, maxZ, res;

maxX = 4.0; // distancia a cada lado del dron (en x)
maxY = 4.0; // distancia a cada lado del dron (en y)
maxZ = 2.0; // distancia a cada lado del dron (en z)
maxX = 3.0; // distancia a cada lado del dron (en x)
maxY = 3.0; // distancia a cada lado del dron (en y)
maxZ = 1.6; // distancia a cada lado del dron (en z)
res = 0.2;


Expand Down
2 changes: 1 addition & 1 deletion include/Planners/AlgorithmBase.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -238,7 +238,7 @@ namespace Planners

utils::DiscreteWorld discrete_world_; /*!< TODO Comment */
unsigned int inflate_steps_{1}; /*!< TODO Comment */
bool do_inflate_{true}; /*!< TODO Comment */
bool do_inflate_{false}; /*!< TODO Comment */

double cost_weight_{0}; /*!< TODO Comment */
unsigned int max_line_of_sight_cells_{0}; /*!< TODO Comment */
Expand Down
6 changes: 3 additions & 3 deletions launch/local_planner.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,9 @@
<arg name="algorithm_name" default="costlazythetastar"/>

<!-- Size of the Local Map -->
<arg name="local_world_size_x" default="4"/>
<arg name="local_world_size_y" default="4"/>
<arg name="local_world_size_z" default="2"/>
<arg name="local_world_size_x" default="3"/>
<arg name="local_world_size_y" default="3"/>
<arg name="local_world_size_z" default="1.6"/>
<arg name="resolution" default="0.2"/>

<arg name="inflate_map" default="true"/>
Expand Down
140 changes: 88 additions & 52 deletions src/ROS/local_planner_ros_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<visualization_msgs::Marker>("local_path_line_markers", 1);
point_markers_pub_ = lnh_.advertise<visualization_msgs::Marker>("local_path_points_markers", 1);
local_line_markers_pub_ = lnh_.advertise<visualization_msgs::Marker>("local_path_line_markers", 1);
local_point_markers_pub_ = lnh_.advertise<visualization_msgs::Marker>("local_path_points_markers", 1);
cloud_test = lnh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("/cloud_PCL", 1, true); // Defined by me to show the point cloud as pcl::PointCloud<pcl::PointXYZ

networkReceivedFlag_ = 1;
Expand Down Expand Up @@ -152,6 +152,10 @@ class HeuristicLocalPlannerROS
// Only perform local planning if global path was received
if(globalPathReceived_ == 1){

auto loop_start = std::chrono::high_resolution_clock::now();



// 1. Update Neural Network State (if new state available)
if(networkReceivedFlag_ == 1)
{
Expand All @@ -160,13 +164,17 @@ class HeuristicLocalPlannerROS
networkReceivedFlag_ = 0;
}

Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_, drone_x_, drone_y_, drone_z_, loaded_sdf_);
// 2. Update local map with the neural network information around the drone
printf("-- Exiting callback --\n");
Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_, drone_x_, drone_y_, drone_z_, loaded_sdf_);
// printf("-- Exiting callback --\n");

// 3. Calculate Local Path
calculatePath3D();

auto loop_end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, std::milli> loop_duration = loop_end - loop_start;
printf("TIEMPO DE LOOP: %.2f ms\n", loop_duration.count());

}
}

Expand Down Expand Up @@ -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<double>::infinity();
Expand All @@ -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;
Expand All @@ -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

Expand All @@ -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<Planners::utils::CoordinateList>(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<Planners::utils::CoordinateList>(local_path_data["path"]);
std::cout << "Local path: " << local_path << std::endl;

if( std::get<bool>(local_path_data["solved"]) ){
Planners::utils::CoordinateList local_path;
local_path = std::get<Planners::utils::CoordinateList>(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<Planners::utils::CoordinateList>(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");

}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

}

Expand All @@ -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);
Expand All @@ -685,15 +721,15 @@ 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;

std::unique_ptr<Local_Grid3d> m_local_grid3d_;

std::unique_ptr<Planners::AlgorithmBase> 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
Expand Down
2 changes: 2 additions & 0 deletions src/ROS/planner_ros_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_);

Expand Down
2 changes: 2 additions & 0 deletions src/utils/ros/ROSInterfaces.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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});
}
}
}
Expand Down

0 comments on commit 1e351ac

Please sign in to comment.