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

git commit -m "first commit"

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

192
navigations/navfn/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,192 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package navfn
^^^^^^^^^^^^^^^^^^^^^^^^^^^
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)
-------------------
* navfn: stop installing test headers (`#1085 <https://github.com/ros-planning/navigation/issues/1085>`_)
The navtest executable is only built if FLTK is installed. However, the
header it uses is installed regardless, and requires FLTK. Nothing else
uses this header, and navfn doesn't depend upon FLTK, so stop installing
the header. Also fix navtest to actually build when FLTK is installed.
* Contributors: Kyle Fazzari
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)
-------------------
* Fix Unknown CMake command check_include_file (navfn & base_local_planner) (`#975 <https://github.com/ros-planning/navigation/issues/975>`_)
* Contributors: Sam Pfeiffer
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>`_)
* Add frame ID to empty NavFn paths (`#964 <https://github.com/cobalt-robotics/navigation/issues/964>`_)
* Don't publish empty paths
RViz will complain about not being able to transform the path if it doesn't have a frame ID, so we'll just drop these
Closes `#963 <https://github.com/cobalt-robotics/navigation/issues/963>`_
* Publish empty paths with a valid frame
* Fix indexing into empty plan for timestamp
* Contributors: Nick Walker, Sean Yen
1.16.3 (2019-11-15)
-------------------
* 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
* Merge pull request `#789 <https://github.com/ros-planning/navigation/issues/789>`_ from ipa-fez/fix/astar_const_melodic
Remove const from create_nav_plan_astar
* remove const from create_nav_plan_astar
* Contributors: David V. Lu, Felix, Michael Ferguson
1.16.2 (2018-07-31)
-------------------
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Remove dependency on PCL
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Merge pull request `#737 <https://github.com/ros-planning/navigation/issues/737>`_ from marting87/minor_comment_fixes
Minor comment corrections
* Contributors: David V. Lu, Martin Ganeff, Michael Ferguson, 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)
* Remove Dead Code [Lunar] (`#646 <https://github.com/ros-planning/navigation/issues/646>`_)
* Clean up navfn
* Cleanup amcl
* 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>`_)
* import only PCL common
* port `#549 <https://github.com/ros-planning/navigation/issues/549>`_ (in alphabetical order)
* address gcc6 build error
* remove GCC warnings
* Contributors: Lukas Bulwahn, Martin Günther, Michael Ferguson, Mikael Arguedas, Vincent Rabaud
1.14.0 (2016-05-20)
-------------------
* navfn: make independent on costmap implementation
navfn::NavfnROS:
* remove direct dependency on costmap_2d::Costmap2DROS
* add constructor for barebone costmap_2d::Costmap2D (user must provide also global_frame)
* NavfnROS::initialize() follows constructor semantics
nav_core::BaseGlobalPlanner interface unchanged
* Contributors: Jiri Horner
1.13.1 (2015-10-29)
-------------------
* Fix for `#337 <https://github.com/ros-planning/navigation/issues/337>`_
* Contributors: David V. Lu!!
1.13.0 (2015-03-17)
-------------------
1.12.0 (2015-02-04)
-------------------
* update maintainer email
* Contributors: Michael Ferguson
1.11.15 (2015-02-03)
--------------------
* Add ARCHIVE_DESTINATION for static builds
* Contributors: Gary Servin
1.11.14 (2014-12-05)
--------------------
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
1.11.11 (2014-07-23)
--------------------
* removes unused param planner_costmap_publish_frequency
* Contributors: Enrique Fernández Perdomo
1.11.10 (2014-06-25)
--------------------
* Remove unnecessary colons
* Contributors: David Lu!!
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)
-------------------
* update build to find eigen using cmake_modules
* Contributors: Michael Ferguson
1.11.5 (2014-01-30)
-------------------
* navfn: fix parallel build error from missing dep
* fixed header installation directory
* check for CATKIN_ENABLE_TESTING
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* fixed `#103 <https://github.com/ros-planning/navigation/issues/103>`_, navfn_node not installed
* Potential missing dependency

141
navigations/navfn/CMakeLists.txt Executable file
View File

