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();
}