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

76
navigations/global_planner/include/global_planner/astar.h Executable file
View File

@@ -0,0 +1,76 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _ASTAR_H
#define _ASTAR_H
#include <global_planner/planner_core.h>
#include <global_planner/expander.h>
#include <vector>
#include <algorithm>
namespace global_planner {
class Index {
public:
Index(int a, float b) {
i = a;
cost = b;
}
int i;
float cost;
};
struct greater1 {
bool operator()(const Index& a, const Index& b) const {
return a.cost > b.cost;
}
};
class AStarExpansion : public Expander {
public:
AStarExpansion(PotentialCalculator* p_calc, int nx, int ny);
virtual ~AStarExpansion() {}
bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles,
float* potential);
private:
void add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x, int end_y);
std::vector<Index> queue_;
};
} //end namespace global_planner
#endif

110
navigations/global_planner/include/global_planner/dijkstra.h Executable file
View File

@@ -0,0 +1,110 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _DIJKSTRA_H
#define _DIJKSTRA_H
#define PRIORITYBUFSIZE 10000
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <global_planner/planner_core.h>
#include <global_planner/expander.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 global_planner {
class DijkstraExpansion : public Expander {
public:
DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny);
virtual ~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 */
};
} //end namespace global_planner
#endif

107
navigations/global_planner/include/global_planner/expander.h Executable file
View File

@@ -0,0 +1,107 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _EXPANDER_H
#define _EXPANDER_H
#include <global_planner/potential_calculator.h>
#include <global_planner/planner_core.h>
namespace global_planner {
class Expander {
public:
Expander(PotentialCalculator* p_calc, int nx, int ny) :
unknown_(true), lethal_cost_(253), neutral_cost_(50), factor_(3.0), p_calc_(p_calc) {
setSize(nx, ny);
}
virtual ~Expander() {}
virtual bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* 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
*/
virtual void setSize(int nx, int ny) {
nx_ = nx;
ny_ = ny;
ns_ = nx * ny;
} /**< sets or resets the size of the map */
void setLethalCost(unsigned char lethal_cost) {
lethal_cost_ = lethal_cost;
}
void setNeutralCost(unsigned char neutral_cost) {
neutral_cost_ = neutral_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_;
PotentialCalculator* p_calc_;
};
} //end namespace global_planner
#endif

78
navigations/global_planner/include/global_planner/gradient_path.h Executable file
View File

@@ -0,0 +1,78 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _GRADIENT_PATH_H
#define _GRADIENT_PATH_H
#include<global_planner/traceback.h>
#include <math.h>
#include <algorithm>
namespace global_planner {
class GradientPath : public Traceback {
public:
GradientPath(PotentialCalculator* p_calc);
virtual ~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 */
};
} //end namespace global_planner
#endif

53
navigations/global_planner/include/global_planner/grid_path.h Executable file
View File

@@ -0,0 +1,53 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _GRID_PATH_H
#define _GRID_PATH_H
#include<vector>
#include<global_planner/traceback.h>
namespace global_planner {
class GridPath : public Traceback {
public:
GridPath(PotentialCalculator* p_calc): Traceback(p_calc){}
virtual ~GridPath() {}
bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path);
};
} //end namespace global_planner
#endif

67
navigations/global_planner/include/global_planner/orientation_filter.h Executable file
View File

@@ -0,0 +1,67 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef GLOBAL_PLANNER_ORIENTATION_FILTER_H
#define GLOBAL_PLANNER_ORIENTATION_FILTER_H
#include <nav_msgs/Path.h>
namespace global_planner {
enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD };
class OrientationFilter {
public:
OrientationFilter() : omode_(NONE) {}
virtual 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 setMode(int new_mode){ setMode((OrientationMode) new_mode); }
void setWindowSize(size_t window_size){ window_size_ = window_size; }
protected:
OrientationMode omode_;
int window_size_;
};
} //end namespace global_planner
#endif

212
navigations/global_planner/include/global_planner/planner_core.h Executable file
View File