@@ -0,0 +1,141 @@
cmake_minimum_required(VERSION 3.0.2)
project(navfn)
include(CheckIncludeFile)
find_package(catkin REQUIRED
COMPONENTS
cmake_modules
costmap_2d
geometry_msgs
message_generation
nav_core
nav_msgs
pluginlib
rosconsole
roscpp
tf2_ros
sensor_msgs
visualization_msgs
)
find_package(Eigen3 REQUIRED)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
# services
add_service_files(
DIRECTORY srv
FILES
MakeNavPlan.srv
SetCostmap.srv
)
generate_messages(
DEPENDENCIES
geometry_msgs
)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
navfn
CATKIN_DEPENDS
geometry_msgs
message_runtime
nav_core
nav_msgs
pluginlib
roscpp
sensor_msgs
visualization_msgs
)
check_include_file(sys/time.h HAVE_SYS_TIME_H)
if (HAVE_SYS_TIME_H)
add_definitions(-DHAVE_SYS_TIME_H)
endif (HAVE_SYS_TIME_H)
add_library (navfn src/navfn.cpp src/navfn_ros.cpp)
target_link_libraries(navfn
${catkin_LIBRARIES}
)
add_dependencies(navfn ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_executable(navfn_node src/navfn_node.cpp)
target_link_libraries(navfn_node
navfn
)
install(TARGETS navfn_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS navfn
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/navfn/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(FILES bgp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
### The problem with FindFLTK is that it only reports success if *all*
### fltk components are installed, but we only need the core library.
# include (FindFLTK)
include (CheckIncludeFileCXX)
check_include_file_cxx (FL/Fl.H NAVFN_HAVE_FLTK)
check_include_file_cxx (pgm.h NAVFN_HAVE_NETPBM)
message (STATUS "NAVFN_HAVE_FLTK: ${NAVFN_HAVE_FLTK}, NETPBM: ${NAVFN_HAVE_NETPBM}")
# Just linking -lfltk is not sufficient on OS X
if (NAVFN_HAVE_FLTK AND NAVFN_HAVE_NETPBM AND NOT APPLE)
message (STATUS "FLTK found: adding navtest to build")
add_executable (navtest src/navtest/navtest.cpp src/navtest/navwin.cpp)
set (FLTK_SKIP_FLUID 1)
set (FLTK_SKIP_FORMS 1)
set (FLTK_SKIP_IMAGES 1)
find_package(FLTK)
if(FLTK_FOUND)
target_link_libraries (navtest navfn netpbm ${FLTK_LIBRARIES})
else (FLTK_FOUND)
target_link_libraries (navtest navfn netpbm fltk)
endif (FLTK_FOUND)
else (NAVFN_HAVE_FLTK)
message (STATUS "FLTK orf NETPBM not found: cannot build navtest")
endif (NAVFN_HAVE_FLTK AND NAVFN_HAVE_NETPBM AND NOT APPLE)
### For some reason (on cmake-2.4.7 at least) the "check" for pgm.h
### always succeeds, even if pgm.h is not installed. It seems to be
### caused by a bug in the rule that attempts to build the C source:
### instead of directly calling e.g. 'gcc -c
### /CMakeFiles/CMakeTmp/CheckIncludeFile.c' it goes through some make
### infrastructure, which reports "Nothing to be done for
### `CMakeFiles/cmTryCompileExec.dir/build'" and calls that a success.
###
### As a workaround we simply force everyone to install libnetpbm
#
# include (CheckIncludeFile)
# check_include_file (pgm.h NAVFN_HAVE_NETPBM)
# message (STATUS "NAVFN_HAVE_NETPBM: ${NAVFN_HAVE_NETPBM}")
# if (NAVFN_HAVE_NETPBM)
# message (STATUS "found pgm.h")
# add_definitions (-DNAVFN_HAVE_NETPBM)
#target_link_libraries (navfn -lnetpbm)
# else (NAVFN_HAVE_NETPBM)
# message (STATUS "pgm.h not found (optional)")
# endif (NAVFN_HAVE_NETPBM)
if(CATKIN_ENABLE_TESTING)
add_subdirectory(test)
endif()

39
navigations/navfn/Makefile.orig Executable file
View File

@@ -0,0 +1,39 @@
#
# Makefile for navigation function planner
#
CC = g++
CXX = g++
LD = g++
CPPFLAGS = -Wall -g -O3 -Iinclude -I/usr/local/include -I/usr/local/include/opencv
CFLAGS = -DGCC -msse2 -mpreferred-stack-boundary=4 -O3 -Wall -Iinclude -I/usr/local/include
GCC = g++
LDFLAGS = -Lbin
SHAREDFLAGS = -shared -Wl,-soname,
LIBS = -lfltk -lnetpbm
OBJECTS = navfn navwin
all: bin/navtest
bin/navtest: obj/navtest.o $(OBJECTS:%=obj/%.o)
$(LD) $(LDFLAGS) -o $@ $(OBJECTS:%=obj/%.o) obj/navtest.o $(LIBS)
# general cpp command from src->obj
obj/%.o:src/%.cpp
$(GCC) $(CPPFLAGS) -c $< -o $@
# general c command from src->obj
obj/%.o:src/%.c
$(GCC) $(CFLAGS) -c $< -o $@
obj/navfn.o: include/navfn/navfn.h
clean:
- rm obj/*.o
- rm bin/navtest
dist:

7
navigations/navfn/bgp_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libnavfn">
<class name="navfn/NavfnROS" type="navfn::NavfnROS" base_class_type="nav_core::BaseGlobalPlanner">
<description>
A implementation of a grid based planner using Dijkstra
</description>
</class>
</library>

267
navigations/navfn/include/navfn/navfn.h Executable file
View File

@@ -0,0 +1,267 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 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.
*********************************************************************/
//
// Navigation function computation
// Uses Dijkstra's method
// Modified for Euclidean-distance computation
//
#ifndef _NAVFN_H
#define _NAVFN_H
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
// cost defs
#define COST_UNKNOWN_ROS 255 // 255 is unknown cost
#define COST_OBS 254 // 254 for forbidden regions
#define COST_OBS_ROS 253 // ROS values of 253 are obstacles
// navfn cost values are set to
// COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
// Incoming costmap cost values are in the range 0 to 252.
// With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
// ensure the input values are spread evenly over the output range, 50
// to 253. If COST_FACTOR is higher, cost values will have a plateau
// around obstacles and the planner will then treat (for example) the
// whole width of a narrow hallway as equally undesirable and thus
// will not plan paths down the center.
#define COST_NEUTRAL 50 // Set this to "open space" value
#define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap()
// Define the cost type in the case that it is not set. However, this allows
// clients to modify it without changing the file. Arguably, it is better to require it to
// be defined by a user explicitly
#ifndef COSTTYPE
#define COSTTYPE unsigned char // Whatever is used...
#endif
// potential defs
#define POT_HIGH 1.0e10 // unassigned cell potential
// priority buffers
#define PRIORITYBUFSIZE 10000
namespace navfn {
/**
Navigation function call.
\param costmap Cost map array, of type COSTTYPE; origin is upper left
NOTE: will be modified to have a border of obstacle costs
\param nx Width of map in cells
\param ny Height of map in cells
\param goal X,Y position of goal cell
\param start X,Y position of start cell
Returns length of plan if found, and fills an array with x,y interpolated
positions at about 1/2 cell resolution; else returns 0.
*/
int create_nav_plan_astar(COSTTYPE *costmap, int nx, int ny,
int* goal, int* start,
float *plan, int nplan);
/**
* @class NavFn
* @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down.
*/
class NavFn
{
public:
/**
* @brief Constructs the planner
* @param nx The x size of the map
* @param ny The y size of the map
*/
NavFn(int nx, int ny); // size of map
~NavFn();
/**
* @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 setNavArr(int nx, int ny); /**< sets or resets the size of the map */
int nx, ny, ns; /**< size of grid, in pixels */
/**
* @brief Set up the cost array for the planner, usually from ROS
* @param cmap The costmap
* @param isROS Whether or not the costmap is coming in in ROS format
* @param allow_unknown Whether or not the planner should be allowed to plan through unknown space
*/
void setCostmap(const COSTTYPE *cmap, bool isROS=true, bool allow_unknown = true); /**< sets up the cost map */
/**
* @brief Calculates a plan using the A* heuristic, returns true if one is found
* @return True if a plan is found, false otherwise
*/
bool calcNavFnAstar(); /**< calculates a plan, returns true if found */
/**
* @brief Caclulates the full navigation function using Dijkstra
*/
bool calcNavFnDijkstra(bool atStart = false); /**< calculates the full navigation function */
/**
* @brief Accessor for the x-coordinates of a path
* @return The x-coordinates of a path
*/
float *getPathX(); /**< x-coordinates of path */
/**
* @brief Accessor for the y-coordinates of a path
* @return The y-coordinates of a path
*/
float *getPathY(); /**< y-coordinates of path */
/**
* @brief Accessor for the length of a path
* @return The length of a path
*/
int getPathLen(); /**< length of path, 0 if not found */
/**
* @brief Gets the cost of the path found the last time a navigation function was computed
* @return The cost of the last path found
*/
float getLastPathCost(); /**< Return cost of path found the last time A* was called */
/** cell arrays */
COSTTYPE *costarr; /**< cost array in 2D configuration space */
float *potarr; /**< potential array, navigation function potential */
bool *pending; /**< pending cells during propagation */
int nobs; /**< number of obstacle cells */
/** block priority buffers */
int *pb1, *pb2, *pb3; /**< storage buffers for priority blocks */
int *curP, *nextP, *overP; /**< priority buffer block ptrs */
int curPe, nextPe, overPe; /**< end points of arrays */
/** block priority thresholds */
float curT; /**< current threshold */
float priInc; /**< priority threshold increment */
/** goal and start positions */
/**
* @brief Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start.
* @param goal the goal position
*/
void setGoal(int *goal);
/**
* @brief Sets the start position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start.
* @param start the start position
*/
void setStart(int *start);
int goal[2];
int start[2];
/**
* @brief Initialize cell k with cost v for propagation
* @param k the cell to initialize
* @param v the cost to give to the cell
*/
void initCost(int k, float v); /**< initialize cell <k> with cost <v>, for propagation */
/** propagation */
/**
* @brief Updates the cell at index n
* @param n The index to update
*/
void updateCell(int n); /**< updates the cell at index <n> */
/**
* @brief Updates the cell at index n using the A* heuristic
* @param n The index to update
*/
void updateCellAstar(int n); /**< updates the cell at index <n>, uses A* heuristic */
void setupNavFn(bool keepit = false); /**< resets all nav fn arrays for propagation */
/**
* @brief Run propagation for <cycles> iterations, or until start is reached using breadth-first Dijkstra method
* @param cycles The maximum number of iterations to run for
* @param atStart Whether or not to stop when the start point is reached
* @return true if the start point is reached
*/
bool propNavFnDijkstra(int cycles, bool atStart = false); /**< returns true if start point found or full prop */
/**
* @brief Run propagation for <cycles> iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic
* @param cycles The maximum number of iterations to run for
* @return true if the start point is reached
*/
bool propNavFnAstar(int cycles); /**< returns true if start point found */
/** gradient and paths */
float *gradx, *grady; /**< gradient arrays, size of potential array */
float *pathx, *pathy; /**< path points, as subpixel cell coordinates */
int npath; /**< number of path points */
int npathbuf; /**< size of pathx, pathy buffers */
float last_path_cost_; /**< Holds the cost of the path found the last time A* was called */
/**
* @brief Calculates the path for at mose <n> cycles
* @param n The maximum number of cycles to run for
* @return The length of the path found
*/
int calcPath(int n, int *st = NULL); /**< calculates path for at most <n> cycles, returns path length, 0 if none */
float gradCell(int n); /**< calculates gradient at cell <n>, returns norm */
float pathStep; /**< step size for following gradient */
/** display callback */
void display(void fn(NavFn *nav), int n = 100); /**< <n> is the number of cycles between updates */
int displayInt; /**< save second argument of display() above */
void (*displayFn)(NavFn *nav); /**< display function itself */
/** save costmap */
void savemap(const char *fname); /**< write out costmap and start/goal states as fname.pgm and fname.txt */
};
};
#endif // NAVFN

188
navigations/navfn/include/navfn/navfn_ros.h Executable file
View File

@@ -0,0 +1,188 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 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
*********************************************************************/
#ifndef NAVFN_NAVFN_ROS_H_
#define NAVFN_NAVFN_ROS_H_
#include <ros/ros.h>
#include <navfn/navfn.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 <navfn/potarr_point.h>
namespace navfn {
/**
* @class NavfnROS
* @brief Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a costmap.
*/
class NavfnROS : public nav_core::BaseGlobalPlanner {
public:
/**
* @brief Default constructor for the NavFnROS object
*/
NavfnROS();
/**
* @brief Constructor for the NavFnROS object
* @param name The name of this planner
* @param costmap A pointer to the ROS wrapper of the costmap to use
*/
NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Constructor for the NavFnROS object
* @param name The name of this planner
* @param costmap A pointer to the costmap to use
* @param global_frame The global frame of the costmap
*/
NavfnROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);
/**
* @brief Initialization function for the NavFnROS object
* @param name The name of this planner
* @param costmap A pointer to the ROS wrapper of the costmap to use for planning
*/
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Initialization function for the NavFnROS object
* @param name The name of this planner
* @param costmap A pointer to the costmap to use for planning
* @param global_frame The global frame of the costmap
*/
void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);
/**
* @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 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(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, double r, double g, double b, double a);
~NavfnROS(){}
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_;
boost::shared_ptr<NavFn> planner_;
ros::Publisher plan_pub_;
ros::Publisher potarr_pub_;
bool initialized_, allow_unknown_, visualize_potential_;
private:
inline double sq_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2){
double dx = p1.pose.position.x - p2.pose.position.x;
double dy = p1.pose.position.y - p2.pose.position.y;
return dx*dx +dy*dy;
}
void mapToWorld(double mx, double my, double& wx, double& wy);
void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
double planner_window_x_, planner_window_y_, default_tolerance_;
boost::mutex mutex_;
ros::ServiceServer make_plan_srv_;
std::string global_frame_;
};
};
#endif

50
navigations/navfn/include/navfn/potarr_point.h Executable file
View File

@@ -0,0 +1,50 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, (Simon) Ye Cheng
* 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 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.
*
*********************************************************************/
#ifndef POTARR_POINT_H_
#define POTARR_POINT_H_
namespace navfn {
struct PotarrPoint {
float x;
float y;
float z;
float pot_value;
};
}
#endif

35
navigations/navfn/include/navfn/read_pgm_costmap.h Executable file
View File

@@ -0,0 +1,35 @@
/*
* Copyright (c) 2012, 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.
*/
#ifndef READ_PGM_COSTMAP_H
#define READ_PGM_COSTMAP_H
// <raw> is true for ROS-generated raw cost maps
COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false);
#endif // READ_PGM_COSTMAP_H

49
navigations/navfn/package.xml Executable file
View File

@@ -0,0 +1,49 @@
<?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>navfn</name>
<version>1.17.3</version>
<description>
navfn provides a fast interpolated navigation function that can be used to create plans for
a mobile base. The planner assumes a circular robot and operates on a costmap to find a
minimum cost plan from a start point to an end point in a grid. The navigation function is
computed with Dijkstra's algorithm, but support for an A* heuristic may also be added in the
near future. navfn also provides a ROS wrapper for the navfn planner that adheres to the
nav_core::BaseGlobalPlanner interface specified in <a href="http://wiki.ros.org/nav_core">nav_core</a>.
</description>
<author>Kurt Konolige</author>
<author>Eitan Marder-Eppstein</author>
<author>contradict@gmail.com</author>
<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>
<url>http://wiki.ros.org/navfn</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>netpbm</build_depend> <!-- This is a test dependency -->
<depend>costmap_2d</depend>
<depend>geometry_msgs</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>rosconsole</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<exec_depend>message_runtime</exec_depend>
<test_depend>rosunit</test_depend>
<export>
<nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>
</package>

1056
navigations/navfn/src/navfn.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

131
navigations/navfn/src/navfn_node.cpp Executable file
View File

@@ -0,0 +1,131 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 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
*********************************************************************/
#include <navfn/navfn_ros.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 navfn {
class NavfnWithCostmap : public NavfnROS
{
public:
NavfnWithCostmap(string name, Costmap2DROS* cmap);
bool makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp);
private:
void poseCallback(const rm::PoseStamped::ConstPtr& goal);
Costmap2DROS* cmap_;
ros::ServiceServer make_plan_service_;
ros::Subscriber pose_sub_;
};
bool NavfnWithCostmap::makePlanService(MakeNavPlan::Request& req, 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 NavfnWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
geometry_msgs::PoseStamped global_pose;
cmap_->getRobotPose(global_pose);
vector<PoseStamped> path;
makePlan(global_pose, *goal, path);
}
NavfnWithCostmap::NavfnWithCostmap(string name, Costmap2DROS* cmap) :
NavfnROS(name, cmap)
{
ros::NodeHandle private_nh("~");
cmap_ = cmap;
make_plan_service_ = private_nh.advertiseService("make_plan", &NavfnWithCostmap::makePlanService, this);
pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &NavfnWithCostmap::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);
navfn::NavfnWithCostmap navfn("navfn_planner", &lcr);
ros::spin();
return 0;
}

