ROS:Dijkstra全局规划器
#ifndef _DIJKSTRA_PLANNER_H_
#define _DIJKSTRA_PLANNER_H_
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#define push_cur(n) { if (n>=0 && n
#define push_next(n) { if (n>=0 && n
#define push_over(n) { if (n>=0 && n
namespace simple_local_planner {
#define PRIORITYBUFSIZE 10000
#define POT_HIGH 1.0e10
#define INVSQRT2 0.707106781
class QuadraticCalculator
{
public:
QuadraticCalculator(int nx, int ny)
{
setSize(nx,ny);
}
~QuadraticCalculator(){}
float calculatePotential(float *potential, unsigned char cost, int n, float prev_potential = 0);
void setSize(int nx, int ny)
{
nx_ = nx;
ny_ = ny;
ns_ = nx * ny;
}
protected:
inline int toIndex(int x, int y)
{
return x + nx_ * y;
}
int nx_, ny_, ns_;
};
class DijkstraExpansion
{
public:
DijkstraExpansion(QuadraticCalculator* p_calc, int nx, int ny);
~DijkstraExpansion();
bool calculatePotentials(unsigned char* costs,
double start_x, double start_y,
double end_x, double end_y,
int cycles,
float* potential);
void setSize(int nx, int ny);
void setNeutralCost(unsigned char neutral_cost)
{
neutral_cost_ = neutral_cost;
priorityIncrement_ = 2 * neutral_cost_;
}
void setPreciseStart(bool precise){ precise_ = precise; }
private:
void updateCell(unsigned char* costs, float* potential, int n);
float getCost(unsigned char* costs, int n)
{
float c = costs[n];
if (c < lethal_cost_ - 1 || (unknown_ && c==255))
{
c = c * factor_ + neutral_cost_;
if (c >= lethal_cost_)
c = lethal_cost_ - 1;
return c;
}
return lethal_cost_;
}
int *buffer1_, *buffer2_, *buffer3_;
int *currentBuffer_, *nextBuffer_, *overBuffer_;
int currentEnd_, nextEnd_, overEnd_;
bool *pending_;
bool precise_;
float threshold_;
float priorityIncrement_;
public:
void setLethalCost(unsigned char lethal_cost)
{
lethal_cost_ = lethal_cost;
}
void setFactor(float factor)
{
factor_ = factor;
}
void setHasUnknown(bool unknown)
{
unknown_ = unknown;
}
void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s)
{
int startCell = toIndex(gx, gy);
for(int i=-s;i<=s;i++)
{
for(int j=-s;j<=s;j++)
{
int n = startCell+i+nx_*j;
if(potential[n]<POT_HIGH)
continue;
float c = costs[n]+neutral_cost_;
float pot = p_calc_->calculatePotential(potential, c, n);
potential[n] = pot;
}
}
}
protected:
inline int toIndex(int x, int y)
{
return x + nx_ * y;
}
int nx_, ny_, ns_;
bool unknown_;
unsigned char lethal_cost_, neutral_cost_;
int cells_visited_;
float factor_;
QuadraticCalculator* p_calc_;
};
class GradientPath
{
public:
GradientPath(QuadraticCalculator* p_calc);
~GradientPath();
void setSize(int xs, int ys);
bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path);
private:
inline int getNearestPoint(int stc, float dx, float dy)
{
int pt = stc + (int)round(dx) + (int)(xs_ * round(dy));
return std::max(0, std::min(xs_ * ys_ - 1, pt));
}
float gradCell(float* potential, int n);
float *gradx_, *grady_;
float pathStep_;
public:
inline int getIndex(int x, int y)
{
return x + y * xs_;
}
void setLethalCost(unsigned char lethal_cost)
{
lethal_cost_ = lethal_cost;
}
protected:
int xs_, ys_;
unsigned char lethal_cost_;
QuadraticCalculator* p_calc_;
};
enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD };
class OrientationFilter
{
public:
OrientationFilter() : omode_(NONE) {}
void processPath(const geometry_msgs::PoseStamped& start,
std::vector<geometry_msgs::PoseStamped>& path);
void setAngleBasedOnPositionDerivative(std::vector<geometry_msgs::PoseStamped>& path, int index);
void interpolate(std::vector<geometry_msgs::PoseStamped>& path,
int start_index, int end_index);
void setMode(OrientationMode new_mode){ omode_ = new_mode; }
void setWindowSize(int window_size){ window_size_ = window_size; }
protected:
OrientationMode omode_;
int window_size_;
};
class DijkstraPlanner
{
public:
DijkstraPlanner();
DijkstraPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
~DijkstraPlanner();
void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
bool makePlan(const geometry_msgs::PoseStamped start,
const geometry_msgs::PoseStamped goal,
double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan);
int optimizationPath(std::vector<geometry_msgs::PoseStamped>& plan,double movement_angle_range);
bool computePotential(const geometry_msgs::Point& world_point);
bool getPlanFromPotential(double start_x, double start_y,
double end_x, double end_y,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
double getPointPotential(const geometry_msgs::Point& world_point);
bool validPointPotential(const geometry_msgs::Point& world_point);
bool validPointPotential(const geometry_msgs::Point& world_point, double tolerance);
protected:
costmap_2d::Costmap2D* costmap_;
std::string frame_id_;
bool initialized_, allow_unknown_;
private:
void mapToWorld(double mx, double my, double& wx, double& wy);
bool worldToMap(double wx, double wy, double& mx, double& my);
void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
void publishPotential(float* potential);
double inline normalizeAngle(double val,double min = -M_PI,double max = M_PI)
{
float norm = 0.0;
if (val >= min)
norm = min + fmod((val - min), (max-min));
else
norm = max - fmod((min - val), (max-min));
return norm;
}
double planner_window_x_, planner_window_y_, default_tolerance_;
boost::mutex mutex_;
QuadraticCalculator* p_calc_;
DijkstraExpansion* planner_;
GradientPath* path_maker_;
OrientationFilter* orientation_filter_;
ros::Publisher potential_pub_;
int publish_scale_;
void outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value);
unsigned char* cost_array_;
float* potential_array_;
unsigned int start_x_, start_y_, end_x_, end_y_;
float convert_offset_;
};
}
#endif
#include
namespace simple_local_planner{
float QuadraticCalculator::calculatePotential(float *potential, unsigned char cost, int n, float prev_potential)
{
prev_potential = 0;
float u, d, l, r;
l = potential[n - 1];
r = potential[n + 1];
u = potential[n - nx_];
d = potential[n + nx_];
float ta, tc;
if (l < r)
tc = l;
else
tc = r;
if (u < d)
ta = u;
else
ta = d;
float hf = cost;
float dc = tc - ta;
if (dc < 0)
{
dc = -dc;
ta = tc;
}
if (dc >= hf)
return ta + hf;
else
{
float d = dc / hf;
float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
return ta + hf * v;
}
}
DijkstraExpansion::DijkstraExpansion(QuadraticCalculator *p_calc, int nx, int ny):
pending_(nullptr),unknown_(true), lethal_cost_(253), neutral_cost_(50), factor_(3.0), p_calc_(p_calc)
{
setSize(nx, ny);
buffer1_ = new int[PRIORITYBUFSIZE];
buffer2_ = new int[PRIORITYBUFSIZE];
buffer3_ = new int[PRIORITYBUFSIZE];
priorityIncrement_ = 2 * neutral_cost_;
}
DijkstraExpansion::~DijkstraExpansion()
{
delete[] buffer1_;
delete[] buffer2_;
delete[] buffer3_;
if (pending_)
delete[] pending_;
}
void DijkstraExpansion::setSize(int xs, int ys)
{
nx_ = xs;
ny_ = ys;
ns_ = xs * ys;
if (pending_)
delete[] pending_;
pending_ = new bool[ns_];
memset(pending_, 0, ns_ * sizeof(bool));
}
bool DijkstraExpansion::calculatePotentials(unsigned char* costs,
double start_x, double start_y,
double end_x, double end_y,
int cycles,
float* potential)
{
cells_visited_ = 0;
threshold_ = lethal_cost_;
currentBuffer_ = buffer1_;
currentEnd_ = 0;
nextBuffer_ = buffer2_;
nextEnd_ = 0;
overBuffer_ = buffer3_;
overEnd_ = 0;
memset(pending_, 0, ns_ * sizeof(bool));
std::fill(potential, potential + ns_, POT_HIGH);
int k = toIndex(start_x, start_y);
if(precise_)
{
double dx = start_x - (int)start_x, dy = start_y - (int)start_y;
dx = floorf(dx * 100 + 0.5) / 100;
dy = floorf(dy * 100 + 0.5) / 100;
potential[k] = neutral_cost_ * 2 * dx * dy;
potential[k+1] = neutral_cost_ * 2 * (1-dx)*dy;
potential[k+nx_] = neutral_cost_*2*dx*(1-dy);
potential[k+nx_+1] = neutral_cost_*2*(1-dx)*(1-dy);
push_cur(k+2);
push_cur(k-1);
push_cur(k+nx_-1);
push_cur(k+nx_+2);
push_cur(k-nx_);
push_cur(k-nx_+1);
push_cur(k+nx_*2);
push_cur(k+nx_*2+1);
}
else
{
potential[k] = 0;
push_cur(k+1);
push_cur(k-1);
push_cur(k-nx_);
push_cur(k+nx_);
}
int nwv = 0;
int nc = 0;
int cycle = 0;
int startCell = toIndex(end_x, end_y);
for (; cycle < cycles; cycle++)
{
if (currentEnd_ == 0 && nextEnd_ == 0)
return false;
nc += currentEnd_;
if (currentEnd_ > nwv)
nwv = currentEnd_;
int *pb = currentBuffer_;
int i = currentEnd_;
while (i-- > 0)
pending_[*(pb++)] = false;
pb = currentBuffer_;
i = currentEnd_;
while (i-- > 0)
updateCell(costs, potential, *pb++);
currentEnd_ = nextEnd_;
nextEnd_ = 0;
pb = currentBuffer_;
currentBuffer_ = nextBuffer_;
nextBuffer_ = pb;
if (currentEnd_ == 0)
{
threshold_ += priorityIncrement_;
currentEnd_ = overEnd_;
overEnd_ = 0;
pb = currentBuffer_;
currentBuffer_ = overBuffer_;
overBuffer_ = pb;
}
if (potential[startCell] < POT_HIGH)
break;
}
if (cycle < cycles)
return true;
else
return false;
}
inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential, int n)
{
cells_visited_++;
float c = getCost(costs, n);
if (c >= lethal_cost_)
return;
float c_near[8] = {0};
int np = n - nx_;
int nn = n + nx_;
c_near[0] = getCost(costs, np-1);
c_near[1] = getCost(costs, np);
c_near[2] = getCost(costs, np+1);
c_near[3] = getCost(costs, n-1);
c_near[4] = getCost(costs, n+1);
c_near[5] = getCost(costs, nn-1);
c_near[6] = getCost(costs, nn);
c_near[7] = getCost(costs, nn+1);
for(unsigned int i=0;i<8;i++)
{
if(c_near[i] > 50)
{
return;
}
}
float pot = p_calc_->calculatePotential(potential, c, n);
if (pot < potential[n])
{
float le = INVSQRT2 * (float)getCost(costs, n - 1);
float re = INVSQRT2 * (float)getCost(costs, n + 1);
float ue = INVSQRT2 * (float)getCost(costs, n - nx_);
float de = INVSQRT2 * (float)getCost(costs, n + nx_);
potential[n] = pot;
if (pot < threshold_)
{
if (potential[n - 1] > pot + le)
push_next(n-1);
if (potential[n + 1] > pot + re)
push_next(n+1);
if (potential[n - nx_] > pot + ue)
push_next(n-nx_);
if (potential[n + nx_] > pot + de)
push_next(n+nx_);
}
else
{
if (potential[n - 1] > pot + le)
push_over(n-1);
if (potential[n + 1] > pot + re)
push_over(n+1);
if (potential[n - nx_] > pot + ue)
push_over(n-nx_);
if (potential[n + nx_] > pot + de)
push_over(n+nx_);
}
}
}
GradientPath::GradientPath(QuadraticCalculator *p_calc) : pathStep_(0.5)
{
p_calc_ = p_calc;
gradx_ = nullptr;
grady_ = nullptr;
}
GradientPath::~GradientPath()
{
if (gradx_)
delete[] gradx_;
if (grady_)
delete[] grady_;
}
void GradientPath::setSize(int xs, int ys)
{
xs_ = xs;
ys_ = ys;
if (gradx_)
delete[] gradx_;
if (grady_)
delete[] grady_;
gradx_ = new float[xs * ys];
grady_ = new float[xs * ys];
}
bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path)
{
std::pair<float, float> current;
int stc = getIndex(static_cast<int>(goal_x), static_cast<int>(goal_y));
float dx = 0.0;
float dy = 0.0;
int ns = xs_ * ys_;
memset(gradx_, 0, static_cast<unsigned long>(ns) * sizeof(float));
memset(grady_, 0, static_cast<unsigned long>(ns) * sizeof(float));
int c = 0;
while (c++<ns*4)
{
float nx = stc % xs_ + dx, ny = stc / xs_ + dy;
if (fabs(nx - start_x) < 0.5 && fabs(ny - start_y) < 0.5)
{
current.first = start_x;
current.second = start_y;
path.push_back(current);
return true;
}
if (stc < xs_ || stc > xs_ * ys_ - xs_)
{
ROS_INFO("would be out of bounds");
return false;
}
current.first = nx;
current.second = ny;
path.push_back(current);
bool oscillation_detected = false;
int npath = path.size();
if (npath > 2 && path[npath - 1].first == path[npath - 3].first
&& path[npath - 1].second == path[npath - 3].second)
{
oscillation_detected = true;
}
int stcnx = stc + xs_;
int stcpx = stc - xs_;
if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
|| potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
|| potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
|| oscillation_detected)
{
int minc = stc;
int minp = potential[stc];
int st = stcpx - 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st = stc - 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st = stc + 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st = stcnx - 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
stc = minc;
dx = 0;
dy = 0;
if (potential[stc] >= POT_HIGH)
{
return false;
}
}
else
{
gradCell(potential, stc);
gradCell(potential, stc + 1);
gradCell(potential, stcnx);
gradCell(potential, stcnx + 1);
float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
float x = (1.0 - dy) * x1 + dy * x2;
float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
float y = (1.0 - dy) * y1 + dy * y2;
if (x == 0.0 && y == 0.0)
{
return false;
}
float ss = pathStep_ / hypot(x, y);
dx += x * ss;
dy += y * ss;
if (dx > 1.0)
{
stc++;
dx -= 1.0;
}
if (dx < -1.0)
{
stc--;
dx += 1.0;
}
if (dy > 1.0)
{
stc += xs_;
dy -= 1.0;
}
if (dy < -1.0)
{
stc -= xs_;
dy += 1.0;
}
}
}
return false;
}
float GradientPath::gradCell(float* potential, int n)
{
if (gradx_[n] + grady_[n] > 0.0)
return 1.0;
if (n < xs_ || n > xs_ * ys_ - xs_)
return 0.0;
float cv = potential[n];
float dx = 0.0;
float dy = 0.0;
if (cv >= POT_HIGH)
{
if (potential[n - 1] < POT_HIGH)
dx = -lethal_cost_;
else if (potential[n + 1] < POT_HIGH)
dx = lethal_cost_;
if (potential[n - xs_] < POT_HIGH)
dy = -lethal_cost_;
else if (potential[n + xs_] < POT_HIGH)
dy = lethal_cost_;
}
else
{
if (potential[n - 1] < POT_HIGH)
dx += potential[n - 1] - cv;
if (potential[n + 1] < POT_HIGH)
dx += cv - potential[n + 1];
if (potential[n - xs_] < POT_HIGH)
dy += potential[n - xs_] - cv;
if (potential[n + xs_] < POT_HIGH)
dy += cv - potential[n + xs_];
}
float norm = hypot(dx, dy);
if (norm > 0)
{
norm = 1.0 / norm;
gradx_[n] = norm * dx;
grady_[n] = norm * dy;
}
return norm;
}
void set_angle(geometry_msgs::PoseStamped* pose, double angle)
{
tf2::Quaternion q;
q.setRPY(0, 0, angle);
tf2::convert(q, pose->pose.orientation);
}
void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
std::vector<geometry_msgs::PoseStamped>& path)
{
int n = path.size();
if (n == 0) return;
switch(omode_) {
case FORWARD:
for(int i=0;i<n-1;i++){
setAngleBasedOnPositionDerivative(path, i);
}
break;
case BACKWARD:
for(int i=0;i<n-1;i++){
setAngleBasedOnPositionDerivative(path, i);
set_angle(&path[i], angles::normalize_angle(tf2::getYaw(path[i].pose.orientation) + M_PI));
}
break;
case LEFTWARD:
for(int i=0;i<n-1;i++){
setAngleBasedOnPositionDerivative(path, i);
set_angle(&path[i], angles::normalize_angle(tf2::getYaw(path[i].pose.orientation) - M_PI_2));
}
break;
case RIGHTWARD:
for(int i=0;i<n-1;i++){
setAngleBasedOnPositionDerivative(path, i);
set_angle(&path[i], angles::normalize_angle(tf2::getYaw(path[i].pose.orientation) + M_PI_2));
}
break;
case INTERPOLATE:
path[0].pose.orientation = start.pose.orientation;
interpolate(path, 0, n-1);
break;
case FORWARDTHENINTERPOLATE:
for(int i=0;i<n-1;i++){
setAngleBasedOnPositionDerivative(path, i);
}
int i=n-3;
const double last = tf2::getYaw(path[i].pose.orientation);
while( i>0 ){
const double new_angle = tf2::getYaw(path[i-1].pose.orientation);
double diff = fabs(angles::shortest_angular_distance(new_angle, last));
if( diff>0.35)
break;
else
i--;
}
path[0].pose.orientation = start.pose.orientation;
interpolate(path, i, n-1);
break;
}
}
void OrientationFilter::setAngleBasedOnPositionDerivative(std::vector<geometry_msgs::PoseStamped>& path, int index)
{
int index0 = std::max(0, index - window_size_);
int index1 = std::min((int)path.size() - 1, index + window_size_);
double x0 = path[index0].pose.position.x,
y0 = path[index0].pose.position.y,
x1 = path[index1].pose.position.x,
y1 = path[index1].pose.position.y;
double angle = atan2(y1-y0,x1-x0);
set_angle(&path[index], angle);
}
void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& path,
int start_index, int end_index)
{
const double start_yaw = tf2::getYaw(path[start_index].pose.orientation),
end_yaw = tf2::getYaw(path[end_index ].pose.orientation);
double diff = angles::shortest_angular_distance(start_yaw, end_yaw);
double increment = diff/(end_index-start_index);
for(int i=start_index; i<=end_index; i++){
double angle = start_yaw + increment * i;
set_angle(&path[i], angle);
}
}
DijkstraPlanner::DijkstraPlanner() :
costmap_(nullptr), initialized_(false), allow_unknown_(true)
{
;
}
DijkstraPlanner::DijkstraPlanner(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id) :
costmap_(nullptr), initialized_(false), allow_unknown_(true)
{
initialize(name, costmap, frame_id);
}
DijkstraPlanner::~DijkstraPlanner()
{
if (p_calc_)
delete p_calc_;
if (planner_)
delete planner_;
if (path_maker_)
delete path_maker_;
}
void DijkstraPlanner::initialize(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id)
{
if (!initialized_)
{
ros::NodeHandle private_nh("~/" + name);
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential",1);
costmap_ = costmap;
frame_id_ = frame_id;
unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
convert_offset_ = 0.5;
p_calc_ = new QuadraticCalculator(cx, cy);
planner_ = new DijkstraExpansion(p_calc_, cx, cy);
planner_->setPreciseStart(true);
path_maker_ = new GradientPath(p_calc_);
orientation_filter_ = new OrientationFilter();
private_nh.param("allow_unknown", allow_unknown_, false);
planner_->setHasUnknown(allow_unknown_);
private_nh.param("planner_window_x", planner_window_x_, 0.0);
private_nh.param("planner_window_y", planner_window_y_, 0.0);
private_nh.param("default_tolerance", default_tolerance_, 0.0);
private_nh.param("publish_scale", publish_scale_, 100);
initialized_ = true;
}
else
{
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
}
void DijkstraPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value)
{
unsigned char* pc = costarr;
for (int i = 0; i < nx; i++)
*pc++ = value;
pc = costarr + (ny - 1) * nx;
for (int i = 0; i < nx; i++)
*pc++ = value;
pc = costarr;
for (int i = 0; i < ny; i++, pc += nx)
*pc = value;
pc = costarr + nx - 1;
for (int i = 0; i < ny; i++, pc += nx)
*pc = value;
}
void DijkstraPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my)
{
if (!initialized_)
{
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return;
}
costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
}
void DijkstraPlanner::mapToWorld(double mx, double my, double& wx, double& wy)
{
wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
}
bool DijkstraPlanner::worldToMap(double wx, double wy, double& mx, double& my)
{
double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
double resolution = costmap_->getResolution();
if (wx < origin_x || wy < origin_y)
return false;
mx = (wx - origin_x) / resolution - convert_offset_;
my = (wy - origin_y) / resolution - convert_offset_;
if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
return true;
return false;
}
void DijkstraPlanner::publishPotential(float* potential)
{
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
double resolution = costmap_->getResolution();
nav_msgs::OccupancyGrid grid;
grid.header.frame_id = frame_id_;
grid.header.stamp = ros::Time::now();
grid.info.resolution = resolution;
grid.info.width = nx;
grid.info.height = ny;
double wx, wy;
costmap_->mapToWorld(0, 0, wx, wy);
grid.info.origin.position.x = wx - resolution / 2;
grid.info.origin.position.y = wy - resolution / 2;
grid.info.origin.position.z = 0.0;
grid.info.origin.orientation.w = 1.0;
grid.data.resize(nx * ny);
float max = 0.0;
for (unsigned int i = 0; i < grid.data.size(); i++)
{
float po = potential[i];
if (po < POT_HIGH)
{
if (po > max)
{
max = po;
}
}
}
for (unsigned int i = 0; i < grid.data.size(); i++)
{
if (potential[i] >= POT_HIGH)
{
grid.data[i] = -1;
} else
grid.data[i] = potential[i] * publish_scale_ / max;
}
potential_pub_.publish(grid);
}
bool DijkstraPlanner::makePlan(const geometry_msgs::PoseStamped start,
const geometry_msgs::PoseStamped goal,
double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan)
{
double toler = tolerance;
toler = 0;
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_)
{
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
plan.clear();
std::string global_frame = frame_id_;
if (goal.header.frame_id != global_frame)
{
ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
return false;
}
if (start.header.frame_id != global_frame)
{
ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
return false;
}
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(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(wx, wy, goal_x, goal_y);
clearRobotCell(start, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(),
start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if (found_legal)
{
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))
{
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
optimizationPath(plan,M_PI/10);
}
else
{
ROS_ERROR("local planner: Failed to get a plan. This shouldn't happen.");
}
}
else
{
ROS_ERROR("local planner:Failed to get a plan.");
}
orientation_filter_->processPath(start, plan);
delete potential_array_;
return !plan.empty();
}
int DijkstraPlanner::optimizationPath(std::vector<geometry_msgs::PoseStamped>& plan,double movement_angle_range)
{
if(plan.empty())
return 0;
size_t pose_size = plan.size() - 1;
double px,py,cx,cy,nx,ny,a_p,a_n;
bool is_run = false;
int ci = 0;
for(ci=0;ci<1000;ci++)
{
is_run = false;
for(size_t i=1;i<pose_size;i++)
{
px = plan[i-1].pose.position.x;
py = plan[i-1].pose.position.y;
cx = plan[i].pose.position.x;
cy = plan[i].pose.position.y;
nx = plan[i+1].pose.position.x;
ny = plan[i+1].pose.position.y;
a_p = normalizeAngle(atan2(cy-py,cx-px),0,2*M_PI);
a_n = normalizeAngle(atan2(ny-cy,nx-cx),0,2*M_PI);
if(std::max(a_p,a_n)-std::min(a_p,a_n) > movement_angle_range)
{
plan[i].pose.position.x = (px + nx)/2;
plan[i].pose.position.y = (py + ny)/2;
is_run = true;
}
}
if(!is_run)
return ci;
}
return ci;
}
bool DijkstraPlanner::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_;
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();
int path_size_num = path.size() -1;
for (int i = path_size_num; i>=0; i--)
{
std::pair<float, float> point = path[i];
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;
if(i != path_size_num)
{
double cx,cy,px,py;
cx = pose.pose.position.x;
cy = pose.pose.position.y;
px = plan.back().pose.position.x;
py = plan.back().pose.position.y;
if( sqrt( (cx-px)*(cx-px) + (cy-py)*(cy-py) ) > 0.05)
{
geometry_msgs::PoseStamped pose_insert = pose;
pose_insert.pose.position.x = (cx+px)/2;
pose_insert.pose.position.y = (cy+py)/2;
plan.push_back(pose_insert);
}
}
plan.push_back(pose);
}
return !plan.empty();
}
};