HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
navigations/global_planner/src/astar.cpp Executable file
View File

@@ -0,0 +1,98 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include<global_planner/astar.h>
#include<costmap_2d/cost_values.h>
namespace global_planner {
AStarExpansion::AStarExpansion(PotentialCalculator* p_calc, int xs, int ys) :
Expander(p_calc, xs, ys) {
}
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) {
queue_.clear();
int start_i = toIndex(start_x, start_y);
queue_.push_back(Index(start_i, 0));
std::fill(potential, potential + ns_, POT_HIGH);
potential[start_i] = 0;
int goal_i = toIndex(end_x, end_y);
int cycle = 0;
while (queue_.size() > 0 && cycle < cycles) {
Index top = queue_[0];
std::pop_heap(queue_.begin(), queue_.end(), greater1());
queue_.pop_back();
int i = top.i;
if (i == goal_i)
return true;
add(costs, potential, potential[i], i + 1, end_x, end_y);
add(costs, potential, potential[i], i - 1, end_x, end_y);
add(costs, potential, potential[i], i + nx_, end_x, end_y);
add(costs, potential, potential[i], i - nx_, end_x, end_y);
cycle++;
}
return false;
}
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
int end_y) {
if (next_i < 0 || next_i >= ns_)
return;
if (potential[next_i] < POT_HIGH)
return;
if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))
return;
potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
int x = next_i % nx_, y = next_i / nx_;
float distance = abs(end_x - x) + abs(end_y - y);
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
std::push_heap(queue_.begin(), queue_.end(), greater1());
}
} //end namespace global_planner

234
navigations/global_planner/src/dijkstra.cpp Executable file
View File

@@ -0,0 +1,234 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include<global_planner/dijkstra.h>
#include <algorithm>
namespace global_planner {
DijkstraExpansion::DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny) :
Expander(p_calc, nx, ny), pending_(NULL), precise_(false) {
// priority buffers
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_;
}
//
// Set/Reset map size
//
void DijkstraExpansion::setSize(int xs, int ys) {
Expander::setSize(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;
}
//ROS_INFO("CYCLES %d/%d ", cycle, cycles);
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
//
#define INVSQRT2 0.707106781
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 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_);
}
}
}
} //end namespace global_planner

315
navigations/global_planner/src/gradient_path.cpp Executable file
View File

@@ -0,0 +1,315 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <global_planner/gradient_path.h>
#include <algorithm>
#include <stdio.h>
#include <global_planner/planner_core.h>
namespace global_planner {
GradientPath::GradientPath(PotentialCalculator* p_calc) :
Traceback(p_calc), pathStep_(0.5) {
gradx_ = grady_ = NULL;
}
GradientPath::~GradientPath() {
if (gradx_)
delete[] gradx_;
if (grady_)
delete[] grady_;
}
void GradientPath::setSize(int xs, int ys) {
Traceback::setSize(xs, 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(goal_x, goal_y);
// set up offset
float dx = goal_x - (int)goal_x;
float dy = goal_y - (int)goal_y;
int ns = xs_ * ys_;
memset(gradx_, 0, ns * sizeof(float));
memset(grady_, 0, ns * sizeof(float));
int c = 0;
while (c++<ns*4) {
// check if near goal
double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .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
{
printf("[PathCalc] Out of bounds\n");
return false;
}
current.first = nx;
current.second = ny;
//ROS_INFO("%d %d | %f %f ", stc%xs_, stc/xs_, dx, dy);
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) {
ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
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) {
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;
//ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
// potential[stc], path[npath-1].first, path[npath-1].second);
if (potential[stc] >= POT_HIGH) {
ROS_DEBUG("[PathCalc] No path found, high potential");
//savemap("navfn_highpot");
return 0;
}
}
// have a good gradient here
else {
// 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
// show gradients
ROS_DEBUG(
"[Path] %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f\n", gradx_[stc], grady_[stc], gradx_[stc+1], grady_[stc+1], gradx_[stcnx], grady_[stcnx], gradx_[stcnx+1], grady_[stcnx+1], x, y);
// check for zero gradient, failed
if (x == 0.0 && y == 0.0) {
ROS_DEBUG("[PathCalc] Zero gradient");
return 0;
}
// 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;
}
}
//printf("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
// potential[stc], dx, dy, path[npath-1].first, path[npath-1].second);
}
return false;
}
/*
int
NavFn::calcPath(int n, int *st)
{
// set up start position at cell
// st is always upper left corner for 4-point bilinear interpolation
if (st == NULL) st = start;
int stc = st[1]*nx + st[0];
// go for <n> cycles at most
for (int i=0; i<n; i++)
{
}
// return npath; // out of cycles, return failure
ROS_DEBUG("[PathCalc] No path found, path too long");
//savemap("navfn_pathlong");
return 0; // out of cycles, return failure
}
*/
//
// 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)
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 // not in an obstacle
{
// dx calc, average to sides
if (potential[n - 1] < POT_HIGH)
dx += potential[n - 1] - cv;
if (potential[n + 1] < POT_HIGH)
dx += cv - potential[n + 1];
// dy calc, average to sides
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;
}
} //end namespace global_planner