432
navigations/navfn/src/navfn_ros.cpp Executable file
View File

@@ -0,0 +1,432 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 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
*********************************************************************/
#include <navfn/navfn_ros.h>
#include <pluginlib/class_list_macros.hpp>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>
#include <sensor_msgs/point_cloud2_iterator.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(navfn::NavfnROS, nav_core::BaseGlobalPlanner)
namespace navfn {
NavfnROS::NavfnROS()
: costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {}
NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
: costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {
//initialize the planner
initialize(name, costmap_ros);
}
NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame)
: costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {
//initialize the planner
initialize(name, costmap, global_frame);
}
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){
if(!initialized_){
costmap_ = costmap;
global_frame_ = global_frame;
planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()));
ros::NodeHandle private_nh("~/" + name);
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
private_nh.param("visualize_potential", visualize_potential_, false);
//if we're going to visualize the potential array we need to advertise
if(visualize_potential_)
potarr_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("potential", 1);
private_nh.param("allow_unknown", allow_unknown_, true);
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);
make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
initialized_ = true;
}
else
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}
bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point){
return validPointPotential(world_point, default_tolerance_);
}
bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point, double tolerance){
if(!initialized_){
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
double resolution = costmap_->getResolution();
geometry_msgs::Point p;
p = world_point;
p.y = world_point.y - tolerance;
while(p.y <= world_point.y + tolerance){
p.x = world_point.x - tolerance;
while(p.x <= world_point.x + tolerance){
double potential = getPointPotential(p);
if(potential < POT_HIGH){
return true;
}
p.x += resolution;
}
p.y += resolution;
}
return false;
}
double NavfnROS::getPointPotential(const geometry_msgs::Point& world_point){
if(!initialized_){
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return -1.0;
}
unsigned int mx, my;
if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my))
return DBL_MAX;
unsigned int index = my * planner_->nx + mx;
return planner_->potarr[index];
}
bool NavfnROS::computePotential(const geometry_msgs::Point& world_point){
if(!initialized_){
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//make sure to resize the underlying array that Navfn uses
planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
unsigned int mx, my;
if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my))
return false;
int map_start[2];
map_start[0] = 0;
map_start[1] = 0;
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
planner_->setStart(map_start);
planner_->setGoal(map_goal);
return planner_->calcNavFnDijkstra();
}
void NavfnROS::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 NavfnROS::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 = global_frame_;
return true;
}
void NavfnROS::mapToWorld(double mx, double my, double& wx, double& wy) {
wx = costmap_->getOriginX() + mx * costmap_->getResolution();
wy = costmap_->getOriginY() + my * costmap_->getResolution();
}
bool NavfnROS::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 NavfnROS::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;
//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 mx, my;
if(!costmap_->worldToMap(wx, wy, mx, my)){
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;
}
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, mx, my);
//make sure to resize the underlying array that Navfn uses
planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
int map_start[2];
map_start[0] = mx;
map_start[1] = my;
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if(!costmap_->worldToMap(wx, wy, mx, my)){
if(tolerance <= 0.0){
ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
mx = 0;
my = 0;
}
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
planner_->setStart(map_goal);
planner_->setGoal(map_start);
//bool success = planner_->calcNavFnAstar();
planner_->calcNavFnDijkstra(true);
double resolution = costmap_->getResolution();
geometry_msgs::PoseStamped p, best_pose;
p = goal;
bool found_legal = false;
double best_sdist = DBL_MAX;
p.pose.position.y = goal.pose.position.y - tolerance;
while(p.pose.position.y <= goal.pose.position.y + tolerance){
p.pose.position.x = goal.pose.position.x - tolerance;
while(p.pose.position.x <= goal.pose.position.x + tolerance){
double potential = getPointPotential(p.pose.position);
double sdist = sq_distance(p, goal);
if(potential < POT_HIGH && sdist < best_sdist){
best_sdist = sdist;
best_pose = p;
found_legal = true;
}
p.pose.position.x += resolution;
}
p.pose.position.y += resolution;
}
if(found_legal){
//extract the plan
if(getPlanFromPotential(best_pose, plan)){
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = best_pose;
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.");
}
}
if (visualize_potential_)
{
// Publish the potentials as a PointCloud2
sensor_msgs::PointCloud2 cloud;
cloud.width = 0;
cloud.height = 0;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = global_frame_;
sensor_msgs::PointCloud2Modifier cloud_mod(cloud);
cloud_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"pot", 1, sensor_msgs::PointField::FLOAT32);
cloud_mod.resize(planner_->ny * planner_->nx);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
PotarrPoint pt;
float *pp = planner_->potarr;
double pot_x, pot_y;
for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++)
{
if (pp[i] < 10e7)
{
mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y);
iter_x[0] = pot_x;
iter_x[1] = pot_y;
iter_x[2] = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
iter_x[3] = pp[i];
++iter_x;
}
}
potarr_pub_.publish(cloud);
}
//publish the plan for visualization purposes
publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return !plan.empty();
}
void NavfnROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a){
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());
if(path.empty()) {
//still set a valid frame so visualization won't hit transform issues
gui_path.header.frame_id = global_frame_;
gui_path.header.stamp = ros::Time::now();
} else {
gui_path.header.frame_id = path[0].header.frame_id;
gui_path.header.stamp = path[0].header.stamp;
}
// 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 NavfnROS::getPlanFromPotential(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;
}
//clear the plan, just in case
plan.clear();
//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;
}
double wx = goal.pose.position.x;
double wy = goal.pose.position.y;
//the potential has already been computed, so we won't update our copy of the costmap
unsigned int mx, my;
if(!costmap_->worldToMap(wx, wy, mx, my)){
ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
planner_->setStart(map_goal);
planner_->calcPath(costmap_->getSizeInCellsX() * 4);
//extract the plan
float *x = planner_->getPathX();
float *y = planner_->getPathY();
int len = planner_->getPathLen();
ros::Time plan_time = ros::Time::now();
for(int i = len - 1; i >= 0; --i){
//convert the plan to world coordinates
double world_x, world_y;
mapToWorld(x[i], y[i], 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);
}
//publish the plan for visualization purposes
publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return !plan.empty();
}
};

