bool VoronoiPlanner::computePlan(costmap_2d::Costmap2D* costmap_, DynamicVoronoi * voronoi_,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan) {
//Sure that plan is clear
plan.clear();
ros::NodeHandle n;
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
worldToMap(costmap_, wx, wy, start_x, start_y);
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
worldToMap(costmap_, wx, wy, goal_x, goal_y);
clearRobotCell(costmap_, start_x_i, start_y_i);
bool ** map;
computeVoronoi(voronoi_, costmap_, map);
ros::Time t_b = ros::Time::now();
ros::Time t = ros::Time::now();
std::vector<std::pair<float, float> > path1;
std::vector<std::pair<float, float> > path2;
std::vector<std::pair<float, float> > path3;
ROS_INFO("start_x %f, start_y %f", start_x, start_y);
bool res1 = false, res2 = false, res3 = false;
// If goal not are in cell of the vornoi diagram, we have that best findPath of goal to voronoi diagram without have a cell occupancie
if (!voronoi_->isVoronoi(goal_x, goal_y)) {
res3 = computePath(&path3, goal_x, goal_y, start_x, start_y, voronoi_, 0,
1);
std::cout << "computePath goal to VD " << res3 << std::endl;
goal_x = std::get < 0 > (path3[path3.size() - 1]);
goal_y = std::get < 1 > (path3[path3.size() - 1]);
ROS_INFO("Is voronoi goal compute %d", voronoi_->isVoronoi(goal_x, goal_y));
std::reverse(path3.begin(), path3.end());
}
if (!voronoi_->isVoronoi(start_x, start_y)) {
res1 = computePath(&path1, start_x, start_y, goal_x, goal_y, voronoi_, 0,
1);
std::cout << "computePath start to VD " << res1 << std::endl;
start_x = std::get < 0 > (path1[path1.size() - 1]);
start_y = std::get < 1 > (path1[path1.size() - 1]);
ROS_INFO("Is voronoi start compute %d", voronoi_->isVoronoi(start_x, start_y));
}
res2 = computePath(&path2, start_x, start_y, goal_x, goal_y, voronoi_, 1, 0);
ROS_INFO("computePath %d", res2);
if (!(res1 && res2 && res3)) {
ROS_INFO("Failed to compute full path");
}
path1.insert(path1.end(), path2.begin(), path2.end());
path1.insert(path1.end(), path3.begin(), path3.end());
/*for (int i = 0; i < path1.size(); i++) {
int x = std::get < 0 > (path1[i]);
int y = std::get < 1 > (path1[i]);
if (x > 0 && y > 0)
map[x][y] = 1;
}*/
smoothPath(&path1);
for (int i = 0; i < path1.size(); i++) {
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);
new_goal.pose.position.x = std::get < 0 > (path1[i]);
new_goal.pose.position.y = std::get < 1 > (path1[i]);
mapToWorld(costmap_, new_goal.pose.position.x, new_goal.pose.position.y,
new_goal.pose.position.x, new_goal.pose.position.y);
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
}
ROS_INFO("\nTime to get plan: %f sec\n", (ros::Time::now() - t_b).toSec());
return !plan.empty();
}
/*
*----------------------------------------------------------
* MainMenu
*----------------------------------------------------------
*/
void mainMenu(int item)
{
switch(item){
case 0:
/* HACK! Moves the camera back by globalScale amount */
Prim[WORLD].trans[Z] *= Prim[WORLD].cosine.z;
showLandMarks = FALSE;
showPrimitives = FALSE;
panSpeed *= 16.0;
break;
case 1:
lightFlag = !lightFlag;
break;
case 2:
windowSplitFlag = !windowSplitFlag;
break;
case 3:
parametrize();
break;
case 4:
textureFlag = !textureFlag;
if ( textureFlag )
{
if ( !textCoordComputed )
{
compTextCoordinates();
}
glEnable(GL_TEXTURE_2D);
}
else
glDisable(GL_TEXTURE_2D);
break;
case 5:
computeVoronoi();
optimizeVoronoiVertex();
break;
case 6: /* added by Thompson 21/05/2002 */
cellPicking = !cellPicking;
if( cellPicking )
glutSetCursor( GLUT_CURSOR_INFO );
else
glutSetCursor( GLUT_CURSOR_LEFT_ARROW );
break;
case 7:{
polygonPicking = !polygonPicking;
if ( polygonPicking )
{glutSetCursor( GLUT_CURSOR_INFO );}
else {glutSetCursor( GLUT_CURSOR_LEFT_ARROW );}
}
break;
case 8:
drawCells = TRUE;
createRandomCells( NumberCells, FALSE );
break;
case 9:
duplicatePrimitive();
break;
case 10:
growthFlag = !growthFlag;
break;
case 11:
animFlag = !animFlag;
break;
case 12:
exit(0);
case 13:
optimizeVoronoiPoligons2();
break;
default:
break;
}
glutPostRedisplay();
}
