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 StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y)
{
if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
return;
useExtraBounds(min_x, min_y, max_x, max_y);
double wx, wy;
mapToWorld(x_, y_, wx, wy);
*min_x = std::min(wx, *min_x);
*min_y = std::min(wy, *min_y);
mapToWorld(x_ + width_, y_ + height_, wx, wy);
*max_x = std::max(wx, *max_x);
*max_y = std::max(wy, *max_y);
has_updated_data_ = false;
}
void DynamicEDTOctomap::getDistanceAndClosestObstacle_unsafe(const octomap::point3d& p, float &distance, octomap::point3d& closestObstacle) const {
int x,y,z;
worldToMap(p, x, y, z);
dataCell c= data[x][y][z];
distance = c.dist*treeResolution;
if(c.obstX != invalidObstData){
mapToWorld(c.obstX, c.obstY, c.obstZ, closestObstacle);
} else {
//If we are at maxDist, it can very well be that there is no valid closest obstacle data for this cell, this is not an error.
}
}
gm::Polygon cellPolygon (const nm::MapMetaData& info, const Cell& c)
{
const float dx[4] = {0.0, 0.0, 1.0, 1.0};
const float dy[4] = {0.0, 1.0, 1.0, 0.0};
const tf::Transform trans = mapToWorld(info);
gm::Polygon p;
p.points.resize(4);
for (unsigned i=0; i<4; i++) {
tf::Point pt((c.x+dx[i])*info.resolution, (c.y+dy[i])*info.resolution, 0.0);
tf::Point transformed = trans*pt;
p.points[i].x = transformed.x();
p.points[i].y = transformed.y();
p.points[i].z = transformed.z();
}
return p;
}
bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) {
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
std::string global_frame = frame_id_;
//clear the plan, just in case
plan.clear();
std::vector<std::pair<float, float> > path;
if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {
ROS_ERROR("NO PATH!");
return false;
}
ros::Time plan_time = ros::Time::now();
for (int i = path.size() -1; i>=0; i--) {
std::pair<float, float> point = path[i];
//convert the plan to world coordinates
double world_x, world_y;
mapToWorld(point.first, point.second, world_x, world_y);
geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = global_frame;
pose.pose.position.x = world_x;
pose.pose.position.y = world_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
plan.push_back(pose);
}
if(old_navfn_behavior_){
plan.push_back(goal);
}
return !plan.empty();
}
gm::Polygon gridPolygon (const nm::MapMetaData& info)
{
const float x[4] = {0.0, 0.0, 1.0, 1.0};
const float y[4] = {0.0, 1.0, 1.0, 0.0};
const tf::Transform trans = mapToWorld(info);
gm::Polygon p;
p.points.resize(4);
for (unsigned i=0; i<4; i++) {
tf::Point pt(x[i]*info.resolution*info.width, y[i]*info.resolution*info.height, 0.0);
tf::Point transformed=trans*pt;
p.points[i].x = transformed.x();
p.points[i].y = transformed.y();
p.points[i].z = transformed.z();
}
return p;
}
bool DynamicEDTOctomap::checkConsistency() const {
for(octomap::KeyBoolMap::const_iterator it = octree->changedKeysBegin(), end=octree->changedKeysEnd(); it!=end; ++it){
//std::cerr<<"Cannot check consistency, you must execute the update() method first."<<std::endl;
return false;
}
for(int x=0; x<sizeX; x++){
for(int y=0; y<sizeY; y++){
for(int z=0; z<sizeZ; z++){
octomap::point3d point;
mapToWorld(x,y,z,point);
octomap::OcTreeNode* node = octree->search(point);
bool mapOccupied = isOccupied(x,y,z);
bool treeOccupied = false;
if(node){
treeOccupied = octree->isNodeOccupied(node);
} else {
if(unknownOccupied)
treeOccupied = true;
}
if(mapOccupied != treeOccupied){
//std::cerr<<"OCCUPANCY MISMATCH BETWEEN TREE AND MAP at "<<x<<","<<y<<","<<z<<std::endl;
//std::cerr<<"Tree "<<treeOccupied<<std::endl;
//std::cerr<<"Map "<<mapOccupied<<std::endl;
return false;
}
}
}
}
return true;
}
void Costmap2D::replaceStaticMapWindow(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y,
const std::vector<unsigned char>& static_data){
unsigned int start_x, start_y;
if(!worldToMap(win_origin_x, win_origin_y, start_x, start_y) || (start_x + data_size_x) > size_x_ || (start_y + data_size_y) > size_y_){
ROS_ERROR("You must call replaceStaticMapWindow with a window origin and size that is contained within the map");
return;
}
//we need to compute the region of the costmap that could change from inflation of new obstacles
unsigned int max_inflation_change = 2 * cell_inflation_radius_;
//make sure that we don't go out of map bounds
unsigned int copy_sx = std::min(std::max(0, (int)start_x - (int)max_inflation_change), (int)size_x_);
unsigned int copy_sy = std::min(std::max(0, (int)start_y - (int)max_inflation_change), (int)size_x_);
unsigned int copy_ex = std::max(std::min((int)size_x_, (int)start_x + (int)data_size_x + (int)max_inflation_change), 0);
unsigned int copy_ey = std::max(std::min((int)size_y_, (int)start_y + (int)data_size_y + (int)max_inflation_change), 0);
unsigned int copy_size_x = copy_ex - copy_sx;
unsigned int copy_size_y = copy_ey - copy_sy;
//now... we have to compute the window
double ll_x, ll_y, ur_x, ur_y;
mapToWorld(copy_sx, copy_sy, ll_x, ll_y);
mapToWorld(copy_ex, copy_ey, ur_x, ur_y);
double mid_x = (ll_x + ur_x) / 2;
double mid_y = (ll_y + ur_y) / 2;
double size_x = ur_x - ll_x;
double size_y = ur_y - ll_y;
//finally... we'll clear all non-lethal costs in the area that could be affected by the map update
//we'll reinflate after the map update is complete
clearNonLethal(mid_x, mid_y, size_x, size_y);
//copy static data into the costmap
unsigned int start_index = start_y * size_x_ + start_x;
unsigned char* costmap_index = costmap_ + start_index;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
for(unsigned int i = 0; i < data_size_y; ++i){
for(unsigned int j = 0; j < data_size_x; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
++costmap_index;
++static_data_index;
}
costmap_index += size_x_ - data_size_x;
}
//now, we're ready to reinflate obstacles in the window that has been updated
//we won't clear all non-lethal obstacles first because the static map update
//may have included non-lethal costs
reinflateWindow(mid_x, mid_y, size_x, size_y, false);
//we also want to keep a copy of the current costmap as the static map... we'll only need to write the region that has changed
copyMapRegion(costmap_, copy_sx, copy_sy, size_x_, static_map_, copy_sx, copy_sy, size_x_, copy_size_x, copy_size_y);
}
bool EpicNavigationNodeHarmonic::srvComputePath(epic::ComputePath::Request &req, epic::ComputePath::Response &res)
{
if (!init_alg) {
ROS_WARN("Warning[EpicNavigationNodeHarmonic::srvComputePath]: Algorithm was not initialized.");
return false;
}
if (gpu) {
if (harmonic_get_potential_values_gpu(&harmonic) != EPIC_SUCCESS) {
ROS_WARN("Error[EpicNavigationNodeHarmonic::srvComputePath]: Failed to get the potential values from the GPU.");
return false;
}
}
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[EpicNavigationNodeHarmonic::srvComputePath]: Could not convert start to floating point map location.");
}
unsigned int k = 0;
float *raw_path = nullptr;
int result = harmonic_compute_path_2d_cpu(&harmonic, x, y, req.step_size, req.precision, req.max_length, k, raw_path);
if (result != EPIC_SUCCESS) {
ROS_ERROR("Error[EpicNavigationNodeHarmonic::srvComputePath]: Failed to compute the path.");
if (raw_path != nullptr) {
delete [] raw_path;
}
return false;
}
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;
}
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();
}
bool EpicNavCorePlugin::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan)
{
if (!initialized) {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: EpicNavCorePlugin has not been initialized yet.");
return false;
}
// Set the goal location on the potential map.
unsigned int x_coord = 0;
unsigned int y_coord = 0;
if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, x_coord, y_coord)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert goal to cost map location.");
x_coord = 0;
y_coord = 0;
}
setGoal(x_coord, y_coord);
ROS_INFO("Information[EpicNavCorePlugin::makePlan]: Solving harmonic function...");
int result = harmonic_complete_gpu(&harmonic, NUM_THREADS_GPU);
if (result != EPIC_SUCCESS) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not execute GPU version of 'epic' library.");
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Trying CPU fallback...");
result = harmonic_complete_cpu(&harmonic);
}
if (result == EPIC_SUCCESS) {
ROS_INFO("Information[EpicNavCorePlugin::makePlan]: Successfully solved harmonic function!");
} else {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: Failed to solve harmonic function.");
return false;
}
//pub_potential.publish(harmonic.u);
plan.clear();
plan.push_back(start);
/*
if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, xCoord, yCoord)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert start to cost map location.");
xCoord = 0;
yCoord = 0;
}
*/
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(start.pose.position.x, start.pose.position.y, x, y)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert start to floating point cost map location.");
}
float step_size = 0.05f;
float cd_precision = 0.5f;
unsigned int max_length = harmonic.m[0] * harmonic.m[1] / step_size;
unsigned int k = 0;
float *raw_plan = nullptr;
result = harmonic_compute_path_2d_cpu(&harmonic, x, y, step_size, cd_precision, max_length, k, raw_plan);
if (result != EPIC_SUCCESS) {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: Failed to compute the path.");
if (raw_plan != nullptr) {
delete [] raw_plan;
}
return false;
}
geometry_msgs::PoseStamped new_goal = goal;
for (unsigned int i = 1; i < k; i++) {
float x = raw_plan[2 * i + 0];
float y = raw_plan[2 * i + 1];
float plan_theta = std::atan2(y - raw_plan[2 * (i - 1) + 1], x - raw_plan[2 * (i - 1) + 0]);
float plan_x = 0.0f;
float plan_y = 0.0f;
mapToWorld(x, y, plan_x, plan_y);
new_goal.pose.position.x = plan_x;
new_goal.pose.position.y = plan_y;
new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(plan_theta);
plan.push_back(new_goal);
}
delete [] raw_plan;
raw_plan = nullptr;
plan.push_back(goal);
publishPlan(plan);
return true;
}