309
navigations/navfn/src/navtest/navtest.cpp Executable file
View File

@@ -0,0 +1,309 @@
//
// simple timing test of the nav fn planner
// expects a cost map in maps/willow-full-0.05.pgm
//
#include <navfn/navfn.h>
#ifdef HAVE_SYS_TIME_H
#include <sys/time.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string>
#include <fstream>
#include "navwin.h"
using namespace navfn;
#ifdef __APPLE__
# include <netpbm/pgm.h>
#else
extern "C" {
#include <stdio.h>
// pgm.h is not very friendly with system headers... need to undef max() and min() afterwards
#include <pgm.h>
#undef max
#undef min
}
#endif
int goal[2];
int start[2];
double get_ms()
{
struct timeval t0;
gettimeofday(&t0,NULL);
double ret = t0.tv_sec * 1000.0;
ret += ((double)t0.tv_usec)*0.001;
return ret;
}
NavWin *nwin;
void
dispPot(NavFn *nav)
{
nwin->drawPot(nav);
Fl::check();
}
// <raw> is true for ROS-generated raw cost maps
COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false);
int main(int argc, char **argv)
{
int dispn = 0;
int res = 50; // 50 mm resolution
double size = 40.0; // 40 m on a side
int inc = 2*COST_NEUTRAL; // thin wavefront
bool got_start_goal = false;
std::string pgm_file_name;
start[0] = 420;
start[1] = 420;
goal[0] = 580;
goal[1] = 400;
if (argc > 1)
{
pgm_file_name = std::string( argv[ 1 ]) + ".pgm";
std::string txt_file_name = std::string( argv[ 1 ]) + ".txt";
std::ifstream txt_stream( txt_file_name.c_str() );
if( txt_stream )
{
std::string name;
int x, y;
for( int i = 0; i < 2; i++ )
{
txt_stream >> name >> x >> y;
if( txt_stream && name == "Goal:" )
{
goal[0] = x;
goal[1] = y;
}
else if( txt_stream && name == "Start:" )
{
start[0] = x;
start[1] = y;
}
}
got_start_goal = true;
printf( "start is %d, %d, goal is %d, %d.\n", start[ 0 ], start[ 1 ], goal[ 0 ], goal[ 1 ]);
}
else
{
printf( "Failed to open file %s, assuming you didn't want to open a file.\n", txt_file_name.c_str() );
}
}
// get resolution (mm) and perhaps size (m)
if( !got_start_goal )
{
if (argc > 1)
res = atoi(argv[1]);
if (argc > 2)
size = atoi(argv[2]);
if (argc > 3)
inc = atoi(argv[3]);
if (argc > 4)
dispn = atoi(argv[4]);
}
NavFn *nav;
// try reading in a file
int sx,sy;
COSTTYPE *cmap = NULL;
cmap = readPGM( pgm_file_name.c_str(),&sx,&sy,true);
if (cmap)
{
nav = new NavFn(sx,sy);
}
else
{
sx = (int)((.001 + size) / (res*.001));
sy = sx;
nav = new NavFn(sx,sy); // size in pixels
goal[0] = sx-10;
goal[1] = sy/2;
start[0] = 20;
start[1] = sy/2;
}
// display
nwin = new NavWin(sx,sy,"Potential Field");
nwin->maxval = 2*sx*COST_NEUTRAL;
Fl::visual(FL_RGB);
nwin->show();
// set goal and robot poses
int *gg = goal;
nav->setGoal(gg);
int *ss = start;
nav->setStart(ss);
// set display function
nav->display(dispPot,dispn);
nav->priInc = inc;
printf("[NavTest] priority increment: %d\n", inc);
double t0 = get_ms();
// set up cost map from file, if it exists
if (cmap)
{
// nav->setCostMap(cmap);
memcpy(nav->costarr,cmap,sx*sy);
nav->setupNavFn(true);
}
else
{
nav->setupNavFn(false);
}
double t1 = get_ms();
// nav->calcNavFnAstar();
nav->calcNavFnDijkstra(true);
double t2 = get_ms();
printf("Setup: %d ms Plan: %d ms Total: %d ms\n",
(int)(t1-t0), (int)(t2-t1), (int)(t2-t0));
// draw potential field
float mmax = 0.0;
float *pp = nav->potarr;
int ntot = 0;
for (int i=0; i<nav->ny*nav->nx; i++, pp++)
{
if (*pp < 10e7 && *pp > mmax)
mmax = *pp;
if (*pp > 10e7)
ntot++; // number of uncalculated cells
}
printf("[NavFn] Cells not touched: %d/%d\n", ntot, nav->nx*nav->ny);
nwin->maxval = 4*mmax/3/15;
dispPot(nav);
while (Fl::check()) {
if( Fl::event_key( 'q' ))
{
break;
}
}
return 0;
}
// read in a PGM file for obstacles
// no expansion yet...
static int CS;
void
setcostobs(COSTTYPE *cmap, int n, int w)
{
CS = 11;
for (int i=-CS/2; i<CS/2; i++)
{
COSTTYPE *cm = i*w + &cmap[n];
for (int j=-CS/2; j<CS/2; j++)
cm[j] = COST_NEUTRAL + 50;
}
CS = 7;
for (int i=-CS/2; i<CS/2; i++)
{
COSTTYPE *cm = i*w + &cmap[n];
for (int j=-CS/2; j<CS/2; j++)
cm[j] = COST_OBS;
}
}
void setcostunk(COSTTYPE *cmap, int n, int w)
{
cmap[n] = COST_OBS;
}
#define unknown_gray 0xCC // seems to be the value of "unknown" in maps
COSTTYPE *
readPGM(const char *fname, int *width, int *height, bool raw)
{
pm_init("navtest",0);
FILE *pgmfile;
pgmfile = fopen(fname,"r");
if (!pgmfile)
{
printf("[NavTest] Can't find file %s\n", fname);
return NULL;
}
printf("[NavTest] Reading costmap file %s\n", fname);
int ncols, nrows;
gray maxval;
int format;
pgm_readpgminit(pgmfile, &ncols, &nrows, &maxval, &format);
printf("[NavTest] Size: %d x %d\n", ncols, nrows);
// set up cost map
COSTTYPE *cmap = (COSTTYPE *)malloc(ncols*nrows*sizeof(COSTTYPE));
if (!raw)
for (int i=0; i<ncols*nrows; i++)
cmap[i] = COST_NEUTRAL;
gray * row(pgm_allocrow(ncols));
int otot = 0;
int utot = 0;
int ftot = 0;
for (int ii = 0; ii < nrows; ii++) {
pgm_readpgmrow(pgmfile, row, ncols, maxval, format);
if (raw) // raw costmap from ROS
{
for (int jj(ncols - 1); jj >= 0; --jj)
{
int v = row[jj];
cmap[ii*ncols+jj] = v;
if (v >= COST_OBS_ROS)
otot++;
if (v == 0)
ftot++;
}
}
else
{
ftot = ncols*nrows;
for (int jj(ncols - 1); jj >= 0; --jj)
{
if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7)
{
setcostobs(cmap,ii*ncols+jj,ncols);
otot++;
ftot--;
}
else if (row[jj] <= unknown_gray)
{
setcostunk(cmap,ii*ncols+jj,ncols);
utot++;
ftot--;
}
}
}
}
printf("[NavTest] Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot);
pgm_freerow(row);
*width = ncols;
*height = nrows;
return cmap;
}

