From 1e8ca8f66a9cd042a6d73e71f4bac5feba8267df Mon Sep 17 00:00:00 2001 From: guillermogilg99 Date: Thu, 3 Oct 2024 14:39:57 +0000 Subject: [PATCH] Local planner receives global wp. Simple timed planning loop added --- CMakeLists.txt | 1 - launch/local_planner.launch | 86 ++++++++++++ src/ROS/local_planner_ros_node.cpp | 207 +++++++++-------------------- 3 files changed, 147 insertions(+), 147 deletions(-) create mode 100755 launch/local_planner.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f758fc..5fdf8bf 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,6 @@ if(BUILD_DOC) set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile) set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) - # request to configure the file configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) message("Doxygen build started") diff --git a/launch/local_planner.launch b/launch/local_planner.launch new file mode 100755 index 0000000..e2b6091 --- /dev/null +++ b/launch/local_planner.launch @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp index ac96439..3013fc4 100755 --- a/src/ROS/local_planner_ros_node.cpp +++ b/src/ROS/local_planner_ros_node.cpp @@ -3,16 +3,13 @@ #include "Planners/AStar.hpp" #include "Planners/AStarM2.hpp" #include "Planners/AStarM1.hpp" -#include "Planners/AStarSIREN.hpp" #include "Planners/ThetaStar.hpp" #include "Planners/ThetaStarM1.hpp" #include "Planners/ThetaStarM2.hpp" -#include "Planners/ThetaStarSIREN.hpp" #include "Planners/LazyThetaStar.hpp" #include "Planners/LazyThetaStarM1.hpp" #include "Planners/LazyThetaStarM1Mod.hpp" #include "Planners/LazyThetaStarM2.hpp" -#include "Planners/LazyThetaStarSIREN.hpp" #include "utils/ros/ROSInterfaces.hpp" #include "utils/SaveDataVariantToFile.hpp" #include "utils/misc.hpp" @@ -40,6 +37,8 @@ #include #include +#include +#include /** @@ -61,9 +60,15 @@ class HeuristicLocalPlannerROS configureAlgorithm(algorithm_name, heuristic_); // pointcloud_local_sub_ = lnh_.subscribe>("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); - pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); + // pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); + + path_local_sub_ = lnh_.subscribe("/planner_ros_node/global_path", 1, &HeuristicLocalPlannerROS::globalPathCallback, this); + + //GLOBAL POSITIONING SUBSCRIBER - for SDF query + // globalposition_local_sub_ = lnh_.subscribe("/ground_truth_to_tf/pose", 1, &HeuristicLocalPlannerROS::globalPositionCallback, this); // pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); //compile occupancy_grid_local_sub_ = lnh_.subscribe("/grid", 1, &HeuristicLocalPlannerROS::occupancyGridCallback, this); + //network_update_sub_ = lnh_.subscribe("/net_update", 1, &HeuristicLocalPlannerROS::networkUpdateCallback, this); // 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); @@ -71,44 +76,16 @@ class HeuristicLocalPlannerROS line_markers_pub_ = lnh_.advertise("path_line_markers", 1); point_markers_pub_ = lnh_.advertise("path_points_markers", 1); cloud_test = lnh_.advertise >("/cloud_PCL", 1, true); // Defined by me to show the point cloud as pcl::PointCloudisActive()) - // { - // clearMarkers(); - // return; - // } - - calculatePath3D(); - // state.reset(new actionlib::SimpleClientGoalState(navigation3DClient->getState())); - - // seconds = finishT.time - startT.time - 1; - // milliseconds = (1000 - startT.millitm) + finishT.millitm; - // publishExecutePathFeedback(); - - // if (*state == actionlib::SimpleClientGoalState::SUCCEEDED) - // { - // clearMarkers(); - // action_result.arrived = true; - // execute_path_srv_ptr->setSucceeded(action_result); - // ROS_ERROR("LocalPlanner: Goal Succed"); - // return; - // } - // else if (*state == actionlib::SimpleClientGoalState::ABORTED) - // { - // ROS_INFO_COND(debug, "Goal aborted by path tracker"); - // resetFlags(); - // } - // else if (*state == actionlib::SimpleClientGoalState::PREEMPTED) - // { - // ROS_INFO_COND(debug, "Goal preempted by path tracker"); - // resetFlags(); - // } - //ROS_INFO_COND(debug, "Before start loop calculation"); } @@ -124,6 +101,36 @@ class HeuristicLocalPlannerROS input_map_ = 1; } + void globalPathCallback(const heuristic_planners::CoordinateList::ConstPtr& msg) + { + std::cout << "Entered global path retrieving callback" << std::endl; + //Vaciamos la variable del path global anteriormente + global_path_.clear(); + + //Pasamos el mensaje a la variable global_path + for (const auto& vec : msg->coordinates) { + // Convert each your_package::Vec3i to Planners::utils::Vec3i + Planners::utils::Vec3i vec_intermedio; + vec_intermedio.x = vec.x; + vec_intermedio.y = vec.y; + vec_intermedio.z = vec.z; + + // Add the converted Vec3i to the path + global_path_.push_back(vec_intermedio); + } + + //Ponemos el flag de recepción a 1 + globalPathReceived_ = 1; + std::cout << "Global path successfully received" << std::endl; + std::cout << global_path_ << std::endl; + + } + + void localtimedCallback(const ros::TimerEvent& event) + { + std::cout << "-----TIMED CALLBACK------" << std::endl; + } + // From lazy_theta_star_planners // void pointCloudCallback(const PointCloud::ConstPtr &points) // void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &_points) @@ -231,7 +238,7 @@ class HeuristicLocalPlannerROS } bool PointCloud2_to_PointXYZ_pcl(sensor_msgs::PointCloud2 &in, pcl::PointCloud &out) - { + { sensor_msgs::PointCloud2Iterator iterX(in, "x"); sensor_msgs::PointCloud2Iterator iterY(in, "y"); sensor_msgs::PointCloud2Iterator iterZ(in, "z"); @@ -254,7 +261,7 @@ class HeuristicLocalPlannerROS } bool PointCloud2_to_PointXYZ(sensor_msgs::PointCloud2 &in, std::vector &out) - { + { sensor_msgs::PointCloud2Iterator iterX(in, "x"); sensor_msgs::PointCloud2Iterator iterY(in, "y"); sensor_msgs::PointCloud2Iterator iterZ(in, "z"); @@ -271,112 +278,9 @@ class HeuristicLocalPlannerROS } void calculatePath3D() -{ - if (mapReceived) // It is true once a pointcloud is received. { - // ROS_INFO("Local Planner 3D: Global trj received and pointcloud received"); - mapReceived = false; - - // //TODO : Set to robot pose to the center of the workspace? - // if (theta3D.setValidInitialPosition(robotPose) || theta3D.searchInitialPosition3d(initialSearchAround)) - // { - // ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Calculating local goal"); - - // if (calculateLocalGoal3D()) //TODO: VER CUAL SERIA EL LOCAL GOAL EN ESTE CASO - // { - // ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Local Goal calculated"); - - // // ROS_INFO("%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z); - // RCLCPP_INFO(this->get_logger(),"%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z); - - // if (!theta3D.isInside(localGoal)) - // { - // ROS_INFO("Returning, not inside :("); - // action_result.arrived=false; - // execute_path_srv_ptr->setAborted(action_result, "Not inside after second check"); - // navigation3DClient->cancelAllGoals(); - // return; - // } - - // if (theta3D.setValidFinalPosition(localGoal) || theta3D.searchFinalPosition3dAheadHorizontalPrior(finalSearchAround)) - // { - // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Computing Local Path"); - - // // JAC: Should we calculate the path with findpath (which return the path instead of the number of points)? - // number_of_points = theta3D.computePath(); - // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Path computed, %d points", number_of_points); - // occGoalCnt = 0; - - // if (number_of_points > 0) - // { - // buildAndPubTrayectory3D(); - // planningStatus.data = "OK"; - // if (impossibleCnt > 0) //If previously the local planner couldn t find solution, reset - // impossibleCnt = 0; - // } - // else if (number_of_points == 0) //!Esto es lo que devuelve el algoritmo cuando NO HAY SOLUCION - // { - - // impossibleCnt++; - // //ROS_INFO_COND(debug,"Local: +1 impossible"); - // if (impossibleCnt > 2) - // { - - // clearMarkers(); - // action_result.arrived=false; - // execute_path_srv_ptr->setAborted(action_result, "Requesting new global path, navigation cancelled"); - // navigation3DClient->cancelAllGoals(); - // planningStatus.data = "Requesting new global path, navigation cancelled"; - // impossibleCnt = 0; - // } - // } - // } - // else if (occGoalCnt > 2) //!Caso GOAL OCUPADO - // { //If it cant find a free position near local goal, it means that there is something there. - // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Pausing planning, final position busy"); - // planningStatus.data = "Final position Busy, Cancelling goal"; - // //TODO What to tell to the path tracker - // action_result.arrived=false; - // execute_path_srv_ptr->setAborted(action_result, "Local goal occupied"); - // //In order to resume planning, someone must call the pause/resume planning Service that will change the flag to true - // occGoalCnt = 0; - // } - // else - // { - // ++occGoalCnt; - // } - // } - // else if (badGoal < 3) - // { - // ROS_INFO_COND(debug, "Local Planner 3D: Bad Goal Calculated: [%.2f, %.2f]", localGoal.x, localGoal.y); - - // ++badGoal; - // } - // else - // { - // navigation3DClient->cancelAllGoals(); - // action_result.arrived=false; - // execute_path_srv_ptr->setAborted(action_result,"Bad goal calculated 3 times"); - // badGoal = 0; - // ROS_INFO("Bad goal calculated 3 times"); - // } - // } - // else - // { - // planningStatus.data = "No initial position found..."; - // action_result.arrived=false; - // execute_path_srv_ptr->setAborted(action_result,"No initial position found"); - // navigation3DClient->cancelAllGoals(); - - // clearMarkers(); - // ROS_INFO_COND(debug, "Local Planner 3D: No initial free position found"); - // } + std::cout << "------ENTERED CALCULATEPATH3D-------" << std::endl; } - // else - // { - // ROS_INFO("Map not received"); - // } -} bool setAlgorithm(heuristic_planners::SetAlgorithmRequest &_req, heuristic_planners::SetAlgorithmResponse &rep){ @@ -387,6 +291,7 @@ class HeuristicLocalPlannerROS void configureAlgorithm(const std::string &algorithm_name, const std::string &_heuristic){ + std::cout << "Entered configureAlgorithm" << std::endl; float ws_x, ws_y, ws_z; lnh_.param("local_world_size_x", ws_x, (float)2.0); // In meters @@ -512,6 +417,7 @@ class HeuristicLocalPlannerROS // Init internal variables: TF transform m_tfCache = false; } + void configureHeuristic(const std::string &_heuristic){ if( _heuristic == "euclidean" ){ @@ -534,6 +440,7 @@ class HeuristicLocalPlannerROS ROS_WARN("Wrong Heuristic param. Using Euclidean Heuristics by default"); } } + std::vector> getClosestObstaclesToPathPoints(const Planners::utils::CoordinateList &_path){ std::vector> result; @@ -549,6 +456,7 @@ class HeuristicLocalPlannerROS } return result; } + void configMarkers(const std::string &_ns, const std::string &_frame, const double &_scale){ path_line_markers_.ns = _ns; @@ -584,6 +492,7 @@ class HeuristicLocalPlannerROS path_points_markers_.scale.z = _scale; } + void publishMarker(visualization_msgs::Marker &_marker, const ros::Publisher &_pub){ //Clear previous marker @@ -620,7 +529,7 @@ class HeuristicLocalPlannerROS ros::NodeHandle lnh_{"~"}; ros::ServiceServer request_path_server_, change_planner_server_; - ros::Subscriber pointcloud_local_sub_, occupancy_grid_local_sub_; + 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; @@ -672,6 +581,14 @@ class HeuristicLocalPlannerROS int number_of_points; bool mapReceived; + //global path variable and flag + Planners::utils::CoordinateList global_path_; + int globalPathReceived_; + + + // local pathplanner loop timer + ros::Timer timed_local_path_; + }; // int main(int argc, char **argv) // { @@ -697,11 +614,9 @@ int main(int argc, char **argv) ros::Rate loop_rate(30); while(ros::ok()){ - ros::spinOnce(); - // Call to Local Plan - heuristic_local_planner_ros.plan(); + // heuristic_local_planner_ros.plan(); loop_rate.sleep(); }