85
navigations/global_planner/src/grid_path.cpp Executable file
View File

@@ -0,0 +1,85 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <global_planner/grid_path.h>
#include <algorithm>
#include <stdio.h>
namespace global_planner {
bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
std::pair<float, float> current;
current.first = end_x;
current.second = end_y;
int start_index = getIndex(start_x, start_y);
path.push_back(current);
int c = 0;
int ns = xs_ * ys_;
while (getIndex(current.first, current.second) != start_index) {
float min_val = 1e10;
int min_x = 0, min_y = 0;
for (int xd = -1; xd <= 1; xd++) {
for (int yd = -1; yd <= 1; yd++) {
if (xd == 0 && yd == 0)
continue;
int x = current.first + xd, y = current.second + yd;
int index = getIndex(x, y);
if (potential[index] < min_val) {
min_val = potential[index];
min_x = x;
min_y = y;
}
}
}
if (min_x == 0 && min_y == 0)
return false;
current.first = min_x;
current.second = min_y;
path.push_back(current);
if(c++>ns*4){
return false;
}
}
return true;
}
} //end namespace global_planner

135
navigations/global_planner/src/orientation_filter.cpp Executable file
View File

@@ -0,0 +1,135 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, David V. Lu!!
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of David V. Lu nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: David V. Lu!!
*********************************************************************/
#include <global_planner/orientation_filter.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <angles/angles.h>
namespace global_planner {
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);
}
}
};

111
navigations/global_planner/src/plan_node.cpp Executable file
View File

@@ -0,0 +1,111 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Bhaskara Marthi
* David V. Lu!!
*********************************************************************/
#include <global_planner/planner_core.h>
#include <navfn/MakeNavPlan.h>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/transform_listener.h>
namespace cm = costmap_2d;
namespace rm = geometry_msgs;
using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;
namespace global_planner {
class PlannerWithCostmap : public GlobalPlanner {
public:
PlannerWithCostmap(string name, Costmap2DROS* cmap);
bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);
private:
void poseCallback(const rm::PoseStamped::ConstPtr& goal);
Costmap2DROS* cmap_;
ros::ServiceServer make_plan_service_;
ros::Subscriber pose_sub_;
};
bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
vector<PoseStamped> path;
req.start.header.frame_id = "map";
req.goal.header.frame_id = "map";
bool success = makePlan(req.start, req.goal, path);
resp.plan_found = success;
if (success) {
resp.path = path;
}
return true;
}
void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
geometry_msgs::PoseStamped global_pose;
cmap_->getRobotPose(global_pose);
vector<PoseStamped> path;
makePlan(global_pose, *goal, path);
}
PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) {
ros::NodeHandle private_nh("~");
cmap_ = cmap;
make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}
} // namespace
int main(int argc, char** argv) {
ros::init(argc, argv, "global_planner");
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
costmap_2d::Costmap2DROS lcr("costmap", buffer);
global_planner::PlannerWithCostmap pppp("planner", &lcr);
ros::spin();
return 0;
}

438
navigations/global_planner/src/planner_core.cpp Executable file
View File