292
navigations/navfn/src/navtest/navwin.cpp Executable file
View File

@@ -0,0 +1,292 @@
//
// simple timing test of the nav fn planner
//
#include <string.h>
#include "navwin.h"
namespace navfn {
NavWin::NavWin(int w, int h, const char *name)
: Fl_Double_Window(w,h,name)
{
nw = w;
nh = h;
dec = 1;
maxval = 90*1000;
im = NULL;
pw = w;
ph = h;
pce = pne = poe = 0;
pc = pn = po = NULL;
pathlen = pathbuflen = 0;
path = NULL;
}
NavWin::~NavWin()
{
}
void
NavWin::drawPot(NavFn *nav)
{
float *pot = nav->potarr;
COSTTYPE *cst = nav->costarr;
int width = nav->nx;
int height = nav->ny;
// params
pw = width;
ph = height;
// figure out decimation or expansion to fit
dec = 1;
inc = 1;
if (width >= nw/2)
{
int ww = width;
while (ww > nw)
{
dec++;
ww = width/dec;
}
int hh = height/dec;
while (hh > nh)
{
dec++;
hh = height/dec;
}
if (im == NULL)
im = new uchar[nw*nh*3];
// draw potential
for (int i=0; i<height-dec+1; i+=dec)
{
float *pp = pot + i*width;
uchar *ii = im + 3*i/dec * nw;
for (int j=0; j<width-dec+1; j+=dec, pp+=dec)
{
int v;
v = (int)((*pp/maxval) * 255.0);
*ii++ = v;
*ii++ = v;
*ii++ = v;
}
}
// draw obstacles
for (int i=0; i<height-dec+1; i+=dec)
{
COSTTYPE *pp = cst + i*width;
uchar *ii = im + 3*i/dec * nw;
for (int j=0; j<width-dec+1; j+=dec, pp+=dec, ii+=3)
{
if (*pp >= COST_OBS)
{
*ii = 120;
*(ii+1) = 60;
*(ii+2) = 60;
}
}
}
}
else // expand
{
int ww = width;
while (ww < nw/2)
{
inc++;
ww = width*inc;
}
int hh = height*inc;
while (hh < nh/2)
{
inc++;
hh = height*inc;
}
if (im == NULL)
im = new uchar[nw*nh*3];
for (int i=0; i<height; i++)
{
float *pp = pot + i*width;
uchar *ii = im + 3*i*inc * nw;
for (int j=0; j<width; j++, pp++)
{
int v;
if (*pp > maxval)
v = 255;
else
v = (int)((*pp/maxval) * 255.0);
for (int k=0; k<inc; k++)
{
uchar *iii = ii + 3*j*inc + 3*k*nw;
for (int kk=0; kk<inc; kk++)
{
*iii++ = v;
*iii++ = v;
*iii++ = v;
}
}
}
}
}
make_current();
fl_draw_image(im, 0,0,nw,nh);
if (pc)
delete [] pc;
pce = nav->curPe;
pc = new int[pce];
memcpy(pc, nav->curP, pce*sizeof(int));
if (pn)
delete [] pn;
pne = nav->nextPe;
pn = new int[pne];
memcpy(pn, nav->nextP, pne*sizeof(int));
if (po)
delete [] po;
poe = nav->overPe;
po = new int[poe];
memcpy(po, nav->overP, poe*sizeof(int));
// start and goal
goal[0] = nav->goal[0];
goal[1] = nav->goal[1];
start[0] = nav->start[0];
start[1] = nav->start[1];
// path
if (nav->npath > 0)
{
pathlen = nav->npath;
if (pathbuflen < pathlen)
{
pathbuflen = pathlen;
if (path) delete [] path;
path = new int[pathbuflen];
}
for (int i=0; i<pathlen; i++)
path[i] = width*(int)(nav->pathy[i])+(int)(nav->pathx[i]);
}
drawOverlay();
redraw();
}
void
NavWin::drawOverlay()
{
if (inc == 1) // decimation
{
fl_color(255,0,0);
if (pce > 0)
for (int i=0; i<pce; i++)
{
int y = pc[i]/pw;
int x = pc[i]%pw;
fl_point(x/dec,y/dec);
}
fl_color(255,0,0);
if (pne > 0)
for (int i=0; i<pne; i++)
{
int y = pn[i]/pw;
int x = pn[i]%pw;
fl_point(x/dec,y/dec);
}
fl_color(0,255,0);
if (poe > 0)
for (int i=0; i<poe; i++)
{
int y = po[i]/pw;
int x = po[i]%pw;
fl_point(x/dec,y/dec);
}
}
else // expansion
{
fl_color(255,0,0);
if (pce > 0)
for (int i=0; i<pce; i++)
{
int y = pc[i]/pw;
int x = pc[i]%pw;
fl_rect(x*inc,y*inc,inc,inc);
}
fl_color(255,0,0);
if (pne > 0)
for (int i=0; i<pne; i++)
{
int y = pn[i]/pw;
int x = pn[i]%pw;
fl_rect(x*inc,y*inc,inc,inc);
}
fl_color(0,255,0);
if (poe > 0)
for (int i=0; i<poe; i++)
{
int y = po[i]/pw;
int x = po[i]%pw;
fl_rect(x*inc,y*inc,inc,inc);
}
}
// draw the goal
fl_color(255,200,0);
int x = goal[0];
int y = goal[1];
if (inc == 1)
fl_rectf(x/dec-2,y/dec-2,5,5);
else
fl_rectf(x*inc-2,y*inc-2,5,5);
// draw the start
fl_color(0,255,0);
x = start[0];
y = start[1];
if (inc == 1)
fl_rectf(x/dec-2,y/dec-2,5,5);
else
fl_rectf(x*inc-2,y*inc-2,5,5);
// draw the path
fl_color(0,255,255);
if (inc == 1 && pathlen > 0) // decimation or equal pixels
{
int y = path[0]/pw;
int x = path[0]%pw;
for (int i=1; i<pathlen; i++)
{
int y1 = path[i]/pw;
int x1 = path[i]%pw;
fl_line(x/dec,y/dec,x1/dec,y1/dec);
x = x1;
y = y1;
}
}
}
void NavWin::draw()
{
if (im)
fl_draw_image(im, 0,0,nw,nh);
drawOverlay();
}
};

