bool EpicNavigationNodeOMPL::srvRemoveGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res)
{
if (occupancy_grid == nullptr) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvRemoveGoals]: Occupancy grid was not initialized.");
res.success = false;
return false;
}
if (req.goals.size() != 1) {
ROS_ERROR("Warning[EpicNavigationNodeOMPL::srvRemoveGoals]: Invalid number of goal locations.");
res.success = false;
return false;
}
float x = 0.0f;
float y = 0.0f;
worldToMap(req.goals[0].pose.position.x, req.goals[0].pose.position.y, x, y);
// If the goal location is not a goal, then we can just return without changing anything.
if (!isCellGoal((unsigned int)(x + 0.5f), (unsigned int)(y + 0.5f))) {
return ;
}
occupancy_grid[(unsigned int)(y + 0.5f) * width + (unsigned int)(x + 0.5f)] = EPIC_CELL_TYPE_FREE;
goal_assigned = false;
// Try to create the planner algorithm now.
initAlg();
res.success = true;
return true;
}
bool EpicNavigationNodeOMPL::srvComputePath(epic::ComputePath::Request &req, epic::ComputePath::Response &res)
{
// First, determine and assign the starting location.
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(req.start.pose.position.x, req.start.pose.position.y, x, y)) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvComputePath]: Could not convert start to floating point map location.");
}
start_location.first = x;
start_location.second = y;
start_assigned = true;
// Try to create the planner algorithm now that we have an initial starting state.
initAlg();
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvComputePath]: Algorithm was not initialized.");
return false;
}
unsigned int k = 0;
float *raw_path = nullptr;
if ((bool)ompl_planner_status) {
// TODO: If an approximate or exact solution was found, then populate raw_path. Otherwise, leave it empty.
//k = ???
//raw_path = new float[2 * k];
//for (...
// ompl_planner.get_path...???
}
res.path.header.frame_id = req.start.header.frame_id;
res.path.header.stamp = req.start.header.stamp;
res.path.poses.resize(k);
res.path.poses[0] = req.start;
for (unsigned int i = 1; i < k; i++) {
x = raw_path[2 * i + 0];
y = raw_path[2 * i + 1];
float path_theta = std::atan2(y - raw_path[2 * (i - 1) + 1], x - raw_path[2 * (i - 1) + 0]);
float path_x = 0.0f;
float path_y = 0.0f;
mapToWorld(x, y, path_x, path_y);
res.path.poses[i] = req.start;
res.path.poses[i].pose.position.x = path_x;
res.path.poses[i].pose.position.y = path_y;
res.path.poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(path_theta);
}
delete [] raw_path;
raw_path = nullptr;
return true;
}
void initAlg (FILE *archivo, FILE *archivo2) {
char cadena[100];
if (fgets(cadena, 100, archivo) != 0) {
putCadena(cadena, archivo2);
initAlg(archivo, archivo2);
}
}
int main() {
FILE *archivo, *archivo2;
char *temporal;
archivo = fopen("texto.txt", "r");
archivo2 = fopen("destino.txt", "w+");
initAlg(archivo, archivo2);
fclose(archivo);
fclose(archivo2);
return 0;
}
void EpicNavigationNodeHarmonic::subOccupancyGrid(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
// If the size changed, then free everything and start over.
// Note we don't care if the resolution or offsets change.
if (msg->info.width != width || msg->info.height != height) {
uninitAlg();
initAlg(msg->info.width, msg->info.height);
}
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::subOccupancyGrid]: Algorithm was not initialized.");
return;
}
// These only affect the final path, not the path computation.
x_origin = msg->info.origin.position.x;
y_origin = msg->info.origin.position.y;
resolution = msg->info.resolution;
// Copy the data to the Harmonic object.
std::vector<unsigned int> v;
std::vector<unsigned int> types;
for (unsigned int y = 1; y < height - 1; y++) {
for (unsigned int x = 1; x < width - 1; x++) {
if (msg->data[y * width + x] == EPIC_OCCUPANCY_GRID_NO_CHANGE || isCellGoal(x, y)) {
// Do nothing.
} else if (msg->data[y * width + x] >= EPIC_OCCUPANCY_GRID_OBSTACLE_THRESHOLD) {
v.push_back(x);
v.push_back(y);
types.push_back(EPIC_CELL_TYPE_OBSTACLE);
} else {
v.push_back(x);
v.push_back(y);
types.push_back(EPIC_CELL_TYPE_FREE);
}
}
}
setCells(v, types);
v.clear();
types.clear();
}
bool EpicNavigationNodeOMPL::srvAddGoals(epic::ModifyGoals::Request &req, epic::ModifyGoals::Response &res)
{
if (occupancy_grid == nullptr) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvAddGoals]: Occupancy grid was not initialized.");
res.success = false;
return false;
}
if (req.goals.size() != 1) {
ROS_ERROR("Warning[EpicNavigationNodeOMPL::srvAddGoals]: Invalid number of goal locations.");
res.success = false;
return false;
}
float x = 0.0f;
float y = 0.0f;
worldToMap(req.goals[0].pose.position.x, req.goals[0].pose.position.y, x, y);
// If the goal location is an obstacle, then do not let it add a goal here.
if (isCellObstacle((unsigned int)(x + 0.5f), (unsigned int)(y + 0.5f))) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvAddGoals]: Attempted to add a goal at an obstacle.");
res.success = false;
return false;
}
goal_location.first = x;
goal_location.second = y;
goal_assigned = true;
occupancy_grid[(unsigned int)(y + 0.5f) * width + (unsigned int)(x + 0.5f)] = EPIC_CELL_TYPE_OBSTACLE;
// Try to create the planner algorithm now.
initAlg();
res.success = true;
return true;
}
bool EpicNavigationNodeOMPL::srvSetCells(epic::SetCells::Request &req, epic::SetCells::Response &res)
{
if (occupancy_grid == nullptr) {
ROS_WARN("Warning[EpicNavigationNodeOMPL::srvSetCells]: Occupancy grid was not initialized.");
res.success = false;
return false;
}
for (unsigned int i = 0; i < req.types.size(); i++) {
// Note: For the SetCells service, these are assigning the raw cells, not picking world coordinates.
// Thus, no worldToMap translation is required.
unsigned int x = req.v[2 * i + 0];
unsigned int y = req.v[2 * i + 1];
if (x >= width || y >= height) {
continue;
}
if (req.types[i] == EPIC_CELL_TYPE_GOAL || req.types[i] == EPIC_CELL_TYPE_OBSTACLE || req.types[i] == EPIC_CELL_TYPE_FREE) {
occupancy_grid[(unsigned int)(y + 0.5f) * width + (unsigned int)(x + 0.5f)] = req.types[i];
}
if (req.types[i] == EPIC_CELL_TYPE_GOAL) {
goal_location.first = x;
goal_location.second = y;
goal_assigned = true;
}
}
// Try to create the planner algorithm now.
initAlg();
res.success = true;
return true;
}
bool EdmondMatching::augmentMatching (int n){ //The main analyzing function, with the
int node,nodeLabel; //goal of finding augmenting paths or
initAlg(n); //concluding that the matching is maximum.
for (int i=0; i<n; i++) if (!match[i]){
label[i].even=empty;
enqueue(i,0); //Initialize the queue with the exposed vertices,
} //making them the roots in the forest.
while (queueFront<queueBack){
node=queue[queueFront].vertex;
nodeLabel=queue[queueFront].type;
if (nodeLabel==0){
for (int i=0; i<n; i++) if (g[node][i]==unmatched){
if (blossom[node]==blossom[i]);
//Do nothing. Edges inside the same blossom have no meaning.
else if (label[i].even!=undef){
/*
The tree has reached a vertex with a label.
The parity of this label indicates that an odd length
alternating path has been found. If this path is between
roots, we have an augmenting path, else there's an
alternating cycle, a blossom.
*/
endPath[0]=endPath[1]=0;
backtrace(node,0,empty,0,reverse);
backtrace(i,1,empty,0,reverse);
//Call the backtracing function to find out.
if (path[0][0]==path[1][0]) newBlossom(node,i);
/*
If the same root node is reached, a blossom was found.
Start the labelling procedure to create pseudo-contraction.
*/
else {
augmentPath();
return true;
/*
If the roots are different, we have an augmenting path.
Improve the matching by augmenting this path.
Now some labels might make no sense, stop the function,
returning that it was successful in improving.
*/
}
}
else if (label[i].even==undef && label[i].odd[0]==undef){
//If an unseen vertex is found, report the existing path
//by labeling it accordingly.
label[i].odd[0]=node;
enqueue(i,1);
}
}
}
else if (nodeLabel==1){ //Similar to above.
for (int i=0; i<n; i++) if (g[node][i]==matched){
if (blossom[node]==blossom[i]);
else if (label[i].odd[0]!=undef){
endPath[0]=endPath[1]=0;
backtrace(node,0,empty,1,reverse);
backtrace(i,1,empty,1,reverse);
if (path[0][0]==path[1][0]) newBlossom(node,i);
else {
augmentPath();
return true;
}
}
else if (label[i].even==undef && label[i].odd[0]==undef){
label[i].even=node;
enqueue(i,0);
}
}
}
/*
The scanning of this label is complete, dequeue it and
keep going to the next one.
*/
queueFront++;
}
/*
If the function reaches this point, the queue is empty, all
labels have been scanned. The algorithm couldn't find an augmenting
path. Therefore, it concludes the matching is maximum.
*/
return false;
}