@@ -0,0 +1,438 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <global_planner/planner_core.h>
#include <pluginlib/class_list_macros.hpp>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>
#include <global_planner/dijkstra.h>
#include <global_planner/astar.h>
#include <global_planner/grid_path.h>
#include <global_planner/gradient_path.h>
#include <global_planner/quadratic_calculator.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
namespace global_planner {
void GlobalPlanner::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;
}
GlobalPlanner::GlobalPlanner() :
costmap_(NULL), initialized_(false), allow_unknown_(true),
p_calc_(NULL), planner_(NULL), path_maker_(NULL), orientation_filter_(NULL),
potential_array_(NULL) {
}
GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
GlobalPlanner() {
//initialize the planner
initialize(name, costmap, frame_id);
}
GlobalPlanner::~GlobalPlanner() {
if (p_calc_)
delete p_calc_;
if (planner_)
delete planner_;
if (path_maker_)
delete path_maker_;
if (dsrv_)
delete dsrv_;
}
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
if (!initialized_) {
ros::NodeHandle private_nh("~/" + name);
costmap_ = costmap;
frame_id_ = frame_id;
unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
if(!old_navfn_behavior_)
convert_offset_ = 0.5;
else
convert_offset_ = 0.0;
bool use_quadratic;
private_nh.param("use_quadratic", use_quadratic, true);
if (use_quadratic)
p_calc_ = new QuadraticCalculator(cx, cy);
else
p_calc_ = new PotentialCalculator(cx, cy);
bool use_dijkstra;
private_nh.param("use_dijkstra", use_dijkstra, true);
if (use_dijkstra)
{
DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
if(!old_navfn_behavior_)
de->setPreciseStart(true);
planner_ = de;
}
else
planner_ = new AStarExpansion(p_calc_, cx, cy);
bool use_grid_path;
private_nh.param("use_grid_path", use_grid_path, false);
if (use_grid_path)
path_maker_ = new GridPath(p_calc_);
else
path_maker_ = new GradientPath(p_calc_);
orientation_filter_ = new OrientationFilter();
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
private_nh.param("allow_unknown", allow_unknown_, true);
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);
make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb =
[this](auto& config, auto level){ reconfigureCB(config, level); };
dsrv_->setCallback(cb);
initialized_ = true;
} else
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
planner_->setLethalCost(config.lethal_cost);
path_maker_->setLethalCost(config.lethal_cost);
planner_->setNeutralCost(config.neutral_cost);
planner_->setFactor(config.cost_factor);
publish_potential_ = config.publish_potential;
orientation_filter_->setMode(config.orientation_mode);
orientation_filter_->setWindowSize(config.orientation_window_size);
}
void GlobalPlanner::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);
}
bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
makePlan(req.start, req.goal, resp.plan.poses);
resp.plan.header.stamp = ros::Time::now();
resp.plan.header.frame_id = frame_id_;
return true;
}
void GlobalPlanner::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 GlobalPlanner::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;
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) {
return makePlan(start, goal, default_tolerance_, plan);
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
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_THROTTLE(1.0,
"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;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
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;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
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];
if(outline_map_)
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_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
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);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR_THROTTLE(5.0, "Failed to get a plan.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
publishPlan(plan);
delete[] potential_array_;
return !plan.empty();
}
void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return;
}
//create a message for the plan
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = frame_id_;
gui_path.header.stamp = ros::Time::now();
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for (unsigned int i = 0; i < path.size(); i++) {
gui_path.poses[i] = path[i];
}
plan_pub_.publish(gui_path);
}
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();
}
void GlobalPlanner::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 potential = potential_array_[i];
if (potential < POT_HIGH) {
if (potential > max) {
max = potential;
}
}
}
for (unsigned int i = 0; i < grid.data.size(); i++) {
if (potential_array_[i] >= POT_HIGH) {
grid.data[i] = -1;
} else {
if (fabs(max) < DBL_EPSILON) {
grid.data[i] = -1;
} else {
grid.data[i] = potential_array_[i] * publish_scale_ / max;
}
}
}
potential_pub_.publish(grid);
}
} //end namespace global_planner

77
navigations/global_planner/src/quadratic_calculator.cpp Executable file
View File

@@ -0,0 +1,77 @@
/*
* Copyright (c) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <global_planner/quadratic_calculator.h>
namespace global_planner {
float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) {
// 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;
}
}
}