admin管理员组文章数量:1569885
ROS:Dijkstra全局规划器
#ifndef _DIJKSTRA_PLANNER_H_
#define _DIJKSTRA_PLANNER_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/cost_values.h>
#include <simple_local_planner/world_model.h>
#include <simple_local_planner/trajectory.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <nav_msgs/OccupancyGrid.h>
#include <iostream>
#include <vector>
#include <cmath>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <angles/angles.h>
// inserting onto the priority blocks
#define push_cur(n) { if (n>=0 && n<ns_ && !pending_[n] && getCost(costs, n)<lethal_cost_ && currentEnd_<PRIORITYBUFSIZE){ currentBuffer_[currentEnd_++]=n; pending_[n]=true; }}
#define push_next(n) { if (n>=0 && n<ns_ && !pending_[n] && getCost(costs, n)<lethal_cost_ && nextEnd_<PRIORITYBUFSIZE){ nextBuffer_[ nextEnd_++]=n; pending_[n]=true; }}
#define push_over(n) { if (n>=0 && n<ns_ && !pending_[n] && getCost(costs, n)<lethal_cost_ && overEnd_<PRIORITYBUFSIZE){ overBuffer_[ overEnd_++]=n; pending_[n]=true; }}
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);
/**
* @brief Sets or resets the size of the map
* @param nx The x size of the map
* @param ny The y size of the map
*/
void setSize(int nx, int ny)
{
nx_ = nx;
ny_ = ny;
ns_ = nx * ny;
} /**< sets or resets the size of the map */
protected:
inline int toIndex(int x, int y)
{
return x + nx_ * y;
}
int nx_, ny_, ns_; /**< size of grid, in pixels */
};
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);
/**
* @brief Sets or resets the size of the map
* @param nx The x size of the map
* @param ny The y size of the map
*/
void setSize(int nx, int ny); /**< sets or resets the size of the map */
void setNeutralCost(unsigned char neutral_cost)
{
neutral_cost_ = neutral_cost;
priorityIncrement_ = 2 * neutral_cost_;
}
void setPreciseStart(bool precise){ precise_ = precise; }
private:
/**
* @brief Updates the cell at index n
* @param costs The costmap
* @param potential The potential array in which we are calculating
* @param n The index to update
*/
void updateCell(unsigned char* costs, float* potential, int n); /** updates the cell at index 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_;
}
/** block priority buffers */
int *buffer1_, *buffer2_, *buffer3_; /**< storage buffers for priority blocks */
int *currentBuffer_, *nextBuffer_, *overBuffer_; /**< priority buffer block ptrs */
int currentEnd_, nextEnd_, overEnd_; /**< end points of arrays */
bool *pending_; /**< pending_ cells during propagation */
bool precise_;
/** block priority thresholds */
float threshold_; /**< current threshold */
float priorityIncrement_; /**< priority threshold increment */
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_; /**< size of grid, in pixels */
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);
//
// Path construction
// Find gradient at array points, interpolate path
// Use step size of pathStep, usually 0.5 pixel
//
// Some sanity checks:
// 1. Stuck at same index position
// 2. Doesn't get near goal
// 3. Surrounded by high potentials
//
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_; /**< gradient arrays, size of potential array */
float pathStep_; /**< step size for following gradient */
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);
/**
* @brief Computes the full navigation function for the map given a point in the world to start from
* @param world_point The point to use for seeding the navigation function
* @return True if the navigation function was computed successfully, false otherwise
*/
bool computePotential(const geometry_msgs::Point& world_point);
/**
* @brief Compute a plan to a goal after the potential for a start point has already been computed (Note: You should call computePotential first)
* @param start_x
* @param start_y
* @param end_x
* @param end_y
* @param goal The goal pose to create a plan to
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
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);
/**
* @brief Get the potential, or naviagation cost, at a given point in the world (Note: You should call computePotential first)
* @param world_point The point to get the potential for
* @return The navigation function's value at that point in the world
*/
double getPointPotential(const geometry_msgs::Point& world_point);
/**
* @brief Check for a valid potential value at a given point in the world (Note: You should call computePotential first)
* @param world_point The point to get the potential for
* @return True if the navigation function is valid at that point in the world, false otherwise
*/
bool validPointPotential(const geometry_msgs::Point& world_point);
/**
* @brief Check for a valid potential value at a given point in the world (Note: You should call computePotential first)
* @param world_point The point to get the potential for
* @param tolerance The tolerance on searching around the world_point specified
* @return True if the navigation function is valid at that point in the world, false otherwise
*/
bool validPointPotential(const geometry_msgs::Point& world_point, double tolerance);
/**
* @brief Publish a path for visualization purposes
*/
// void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
// bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);
protected:
/**
* @brief Store a copy of the current costmap in \a costmap. Called by makePlan.
*/
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 <dijkstra_planner.h>
namespace simple_local_planner{
float QuadraticCalculator::calculatePotential(float *potential, unsigned char cost, int n, float prev_potential)
{
prev_potential = 0;
// get neighbors
float u, d, l, r;
l = potential[n - 1];
r = potential[n + 1];
u = potential[n - nx_];
d = potential[n + nx_];
// ROS_INFO("[Update] c: %f l: %f r: %f u: %f d: %f\n",
// potential[n], l, r, u, d);
// ROS_INFO("[Update] cost: %d\n", costs[n]);
// find lowest, and its lowest neighbor
float ta, tc;
if (l < r)
tc = l;
else
tc = r;
if (u < d)
ta = u;
else
ta = d;
float hf = cost; // traversability factor
float dc = tc - ta; // relative cost between ta,tc
if (dc < 0) // tc is lowest
{
dc = -dc;
ta = tc;
}
// calculate new potential
if (dc >= hf) // if too large, use ta-only update
return ta + hf;
else // two-neighbor interpolation update
{
// use quadratic approximation
// might speed this up through table lookup, but still have to
// do the divide
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) //Set/Reset map size
{
nx_ = xs;
ny_ = ys;
ns_ = xs * ys;
if (pending_)
delete[] pending_;
pending_ = new bool[ns_];
memset(pending_, 0, ns_ * sizeof(bool));
}
//
// main propagation function
// Dijkstra method, breadth-first
// runs for a specified number of cycles,
// or until it runs out of cells to update,
// or until the Start cell is found (atStart = true)
//
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;
// priority buffers
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);
// set goal
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; // max priority block size
int nc = 0; // number of cells put into priority blocks
int cycle = 0; // which cycle we're on
// set up start cell
int startCell = toIndex(end_x, end_y);
for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
{
if (currentEnd_ == 0 && nextEnd_ == 0) // priority blocks empty
return false;
// stats
nc += currentEnd_;
if (currentEnd_ > nwv)
nwv = currentEnd_;
// reset pending_ flags on current priority buffer
int *pb = currentBuffer_;
int i = currentEnd_;
while (i-- > 0)
pending_[*(pb++)] = false;
// process current priority buffer
pb = currentBuffer_;
i = currentEnd_;
while (i-- > 0)
updateCell(costs, potential, *pb++);
// swap priority blocks currentBuffer_ <=> nextBuffer_
currentEnd_ = nextEnd_;
nextEnd_ = 0;
pb = currentBuffer_; // swap buffers
currentBuffer_ = nextBuffer_;
nextBuffer_ = pb;
// see if we're done with this priority level
if (currentEnd_ == 0)
{
threshold_ += priorityIncrement_; // increment priority threshold
currentEnd_ = overEnd_; // set current to overflow block
overEnd_ = 0;
pb = currentBuffer_; // swap buffers
currentBuffer_ = overBuffer_;
overBuffer_ = pb;
}
// check if we've hit the Start cell
if (potential[startCell] < POT_HIGH)
break;
}
if (cycle < cycles)
return true; // finished up here
else
return false;
}
//
// Critical function: calculate updated potential value of a cell,
// given its neighbors' values
// Planar-wave update calculation from two lowest neighbors in a 4-grid
// Quadratic approximation to the interpolated value
// No checking of bounds here, this function should be fast
//
inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential, int n)
{
cells_visited_++;
// do planar wave update
float c = getCost(costs, n);
if (c >= lethal_cost_) // don't propagate into obstacles
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);
// now add affected neighbors to priority blocks
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;
//ROS_INFO("UPDATE %d %d %d %f", n, n%nx, n/nx, potential[n]);
if (pot < threshold_) // low-cost buffer block
{
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 // overflow block
{
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));
// set up offset
float dx = 0.0; //goal_x - (int)goal_x;
float dy = 0.0; //goal_y - (int)goal_y;
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;
// check if near goal
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_) // would be out of bounds
{
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_;
// check for potentials at eight positions near cell
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)
{ //产生了振荡或者8个方向有其中一个为障碍物
//ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
// check eight neighbors to find the lowest
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 // have a good gradient here 八个方向都没有障碍物
{
// get grad at four positions near cell
gradCell(potential, stc);
gradCell(potential, stc + 1);
gradCell(potential, stcnx);
gradCell(potential, stcnx + 1);
// get interpolated gradient
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; // interpolated x
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; // interpolated y
// check for zero gradient, failed
if (x == 0.0 && y == 0.0)
{
return false;
}
// move in the right direction
float ss = pathStep_ / hypot(x, y);
dx += x * ss;
dy += y * ss;
// check for overflow
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;
}
// gradient calculations
// calculate gradient at a cell
// positive value are to the right and down
float GradientPath::gradCell(float* potential, int n)
{
if (gradx_[n] + grady_[n] > 0.0) // check this cell
return 1.0;
if (n < xs_ || n > xs_ * ys_ - xs_) // would be out of bounds
return 0.0;
float cv = potential[n];
float dx = 0.0;
float dy = 0.0;
// check for in an obstacle
if (cv >= POT_HIGH) //如果当前点是障碍物
{
if (potential[n - 1] < POT_HIGH) //X方向的前一个点不是障碍物
dx = -lethal_cost_; //梯度增量很小
else if (potential[n + 1] < POT_HIGH) //X方向的下一个点不是障碍物
dx = lethal_cost_; //梯度增量很大
if (potential[n - xs_] < POT_HIGH) //y方向的上一个点不是障碍物
dy = -lethal_cost_; //梯度增量很小
else if (potential[n + xs_] < POT_HIGH) //y方向的下一个点不是障碍物
dy = lethal_cost_; //梯度增量很大
}
else // not in an obstacle
{
// dx calc, average to sides
//X方向的梯度增量为(X方向上前一个点的势减去当前点的势)+(X方向上当前点的势减去下一个点的势)
if (potential[n - 1] < POT_HIGH) //X方向的前一个点不是障碍物
dx += potential[n - 1] - cv; //梯度加上前一个点的势减去当前点的势
if (potential[n + 1] < POT_HIGH)
dx += cv - potential[n + 1];
// dy calc, average to sides
//Y方向的梯度增量为(Y方向上前一个点的势减去当前点的势)+(Y方向上当前点的势减去下一个点的势)
if (potential[n - xs_] < POT_HIGH)
dy += potential[n - xs_] - cv;
if (potential[n + xs_] < POT_HIGH)
dy += cv - potential[n + xs_];
}
// normalize
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);
// private_nh.param("outline_map", outline_map_, true);
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;
}
//set the associated costs in the cost map to be free
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;
// Publish Whole 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;
}
//clear the plan, just in case
plan.clear();
// ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
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);
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
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);
// this->publishPotential(potential_array_);
if (found_legal)
{
//extract the plan
//从可行点矩阵提取路径
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))
{
//make sure the goal we push on has the same timestamp as the rest of the 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.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
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_;
//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();
int path_size_num = path.size() -1;
for (int i = path_size_num; 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;
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();
}
};
版权声明:本文标题:ROS:Dijkstra全局规划器 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/xitong/1725280073a1016383.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论