git commit -m "first commit"
This commit is contained in:
194
navigations/global_planner/CHANGELOG.rst
Executable file
194
navigations/global_planner/CHANGELOG.rst
Executable file
@@ -0,0 +1,194 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package global_planner
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.17.3 (2023-01-10)
|
||||
-------------------
|
||||
* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
|
||||
* do not specify obsolete c++11 standard
|
||||
this breaks with current versions of log4cxx.
|
||||
* update pluginlib include paths
|
||||
the non-hpp headers have been deprecated since kinetic
|
||||
* use lambdas in favor of boost::bind
|
||||
Using boost's _1 as a global system is deprecated since C++11.
|
||||
The ROS packages in Debian removed the implicit support for the global symbols,
|
||||
so this code fails to compile there without the patch.
|
||||
* Contributors: Michael Görner
|
||||
|
||||
1.17.2 (2022-06-20)
|
||||
-------------------
|
||||
* Throttled failed to get plan error to 5 seconds (`#1102 <https://github.com/ros-planning/navigation/issues/1102>`_)
|
||||
* Fixed the risk of divide by zero. (`#1099 <https://github.com/ros-planning/navigation/issues/1099>`_)
|
||||
* No virtual destructor polyformic classes fixed. (`#1100 <https://github.com/ros-planning/navigation/issues/1100>`_)
|
||||
* Fixes `#1026 <https://github.com/ros-planning/navigation/issues/1026>`_ (`#1028 <https://github.com/ros-planning/navigation/issues/1028>`_)
|
||||
* correctly delete previously allocated array (`#1025 <https://github.com/ros-planning/navigation/issues/1025>`_)
|
||||
Co-authored-by: Fedor Vlasov <fedor.vlasov@de.kaercher.com>
|
||||
* Contributors: David V. Lu!!, Noriaki Ando, Orhan G. Hafif, mechatheo
|
||||
|
||||
1.17.1 (2020-08-27)
|
||||
-------------------
|
||||
|
||||
1.17.0 (2020-04-02)
|
||||
-------------------
|
||||
* Merge pull request `#982 <https://github.com/ros-planning/navigation/issues/982>`_ from ros-planning/noetic_prep
|
||||
Noetic Migration
|
||||
* increase required cmake version
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.16.6 (2020-03-18)
|
||||
-------------------
|
||||
|
||||
1.16.5 (2020-03-15)
|
||||
-------------------
|
||||
* [melodic] updated install for better portability. (`#973 <https://github.com/ros-planning/navigation/issues/973>`_)
|
||||
* Contributors: Sean Yen
|
||||
|
||||
1.16.4 (2020-03-04)
|
||||
-------------------
|
||||
* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#851 <https://github.com/cobalt-robotics/navigation/issues/851>`_)
|
||||
* parameter for enabling outlineMap added (`#926 <https://github.com/cobalt-robotics/navigation/issues/926>`_)
|
||||
* Contributors: Pavel Shumejko, Sean Yen
|
||||
|
||||
1.16.3 (2019-11-15)
|
||||
-------------------
|
||||
* Remove unused visualize_potential (`#866 <https://github.com/ros-planning/navigation/issues/866>`_)
|
||||
* remove unused costmap_pub_frequency (`#858 <https://github.com/ros-planning/navigation/issues/858>`_)
|
||||
* Fix `#845 <https://github.com/ros-planning/navigation/issues/845>`_ (`#846 <https://github.com/ros-planning/navigation/issues/846>`_)
|
||||
* Merge pull request `#831 <https://github.com/ros-planning/navigation/issues/831>`_ from ros-planning/feature/remove_slashes
|
||||
[melodic] Remove leading slashes from default frame_id parameters
|
||||
* Remove leading slashes from default frame_id parameters
|
||||
* Contributors: David V. Lu, David V. Lu!!, Michael Ferguson, Stepan Kostusiev
|
||||
|
||||
1.16.2 (2018-07-31)
|
||||
-------------------
|
||||
|
||||
1.16.1 (2018-07-28)
|
||||
-------------------
|
||||
|
||||
1.16.0 (2018-07-25)
|
||||
-------------------
|
||||
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
|
||||
* feat(orientation_filter): Added additional orientation filter options (`#739 <https://github.com/ros-planning/navigation/issues/739>`_)
|
||||
* feat(orientation_filter): Added additional orientation filter options
|
||||
Enables plan references with different orientations for omni-base
|
||||
controllers. The following options are added:
|
||||
- Backward (backward path traversal, pose points to previous point)
|
||||
- Leftward (lateral path traversal in the positive y direction)
|
||||
- Rightward (lateral path traversal in the negative y direction)
|
||||
* Updated orientation filter option description
|
||||
* Added window size parameter to orientation filter
|
||||
Previously, the orientation was calculated using the current and the
|
||||
next point. However, when the path is somewhat jumpy, this results in
|
||||
poor orientations. By adding this parameter and altering the orientation
|
||||
calculation, the calculated orientation can be smoothened along the path
|
||||
by taking into account a larger window. The orientation of index point i
|
||||
will be calculated using the positions of i - window_size and i +
|
||||
window_size.
|
||||
* Contributors: Michael Ferguson, Rein Appeldoorn, Vincent Rabaud
|
||||
|
||||
1.15.2 (2018-03-22)
|
||||
-------------------
|
||||
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
|
||||
update maintainer email (lunar)
|
||||
* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/649>`_ from aaronhoy/lunar_add_ahoy
|
||||
Add myself as a maintainer.
|
||||
* Rebase PRs from Indigo/Kinetic (`#637 <https://github.com/ros-planning/navigation/issues/637>`_)
|
||||
* Respect planner_frequency intended behavior (`#622 <https://github.com/ros-planning/navigation/issues/622>`_)
|
||||
* Only do a getRobotPose when no start pose is given (`#628 <https://github.com/ros-planning/navigation/issues/628>`_)
|
||||
Omit the unnecessary call to getRobotPose when the start pose was
|
||||
already given, so that move_base can also generate a path in
|
||||
situations where getRobotPose would fail.
|
||||
This is actually to work around an issue of getRobotPose randomly
|
||||
failing.
|
||||
* Update gradient_path.cpp (`#576 <https://github.com/ros-planning/navigation/issues/576>`_)
|
||||
* Update gradient_path.cpp
|
||||
* Update navfn.cpp
|
||||
* update to use non deprecated pluginlib macro (`#630 <https://github.com/ros-planning/navigation/issues/630>`_)
|
||||
* update to use non deprecated pluginlib macro
|
||||
* multiline version as well
|
||||
* Print SDL error on IMG_Load failure in server_map (`#631 <https://github.com/ros-planning/navigation/issues/631>`_)
|
||||
* Contributors: Aaron Hoy, David V. Lu!!, Michael Ferguson
|
||||
|
||||
1.15.1 (2017-08-14)
|
||||
-------------------
|
||||
|
||||
1.15.0 (2017-08-07)
|
||||
-------------------
|
||||
* convert packages to format2
|
||||
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
|
||||
* Fix to increment 'cycle' in while loop (`#546 <https://github.com/ros-planning/navigation/issues/546>`_)
|
||||
* Fix to check index value before accessing to element of potential array (`#547 <https://github.com/ros-planning/navigation/issues/547>`_)
|
||||
* Set frame_id and stamp on Path message even if path is not found. (`#533 <https://github.com/ros-planning/navigation/issues/533>`_)
|
||||
* Contributors: Junya Hayashi, Martin Günther, Mikael Arguedas
|
||||
|
||||
1.14.0 (2016-05-20)
|
||||
-------------------
|
||||
|
||||
1.13.1 (2015-10-29)
|
||||
-------------------
|
||||
* Add missing angles dependecy
|
||||
* Fix for `#337 <https://github.com/ros-planning/navigation/issues/337>`_
|
||||
* Contributors: David V. Lu!!, Gary Servin
|
||||
|
||||
1.13.0 (2015-03-17)
|
||||
-------------------
|
||||
* Fixing various memory freeing operations
|
||||
* Add Orientation Filter to Global Planner
|
||||
* Contributors: Alex Bencz, David V. Lu!!, James Servos, Michael Ferguson
|
||||
|
||||
1.12.0 (2015-02-04)
|
||||
-------------------
|
||||
* update maintainer email
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.11.15 (2015-02-03)
|
||||
--------------------
|
||||
|
||||
1.11.14 (2014-12-05)
|
||||
--------------------
|
||||
|
||||
1.11.13 (2014-10-02)
|
||||
--------------------
|
||||
|
||||
1.11.12 (2014-10-01)
|
||||
--------------------
|
||||
* Add Gradient Path's cycle limit to GridPath
|
||||
* When clearing endpoint, do not overwrite potentials
|
||||
* Consolidate usage of POT_HIGH
|
||||
* Contributors: David V. Lu!!
|
||||
|
||||
1.11.11 (2014-07-23)
|
||||
--------------------
|
||||
* Minor code cleanup
|
||||
* Update package.xml
|
||||
* Contributors: David Lu!!, Enrique Fernández Perdomo
|
||||
|
||||
1.11.10 (2014-06-25)
|
||||
--------------------
|
||||
* Remove unnecessary colons
|
||||
* global_planner now pushes the goal onto the end of the global path
|
||||
* move_base planService now searches out from desired goal
|
||||
* Contributors: David Lu!!, Kaijen Hsiao
|
||||
|
||||
1.11.9 (2014-06-10)
|
||||
-------------------
|
||||
* uses ::hypot(x, y) instead of sqrt(x*x, y*y)
|
||||
* Contributors: Enrique Fernández Perdomo
|
||||
|
||||
1.11.8 (2014-05-21)
|
||||
-------------------
|
||||
|
||||
1.11.7 (2014-05-21)
|
||||
-------------------
|
||||
|
||||
1.11.5 (2014-01-30)
|
||||
-------------------
|
||||
* Global Planner Cleanup
|
||||
* Create the vector reversed instead of reverse it after
|
||||
* Reversed the plan vector
|
||||
* global_planner: Fix bgp_plugin.xml file and install it
|
||||
* Improved Global Planner (from Groovy branch)
|
||||
|
||||
1.11.4 (2013-09-27)
|
||||
-------------------
|
||||
* Package URL Updates
|
||||
77
navigations/global_planner/CMakeLists.txt
Executable file
77
navigations/global_planner/CMakeLists.txt
Executable file
@@ -0,0 +1,77 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(global_planner)
|
||||
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS
|
||||
angles
|
||||
costmap_2d
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
nav_core
|
||||
navfn
|
||||
nav_msgs
|
||||
pluginlib
|
||||
roscpp
|
||||
tf2_geometry_msgs
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
generate_dynamic_reconfigure_options(
|
||||
cfg/GlobalPlanner.cfg
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
costmap_2d
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
nav_core
|
||||
navfn
|
||||
nav_msgs
|
||||
pluginlib
|
||||
roscpp
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/quadratic_calculator.cpp
|
||||
src/dijkstra.cpp
|
||||
src/astar.cpp
|
||||
src/grid_path.cpp
|
||||
src/gradient_path.cpp
|
||||
src/orientation_filter.cpp
|
||||
src/planner_core.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(planner
|
||||
src/plan_node.cpp
|
||||
)
|
||||
add_dependencies(planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(planner
|
||||
${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
|
||||
|
||||
install(TARGETS planner
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
PATTERN ".svn" EXCLUDE)
|
||||
|
||||
install(FILES bgp_plugin.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
7
navigations/global_planner/bgp_plugin.xml
Executable file
7
navigations/global_planner/bgp_plugin.xml
Executable file
@@ -0,0 +1,7 @@
|
||||
<library path="lib/libglobal_planner">
|
||||
<class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
|
||||
<description>
|
||||
A implementation of a grid based planner using Dijkstras or A*
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
33
navigations/global_planner/cfg/GlobalPlanner.cfg
Executable file
33
navigations/global_planner/cfg/GlobalPlanner.cfg
Executable file
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "global_planner"
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, bool_t
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("lethal_cost", int_t, 0, "Lethal Cost", 253, 1, 255)
|
||||
gen.add("neutral_cost", int_t, 0, "Neutral Cost", 50, 1, 255)
|
||||
gen.add("cost_factor", double_t, 0, "Factor to multiply each cost from costmap by", 3.0, 0.01, 5.0)
|
||||
gen.add("publish_potential", bool_t, 0, "Publish Potential Costmap", True)
|
||||
|
||||
orientation_enum = gen.enum([
|
||||
gen.const("None", int_t, 0, "No orientations added except goal orientation"),
|
||||
gen.const("Forward", int_t, 1,
|
||||
"Positive x axis points along path, except for the goal orientation"),
|
||||
gen.const("Interpolate", int_t, 2, "Orientations are a linear blend of start and goal pose"),
|
||||
gen.const("ForwardThenInterpolate",
|
||||
int_t, 3, "Forward orientation until last straightaway, then a linear blend until the goal pose"),
|
||||
gen.const("Backward", int_t, 4,
|
||||
"Negative x axis points along the path, except for the goal orientation"),
|
||||
gen.const("Leftward", int_t, 5,
|
||||
"Positive y axis points along the path, except for the goal orientation"),
|
||||
gen.const("Rightward", int_t, 6,
|
||||
"Negative y axis points along the path, except for the goal orientation"),
|
||||
], "How to set the orientation of each point")
|
||||
|
||||
gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 6,
|
||||
edit_method=orientation_enum)
|
||||
gen.add("orientation_window_size", int_t, 0, "What window to use to determine the orientation based on the "
|
||||
"position derivative specified by the orientation mode", 1, 1, 255)
|
||||
|
||||
exit(gen.generate(PACKAGE, "global_planner", "GlobalPlanner"))
|
||||
76
navigations/global_planner/include/global_planner/astar.h
Executable file
76
navigations/global_planner/include/global_planner/astar.h
Executable 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
110
navigations/global_planner/include/global_planner/dijkstra.h
Executable 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
107
navigations/global_planner/include/global_planner/expander.h
Executable 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
78
navigations/global_planner/include/global_planner/gradient_path.h
Executable 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
53
navigations/global_planner/include/global_planner/grid_path.h
Executable 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
67
navigations/global_planner/include/global_planner/orientation_filter.h
Executable 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
212
navigations/global_planner/include/global_planner/planner_core.h
Executable 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
82
navigations/global_planner/include/global_planner/potential_calculator.h
Executable 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
54
navigations/global_planner/include/global_planner/quadratic_calculator.h
Executable 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
67
navigations/global_planner/include/global_planner/traceback.h
Executable 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
|
||||
36
navigations/global_planner/package.xml
Executable file
36
navigations/global_planner/package.xml
Executable file
@@ -0,0 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="2">
|
||||
<name>global_planner</name>
|
||||
<version>1.17.3</version>
|
||||
<description>
|
||||
A path planner library and node.
|
||||
</description>
|
||||
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
|
||||
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
|
||||
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<author>David Lu!!</author>
|
||||
<url>http://wiki.ros.org/global_planner</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>angles</build_depend>
|
||||
<build_depend>tf2_geometry_msgs</build_depend>
|
||||
|
||||
<depend>costmap_2d</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_core</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>navfn</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<export>
|
||||
<nav_core plugin="${prefix}/bgp_plugin.xml" />
|
||||
</export>
|
||||
|
||||
</package>
|
||||
98
navigations/global_planner/src/astar.cpp
Executable file
98
navigations/global_planner/src/astar.cpp
Executable 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
234
navigations/global_planner/src/dijkstra.cpp
Executable 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
315
navigations/global_planner/src/gradient_path.cpp
Executable 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
85
navigations/global_planner/src/grid_path.cpp
Executable 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
135
navigations/global_planner/src/orientation_filter.cpp
Executable 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
111
navigations/global_planner/src/plan_node.cpp
Executable 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
438
navigations/global_planner/src/planner_core.cpp
Executable 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
77
navigations/global_planner/src/quadratic_calculator.cpp
Executable 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user