45
navigations/navfn/src/navtest/navwin.h Executable file
View File

@@ -0,0 +1,45 @@
//
// drawing fns for nav fn
//
#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Group.H>
#include <FL/Fl_Window.H>
#include <FL/fl_draw.H>
#include <navfn/navfn.h>
namespace navfn {
class NavWin
: public Fl_Double_Window
{
public:
NavWin(int w, int h, const char *name);
~NavWin();
int nw,nh; // width and height of image
int pw,ph; // width and height of pot field
int dec, inc; // decimation or expansion for display
float maxval; // max potential value
void drawPot(NavFn *nav); // draw everything...
void drawOverlay();
uchar *im; // image for drawing
int *pc, *pn, *po; // priority buffers
int pce, pne, poe; // buffer sizes
int goal[2];
int start[2];
int *path; // path buffer, cell indices
int pathlen; // how many we have
int pathbuflen; // how big the path buffer is
void draw(); // draw the image
};
};

141
navigations/navfn/src/read_pgm_costmap.cpp Executable file
View File

@@ -0,0 +1,141 @@
/*
* Copyright (c) 2012, 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 <navfn/navfn.h>
#include <stdlib.h>
#include <stdint.h>
#ifdef __APPLE__
# include <netpbm/pgm.h>
#else
extern "C" {
#include <stdio.h>
// pgm.h is not very friendly with system headers... need to undef max() and min() afterwards
#include <pgm.h>
#undef max
#undef min
}
#endif
void
setcostobs(COSTTYPE *cmap, int n, int w)
{
int CS = 11;
for (int i=-CS/2; i<CS/2; i++)
{
COSTTYPE *cm = i*w + &cmap[n];
for (int j=-CS/2; j<CS/2; j++)
cm[j] = COST_NEUTRAL + 50;
}
CS = 7;
for (int i=-CS/2; i<CS/2; i++)
{
COSTTYPE *cm = i*w + &cmap[n];
for (int j=-CS/2; j<CS/2; j++)
cm[j] = COST_OBS;
}
}
void setcostunk(COSTTYPE *cmap, int n, int w)
{
cmap[n] = COST_OBS;
}
#define unknown_gray 0xCC // seems to be the value of "unknown" in maps
COSTTYPE *
readPGM(const char *fname, int *width, int *height, bool raw)
{
pm_init("navfn_tests",0);
FILE *pgmfile;
pgmfile = fopen(fname,"r");
if (!pgmfile)
{
printf("readPGM() Can't find file %s\n", fname);
return NULL;
}
printf("readPGM() Reading costmap file %s\n", fname);
int ncols, nrows;
gray maxval;
int format;
pgm_readpgminit(pgmfile, &ncols, &nrows, &maxval, &format);
printf("readPGM() Size: %d x %d\n", ncols, nrows);
// set up cost map
COSTTYPE *cmap = (COSTTYPE *)malloc(ncols*nrows*sizeof(COSTTYPE));
if (!raw)
for (int i=0; i<ncols*nrows; i++)
cmap[i] = COST_NEUTRAL;
gray * row(pgm_allocrow(ncols));
int otot = 0;
int utot = 0;
int ftot = 0;
for (int ii = 0; ii < nrows; ii++) {
pgm_readpgmrow(pgmfile, row, ncols, maxval, format);
if (raw) // raw costmap from ROS
{
for (int jj(ncols - 1); jj >= 0; --jj)
{
int v = row[jj];
cmap[ii*ncols+jj] = v;
if (v >= COST_OBS_ROS)
otot++;
if (v == 0)
ftot++;
}
}
else
{
ftot = ncols*nrows;
for (int jj(ncols - 1); jj >= 0; --jj)
{
if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7)
{
setcostobs(cmap,ii*ncols+jj,ncols);
otot++;
ftot--;
}
else if (row[jj] <= unknown_gray)
{
setcostunk(cmap,ii*ncols+jj,ncols);
utot++;
ftot--;
}
}
}
}
printf("readPGM() Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot);
pgm_freerow(row);
*width = ncols;
*height = nrows;
return cmap;
}

9
navigations/navfn/srv/MakeNavPlan.srv Executable file
View File

@@ -0,0 +1,9 @@
geometry_msgs/PoseStamped start
geometry_msgs/PoseStamped goal
---
uint8 plan_found
string error_message
# if plan_found is true, this is an array of waypoints from start to goal, where the first one equals start and the last one equals goal
geometry_msgs/PoseStamped[] path

4
navigations/navfn/srv/SetCostmap.srv Executable file
View File

@@ -0,0 +1,4 @@
uint8[] costs
uint16 height
uint16 width
---

2
navigations/navfn/test/CMakeLists.txt Executable file
View File

@@ -0,0 +1,2 @@
catkin_add_gtest(path_calc_test path_calc_test.cpp ../src/read_pgm_costmap.cpp)
target_link_libraries(path_calc_test navfn netpbm)

160
navigations/navfn/test/path_calc_test.cpp Executable file
View File

@@ -0,0 +1,160 @@
/*
* Copyright (c) 2012, 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 <string>
#include <ros/package.h>
#include <gtest/gtest.h>
#include <navfn/navfn.h>
#include <navfn/read_pgm_costmap.h>
// Load a willow garage costmap and return a NavFn instance using it.
// If the costmap file fails to load, returns NULL.
navfn::NavFn* make_willow_nav()
{
int sx,sy;
std::string path = ros::package::getPath( ROS_PACKAGE_NAME ) + "/test/willow_costmap.pgm";
COSTTYPE *cmap = readPGM( path.c_str(), &sx, &sy, true );
if( cmap == NULL )
{
return NULL;
}
navfn::NavFn* nav = new navfn::NavFn(sx,sy);
nav->priInc = 2*COST_NEUTRAL; // thin wavefront
memcpy( nav->costarr, cmap, sx*sy );
return nav;
}
void print_neighborhood_of_last_path_entry( navfn::NavFn* nav )
{
printf("last path entries:\n");
for( int i = nav->npath - 4; i < nav->npath; i++ )
{
printf("%.3f, %.3f\n", nav->pathx[ i ], nav->pathy[ i ]);
}
printf("potential field neighborhood of last entry:\n");
int xf = nav->pathx[ nav->npath-1 ];
int yf = nav->pathy[ nav->npath-1 ];
printf( " " );
for( int x = xf - 2; x <= xf + 2; x++ )
{
printf( " %6d", x );
}
printf( "\n" );
for( int y = yf - 2; y <= yf + 2; y++ )
{
printf( "%5d:", y );
for( int x = xf - 2; x <= xf + 2; x++ )
{
printf( " %5.1f", nav->potarr[ y * nav->nx + x ] );
}
printf( "\n" );
}
printf("gradient neighborhood of last entry:\n");
printf( " " );
for( int x = xf - 2; x <= xf + 2; x++ )
{
printf( " %6d", x );
}
printf( "\n" );
for( int y = yf - 2; y <= yf + 2; y++ )
{
printf( "%5d x:", y );
for( int x = xf - 2; x <= xf + 2; x++ )
{
printf( " %5.1f", nav->gradx[ y * nav->nx + x ] );
}
printf( "\n" );
printf( " y:" );
for( int x = xf - 2; x <= xf + 2; x++ )
{
printf( " %5.1f", nav->grady[ y * nav->nx + x ] );
}
printf( "\n" );
}
}
TEST(PathCalc, oscillate_in_pinch_point)
{
navfn::NavFn* nav = make_willow_nav();
ASSERT_TRUE( nav != NULL );
int goal[2];
int start[2];
start[0] = 428;
start[1] = 746;
goal[0] = 350;
goal[1] = 450;
nav->setGoal( goal );
nav->setStart( start );
bool plan_success = nav->calcNavFnDijkstra( true );
EXPECT_TRUE( plan_success );
if( !plan_success )
{
print_neighborhood_of_last_path_entry( nav );
}
}
TEST(PathCalc, easy_nav_should_always_work)
{
navfn::NavFn* nav = make_willow_nav();
ASSERT_TRUE( nav != NULL );
int goal[2];
int start[2];
start[0] = 350;
start[1] = 400;
goal[0] = 350;
goal[1] = 450;
nav->setGoal( goal );
nav->setStart( start );
EXPECT_TRUE( nav->calcNavFnDijkstra( true ));
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

5
navigations/navfn/test/willow_costmap.pgm Executable file
View File

File diff suppressed because one or more lines are too long