@@ -0,0 +1,212 @@
#ifndef _PLANNERCORE_H
#define _PLANNERCORE_H
/*********************************************************************
*
* 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!!
*********************************************************************/
#define POT_HIGH 1.0e10 // unassigned cell potential
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Path.h>
#include <vector>
#include <nav_core/base_global_planner.h>
#include <nav_msgs/GetPlan.h>
#include <dynamic_reconfigure/server.h>
#include <global_planner/potential_calculator.h>
#include <global_planner/expander.h>
#include <global_planner/traceback.h>
#include <global_planner/orientation_filter.h>
#include <global_planner/GlobalPlannerConfig.h>
namespace global_planner {
class Expander;
class GridPath;
/**
* @class PlannerCore
* @brief Provides a ROS wrapper for the global_planner planner which runs a fast, interpolated navigation function on a costmap.
*/
class GlobalPlanner : public nav_core::BaseGlobalPlanner {
public:
/**
* @brief Default constructor for the PlannerCore object
*/
GlobalPlanner();
/**
* @brief Constructor for the PlannerCore object
* @param name The name of this planner
* @param costmap A pointer to the costmap to use
* @param frame_id Frame of the costmap
*/
GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
/**
* @brief Default deconstructor for the PlannerCore object
*/
~GlobalPlanner();
/**
* @brief Initialization function for the PlannerCore object
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
*/
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param tolerance The tolerance on the goal point for the planner
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan);
/**
* @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_;
ros::Publisher plan_pub_;
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 planner_window_x_, planner_window_y_, default_tolerance_;
boost::mutex mutex_;
ros::ServiceServer make_plan_srv_;
PotentialCalculator* p_calc_;
Expander* planner_;
Traceback* path_maker_;
OrientationFilter* orientation_filter_;
bool publish_potential_;
ros::Publisher potential_pub_;
int publish_scale_;
void outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value);
float* potential_array_;
unsigned int start_x_, start_y_, end_x_, end_y_;
bool old_navfn_behavior_;
float convert_offset_;
bool outline_map_;
dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig> *dsrv_;
void reconfigureCB(global_planner::GlobalPlannerConfig &config, uint32_t level);
};
} //end namespace global_planner
#endif

82
navigations/global_planner/include/global_planner/potential_calculator.h Executable file
View File

@@ -0,0 +1,82 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _POTENTIAL_CALCULATOR_H
#define _POTENTIAL_CALCULATOR_H
#include <algorithm>
namespace global_planner {
class PotentialCalculator {
public:
PotentialCalculator(int nx, int ny) {
setSize(nx, ny);
}
virtual ~PotentialCalculator() {}
virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){
if(prev_potential < 0){
// get min of neighbors
float min_h = std::min( potential[n - 1], potential[n + 1] ),
min_v = std::min( potential[n - nx_], potential[n + nx_]);
prev_potential = std::min(min_h, min_v);
}
return prev_potential + cost;
}
/**
* @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
*/
virtual 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 */
};
} //end namespace global_planner
#endif

54
navigations/global_planner/include/global_planner/quadratic_calculator.h Executable file
View File

@@ -0,0 +1,54 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _QUADRATIC_CALCULATOR_H
#define _QUADRATIC_CALCULATOR_H
#include<vector>
#include<global_planner/potential_calculator.h>
namespace global_planner {
class QuadraticCalculator : public PotentialCalculator {
public:
QuadraticCalculator(int nx, int ny): PotentialCalculator(nx,ny) {}
float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential);
};
} //end namespace global_planner
#endif

67
navigations/global_planner/include/global_planner/traceback.h Executable file
View File

@@ -0,0 +1,67 @@
/*********************************************************************
*
* 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!!
*********************************************************************/
#ifndef _TRACEBACK_H
#define _TRACEBACK_H
#include<vector>
#include<global_planner/potential_calculator.h>
namespace global_planner {
class Traceback {
public:
Traceback(PotentialCalculator* p_calc) : p_calc_(p_calc) {}
virtual ~Traceback() {}
virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) = 0;
virtual void setSize(int xs, int ys) {
xs_ = xs;
ys_ = ys;
}
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_;
PotentialCalculator* p_calc_;
};
} //end namespace global_planner
#endif