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

214
navigations/base_local_planner/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,214 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package base_local_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)
-------------------
* Commit 89a8593 removed footprint scaling. This brings it back. (`#886 <https://github.com/ros-planning/navigation/issues/886>`_) (`#1204 <https://github.com/ros-planning/navigation/issues/1204>`_)
Co-authored-by: Frank Höller <frank.hoeller@fkie.fraunhofer.de>
* Contributors: Michael Ferguson
1.17.1 (2020-08-27)
-------------------
* occdist_scale should not be scaled by the costmap resolution as it doesn't multiply a value that includes a distance. (`#1000 <https://github.com/ros-planning/navigation/issues/1000>`_)
* Contributors: wjwagner
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)
-------------------
* Fixes gdist- and pdist_scale node paramter names (`#936 <https://github.com/cobalt-robotics/navigation/issues/936>`_)
Renames goal and path distance dynamic reconfigure parameter
names in the cfg file in order to actually make the parameters
used by the trajectory planner changeable.
Fixes `#935 <https://github.com/cobalt-robotics/navigation/issues/935>`_
* don't include a main() function in base_local_planner library (`#969 <https://github.com/cobalt-robotics/navigation/issues/969>`_)
* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#851 <https://github.com/cobalt-robotics/navigation/issues/851>`_)
* Contributors: David Leins, Sean Yen, ipa-fez
1.16.3 (2019-11-15)
-------------------
* Merge branch 'melodic-devel' into layer_clear_area-melodic
* Provide different negative values for unknown and out-of-map costs (`#833 <https://github.com/ros-planning/navigation/issues/833>`_)
* Merge pull request `#857 <https://github.com/ros-planning/navigation/issues/857>`_ from jspricke/add_include
Add missing header
* Add missing header
* [kinetic] Fix for adjusting plan resolution (`#819 <https://github.com/ros-planning/navigation/issues/819>`_)
* Fix for adjusting plan resolution
* Simpler math expression
* Contributors: David V. Lu!!, Jochen Sprickerhof, Jorge Santos Simón, Michael Ferguson, Steven Macenski
1.16.2 (2018-07-31)
-------------------
* Merge pull request `#773 <https://github.com/ros-planning/navigation/issues/773>`_ from ros-planning/packaging_fixes
packaging fixes
* add explicit sensor_msgs, tf2 depends for base_local_planner
* Contributors: Michael Ferguson
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Remove PCL `#765 <https://github.com/ros-planning/navigation/issues/765>`_
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Fix trajectory obstacle scoring in dwa_local_planner.
* Make trajectory scoring scales consistent.
* unify parameter names between base_local_planner and dwa_local_planner
addresses parts of `#90 <https://github.com/ros-planning/navigation/issues/90>`_
* fix param to min_in_place_vel_theta, closes `#487 <https://github.com/ros-planning/navigation/issues/487>`_
* add const to getLocalPlane, fixes `#709 <https://github.com/ros-planning/navigation/issues/709>`_
* Merge pull request `#732 <https://github.com/ros-planning/navigation/issues/732>`_ from marting87/small_typo_fixes
Small typo fixes in ftrajectory_planner_ros and robot_pose_ekf
* Fixed typos for base_local_planner
* Contributors: Alexander Moriarty, David V. Lu, Martin Ganeff, Michael Ferguson, Pavlo Kolomiiets, Rein Appeldoorn, Vincent Rabaud, moriarty
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)
* CostmapModel: Make lineCost and pointCost public (`#658 <https://github.com/ros-planning/navigation/issues/658>`_)
Make the methods `lineCost` and `pointCost` of the CostmapModel class
public so they can be used outside of the class.
Both methods are not changing the instance, so this should not cause any
problems. To emphasise their constness, add the actual `const` keyword.
* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/649>`_ from aaronhoy/lunar_add_ahoy
Add myself as a maintainer.
* Contributors: Aaron Hoy, Felix Widmaier, Michael Ferguson
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* set message_generation build and runtime dependency
* convert packages to format2
* cleaner logic, fixes `#156 <https://github.com/ros-planning/navigation/issues/156>`_
* Merge pull request `#596 <https://github.com/ros-planning/navigation/issues/596>`_ from ros-planning/lunar_548
* Add cost function to prevent unnecessary spinning
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* add missing deps on libpcl
* import only PCL common
* pcl proagate -lQt5::Widgets flag so we need to find_package Qt5Widgets (`#578 <https://github.com/ros-planning/navigation/issues/578>`_)
* make rostest in CMakeLists optional (`ros/rosdistro#3010 <https://github.com/ros/rosdistro/issues/3010>`_)
* remove GCC warnings
* Contributors: Lukas Bulwahn, Martin Günther, Michael Ferguson, Mikael Arguedas, Morgan Quigley, Vincent Rabaud, lengly
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
* base_local_planner: some fixes in goal_functions
* Merge pull request `#348 <https://github.com/ros-planning/navigation/issues/348>`_ from mikeferguson/trajectory_planner_fixes
* fix stuck_left/right calculation
* fix calculation of heading diff
* Contributors: Gael Ecorchard, Michael Ferguson
1.13.0 (2015-03-17)
-------------------
* remove previously deprecated param
* Contributors: Michael Ferguson
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)
--------------------
* Fixed setting child_frame_id in base_local_planner::OdometryHelperRos
* Contributors: Mani Monajjemi
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
* Bugfix uninitialised occ_cost variable usage
This fixes `#256 <https://github.com/ros-planning/navigation/issues/256>`_.
* base_local_planner: adds waitForTransform
* Fixed issue causing trajectory planner returning false to isGoalReach ed even when it's control thread finishes executing
* Contributors: Daniel Stonier, Marcus Liebhardt, hes3pal
1.11.11 (2014-07-23)
--------------------
* Minor code cleanup
* Contributors: Enrique Fernández Perdomo
1.11.10 (2014-06-25)
--------------------
* Remove unnecessary colons
* renames acc_lim_th to acc_lim_theta, add warning if using acc_lim_th
* uses odom child_frame_id to set robot_vel frame_id
* Contributors: David Lu!!, Michael Ferguson, Enrique Fernández Perdomo
1.11.9 (2014-06-10)
-------------------
* uses ::hypot(x, y) instead of sqrt(x*x, y*y)
* No need to use `limits->`
* Contributors: Enrique Fernández Perdomo
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* fixes latch_xy_goal_tolerance param not taken
* update build to find eigen using cmake_modules
* Trajectory: fix constness of getter methods
* Use hypot() instead of sqrt(x*x, y*y)
* Fix bug in distance calculation for trajectory rollout
* Some documentation fixes in SimpleTrajectoryGenerator
* Contributors: Michael Ferguson, Siegfried-A. Gevatter Pujals, enriquefernandez
1.11.5 (2014-01-30)
-------------------
* Merge pull request `#152 <https://github.com/ros-planning/navigation/issues/152>`_ from KaijenHsiao/hydro-devel
uncommented trajectory_planner_ros from catkin_package LIBRARIES so other packages can find it
* Fix negative score bug, add ability to sum scores
* Ignore pyc files from running in devel
* Correct type of prefer_forward penalty member variable
* uncommented trajectory_planner_ros from catkin_package LIBRARIES so other packages can find it
* Better handling of frame param in MapGridVisualizer
* check for CATKIN_ENABLE_TESTING
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* Changed new Odom-Helper::initialize() function to setOdomTopic().
* Converted to a pointcloud pointer in Observation in more places.

165
navigations/base_local_planner/CMakeLists.txt Executable file
View File

@@ -0,0 +1,165 @@
cmake_minimum_required(VERSION 3.0.2)
project(base_local_planner)
include(CheckIncludeFile)
find_package(catkin REQUIRED
COMPONENTS
angles
cmake_modules
costmap_2d
dynamic_reconfigure
geometry_msgs
message_generation
nav_core
nav_msgs
pluginlib
roscpp
rospy
sensor_msgs
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
voxel_grid
)
find_package(Boost REQUIRED
COMPONENTS
thread
)
find_package(Eigen3 REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_python_setup()
# messages
add_message_files(
DIRECTORY msg
FILES
Position2DInt.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
# dynamic reconfigure
generate_dynamic_reconfigure_options(
cfg/BaseLocalPlanner.cfg
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES
base_local_planner
trajectory_planner_ros
CATKIN_DEPENDS
angles
costmap_2d
dynamic_reconfigure
geometry_msgs
message_runtime
nav_core
nav_msgs
pluginlib
roscpp
sensor_msgs
std_msgs
tf2
tf2_ros
voxel_grid
)
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)
#uncomment for profiling
#set(ROS_COMPILE_FLAGS "-g -pg" ${ROS_COMPILE_FLAGS})
#set(ROS_LINK_FLAGS "-g -pg" ${ROS_LINK_FLAGS})
#set(ROS_COMPILE_FLAGS "-g" ${ROS_COMPILE_FLAGS})
#set(ROS_LINK_FLAGS "-g" ${ROS_LINK_FLAGS})
add_library(base_local_planner
src/footprint_helper.cpp
src/goal_functions.cpp
src/map_cell.cpp
src/map_grid.cpp
src/map_grid_visualizer.cpp
src/map_grid_cost_function.cpp
src/latched_stop_rotate_controller.cpp
src/local_planner_util.cpp
src/odometry_helper_ros.cpp
src/obstacle_cost_function.cpp
src/oscillation_cost_function.cpp
src/prefer_forward_cost_function.cpp
src/point_grid.cpp
src/costmap_model.cpp
src/simple_scored_sampling_planner.cpp
src/simple_trajectory_generator.cpp
src/trajectory.cpp
src/twirling_cost_function.cpp
src/voxel_grid_model.cpp)
add_dependencies(base_local_planner base_local_planner_gencfg)
add_dependencies(base_local_planner base_local_planner_generate_messages_cpp)
add_dependencies(base_local_planner nav_msgs_generate_messages_cpp)
target_link_libraries(base_local_planner
${catkin_LIBRARIES}
${Boost_LIBRARIES}
${Eigen_LIBRARIES}
)
add_library(trajectory_planner_ros
src/trajectory_planner.cpp
src/trajectory_planner_ros.cpp)
add_dependencies(trajectory_planner_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(trajectory_planner_ros
base_local_planner)
add_executable(point_grid src/point_grid_node.cpp)
add_dependencies(point_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(point_grid ${catkin_LIBRARIES} base_local_planner)
install(TARGETS
base_local_planner
trajectory_planner_ros
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(FILES blp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
PATTERN ".svn" EXCLUDE
)
if(CATKIN_ENABLE_TESTING)
find_package(rostest)
catkin_add_gtest(base_local_planner_utest
test/gtest_main.cpp
test/utest.cpp
test/velocity_iterator_test.cpp
test/footprint_helper_test.cpp
test/trajectory_generator_test.cpp
test/map_grid_test.cpp)
target_link_libraries(base_local_planner_utest
base_local_planner trajectory_planner_ros
)
catkin_add_gtest(line_iterator
test/line_iterator_test.cpp)
endif()

7
navigations/base_local_planner/blp_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libtrajectory_planner_ros">
<class name="base_local_planner/TrajectoryPlannerROS" type="base_local_planner::TrajectoryPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
<description>
A implementation of a local planner using either a DWA or Trajectory Rollout approach based on configuration parameters.
</description>
</class>
</library>

57
navigations/base_local_planner/cfg/BaseLocalPlanner.cfg Executable file
View File

@@ -0,0 +1,57 @@
#!/usr/bin/env python
PACKAGE = 'base_local_planner'
from math import pi
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t, str_t
gen = ParameterGenerator()
# gen.add("inscribed_radius", double_t, 0, "The radius of the inscribed circle of the robot", 1, 0)
# gen.add("circumscribed_radius", double_t, 0, "The radius of the circumscribed circle of the robot", 1, 0)
gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0)
gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 2.5, 0, 20.0)
gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 3.2, 0, 20.0)
gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55, 0, 20.0)
gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0, 0, 20.0)
gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0, 20.0)
gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", -1.0, -20.0, 0.0)
gen.add("min_in_place_vel_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.4, 0, 20.0)
gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5)
gen.add("angular_sim_granularity", double_t, 0, "The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things", 0.025, 0, pi/2)
gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0, 5)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0, 5)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0, 5)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0, 5)
gen.add("escape_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.10, 0, 5)
gen.add("escape_reset_theta", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", pi/2, 0, 5)
gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 20, 1, 300)
gen.add("vtheta_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1, 300)
gen.add("heading_lookahead", double_t, 0, "How far the robot should look ahead of itself when differentiating between different rotational velocities", 0.325, 0, 5)
gen.add("holonomic_robot", bool_t, 0, "Set this to true if the robot being controlled can take y velocities and false otherwise", True)
gen.add("escape_vel", double_t, 0, "The velocity to use while backing up", -0.1, -2, 2)
gen.add("dwa", bool_t, 0, "Set this to true to use the Dynamic Window Approach, false to use acceleration limits", False)
gen.add("heading_scoring", bool_t, 0, "Set this to true to use the Dynamic Window Approach, false to use acceleration limits", False)
gen.add("heading_scoring_timestep", double_t, 0, "How far to look ahead in time when we score heading based trajectories", 0.1, 0, 1)
gen.add("simple_attractor", bool_t, 0, "Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation", False)
gen.add("y_vels", str_t, 0, "A comma delimited list of the y velocities the controller will explore", "-0.3,-0.1,0.1,-0.3")
gen.add("restore_defaults", bool_t, 0, "Retore to the default configuration", False)
exit(gen.generate(PACKAGE, "base_local_planner", "BaseLocalPlanner"))

10
navigations/base_local_planner/cfg/LocalPlannerLimits.cfg Executable file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env python
# Generic Local Planner configuration
# from dynamic_reconfigure.parameter_generator_catkin import *
# if __name__ == "__main__":
# gen = ParameterGenerator()
# add_generic_localplanner_params(gen)
# exit(gen.generate(PACKAGE, "base_local_planner", "LocalPlannerLimits"))

102
navigations/base_local_planner/include/base_local_planner/costmap_model.h Executable file
View File

@@ -0,0 +1,102 @@
/*********************************************************************
*
* 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 TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#define TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#include <base_local_planner/world_model.h>
// For obstacle data access
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
/**
* @class CostmapModel
* @brief A class that implements the WorldModel interface to provide grid
* based collision checks for the trajectory controller using the costmap.
*/
class CostmapModel : public WorldModel {
public:
/**
* @brief Constructor for the CostmapModel
* @param costmap The costmap that should be used
* @return
*/
CostmapModel(const costmap_2d::Costmap2D& costmap);
/**
* @brief Destructor for the world model
*/
virtual ~CostmapModel(){}
using WorldModel::footprintCost;
/**
* @brief Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise:
* -1 if footprint covers at least a lethal obstacle cell, or
* -2 if footprint covers at least a no-information cell, or
* -3 if footprint is [partially] outside of the map
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost(int x0, int x1, int y0, int y1) const;
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
double pointCost(int x, int y) const;
private:
const costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
};
#endif

87
navigations/base_local_planner/include/base_local_planner/footprint_helper.h Executable file
View File

@@ -0,0 +1,87 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef FOOTPRINT_HELPER_H_
#define FOOTPRINT_HELPER_H_
#include <vector>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/Point.h>
#include <Eigen/Core>
#include <base_local_planner/Position2DInt.h>
namespace base_local_planner {
class FootprintHelper {
public:
FootprintHelper();
virtual ~FootprintHelper();
/**
* @brief Used to get the cells that make up the footprint of the robot
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @param fill If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.
* @return The cells that make up either the outline or entire footprint of the robot depending on fill
*/
std::vector<base_local_planner::Position2DInt> getFootprintCells(
Eigen::Vector3f pos,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D&,
bool fill);
/**
* @brief Use Bresenham's algorithm to trace a line between two points in a grid
* @param x0 The x coordinate of the first point
* @param x1 The x coordinate of the second point
* @param y0 The y coordinate of the first point
* @param y1 The y coordinate of the second point
* @param pts Will be filled with the cells that lie on the line in the grid
*/
void getLineCells(int x0, int x1, int y0, int y1, std::vector<base_local_planner::Position2DInt>& pts);
/**
* @brief Fill the outline of a polygon, in this case the robot footprint, in a grid
* @param footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint
*/
void getFillCells(std::vector<base_local_planner::Position2DInt>& footprint);
};
} /* namespace base_local_planner */
#endif /* FOOTPRINT_HELPER_H_ */

153
navigations/base_local_planner/include/base_local_planner/goal_functions.h Executable file
View File

@@ -0,0 +1,153 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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
*********************************************************************/
#ifndef BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#define BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf2_ros/buffer.h>
#include <string>
#include <cmath>
#include <angles/angles.h>
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
/**
* @brief return squared distance to check if the goal position has been achieved
* @param global_pose The pose of the robot in the global frame
* @param goal_x The desired x value for the goal
* @param goal_y The desired y value for the goal
* @return distance to goal
*/
double getGoalPositionDistance(const geometry_msgs::PoseStamped& global_pose, double goal_x, double goal_y);
/**
* @brief return angle difference to goal to check if the goal orientation has been achieved
* @param global_pose The pose of the robot in the global frame
* @param goal_x The desired x value for the goal
* @param goal_y The desired y value for the goal
* @return angular difference
*/
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped& global_pose, double goal_th);
/**
* @brief Publish a plan for visualization purposes
* @param path The plan to publish
* @param pub The published to use
* @param r,g,b,a The color and alpha value to use when publishing
*/
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub);
/**
* @brief Trim off parts of the global plan that are far enough behind the robot
* @param global_pose The pose of the robot in the global frame
* @param plan The plan to be pruned
* @param global_plan The plan to be pruned in the frame of the planner
*/
void prunePlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Transforms the global plan of the robot from the planner frame to the frame of the costmap,
* selects only the (first) part of the plan that is within the costmap area.
* @param tf A reference to a transform listener
* @param global_plan The plan to be transformed
* @param robot_pose The pose of the robot in the global frame (same as costmap)
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param transformed_plan Populated with the transformed plan
*/
bool transformGlobalPlan(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_robot_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan);
/**
* @brief Returns last pose in plan
* @param tf A reference to a transform listener
* @param global_plan The plan being followed
* @param global_frame The global frame of the local planner
* @param goal_pose the pose to copy into
* @return True if achieved, false otherwise
*/
bool getGoalPose(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame,
geometry_msgs::PoseStamped &goal_pose);
/**
* @brief Check if the goal pose has been achieved
* @param tf A reference to a transform listener
* @param global_plan The plan being followed
* @param costmap_ros A reference to the costmap object being used by the planner
* @param global_frame The global frame of the local planner
* @param base_odom The current odometry information for the robot
* @param rot_stopped_vel The rotational velocity below which the robot is considered stopped
* @param trans_stopped_vel The translational velocity below which the robot is considered stopped
* @param xy_goal_tolerance The translational tolerance on reaching the goal
* @param yaw_goal_tolerance The rotational tolerance on reaching the goal
* @return True if achieved, false otherwise
*/
bool isGoalReached(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
geometry_msgs::PoseStamped& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Check whether the robot is stopped or not
* @param base_odom The current odometry information for the robot
* @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
* @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
* @return True if the robot is stopped, false otherwise
*/
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity,
const double& trans_stopped_velocity);
};
#endif

93
navigations/base_local_planner/include/base_local_planner/latched_stop_rotate_controller.h Executable file
View File

@@ -0,0 +1,93 @@
/*
* latched_stop_rotate_controller.h
*
* Created on: Apr 16, 2012
* Author: tkruse
*/
#ifndef LATCHED_STOP_ROTATE_CONTROLLER_H_
#define LATCHED_STOP_ROTATE_CONTROLLER_H_
#include <string>
#include <Eigen/Core>
#include <base_local_planner/local_planner_util.h>
#include <base_local_planner/odometry_helper_ros.h>
namespace base_local_planner {
class LatchedStopRotateController {
public:
LatchedStopRotateController(const std::string& name = "");
virtual ~LatchedStopRotateController();
bool isPositionReached(LocalPlannerUtil* planner_util,
const geometry_msgs::PoseStamped& global_pose);
bool isGoalReached(LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper,
const geometry_msgs::PoseStamped& global_pose);
void resetLatching() {
xy_tolerance_latch_ = false;
}
/**
* @brief Stop the robot taking into account acceleration limits
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check);
/**
* @brief Once a goal position is reached... rotate to the goal orientation
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param goal_th The desired th value for the goal
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool rotateToGoal(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
double goal_th,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
base_local_planner::LocalPlannerLimits& limits,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check);
bool computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper,
const geometry_msgs::PoseStamped& global_pose,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check);
private:
inline double sign(double x){
return x < 0.0 ? -1.0 : 1.0;
}
// whether to latch at all, and whether in this turn we have already been in goal area
bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
bool rotating_to_goal_;
};
} /* namespace base_local_planner */
#endif /* LATCHED_STOP_ROTATE_CONTROLLER_H_ */

143
navigations/base_local_planner/include/base_local_planner/line_iterator.h Executable file
View File

@@ -0,0 +1,143 @@
/*
* 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 LINE_ITERATOR_H
#define LINE_ITERATOR_H
#include <stdlib.h>
namespace base_local_planner
{
/** An iterator implementing Bresenham Ray-Tracing. */
class LineIterator
{
public:
LineIterator( int x0, int y0, int x1, int y1 )
: x0_( x0 )
, y0_( y0 )
, x1_( x1 )
, y1_( y1 )
, x_( x0 ) // X and Y start of at first endpoint.
, y_( y0 )
, deltax_( abs( x1 - x0 ))
, deltay_( abs( y1 - y0 ))
, curpixel_( 0 )
{
if( x1_ >= x0_ ) // The x-values are increasing
{
xinc1_ = 1;
xinc2_ = 1;
}
else // The x-values are decreasing
{
xinc1_ = -1;
xinc2_ = -1;
}
if( y1_ >= y0_) // The y-values are increasing
{
yinc1_ = 1;
yinc2_ = 1;
}
else // The y-values are decreasing
{
yinc1_ = -1;
yinc2_ = -1;
}
if( deltax_ >= deltay_ ) // There is at least one x-value for every y-value
{
xinc1_ = 0; // Don't change the x when numerator >= denominator
yinc2_ = 0; // Don't change the y for every iteration
den_ = deltax_;
num_ = deltax_ / 2;
numadd_ = deltay_;
numpixels_ = deltax_; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2_ = 0; // Don't change the x for every iteration
yinc1_ = 0; // Don't change the y when numerator >= denominator
den_ = deltay_;
num_ = deltay_ / 2;
numadd_ = deltax_;
numpixels_ = deltay_; // There are more y-values than x-values
}
}
bool isValid() const
{
return curpixel_ <= numpixels_;
}
void advance()
{
num_ += numadd_; // Increase the numerator by the top of the fraction
if( num_ >= den_ ) // Check if numerator >= denominator
{
num_ -= den_; // Calculate the new numerator value
x_ += xinc1_; // Change the x as appropriate
y_ += yinc1_; // Change the y as appropriate
}
x_ += xinc2_; // Change the x as appropriate
y_ += yinc2_; // Change the y as appropriate
curpixel_++;
}
int getX() const { return x_; }
int getY() const { return y_; }
int getX0() const { return x0_; }
int getY0() const { return y0_; }
int getX1() const { return x1_; }
int getY1() const { return y1_; }
private:
int x0_; ///< X coordinate of first end point.
int y0_; ///< Y coordinate of first end point.
int x1_; ///< X coordinate of second end point.
int y1_; ///< Y coordinate of second end point.
int x_; ///< X coordinate of current point.
int y_; ///< Y coordinate of current point.
int deltax_; ///< Difference between Xs of endpoints.
int deltay_; ///< Difference between Ys of endpoints.
int curpixel_; ///< index of current point in line loop.
int xinc1_, xinc2_, yinc1_, yinc2_;
int den_, num_, numadd_, numpixels_;
};
} // end namespace base_local_planner
#endif // LINE_ITERATOR_H

121
navigations/base_local_planner/include/base_local_planner/local_planner_limits.h Executable file
View File

@@ -0,0 +1,121 @@
/***********************************************************
* 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.
***********************************************************/
#ifndef __base_local_planner__LOCALPLANNERLIMITS_H__
#define __base_local_planner__LOCALPLANNERLIMITS_H__
#include <Eigen/Core>
namespace base_local_planner
{
class LocalPlannerLimits
{
public:
double max_vel_trans;
double min_vel_trans;
double max_vel_x;
double min_vel_x;
double max_vel_y;
double min_vel_y;
double max_vel_theta;
double min_vel_theta;
double acc_lim_x;
double acc_lim_y;
double acc_lim_theta;
double acc_lim_trans;
bool prune_plan;
double xy_goal_tolerance;
double yaw_goal_tolerance;
double trans_stopped_vel;
double theta_stopped_vel;
bool restore_defaults;
LocalPlannerLimits() {}
LocalPlannerLimits(
double nmax_vel_trans,
double nmin_vel_trans,
double nmax_vel_x,
double nmin_vel_x,
double nmax_vel_y,
double nmin_vel_y,
double nmax_vel_theta,
double nmin_vel_theta,
double nacc_lim_x,
double nacc_lim_y,
double nacc_lim_theta,
double nacc_lim_trans,
double nxy_goal_tolerance,
double nyaw_goal_tolerance,
bool nprune_plan = true,
double ntrans_stopped_vel = 0.1,
double ntheta_stopped_vel = 0.1):
max_vel_trans(nmax_vel_trans),
min_vel_trans(nmin_vel_trans),
max_vel_x(nmax_vel_x),
min_vel_x(nmin_vel_x),
max_vel_y(nmax_vel_y),
min_vel_y(nmin_vel_y),
max_vel_theta(nmax_vel_theta),
min_vel_theta(nmin_vel_theta),
acc_lim_x(nacc_lim_x),
acc_lim_y(nacc_lim_y),
acc_lim_theta(nacc_lim_theta),
acc_lim_trans(nacc_lim_trans),
prune_plan(nprune_plan),
xy_goal_tolerance(nxy_goal_tolerance),
yaw_goal_tolerance(nyaw_goal_tolerance),
trans_stopped_vel(ntrans_stopped_vel),
theta_stopped_vel(ntheta_stopped_vel) {}
~LocalPlannerLimits() {}
/**
* @brief Get the acceleration limits of the robot
* @return The acceleration limits of the robot
*/
Eigen::Vector3f getAccLimits() {
Eigen::Vector3f acc_limits;
acc_limits[0] = acc_lim_x;
acc_limits[1] = acc_lim_y;
acc_limits[2] = acc_lim_theta;
return acc_limits;
}
};
}
#endif // __LOCALPLANNERLIMITS_H__

111
navigations/base_local_planner/include/base_local_planner/local_planner_util.h Executable file
View File

@@ -0,0 +1,111 @@
/*********************************************************************
*
* 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 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
*********************************************************************/
#ifndef ABSTRACT_LOCAL_PLANNER_ODOM_H_
#define ABSTRACT_LOCAL_PLANNER_ODOM_H_
#include <nav_core/base_local_planner.h>
#include <boost/thread.hpp>
#include <costmap_2d/costmap_2d.h>
#include <tf2_ros/buffer.h>
#include <base_local_planner/local_planner_limits.h>
namespace base_local_planner {
/**
* @class LocalPlannerUtil
* @brief Helper class implementing infrastructure code many local planner implementations may need.
*/
class LocalPlannerUtil {
private:
// things we get from move_base
std::string name_;
std::string global_frame_;
costmap_2d::Costmap2D* costmap_;
tf2_ros::Buffer* tf_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
boost::mutex limits_configuration_mutex_;
bool setup_;
LocalPlannerLimits default_limits_;
LocalPlannerLimits limits_;
bool initialized_;
public:
/**
* @brief Callback to update the local planner's parameters
*/
void reconfigureCB(LocalPlannerLimits &config, bool restore_defaults);
LocalPlannerUtil() : initialized_(false) {}
~LocalPlannerUtil() {
}
void initialize(tf2_ros::Buffer* tf,
costmap_2d::Costmap2D* costmap,
std::string global_frame);
bool getGoal(geometry_msgs::PoseStamped& goal_pose);
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan);
costmap_2d::Costmap2D* getCostmap();
LocalPlannerLimits getCurrentLimits();
std::string getGlobalFrame(){ return global_frame_; }
};
};
#endif /* ABSTRACT_LOCAL_PLANNER_ODOM_H_ */

67
navigations/base_local_planner/include/base_local_planner/map_cell.h Executable file
View File

@@ -0,0 +1,67 @@
/*********************************************************************
* 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_MAP_CELL_H_
#define TRAJECTORY_ROLLOUT_MAP_CELL_H_
#include <base_local_planner/trajectory_inc.h>
namespace base_local_planner {
/**
* @class MapCell
* @brief Stores path distance and goal distance information used for scoring trajectories
*/
class MapCell{
public:
/**
* @brief Default constructor
*/
MapCell();
/**
* @brief Copy constructor
* @param mc The MapCell to be copied
*/
MapCell(const MapCell& mc);
unsigned int cx, cy; ///< @brief Cell index in the grid map
double target_dist; ///< @brief Distance to planner's path
bool target_mark; ///< @brief Marks for computing path/goal distances
bool within_robot; ///< @brief Mark for cells within the robot footprint
};
};
#endif

200
navigations/base_local_planner/include/base_local_planner/map_grid.h Executable file
View File

@@ -0,0 +1,200 @@
/*********************************************************************
* 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
#define TRAJECTORY_ROLLOUT_MAP_GRID_H_
#include <vector>
#include <iostream>
#include <base_local_planner/trajectory_inc.h>
#include <ros/console.h>
#include <ros/ros.h>
#include <base_local_planner/map_cell.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
namespace base_local_planner{
/**
* @class MapGrid
* @brief A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controller.
*/
class MapGrid{
public:
/**
* @brief Creates a 0x0 map by default
*/
MapGrid();
/**
* @brief Creates a map of size_x by size_y
* @param size_x The width of the map
* @param size_y The height of the map
*/
MapGrid(unsigned int size_x, unsigned int size_y);
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A reference to the desired cell
*/
inline MapCell& operator() (unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A copy of the desired cell
*/
inline MapCell operator() (unsigned int x, unsigned int y) const {
return map_[size_x_ * y + x];
}
inline MapCell& getCell(unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Destructor for a MapGrid
*/
~MapGrid(){}
/**
* @brief Copy constructor for a MapGrid
* @param mg The MapGrid to copy
*/
MapGrid(const MapGrid& mg);
/**
* @brief Assignment operator for a MapGrid
* @param mg The MapGrid to assign from
*/
MapGrid& operator= (const MapGrid& mg);
/**
* @brief reset path distance fields for all cells
*/
void resetPathDist();
/**
* @brief check if we need to resize
* @param size_x The desired width
* @param size_y The desired height
*/
void sizeCheck(unsigned int size_x, unsigned int size_y);
/**
* @brief Utility to share initialization code across constructors
*/
void commonInit();
/**
* @brief Returns a 1D index into the MapCell array for a 2D index
* @param x The desired x coordinate
* @param y The desired y coordinate
* @return The associated 1D index
*/
size_t getIndex(int x, int y);
/**
* return a value that indicates cell is in obstacle
*/
inline double obstacleCosts() {
return map_.size();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
inline double unreachableCellCosts() {
return map_.size() + 1;
}
/**
* @brief Used to update the distance of a cell in path distance computation
* @param current_cell The cell we're currently in
* @param check_cell The cell to be updated
*/
inline bool updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap);
/**
* increase global plan resolution to match that of the costmap by adding points linearly between global plan points
* This is necessary where global planners produce plans with few points.
* @param global_plan_in input
* @param global_plan_output output
* @param resolution desired distance between waypoints
*/
static void adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution);
/**
* @brief Compute the distance from each cell in the local map grid to the planned path
* @param dist_queue A queue of the initial cells on the path
*/
void computeTargetDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Compute the distance from each cell in the local map grid to the local goal point
* @param goal_queue A queue containing the local goal cell
*/
void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Update what cells are considered path based on the global plan
*/
void setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Update what cell is considered the next local goal
*/
void setLocalGoal(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan);
double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
private:
std::vector<MapCell> map_; ///< @brief Storage for the MapCells
};
};
#endif

139
navigations/base_local_planner/include/base_local_planner/map_grid_cost_function.h Executable file
View File

@@ -0,0 +1,139 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef MAP_GRID_COST_FUNCTION_H_
#define MAP_GRID_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
#include <costmap_2d/costmap_2d.h>
#include <base_local_planner/map_grid.h>
namespace base_local_planner {
/**
* when scoring a trajectory according to the values in mapgrid, we can take
*return the value of the last point (if no of the earlier points were in
* return collision), the sum for all points, or the product of all (non-zero) points
*/
enum CostAggregationType { Last, Sum, Product};
/**
* This class provides cost based on a map_grid of a small area of the world.
* The map_grid covers a the costmap, the costmap containing the information
* about sensed obstacles. The map_grid is used by setting
* certain cells to distance 0, and then propagating distances around them,
* filling up the area reachable around them.
*
* The approach using grid_maps is used for computational efficiency, allowing to
* score hundreds of trajectories very quickly.
*
* This can be used to favor trajectories which stay on a given path, or which
* approach a given goal.
* @param costmap_ros Reference to object giving updates of obstacles around robot
* @param xshift where the scoring point is with respect to robot center pose
* @param yshift where the scoring point is with respect to robot center pose
* @param is_local_goal_function, scores for local goal rather than whole path
* @param aggregationType how to combine costs along trajectory
*/
class MapGridCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
MapGridCostFunction(costmap_2d::Costmap2D* costmap,
double xshift = 0.0,
double yshift = 0.0,
bool is_local_goal_function = false,
CostAggregationType aggregationType = Last);
~MapGridCostFunction() {}
/**
* set line segments on the grid with distance 0, resets the grid
*/
void setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses);
void setXShift(double xshift) {xshift_ = xshift;}
void setYShift(double yshift) {yshift_ = yshift;}
/** @brief If true, failures along the path cause the entire path to be rejected.
*
* Default is true. */
void setStopOnFailure(bool stop_on_failure) {stop_on_failure_ = stop_on_failure;}
/**
* propagate distances
*/
bool prepare();
double scoreTrajectory(Trajectory &traj);
/**
* return a value that indicates cell is in obstacle
*/
double obstacleCosts() {
return map_.obstacleCosts();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
double unreachableCellCosts() {
return map_.unreachableCellCosts();
}
// used for easier debugging
double getCellCosts(unsigned int cx, unsigned int cy);
private:
std::vector<geometry_msgs::PoseStamped> target_poses_;
costmap_2d::Costmap2D* costmap_;
base_local_planner::MapGrid map_;
CostAggregationType aggregationType_;
/// xshift and yshift allow scoring for different
// ooints of robots than center, like fron or back
// this can help with alignment or keeping specific
// wheels on tracks both default to 0
double xshift_;
double yshift_;
// if true, we look for a suitable local goal on path, else we use the full path for costs
bool is_local_goal_function_;
bool stop_on_failure_;
};
} /* namespace base_local_planner */
#endif /* MAP_GRID_COST_FUNCTION_H_ */

71
navigations/base_local_planner/include/base_local_planner/map_grid_visualizer.h Executable file
View File

@@ -0,0 +1,71 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Eric Perko
* 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 Eric Perko 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 MAP_GRID_VISUALIZER_H_
#define MAP_GRID_VISUALIZER_H_
#include <ros/ros.h>
#include <base_local_planner/map_grid.h>
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
class MapGridVisualizer {
public:
/**
* @brief Default constructor
*/
MapGridVisualizer();
/**
* @brief Initializes the MapGridVisualizer
* @param name The name to be appended to ~/ in order to get the proper namespace for parameters
* @param costmap The costmap instance to use to get the size of the map to generate a point cloud for
* @param cost_function The function to use to compute the cost values to be inserted into each point in the output PointCloud as well as whether to include a given point or not
*/
void initialize(const std::string& name, std::string frame, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function);
/**
* @brief Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points for which the cost_function at (cx,cy) returns true.
*/
void publishCostCloud(const costmap_2d::Costmap2D* costmap_p_);
private:
std::string name_; ///< @brief The name to get parameters relative to.
boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function_; ///< @brief The function to be used to generate the cost components for the output PointCloud
ros::NodeHandle ns_nh_;
std::string frame_id_;
ros::Publisher pub_;
};
};
#endif

89
navigations/base_local_planner/include/base_local_planner/obstacle_cost_function.h Executable file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef OBSTACLE_COST_FUNCTION_H_
#define OBSTACLE_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
/**
* class ObstacleCostFunction
* @brief Uses costmap 2d to assign negative costs if robot footprint
* is in obstacle on any point of the trajectory.
*/
class ObstacleCostFunction : public TrajectoryCostFunction {
public:
ObstacleCostFunction(costmap_2d::Costmap2D* costmap);
~ObstacleCostFunction();
bool prepare();
double scoreTrajectory(Trajectory &traj);
void setSumScores(bool score_sums){ sum_scores_=score_sums; }
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed);
void setFootprint(std::vector<geometry_msgs::Point> footprint_spec);
// helper functions, made static for easy unit testing
static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor);
static double footprintCost(
const double& x,
const double& y,
const double& th,
double scale,
std::vector<geometry_msgs::Point> footprint_spec,
costmap_2d::Costmap2D* costmap,
base_local_planner::WorldModel* world_model);
private:
costmap_2d::Costmap2D* costmap_;
std::vector<geometry_msgs::Point> footprint_spec_;
base_local_planner::WorldModel* world_model_;
double max_trans_vel_;
bool sum_scores_;
//footprint scaling with velocity;
double max_scaling_factor_, scaling_speed_;
};
} /* namespace base_local_planner */
#endif /* OBSTACLE_COST_FUNCTION_H_ */

92
navigations/base_local_planner/include/base_local_planner/odometry_helper_ros.h Executable file
View File

@@ -0,0 +1,92 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ODOMETRY_HELPER_ROS2_H_
#define ODOMETRY_HELPER_ROS2_H_
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <geometry_msgs/PoseStamped.h>
namespace base_local_planner {
class OdometryHelperRos {
public:
/** @brief Constructor.
* @param odom_topic The topic on which to subscribe to Odometry
* messages. If the empty string is given (the default), no
* subscription is done. */
OdometryHelperRos(std::string odom_topic = "");
~OdometryHelperRos() {}
/**
* @brief Callback for receiving odometry data
* @param msg An Odometry message
*/
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
void getOdom(nav_msgs::Odometry& base_odom);
void getRobotVel(geometry_msgs::PoseStamped& robot_vel);
/** @brief Set the odometry topic. This overrides what was set in the constructor, if anything.
*
* This unsubscribes from the old topic (if any) and subscribes to the new one (if any).
*
* If odom_topic is the empty string, this just unsubscribes from the previous topic. */
void setOdomTopic(std::string odom_topic);
/** @brief Return the current odometry topic. */
std::string getOdomTopic() const { return odom_topic_; }
private:
//odom topic
std::string odom_topic_;
// we listen on odometry on the odom topic
ros::Subscriber odom_sub_;
nav_msgs::Odometry base_odom_;
boost::mutex odom_mutex_;
// global tf frame id
std::string frame_id_; ///< The frame_id associated this data
};
} /* namespace base_local_planner */
#define CHUNKY 1
#endif /* ODOMETRY_HELPER_ROS2_H_ */

89
navigations/base_local_planner/include/base_local_planner/oscillation_cost_function.h Executable file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef OSCILLATION_COST_FUNCTION_H_
#define OSCILLATION_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
#include <Eigen/Core>
namespace base_local_planner {
class OscillationCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
OscillationCostFunction();
virtual ~OscillationCostFunction();
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
/**
* @brief Reset the oscillation flags for the local planner
*/
void resetOscillationFlags();
void updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory* traj, double min_vel_trans);
void setOscillationResetDist(double dist, double angle);
private:
void resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev);
/**
* @brief Given a trajectory that's selected, set flags if needed to
* prevent the robot from oscillating
* @param t The selected trajectory
* @return True if a flag was set, false otherwise
*/
bool setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans);
// flags
bool strafe_pos_only_, strafe_neg_only_, strafing_pos_, strafing_neg_;
bool rot_pos_only_, rot_neg_only_, rotating_pos_, rotating_neg_;
bool forward_pos_only_, forward_neg_only_, forward_pos_, forward_neg_;
// param
double oscillation_reset_dist_, oscillation_reset_angle_;
Eigen::Vector3f prev_stationary_pos_;
};
} /* namespace base_local_planner */
#endif /* OSCILLATION_COST_FUNCTION_H_ */

57
navigations/base_local_planner/include/base_local_planner/planar_laser_scan.h Executable file
View File

@@ -0,0 +1,57 @@
/*********************************************************************
*
* 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 TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#define TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#include <geometry_msgs/Point32.h>
#include <sensor_msgs/PointCloud.h>
namespace base_local_planner {
/**
* @class PlanarLaserScan
* @brief Stores a scan from a planar laser that can be used to clear freespace
*/
class PlanarLaserScan {
public:
PlanarLaserScan() {}
geometry_msgs::Point32 origin;
sensor_msgs::PointCloud cloud;
double angle_min, angle_max, angle_increment;
};
};
#endif

326
navigations/base_local_planner/include/base_local_planner/point_grid.h Executable file
View File

@@ -0,0 +1,326 @@
/*********************************************************************
*
* 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 POINT_GRID_H_
#define POINT_GRID_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point.h>
#include <costmap_2d/observation.h>
#include <base_local_planner/world_model.h>
#include <sensor_msgs/PointCloud2.h>
namespace base_local_planner {
/**
* @class PointGrid
* @brief A class that implements the WorldModel interface to provide
* free-space collision checks for the trajectory controller. This class
* stores points binned into a grid and performs point-in-polygon checks when
* necessary to determine the legality of a footprint at a given
* position/orientation.
*/
class PointGrid : public WorldModel {
public:
/**
* @brief Constuctor for a grid that stores points in the plane
* @param width The width in meters of the grid
* @param height The height in meters of the gird
* @param resolution The resolution of the grid in meters/cell
* @param origin The origin of the bottom left corner of the grid
* @param max_z The maximum height for an obstacle to be added to the grid
* @param obstacle_range The maximum distance for obstacles to be added to the grid
* @param min_separation The minimum distance between points in the grid
*/
PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
double max_z, double obstacle_range, double min_separation);
/**
* @brief Destructor for a point grid
*/
virtual ~PointGrid(){}
/**
* @brief Returns the points that lie within the cells contained in the specified range. Some of these points may be outside the range itself.
* @param lower_left The lower left corner of the range search
* @param upper_right The upper right corner of the range search
* @param points A vector of pointers to lists of the relevant points
*/
void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<geometry_msgs::Point32>* >& points);
/**
* @brief Checks if any points in the grid lie inside a convex footprint
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
using WorldModel::footprintCost;
/**
* @brief Inserts observations from sensors into the point grid
* @param footprint The footprint of the robot in its current location
* @param observations The observations from various sensors
* @param laser_scans The laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)
*/
void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
* @brief Convert from world coordinates to grid coordinates
* @param pt A point in world space
* @param gx The x coordinate of the corresponding grid cell to be set by the function
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
return false;
}
gx = (int) ((pt.x - origin_.x)/resolution_);
gy = (int) ((pt.y - origin_.y)/resolution_);
if(gx >= width_ || gy >= height_){
gx = 0;
gy = 0;
return false;
}
return true;
}
/**
* @brief Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called
* @param gx The x coordinate of the grid cell
* @param gy The y coordinate of the grid cell
* @param lower_left The lower left bounds of the cell in world coordinates to be filled in
* @param upper_right The upper right bounds of the cell in world coordinates to be filled in
*/
inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
lower_left.x = gx * resolution_ + origin_.x;
lower_left.y = gy * resolution_ + origin_.y;
upper_right.x = lower_left.x + resolution_;
upper_right.y = lower_left.y + resolution_;
}
/**
* @brief Compute the squared distance between two points
* @param pt1 The first point
* @param pt2 The second point
* @return The squared distance between the two points
*/
inline double sq_distance(const geometry_msgs::Point32& pt1, const geometry_msgs::Point32& pt2){
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
}
/**
* @brief Convert from world coordinates to grid coordinates
* @param pt A point in world space
* @param gx The x coordinate of the corresponding grid cell to be set by the function
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
inline bool gridCoords(const geometry_msgs::Point32& pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
return false;
}
gx = (int) ((pt.x - origin_.x)/resolution_);
gy = (int) ((pt.y - origin_.y)/resolution_);
if(gx >= width_ || gy >= height_){
gx = 0;
gy = 0;
return false;
}
return true;
}
/**
* @brief Converts cell coordinates to an index value that can be used to look up the correct grid cell
* @param gx The x coordinate of the cell
* @param gy The y coordinate of the cell
* @return The index of the cell in the stored cell list
*/
inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
/*
* (0, 0) ---------------------- (width, 0)
* | |
* | |
* | |
* | |
* | |
* (0, height) ----------------- (width, height)
*/
return(gx + gy * width_);
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point32& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
template<typename T>
inline double orient(const T& a, const T& b, const T& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check if two line segmenst intersect
* @param v1 The first point of the first segment
* @param v2 The second point of the first segment
* @param u1 The first point of the second segment
* @param u2 The second point of the second segment
* @return True if the segments intersect, false otherwise
*/
inline bool segIntersect(const geometry_msgs::Point32& v1, const geometry_msgs::Point32& v2,
const geometry_msgs::Point32& u1, const geometry_msgs::Point32& u2){
return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
}
/**
* @brief Find the intersection point of two lines
* @param v1 The first point of the first segment
* @param v2 The second point of the first segment
* @param u1 The first point of the second segment
* @param u2 The second point of the second segment
* @param result The point to be filled in
*/
void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
geometry_msgs::Point& result);
/**
* @brief Check if a point is in a polygon
* @param pt The point to be checked
* @param poly The polygon to check against
* @return True if the point is in the polygon, false otherwise
*/
bool ptInPolygon(const geometry_msgs::Point32& pt, const std::vector<geometry_msgs::Point>& poly);
/**
* @brief Insert a point into the point grid
* @param pt The point to be inserted
*/
void insert(const geometry_msgs::Point32& pt);
/**
* @brief Find the distance between a point and its nearest neighbor in the grid
* @param pt The point used for comparison
* @return The distance between the point passed in and its nearest neighbor in the point grid
*/
double nearestNeighborDistance(const geometry_msgs::Point32& pt);
/**
* @brief Find the distance between a point and its nearest neighbor in a cell
* @param pt The point used for comparison
* @param gx The x coordinate of the cell
* @param gy The y coordinate of the cell
* @return The distance between the point passed in and its nearest neighbor in the cell
*/
double getNearestInCell(const geometry_msgs::Point32& pt, unsigned int gx, unsigned int gy);
/**
* @brief Removes points from the grid that lie within the polygon
* @param poly A specification of the polygon to clear from the grid
*/
void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
/**
* @brief Removes points from the grid that lie within a laser scan
* @param laser_scan A specification of the laser scan to use for clearing
*/
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
/**
* @brief Checks to see if a point is within a laser scan specification
* @param pt The point to check
* @param laser_scan The specification of the scan to check against
* @return True if the point is contained within the scan, false otherwise
*/
bool ptInScan(const geometry_msgs::Point32& pt, const PlanarLaserScan& laser_scan);
/**
* @brief Get the points in the point grid
* @param cloud The point cloud to insert the points into
*/
void getPoints(sensor_msgs::PointCloud2& cloud);
private:
double resolution_; ///< @brief The resolution of the grid in meters/cell
geometry_msgs::Point origin_; ///< @brief The origin point of the grid
unsigned int width_; ///< @brief The width of the grid in cells
unsigned int height_; ///< @brief The height of the grid in cells
std::vector< std::list<geometry_msgs::Point32> > cells_; ///< @brief Storage for the cells in the grid
double max_z_; ///< @brief The height cutoff for adding points as obstacles
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
double sq_min_separation_; ///< @brief The minimum square distance required between points in the grid
std::vector< std::list<geometry_msgs::Point32>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
};
};
#endif

64
navigations/base_local_planner/include/base_local_planner/prefer_forward_cost_function.h Executable file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef PREFER_FORWARD_COST_FUNCTION_H_
#define PREFER_FORWARD_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
namespace base_local_planner {
class PreferForwardCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
PreferForwardCostFunction(double penalty) : penalty_(penalty) {}
~PreferForwardCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
void setPenalty(double penalty) {
penalty_ = penalty;
}
private:
double penalty_;
};
} /* namespace base_local_planner */
#endif /* PREFER_FORWARD_COST_FUNCTION_H_ */

109
navigations/base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h Executable file
View File

@@ -0,0 +1,109 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef SIMPLE_SCORED_SAMPLING_PLANNER_H_
#define SIMPLE_SCORED_SAMPLING_PLANNER_H_
#include <vector>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/trajectory_cost_function.h>
#include <base_local_planner/trajectory_sample_generator.h>
#include <base_local_planner/trajectory_search.h>
namespace base_local_planner {
/**
* @class SimpleScoredSamplingPlanner
* @brief Generates a local plan using the given generator and cost functions.
* Assumes less cost are best, and negative costs indicate infinite costs
*
* This is supposed to be a simple and robust implementation of
* the TrajectorySearch interface. More efficient search may well be
* possible using search heuristics, parallel search, etc.
*/
class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch {
public:
~SimpleScoredSamplingPlanner() {}
SimpleScoredSamplingPlanner() {}
/**
* Takes a list of generators and critics. Critics return costs > 0, or negative costs for invalid trajectories.
* Generators other than the first are fallback generators, meaning they only get to generate if the previous
* generator did not find a valid trajectory.
* Will use every generator until it stops returning trajectories or count reaches max_samples.
* Then resets count and tries for the next in the list.
* passing max_samples = -1 (default): Each Sampling planner will continue to call
* generator until generator runs out of samples (or forever if that never happens)
*/
SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples = -1);
/**
* runs all scoring functions over the trajectory creating a weigthed sum
* of positive costs, aborting as soon as a negative cost are found or costs greater
* than positive best_traj_cost accumulated
*/
double scoreTrajectory(Trajectory& traj, double best_traj_cost);
/**
* Calls generator until generator has no more samples or max_samples is reached.
* For each generated traj, calls critics in turn. If any critic returns negative
* value, that value is assumed as costs, else the costs are the sum of all critics
* result. Returns true and sets the traj parameter to the first trajectory with
* minimal non-negative costs if sampling yields trajectories with non-negative costs,
* else returns false.
*
* @param traj The container to write the result to
* @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty)
*/
bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored = 0);
private:
std::vector<TrajectorySampleGenerator*> gen_list_;
std::vector<TrajectoryCostFunction*> critics_;
int max_samples_;
};
} // namespace
#endif /* SIMPLE_SCORED_SAMPLING_PLANNER_H_ */

160
navigations/base_local_planner/include/base_local_planner/simple_trajectory_generator.h Executable file
View File

@@ -0,0 +1,160 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef SIMPLE_TRAJECTORY_GENERATOR_H_
#define SIMPLE_TRAJECTORY_GENERATOR_H_
#include <base_local_planner/trajectory_sample_generator.h>
#include <base_local_planner/local_planner_limits.h>
#include <Eigen/Core>
namespace base_local_planner {
/**
* generates trajectories based on equi-distant discretisation of the degrees of freedom.
* This is supposed to be a simple and robust implementation of the TrajectorySampleGenerator
* interface, more efficient implementations are thinkable.
*
* This can be used for both dwa and trajectory rollout approaches.
* As an example, assuming these values:
* sim_time = 1s, sim_period=200ms, dt = 200ms,
* vsamples_x=5,
* acc_limit_x = 1m/s^2, vel_x=0 (robot at rest, values just for easy calculations)
* dwa_planner will sample max-x-velocities from 0m/s to 0.2m/s.
* trajectory rollout approach will sample max-x-velocities 0m/s up to 1m/s
* trajectory rollout approach does so respecting the acceleration limit, so it gradually increases velocity
*/
class SimpleTrajectoryGenerator: public base_local_planner::TrajectorySampleGenerator {
public:
SimpleTrajectoryGenerator() {
limits_ = NULL;
}
~SimpleTrajectoryGenerator() {}
/**
* @param pos current robot position
* @param vel current robot velocity
* @param limits Current velocity limits
* @param vsamples: in how many samples to divide the given dimension
* @param use_acceleration_limits: if true use physical model, else idealized robot model
* @param additional_samples (deprecated): Additional velocity samples to generate individual trajectories from.
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
std::vector<Eigen::Vector3f> additional_samples,
bool discretize_by_time = false);
/**
* @param pos current robot position
* @param vel current robot velocity
* @param limits Current velocity limits
* @param vsamples: in how many samples to divide the given dimension
* @param use_acceleration_limits: if true use physical model, else idealized robot model
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time = false);
/**
* This function is to be called only when parameters change
*
* @param sim_granularity granularity of collision detection
* @param angular_sim_granularity angular granularity of collision detection
* @param use_dwa whether to use DWA or trajectory rollout
* @param sim_period distance between points in one trajectory
*/
void setParameters(double sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_dwa = false,
double sim_period = 0.0);
/**
* Whether this generator can create more trajectories
*/
bool hasMoreTrajectories();
/**
* Whether this generator can create more trajectories
*/
bool nextTrajectory(Trajectory &traj);
static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel, double dt);
static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt);
bool generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj);
protected:
unsigned int next_sample_index_;
// to store sample params of each sample between init and generation
std::vector<Eigen::Vector3f> sample_params_;
base_local_planner::LocalPlannerLimits* limits_;
Eigen::Vector3f pos_;
Eigen::Vector3f vel_;
// whether velocity of trajectory changes over time or not
bool continued_acceleration_;
bool discretize_by_time_;
double sim_time_, sim_granularity_, angular_sim_granularity_;
bool use_dwa_;
double sim_period_; // only for dwa
};
} /* namespace base_local_planner */
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */

118
navigations/base_local_planner/include/base_local_planner/trajectory.h Executable file
View File

@@ -0,0 +1,118 @@
/*********************************************************************
* 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#include <vector>
namespace base_local_planner {
/**
* @class Trajectory
* @brief Holds a trajectory generated by considering an x, y, and theta velocity
*/
class Trajectory {
public:
/**
* @brief Default constructor
*/
Trajectory();
/**
* @brief Constructs a trajectory
* @param xv The x velocity used to seed the trajectory
* @param yv The y velocity used to seed the trajectory
* @param thetav The theta velocity used to seed the trajectory
* @param num_pts The expected number of points for a trajectory
*/
Trajectory(double xv, double yv, double thetav, double time_delta, unsigned int num_pts);
double xv_, yv_, thetav_; ///< @brief The x, y, and theta velocities of the trajectory
double cost_; ///< @brief The cost/score of the trajectory
double time_delta_; ///< @brief The time gap between points
/**
* @brief Get a point within the trajectory
* @param index The index of the point to get
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getPoint(unsigned int index, double& x, double& y, double& th) const;
/**
* @brief Set a point within the trajectory
* @param index The index of the point to set
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void setPoint(unsigned int index, double x, double y, double th);
/**
* @brief Add a point to the end of a trajectory
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void addPoint(double x, double y, double th);
/**
* @brief Get the last point of the trajectory
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getEndpoint(double& x, double& y, double& th) const;
/**
* @brief Clear the trajectory's points
*/
void resetPoints();
/**
* @brief Return the number of points in the trajectory
* @return The number of points in the trajectory
*/
unsigned int getPointsSize() const;
private:
std::vector<double> x_pts_; ///< @brief The x points in the trajectory
std::vector<double> y_pts_; ///< @brief The y points in the trajectory
std::vector<double> th_pts_; ///< @brief The theta points in the trajectory
};
};
#endif

86
navigations/base_local_planner/include/base_local_planner/trajectory_cost_function.h Executable file
View File

@@ -0,0 +1,86 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef TRAJECTORYCOSTFUNCTION_H_
#define TRAJECTORYCOSTFUNCTION_H_
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
/**
* @class TrajectoryCostFunction
* @brief Provides an interface for critics of trajectories
* During each sampling run, a batch of many trajectories will be scored using such a cost function.
* The prepare method is called before each batch run, and then for each
* trajectory of the sampling set, score_trajectory may be called.
*/
class TrajectoryCostFunction {
public:
/**
*
* General updating of context values if required.
* Subclasses may overwrite. Return false in case there is any error.
*/
virtual bool prepare() = 0;
/**
* return a score for trajectory traj
*/
virtual double scoreTrajectory(Trajectory &traj) = 0;
double getScale() {
return scale_;
}
void setScale(double scale) {
scale_ = scale;
}
virtual ~TrajectoryCostFunction() {}
protected:
TrajectoryCostFunction(double scale = 1.0): scale_(scale) {}
private:
double scale_;
};
}
#endif /* TRAJECTORYCOSTFUNCTION_H_ */

47
navigations/base_local_planner/include/base_local_planner/trajectory_inc.h Executable file
View File

@@ -0,0 +1,47 @@
/*********************************************************************
* 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.
*********************************************************************/
#ifndef TRAJECTORY_INC_H_
#define TRAJECTORY_INC_H_
#include <limits>
#ifndef DBL_MAX /* Max decimal value of a double */
#define DBL_MAX std::numeric_limits<double>::max() // 1.7976931348623157e+308
#endif
#ifndef DBL_MIN //Min decimal value of a double
#define DBL_MIN std::numeric_limits<double>::min() // 2.2250738585072014e-308
#endif
#endif

385
navigations/base_local_planner/include/base_local_planner/trajectory_planner.h Executable file
View File

@@ -0,0 +1,385 @@
/*********************************************************************
*
* 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 TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#include <vector>
#include <cmath>
//for obstacle data access
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/cost_values.h>
#include <base_local_planner/footprint_helper.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/Position2DInt.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>
//we'll take in a path as a vector of poses
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
//for creating a local cost grid
#include <base_local_planner/map_cell.h>
#include <base_local_planner/map_grid.h>
namespace base_local_planner {
/**
* @class TrajectoryPlanner
* @brief Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world.
*/
class TrajectoryPlanner{
friend class TrajectoryPlannerTest; //Need this for gtest to work
public:
/**
* @brief Constructs a trajectory controller
* @param world_model The WorldModel the trajectory controller uses to check for collisions
* @param costmap A reference to the Costmap the controller should use
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @param acc_lim_x The acceleration limit of the robot in the x direction
* @param acc_lim_y The acceleration limit of the robot in the y direction
* @param acc_lim_theta The acceleration limit of the robot in the theta direction
* @param sim_time The number of seconds to "roll-out" each trajectory
* @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things
* @param vx_samples The number of trajectories to sample in the x dimension
* @param vtheta_samples The number of trajectories to sample in the theta dimension
* @param path_distance_bias A scaling factor for how close the robot should stay to the path
* @param goal_distance_bias A scaling factor for how aggresively the robot should pursue a local goal
* @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles
* @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities
* @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
* @param escape_reset_dist The distance the robot must travel before it can exit escape mode
* @param escape_reset_theta The distance the robot must rotate before it can exit escape mode
* @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise
* @param max_vel_x The maximum x velocity the controller will explore
* @param min_vel_x The minimum x velocity the controller will explore
* @param max_vel_th The maximum rotational velocity the controller will explore
* @param min_vel_th The minimum rotational velocity the controller will explore
* @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore
* @param backup_vel The velocity to use while backing up
* @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits
* @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep
* @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories
* @param meter_scoring adapt parameters to costmap resolution
* @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
* @param y_vels A vector of the y velocities the controller will explore
* @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things
*/
TrajectoryPlanner(WorldModel& world_model,
const costmap_2d::Costmap2D& costmap,
std::vector<geometry_msgs::Point> footprint_spec,
double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
double sim_time = 1.0, double sim_granularity = 0.025,
int vx_samples = 20, int vtheta_samples = 20,
double path_distance_bias = 0.6, double goal_distance_bias = 0.8, double occdist_scale = 0.2,
double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05,
double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2,
bool holonomic_robot = true,
double max_vel_x = 0.5, double min_vel_x = 0.1,
double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4,
double backup_vel = -0.1,
bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1,
bool meter_scoring = true,
bool simple_attractor = false,
std::vector<double> y_vels = std::vector<double>(0),
double stop_time_buffer = 0.2,
double sim_period = 0.1, double angular_sim_granularity = 0.025);
/**
* @brief Destructs a trajectory controller
*/
~TrajectoryPlanner();
/**
* @brief Reconfigures the trajectory planner
*/
void reconfigure(BaseLocalPlannerConfig &cfg);
/**
* @brief Given the current position, orientation, and velocity of the robot, return a trajectory to follow
* @param global_pose The current pose of the robot in world space
* @param global_vel The current velocity of the robot in world space
* @param drive_velocities Will be set to velocities to send to the robot base
* @return The selected path or trajectory
*/
Trajectory findBestPath(const geometry_msgs::PoseStamped& global_pose,
geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities);
/**
* @brief Update the plan that the controller is following
* @param new_plan A new plan for the controller to follow
* @param compute_dists Wheter or not to compute path/goal distances when a plan is updated
*/
void updatePlan(const std::vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists = false);
/**
* @brief Accessor for the goal the robot is currently pursuing in world corrdinates
* @param x Will be set to the x position of the local goal
* @param y Will be set to the y position of the local goal
*/
void getLocalGoal(double& x, double& y);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @return True if the trajectory is legal, false otherwise
*/
bool checkTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @return The score (as double)
*/
double scoreTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
/**
* @brief Compute the components and total cost for a map grid cell
* @param cx The x coordinate of the cell in the map grid
* @param cy The y coordinate of the cell in the map grid
* @param path_cost Will be set to the path distance component of the cost function
* @param goal_cost Will be set to the goal distance component of the cost function
* @param occ_cost Will be set to the costmap value of the cell
* @param total_cost Will be set to the value of the overall cost function, taking into account the scaling parameters
* @return True if the cell is traversible and therefore a legal location for the robot to move to
*/
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
/** @brief Set the footprint specification of the robot. */
void setFootprint( std::vector<geometry_msgs::Point> footprint ) { footprint_spec_ = footprint; }
/** @brief Return the footprint specification of the robot. */
geometry_msgs::Polygon getFootprintPolygon() const { return costmap_2d::toPolygon(footprint_spec_); }
std::vector<geometry_msgs::Point> getFootprint() const { return footprint_spec_; }
private:
/**
* @brief Create the trajectories we wish to explore, score them, and return the best option
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param acc_x The x acceleration limit of the robot
* @param acc_y The y acceleration limit of the robot
* @param acc_theta The theta acceleration limit of the robot
* @return
*/
Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
double acc_x, double acc_y, double acc_theta);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param acc_x The x acceleration limit of the robot
* @param acc_y The y acceleration limit of the robot
* @param acc_theta The theta acceleration limit of the robot
* @param impossible_cost The cost value of a cell in the local map grid that is considered impassable
* @param traj Will be set to the generated trajectory with its associated score
*/
void generateTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
double acc_theta, double impossible_cost, Trajectory& traj);
/**
* @brief Checks the legality of the robot footprint at a position and orientation using the world model
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @return
*/
double footprintCost(double x_i, double y_i, double theta_i);
base_local_planner::FootprintHelper footprint_helper_;
MapGrid path_map_; ///< @brief The local map grid where we propagate path distance
MapGrid goal_map_; ///< @brief The local map grid where we propagate goal distance
const costmap_2d::Costmap2D& costmap_; ///< @brief Provides access to cost map information
WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
std::vector<geometry_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
std::vector<geometry_msgs::PoseStamped> global_plan_; ///< @brief The global path for the robot to follow
bool stuck_left, stuck_right; ///< @brief Booleans to keep the robot from oscillating during rotation
bool rotating_left, rotating_right; ///< @brief Booleans to keep track of the direction of rotation for the robot
bool stuck_left_strafe, stuck_right_strafe; ///< @brief Booleans to keep the robot from oscillating during strafing
bool strafe_right, strafe_left; ///< @brief Booleans to keep track of strafe direction for the robot
bool escaping_; ///< @brief Boolean to keep track of whether we're in escape mode
bool meter_scoring_;
double goal_x_,goal_y_; ///< @brief Storage for the local goal the robot is pursuing
double final_goal_x_, final_goal_y_; ///< @brief The end position of the plan.
bool final_goal_position_valid_; ///< @brief True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent.
double sim_time_; ///< @brief The number of seconds each trajectory is "rolled-out"
double sim_granularity_; ///< @brief The distance between simulation points
double angular_sim_granularity_; ///< @brief The distance between angular simulation points
int vx_samples_; ///< @brief The number of samples we'll take in the x dimenstion of the control space
int vtheta_samples_; ///< @brief The number of samples we'll take in the theta dimension of the control space
double path_distance_bias_, goal_distance_bias_, occdist_scale_; ///< @brief Scaling factors for the controller's cost function
double acc_lim_x_, acc_lim_y_, acc_lim_theta_; ///< @brief The acceleration limits of the robot
double prev_x_, prev_y_; ///< @brief Used to calculate the distance the robot has traveled before reseting oscillation booleans
double escape_x_, escape_y_, escape_theta_; ///< @brief Used to calculate the distance the robot has traveled before reseting escape booleans
Trajectory traj_one, traj_two; ///< @brief Used for scoring trajectories
double heading_lookahead_; ///< @brief How far the robot should look ahead of itself when differentiating between different rotational velocities
double oscillation_reset_dist_; ///< @brief The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
double escape_reset_dist_, escape_reset_theta_; ///< @brief The distance the robot must travel before it can leave escape mode
bool holonomic_robot_; ///< @brief Is the robot holonomic or not?
double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_; ///< @brief Velocity limits for the controller
double backup_vel_; ///< @brief The velocity to use while backing up
bool dwa_; ///< @brief Should we use the dynamic window approach?
bool heading_scoring_; ///< @brief Should we score based on the rollout approach or the heading approach
double heading_scoring_timestep_; ///< @brief How far to look ahead in time when we score a heading
bool simple_attractor_; ///< @brief Enables simple attraction to a goal point
std::vector<double> y_vels_; ///< @brief Y velocities to explore
double stop_time_buffer_; ///< @brief How long before hitting something we're going to enforce that the robot stop
double sim_period_; ///< @brief The number of seconds to use to compute max/min vels for dwa
double inscribed_radius_, circumscribed_radius_;
boost::mutex configuration_mutex_;
/**
* @brief Compute x position based on velocity
* @param xi The current x position
* @param vx The current x velocity
* @param vy The current y velocity
* @param theta The current orientation
* @param dt The timestep to take
* @return The new x position
*/
inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
}
/**
* @brief Compute y position based on velocity
* @param yi The current y position
* @param vx The current x velocity
* @param vy The current y velocity
* @param theta The current orientation
* @param dt The timestep to take
* @return The new y position
*/
inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
}
/**
* @brief Compute orientation based on velocity
* @param thetai The current orientation
* @param vth The current theta velocity
* @param dt The timestep to take
* @return The new orientation
*/
inline double computeNewThetaPosition(double thetai, double vth, double dt){
return thetai + vth * dt;
}
//compute velocity based on acceleration
/**
* @brief Compute velocity based on acceleration
* @param vg The desired velocity, what we're accelerating up to
* @param vi The current velocity
* @param a_max An acceleration limit
* @param dt The timestep to take
* @return The new velocity
*/
inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
if((vg - vi) >= 0) {
return std::min(vg, vi + a_max * dt);
}
return std::max(vg, vi - a_max * dt);
}
void getMaxSpeedToStopInTime(double time, double& vx, double& vy, double& vth){
vx = acc_lim_x_ * std::max(time, 0.0);
vy = acc_lim_y_ * std::max(time, 0.0);
vth = acc_lim_theta_ * std::max(time, 0.0);
}
double lineCost(int x0, int x1, int y0, int y1);
double pointCost(int x, int y);
double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
};
};
#endif

231
navigations/base_local_planner/include/base_local_planner/trajectory_planner_ros.h Executable file
View File

@@ -0,0 +1,231 @@
/*********************************************************************
*
* 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 TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/point_grid.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/voxel_grid_model.h>
#include <base_local_planner/trajectory_planner.h>
#include <base_local_planner/map_grid_visualizer.h>
#include <base_local_planner/planar_laser_scan.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf2_ros/buffer.h>
#include <boost/thread.hpp>
#include <string>
#include <angles/angles.h>
#include <nav_core/base_local_planner.h>
#include <dynamic_reconfigure/server.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>
#include <base_local_planner/odometry_helper_ros.h>
namespace base_local_planner {
/**
* @class TrajectoryPlannerROS
* @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
*/
class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner {
public:
/**
* @brief Default constructor for the ros wrapper
*/
TrajectoryPlannerROS();
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
TrajectoryPlannerROS(std::string name,
tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
void initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Destructor for the wrapper
*/
~TrajectoryPlannerROS();
/**
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
* @brief Set the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
bool isGoalReached();
/**
* @brief Generate and score a single trajectory
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param update_map Whether or not to update the map for the planner
* when computing the legality of the trajectory, this is useful to set
* to false if you're going to be doing a lot of trajectory checking over
* a short period of time
* @return True if the trajectory is legal, false otherwise
*/
bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
/**
* @brief Generate and score a single trajectory
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param update_map Whether or not to update the map for the planner
* when computing the legality of the trajectory, this is useful to set
* to false if you're going to be doing a lot of trajectory checking over
* a short period of time
* @return score of trajectory (double)
*/
double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
bool isInitialized() {
return initialized_;
}
/** @brief Return the inner TrajectoryPlanner object. Only valid after initialize(). */
TrajectoryPlanner* getPlanner() const { return tc_; }
private:
/**
* @brief Callback to update the local planner's parameters based on dynamic reconfigure
*/
void reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level);
/**
* @brief Once a goal position is reached... rotate to the goal orientation
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param goal_th The desired th value for the goal
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool rotateToGoal(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel);
/**
* @brief Stop the robot taking into account acceleration limits
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, geometry_msgs::Twist& cmd_vel);
std::vector<double> loadYVels(ros::NodeHandle node);
double sign(double x){
return x < 0.0 ? -1.0 : 1.0;
}
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
costmap_2d::Costmap2D* costmap_; ///< @brief The costmap the controller will use
MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
tf2_ros::Buffer* tf_; ///< @brief Used for transforming point clouds
std::string global_frame_; ///< @brief The frame in which the controller will run
double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
nav_msgs::Odometry base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
bool prune_plan_;
boost::recursive_mutex odom_lock_;
double max_vel_th_, min_vel_th_;
double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
double sim_period_;
bool rotating_to_goal_;
bool reached_goal_;
bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
ros::Publisher g_plan_pub_, l_plan_pub_;
dynamic_reconfigure::Server<BaseLocalPlannerConfig> *dsrv_;
base_local_planner::BaseLocalPlannerConfig default_config_;
bool setup_;
bool initialized_;
base_local_planner::OdometryHelperRos odom_helper_;
std::vector<geometry_msgs::Point> footprint_spec_;
};
};
#endif

74
navigations/base_local_planner/include/base_local_planner/trajectory_sample_generator.h Executable file
View File

@@ -0,0 +1,74 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef TRAJECTORY_SAMPLE_GENERATOR_H_
#define TRAJECTORY_SAMPLE_GENERATOR_H_
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
/**
* @class TrajectorySampleGenerator
* @brief Provides an interface for navigation trajectory generators
*/
class TrajectorySampleGenerator {
public:
/**
* Whether this generator can create more trajectories
*/
virtual bool hasMoreTrajectories() = 0;
/**
* Whether this generator can create more trajectories
*/
virtual bool nextTrajectory(Trajectory &traj) = 0;
/**
* @brief Virtual destructor for the interface
*/
virtual ~TrajectorySampleGenerator() {}
protected:
TrajectorySampleGenerator() {}
};
} // end namespace
#endif /* TRAJECTORY_SAMPLE_GENERATOR_H_ */

71
navigations/base_local_planner/include/base_local_planner/trajectory_search.h Executable file
View File

@@ -0,0 +1,71 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef TRAJECTORY_SEARCH_H_
#define TRAJECTORY_SEARCH_H_
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
/**
* @class TrajectorySearch
* @brief Interface for modules finding a trajectory to use for navigation commands next
*/
class TrajectorySearch {
public:
/**
* searches the space of allowed trajectory and
* returns one considered the optimal given the
* constraints of the particular search.
*
* @param traj The container to write the result to
* @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty)
*/
virtual bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) = 0;
virtual ~TrajectorySearch() {}
protected:
TrajectorySearch() {}
};
}
#endif /* TRAJECTORY_SEARCH_H_ */

64
navigations/base_local_planner/include/base_local_planner/twirling_cost_function.h Executable file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Open Source Robotics Foundation, 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: Morgan Quigley
*********************************************************************/
#ifndef TWIRLING_COST_FUNCTION_H
#define TWIRLING_COST_FUNCTION_H
#include <base_local_planner/trajectory_cost_function.h>
namespace base_local_planner {
/**
* This class provides a cost based on how much a robot "twirls" on its
* way to the goal. With differential-drive robots, there isn't a choice,
* but with holonomic or near-holonomic robots, sometimes a robot spins
* more than you'd like on its way to a goal. This class provides a way
* to assign a penalty purely to rotational velocities.
*/
class TwirlingCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
TwirlingCostFunction() {}
~TwirlingCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
};
} /* namespace base_local_planner */
#endif /* TWIRLING_COST_FUNCTION_H_ */

99
navigations/base_local_planner/include/base_local_planner/velocity_iterator.h Executable file
View File

@@ -0,0 +1,99 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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
*********************************************************************/
#ifndef DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#define DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#include <algorithm>
#include <cmath>
#include <vector>
namespace base_local_planner {
/**
* We use the class to get even sized samples between min and max, inluding zero if it is not included (and range goes from negative to positive
*/
class VelocityIterator {
public:
VelocityIterator(double min, double max, int num_samples):
current_index(0)
{
if (min == max) {
samples_.push_back(min);
} else {
num_samples = std::max(2, num_samples);
// e.g. for 4 samples, split distance in 3 even parts
double step_size = (max - min) / double(std::max(1, (num_samples - 1)));
// we make sure to avoid rounding errors around min and max.
double current;
double next = min;
for (int j = 0; j < num_samples - 1; ++j) {
current = next;
next += step_size;
samples_.push_back(current);
// if 0 is among samples, this is never true. Else it inserts a 0 between the positive and negative samples
if ((current < 0) && (next > 0)) {
samples_.push_back(0.0);
}
}
samples_.push_back(max);
}
}
double getVelocity(){
return samples_.at(current_index);
}
VelocityIterator& operator++(int){
current_index++;
return *this;
}
void reset(){
current_index = 0;
}
bool isFinished(){
return current_index >= samples_.size();
}
private:
std::vector<double> samples_;
unsigned int current_index;
};
};
#endif

179
navigations/base_local_planner/include/base_local_planner/voxel_grid_model.h Executable file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
*
* 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 TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point.h>
#include <costmap_2d/observation.h>
#include <base_local_planner/world_model.h>
//voxel grid stuff
#include <voxel_grid/voxel_grid.h>
namespace base_local_planner {
/**
* @class VoxelGridModel
* @brief A class that implements the WorldModel interface to provide grid
* based collision checks for the trajectory controller using a 3D voxel grid.
*/
class VoxelGridModel : public WorldModel {
public:
/**
* @brief Constructor for the VoxelGridModel
* @param size_x The x size of the map
* @param size_y The y size of the map
* @param size_z The z size of the map... up to 32 cells
* @param xy_resolution The horizontal resolution of the map in meters/cell
* @param z_resolution The vertical resolution of the map in meters/cell
* @param origin_x The x value of the origin of the map
* @param origin_y The y value of the origin of the map
* @param origin_z The z value of the origin of the map
* @param max_z The maximum height for an obstacle to be added to the grid
* @param obstacle_range The maximum distance for obstacles to be added to the grid
*/
VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range);
/**
* @brief Destructor for the world model
*/
virtual ~VoxelGridModel(){}
/**
* @brief Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
using WorldModel::footprintCost;
/**
* @brief The costmap already keeps track of world observations, so for this world model this method does nothing
* @param footprint The footprint of the robot in its current location
* @param observations The observations from various sensors
* @param laser_scan The scans used to clear freespace
*/
void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
* @brief Function copying the Voxel points into a point cloud
* @param cloud the point cloud to copy data to. It has the usual x,y,z channels
*/
void getPoints(sensor_msgs::PointCloud2& cloud);
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost(int x0, int x1, int y0, int y1);
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
double pointCost(int x, int y);
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range);
inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz){
if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
return false;
mx = (int) ((wx - origin_x_) / xy_resolution_);
my = (int) ((wy - origin_y_) / xy_resolution_);
mz = (int) ((wz - origin_z_) / z_resolution_);
return true;
}
inline bool worldToMap2D(double wx, double wy, unsigned int& mx, unsigned int& my){
if(wx < origin_x_ || wy < origin_y_)
return false;
mx = (int) ((wx - origin_x_) / xy_resolution_);
my = (int) ((wy - origin_y_) / xy_resolution_);
return true;
}
inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz){
//returns the center point of the cell
wx = origin_x_ + (mx + 0.5) * xy_resolution_;
wy = origin_y_ + (my + 0.5) * xy_resolution_;
wz = origin_z_ + (mz + 0.5) * z_resolution_;
}
inline void mapToWorld2D(unsigned int mx, unsigned int my, double& wx, double& wy){
//returns the center point of the cell
wx = origin_x_ + (mx + 0.5) * xy_resolution_;
wy = origin_y_ + (my + 0.5) * xy_resolution_;
}
inline double dist(double x0, double y0, double z0, double x1, double y1, double z1){
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
}
inline void insert(const geometry_msgs::Point32& pt){
unsigned int cell_x, cell_y, cell_z;
if(!worldToMap3D(pt.x, pt.y, pt.z, cell_x, cell_y, cell_z))
return;
obstacle_grid_.markVoxel(cell_x, cell_y, cell_z);
}
voxel_grid::VoxelGrid obstacle_grid_;
double xy_resolution_;
double z_resolution_;
double origin_x_;
double origin_y_;
double origin_z_;
double max_z_; ///< @brief The height cutoff for adding points as obstacles
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
};
};
#endif

114
navigations/base_local_planner/include/base_local_planner/world_model.h Executable file
View File

@@ -0,0 +1,114 @@
/*********************************************************************
*
* 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 TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#include <vector>
#include <costmap_2d/observation.h>
#include <costmap_2d/footprint.h>
#include <geometry_msgs/Point.h>
#include <base_local_planner/planar_laser_scan.h>
namespace base_local_planner {
/**
* @class WorldModel
* @brief An interface the trajectory controller uses to interact with the
* world regardless of the underlying world model.
*/
class WorldModel{
public:
/**
* @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise:
* -1 if footprint covers at least a lethal obstacle cell, or
* -2 if footprint covers at least a no-information cell, or
* -3 if footprint is partially or totally outside of the map
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius) = 0;
double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){
double cos_th = cos(theta);
double sin_th = sin(theta);
std::vector<geometry_msgs::Point> oriented_footprint;
for(unsigned int i = 0; i < footprint_spec.size(); ++i){
geometry_msgs::Point new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
geometry_msgs::Point robot_position;
robot_position.x = x;
robot_position.y = y;
if(inscribed_radius==0.0){
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
}
return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
}
/**
* @brief Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius, double extra) {
return footprintCost(position, footprint, inscribed_radius, circumscribed_radius);
}
/**
* @brief Subclass will implement a destructor
*/
virtual ~WorldModel(){}
protected:
WorldModel(){}
};
};
#endif

2
navigations/base_local_planner/msg/Position2DInt.msg Executable file
View File

@@ -0,0 +1,2 @@
int64 x
int64 y

52
navigations/base_local_planner/package.xml Executable file
View File

@@ -0,0 +1,52 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>base_local_planner</name>
<version>1.17.3</version>
<description>
This package provides implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation on a plane. Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base. This package supports both holonomic and non-holonomic robots, any robot footprint that can be represented as a convex polygon or circle, and exposes its configuration as ROS parameters that can be set in a launch file. This package's ROS wrapper adheres to the BaseLocalPlanner interface specified in the <a href="http://wiki.ros.org/nav_core">nav_core</a> package.
</description>
<author>Eitan Marder-Eppstein</author>
<author>Eric Perko</author>
<author>contradict@gmail.com</author>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/base_local_planner</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<depend>angles</depend>
<depend>costmap_2d</depend>
<depend>dynamic_reconfigure</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>rosconsole</depend>
<depend>roscpp</depend>
<depend>rospy</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>voxel_grid</depend>
<exec_depend>message_runtime</exec_depend>
<test_depend>rosunit</test_depend>
<export>
<nav_core plugin="${prefix}/blp_plugin.xml" />
</export>
</package>

10
navigations/base_local_planner/setup.py Executable file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env python
from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages = ['local_planner_limits'],
package_dir = {'': 'src'},
)
setup(**d)

145
navigations/base_local_planner/src/costmap_model.cpp Executable file
View File

@@ -0,0 +1,145 @@
/*********************************************************************
*
* 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 <base_local_planner/line_iterator.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/cost_values.h>
using namespace std;
using namespace costmap_2d;
namespace base_local_planner {
CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
// returns:
// -1 if footprint covers at least a lethal obstacle cell, or
// -2 if footprint covers at least a no-information cell, or
// -3 if footprint is [partially] outside of the map, or
// a positive value for traversable space
//used to put things into grid coordinates
unsigned int cell_x, cell_y;
//get the cell coord of the center point of the robot
if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
return -3.0;
//if number of points in the footprint is less than 3, we'll just assume a circular robot
if(footprint.size() < 3){
unsigned char cost = costmap_.getCost(cell_x, cell_y);
if(cost == NO_INFORMATION)
return -2.0;
if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
return -1.0;
return cost;
}
//now we really have to lay down the footprint in the costmap grid
unsigned int x0, x1, y0, y1;
double line_cost = 0.0;
double footprint_cost = 0.0;
//we need to rasterize each line in the footprint
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
return -3.0;
//get the cell coord of the second point
if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
//if there is an obstacle that hits the line... we know that we can return false right away
if(line_cost < 0)
return line_cost;
}
//we also need to connect the first point in the footprint to the last point
//get the cell coord of the last point
if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
return -3.0;
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
if(line_cost < 0)
return line_cost;
//if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
}
//calculate the cost of a ray-traced line
double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const {
double line_cost = 0.0;
double point_cost = -1.0;
for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
{
point_cost = pointCost( line.getX(), line.getY() ); //Score the current point
if(point_cost < 0)
return point_cost;
if(line_cost < point_cost)
line_cost = point_cost;
}
return line_cost;
}
double CostmapModel::pointCost(int x, int y) const {
unsigned char cost = costmap_.getCost(x, y);
//if the cell is in an obstacle the path is invalid
if(cost == NO_INFORMATION)
return -2;
if(cost == LETHAL_OBSTACLE)
return -1;
return cost;
}
};

248
navigations/base_local_planner/src/footprint_helper.cpp Executable file
View File

@@ -0,0 +1,248 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#include <base_local_planner/footprint_helper.h>
namespace base_local_planner {
FootprintHelper::FootprintHelper() {
// TODO Auto-generated constructor stub
}
FootprintHelper::~FootprintHelper() {
// TODO Auto-generated destructor stub
}
void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector<base_local_planner::Position2DInt>& pts) {
//Bresenham Ray-Tracing
int deltax = abs(x1 - x0); // The difference between the x's
int deltay = abs(y1 - y0); // The difference between the y's
int x = x0; // Start x off at the first pixel
int y = y0; // Start y off at the first pixel
int xinc1, xinc2, yinc1, yinc2;
int den, num, numadd, numpixels;
base_local_planner::Position2DInt pt;
if (x1 >= x0) // The x-values are increasing
{
xinc1 = 1;
xinc2 = 1;
}
else // The x-values are decreasing
{
xinc1 = -1;
xinc2 = -1;
}
if (y1 >= y0) // The y-values are increasing
{
yinc1 = 1;
yinc2 = 1;
}
else // The y-values are decreasing
{
yinc1 = -1;
yinc2 = -1;
}
if (deltax >= deltay) // There is at least one x-value for every y-value
{
xinc1 = 0; // Don't change the x when numerator >= denominator
yinc2 = 0; // Don't change the y for every iteration
den = deltax;
num = deltax / 2;
numadd = deltay;
numpixels = deltax; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2 = 0; // Don't change the x for every iteration
yinc1 = 0; // Don't change the y when numerator >= denominator
den = deltay;
num = deltay / 2;
numadd = deltax;
numpixels = deltay; // There are more y-values than x-values
}
for (int curpixel = 0; curpixel <= numpixels; curpixel++)
{
pt.x = x; //Draw the current pixel
pt.y = y;
pts.push_back(pt);
num += numadd; // Increase the numerator by the top of the fraction
if (num >= den) // Check if numerator >= denominator
{
num -= den; // Calculate the new numerator value
x += xinc1; // Change the x as appropriate
y += yinc1; // Change the y as appropriate
}
x += xinc2; // Change the x as appropriate
y += yinc2; // Change the y as appropriate
}
}
void FootprintHelper::getFillCells(std::vector<base_local_planner::Position2DInt>& footprint){
//quick bubble sort to sort pts by x
base_local_planner::Position2DInt swap, pt;
unsigned int i = 0;
while (i < footprint.size() - 1) {
if (footprint[i].x > footprint[i + 1].x) {
swap = footprint[i];
footprint[i] = footprint[i + 1];
footprint[i + 1] = swap;
if(i > 0) {
--i;
}
} else {
++i;
}
}
i = 0;
base_local_planner::Position2DInt min_pt;
base_local_planner::Position2DInt max_pt;
unsigned int min_x = footprint[0].x;
unsigned int max_x = footprint[footprint.size() -1].x;
//walk through each column and mark cells inside the footprint
for (unsigned int x = min_x; x <= max_x; ++x) {
if (i >= footprint.size() - 1) {
break;
}
if (footprint[i].y < footprint[i + 1].y) {
min_pt = footprint[i];
max_pt = footprint[i + 1];
} else {
min_pt = footprint[i + 1];
max_pt = footprint[i];
}
i += 2;
while (i < footprint.size() && footprint[i].x == x) {
if(footprint[i].y < min_pt.y) {
min_pt = footprint[i];
} else if(footprint[i].y > max_pt.y) {
max_pt = footprint[i];
}
++i;
}
//loop though cells in the column
for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
pt.x = x;
pt.y = y;
footprint.push_back(pt);
}
}
}
/**
* get the cellsof a footprint at a given position
*/
std::vector<base_local_planner::Position2DInt> FootprintHelper::getFootprintCells(
Eigen::Vector3f pos,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D& costmap,
bool fill){
double x_i = pos[0];
double y_i = pos[1];
double theta_i = pos[2];
std::vector<base_local_planner::Position2DInt> footprint_cells;
//if we have no footprint... do nothing
if (footprint_spec.size() <= 1) {
unsigned int mx, my;
if (costmap.worldToMap(x_i, y_i, mx, my)) {
Position2DInt center;
center.x = mx;
center.y = my;
footprint_cells.push_back(center);
}
return footprint_cells;
}
//pre-compute cos and sin values
double cos_th = cos(theta_i);
double sin_th = sin(theta_i);
double new_x, new_y;
unsigned int x0, y0, x1, y1;
unsigned int last_index = footprint_spec.size() - 1;
for (unsigned int i = 0; i < last_index; ++i) {
//find the cell coordinates of the first segment point
new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
//find the cell coordinates of the second segment point
new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
}
//we need to close the loop, so we also have to raytrace from the last pt to first pt
new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
if(fill) {
getFillCells(footprint_cells);
}
return footprint_cells;
}
} /* namespace base_local_planner */

245
navigations/base_local_planner/src/goal_functions.cpp Executable file
View File

@@ -0,0 +1,245 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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
*********************************************************************/
#include <base_local_planner/goal_functions.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#ifdef _MSC_VER
#define GOAL_ATTRIBUTE_UNUSED
#else
#define GOAL_ATTRIBUTE_UNUSED __attribute__ ((unused))
#endif
namespace base_local_planner {
double getGoalPositionDistance(const geometry_msgs::PoseStamped& global_pose, double goal_x, double goal_y) {
return hypot(goal_x - global_pose.pose.position.x, goal_y - global_pose.pose.position.y);
}
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped& global_pose, double goal_th) {
double yaw = tf2::getYaw(global_pose.pose.orientation);
return angles::shortest_angular_distance(yaw, goal_th);
}
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub) {
//given an empty path we won't do anything
if(path.empty())
return;
//create a path message
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
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];
}
pub.publish(gui_path);
}
void prunePlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const geometry_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
double x_diff = global_pose.pose.position.x - w.pose.position.x;
double y_diff = global_pose.pose.position.y - w.pose.position.y;
double distance_sq = x_diff * x_diff + y_diff * y_diff;
if(distance_sq < 1){
ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.pose.position.x, global_pose.pose.position.y, w.pose.position.x, w.pose.position.y);
break;
}
it = plan.erase(it);
global_it = global_plan.erase(global_it);
}
}
bool transformGlobalPlan(
const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
transformed_plan.clear();
if (global_plan.empty()) {
ROS_ERROR("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
try {
// get plan_to_global_transform from plan frame to global_frame
geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5));
//let's get the pose of the robot in the frame of the plan
geometry_msgs::PoseStamped robot_pose;
tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
tf2::doTransform(pose, newer_pose, plan_to_global_transform);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf2::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf2::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf2::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (!global_plan.empty())
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool getGoalPose(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, geometry_msgs::PoseStamped &goal_pose) {
if (global_plan.empty())
{
ROS_ERROR("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try{
geometry_msgs::TransformStamped transform = tf.lookupTransform(global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration(0.5));
tf2::doTransform(plan_goal_pose, goal_pose, transform);
}
catch(tf2::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf2::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf2::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool isGoalReached(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap GOAL_ATTRIBUTE_UNUSED,
const std::string& global_frame,
geometry_msgs::PoseStamped& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance){
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
getGoalPose(tf, global_plan, global_frame, goal_pose);
double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
double goal_th = tf2::getYaw(goal_pose.pose.orientation);
//check to see if we've reached the goal position
if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
//check to see if the goal orientation has been reached
if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
//make sure that we're actually stopped before returning success
if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
return true;
}
}
return false;
}
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity, const double& trans_stopped_velocity){
return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
&& fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
&& fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
}
};

278
navigations/base_local_planner/src/latched_stop_rotate_controller.cpp Executable file
View File

@@ -0,0 +1,278 @@
/*
* latched_stop_rotate_controller.cpp
*
* Created on: Apr 16, 2012
* Author: tkruse
*/
#include <base_local_planner/latched_stop_rotate_controller.h>
#include <cmath>
#include <Eigen/Core>
#include <angles/angles.h>
#include <nav_msgs/Odometry.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/local_planner_limits.h>
#include <tf2/utils.h>
namespace base_local_planner {
LatchedStopRotateController::LatchedStopRotateController(const std::string& name) {
ros::NodeHandle private_nh("~/" + name);
private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
rotating_to_goal_ = false;
}
LatchedStopRotateController::~LatchedStopRotateController() {}
/**
* returns true if we have passed the goal position.
* Meaning we might have overshot on the position beyond tolerance, yet still return true.
* Also goal orientation might not yet be true
*/
bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
const geometry_msgs::PoseStamped& global_pose) {
double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
if ( ! planner_util->getGoal(goal_pose)) {
return false;
}
double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
//check to see if we've reached the goal position
if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
xy_tolerance_latch_ = true;
return true;
}
return false;
}
/**
* returns true if we have passed the goal position and have reached goal orientation.
* Meaning we might have overshot on the position beyond tolerance, yet still return true.
*/
bool LatchedStopRotateController::isGoalReached(LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper,
const geometry_msgs::PoseStamped& global_pose) {
double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
double theta_stopped_vel = planner_util->getCurrentLimits().theta_stopped_vel;
double trans_stopped_vel = planner_util->getCurrentLimits().trans_stopped_vel;
//copy over the odometry information
nav_msgs::Odometry base_odom;
odom_helper.getOdom(base_odom);
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
if ( ! planner_util->getGoal(goal_pose)) {
return false;
}
double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
//check to see if we've reached the goal position
if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
//if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
//just rotate in place
if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
ROS_DEBUG("Goal position reached (check), stopping and turning in place");
xy_tolerance_latch_ = true;
}
double goal_th = tf2::getYaw(goal_pose.pose.orientation);
double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
//check to see if the goal orientation has been reached
if (fabs(angle) <= limits.yaw_goal_tolerance) {
//make sure that we're actually stopped before returning success
if (base_local_planner::stopped(base_odom, theta_stopped_vel, trans_stopped_vel)) {
return true;
}
}
}
return false;
}
bool LatchedStopRotateController::stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check) {
//slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
//but we'll use a tenth of a second to be consistent with the implementation of the local planner.
double vx = sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) - acc_lim[0] * sim_period));
double vy = sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) - acc_lim[1] * sim_period));
double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim[2] * sim_period));
//we do want to check whether or not the command is valid
double yaw = tf2::getYaw(global_pose.pose.orientation);
bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
Eigen::Vector3f(vx, vy, vth));
//if we have a valid command, we'll pass it on, otherwise we'll command all zeros
if(valid_cmd){
ROS_DEBUG_NAMED("latched_stop_rotate", "Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
cmd_vel.linear.x = vx;
cmd_vel.linear.y = vy;
cmd_vel.angular.z = vth;
return true;
}
ROS_WARN("Stopping cmd in collision");
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
return false;
}
bool LatchedStopRotateController::rotateToGoal(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
double goal_th,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
base_local_planner::LocalPlannerLimits& limits,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check) {
double yaw = tf2::getYaw(global_pose.pose.orientation);
double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
double v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, fabs(ang_diff)));
//take the acceleration limits of the robot into account
double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period;
double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period;
v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
//we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff));
v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp));
v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, v_theta_samp));
if (ang_diff < 0) {
v_theta_samp = - v_theta_samp;
}
//we still want to lay down the footprint of the robot and check if the action is legal
bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
Eigen::Vector3f( 0.0, 0.0, v_theta_samp));
if (valid_cmd) {
ROS_DEBUG_NAMED("dwa_local_planner", "Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
cmd_vel.angular.z = v_theta_samp;
return true;
}
ROS_WARN("Rotation cmd in collision");
cmd_vel.angular.z = 0.0;
return false;
}
bool LatchedStopRotateController::computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper_,
const geometry_msgs::PoseStamped& global_pose,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check) {
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
if ( ! planner_util->getGoal(goal_pose)) {
ROS_ERROR("Could not get goal pose");
return false;
}
base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
//if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
//just rotate in place
if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ ) {
ROS_INFO("Goal position reached, stopping and turning in place");
xy_tolerance_latch_ = true;
}
//check to see if the goal orientation has been reached
double goal_th = tf2::getYaw(goal_pose.pose.orientation);
double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
if (fabs(angle) <= limits.yaw_goal_tolerance) {
//set the velocity command to zero
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
rotating_to_goal_ = false;
} else {
ROS_DEBUG("Angle: %f Tolerance: %f", angle, limits.yaw_goal_tolerance);
geometry_msgs::PoseStamped robot_vel;
odom_helper_.getRobotVel(robot_vel);
nav_msgs::Odometry base_odom;
odom_helper_.getOdom(base_odom);
//if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, limits.theta_stopped_vel, limits.trans_stopped_vel)) {
if ( ! stopWithAccLimits(
global_pose,
robot_vel,
cmd_vel,
acc_lim,
sim_period,
obstacle_check)) {
ROS_INFO("Error when stopping.");
return false;
}
ROS_DEBUG("Stopping...");
}
//if we're stopped... then we want to rotate to goal
else {
//set this so that we know its OK to be moving
rotating_to_goal_ = true;
if ( ! rotateToGoal(
global_pose,
robot_vel,
goal_th,
cmd_vel,
acc_lim,
sim_period,
limits,
obstacle_check)) {
ROS_INFO("Error when rotating.");
return false;
}
ROS_DEBUG("Rotating...");
}
}
return true;
}
} /* namespace base_local_planner */

1
navigations/base_local_planner/src/local_planner_limits/.gitignore vendored Executable file
View File

@@ -0,0 +1 @@
*.pyc

41
navigations/base_local_planner/src/local_planner_limits/__init__.py Executable file
View File

@@ -0,0 +1,41 @@
# Generic set of parameters to use with base local planners
# To use:
#
# from local_planner_limits import add_generic_localplanner_params
# gen = ParameterGenerator()
# add_generic_localplanner_params(gen)
# ...
#
# Using these standard parameters instead of your own allows easier switching of local planners
# need this only for dataype declarations
from dynamic_reconfigure.parameter_generator_catkin import double_t, bool_t
def add_generic_localplanner_params(gen):
# velocities
gen.add("max_vel_trans", double_t, 0, "The absolute value of the maximum translational velocity for the robot in m/s", 0.55, 0)
gen.add("min_vel_trans", double_t, 0, "The absolute value of the minimum translational velocity for the robot in m/s", 0.1, 0)
gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55)
gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0)
gen.add("max_vel_y", double_t, 0, "The maximum y velocity for the robot in m/s", 0.1)
gen.add("min_vel_y", double_t, 0, "The minimum y velocity for the robot in m/s", -0.1)
gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0)
gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.4, 0)
# acceleration
gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0)
gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 2.5, 0, 20.0)
gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 3.2, 0, 20.0)
gen.add("acc_lim_trans", double_t, 0, "The absolute value of the maximum translational acceleration for the robot in m/s^2", 0.1, 0)
gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False)
gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1)
gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
gen.add("theta_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)

128
navigations/base_local_planner/src/local_planner_util.cpp Executable file
View File

@@ -0,0 +1,128 @@
/*********************************************************************
*
* 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 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
*********************************************************************/
#include <base_local_planner/local_planner_util.h>
#include <base_local_planner/goal_functions.h>
namespace base_local_planner {
void LocalPlannerUtil::initialize(
tf2_ros::Buffer* tf,
costmap_2d::Costmap2D* costmap,
std::string global_frame) {
if(!initialized_) {
tf_ = tf;
costmap_ = costmap;
global_frame_ = global_frame;
initialized_ = true;
}
else{
ROS_WARN("Planner utils have already been initialized, doing nothing.");
}
}
void LocalPlannerUtil::reconfigureCB(LocalPlannerLimits &config, bool restore_defaults)
{
if(setup_ && restore_defaults) {
config = default_limits_;
}
if(!setup_) {
default_limits_ = config;
setup_ = true;
}
boost::mutex::scoped_lock l(limits_configuration_mutex_);
limits_ = LocalPlannerLimits(config);
}
costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() {
return costmap_;
}
LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() {
boost::mutex::scoped_lock l(limits_configuration_mutex_);
return limits_;
}
bool LocalPlannerUtil::getGoal(geometry_msgs::PoseStamped& goal_pose) {
//we assume the global goal is the last point in the global plan
return base_local_planner::getGoalPose(*tf_,
global_plan_,
global_frame_,
goal_pose);
}
bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if(!initialized_){
ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
return false;
}
//reset the global plan
global_plan_.clear();
global_plan_ = orig_global_plan;
return true;
}
bool LocalPlannerUtil::getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
//get the global plan in our frame
if(!base_local_planner::transformGlobalPlan(
*tf_,
global_plan_,
global_pose,
*costmap_,
global_frame_,
transformed_plan)) {
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
//now we'll prune the plan based on the position of the robot
if(limits_.prune_plan) {
base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}
return true;
}
} // namespace

52
navigations/base_local_planner/src/map_cell.cpp Executable file
View File

@@ -0,0 +1,52 @@
/*********************************************************************
* 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.
*********************************************************************/
#include <base_local_planner/map_cell.h>
namespace base_local_planner{
MapCell::MapCell()
: cx(0), cy(0),
target_dist(DBL_MAX),
target_mark(false),
within_robot(false)
{}
MapCell::MapCell(const MapCell& mc)
: cx(mc.cx), cy(mc.cy),
target_dist(mc.target_dist),
target_mark(mc.target_mark),
within_robot(mc.within_robot)
{}
};

309
navigations/base_local_planner/src/map_grid.cpp Executable file
View File

@@ -0,0 +1,309 @@
/*********************************************************************
* 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.
*********************************************************************/
#include <base_local_planner/map_grid.h>
#include <costmap_2d/cost_values.h>
using namespace std;
namespace base_local_planner{
MapGrid::MapGrid()
: size_x_(0), size_y_(0)
{
}
MapGrid::MapGrid(unsigned int size_x, unsigned int size_y)
: size_x_(size_x), size_y_(size_y)
{
commonInit();
}
MapGrid::MapGrid(const MapGrid& mg){
size_y_ = mg.size_y_;
size_x_ = mg.size_x_;
map_ = mg.map_;
}
void MapGrid::commonInit(){
//don't allow construction of zero size grid
ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
map_.resize(size_y_ * size_x_);
//make each cell aware of its location in the grid
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
unsigned int id = size_x_ * i + j;
map_[id].cx = j;
map_[id].cy = i;
}
}
}
size_t MapGrid::getIndex(int x, int y){
return size_x_ * y + x;
}
MapGrid& MapGrid::operator= (const MapGrid& mg){
size_y_ = mg.size_y_;
size_x_ = mg.size_x_;
map_ = mg.map_;
return *this;
}
void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y){
if(map_.size() != size_x * size_y)
map_.resize(size_x * size_y);
if(size_x_ != size_x || size_y_ != size_y){
size_x_ = size_x;
size_y_ = size_y;
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
int index = size_x_ * i + j;
map_[index].cx = j;
map_[index].cy = i;
}
}
}
}
inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap){
//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION)){
check_cell->target_dist = obstacleCosts();
return false;
}
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}
return true;
}
//reset the path_dist and goal_dist fields for all cells
void MapGrid::resetPathDist(){
for(unsigned int i = 0; i < map_.size(); ++i) {
map_[i].target_dist = unreachableCellCosts();
map_[i].target_mark = false;
map_[i].within_robot = false;
}
}
void MapGrid::adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution) {
if (global_plan_in.size() == 0) {
return;
}
double last_x = global_plan_in[0].pose.position.x;
double last_y = global_plan_in[0].pose.position.y;
global_plan_out.push_back(global_plan_in[0]);
double min_sq_resolution = resolution * resolution;
for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
double loop_x = global_plan_in[i].pose.position.x;
double loop_y = global_plan_in[i].pose.position.y;
double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
if (sqdist > min_sq_resolution) {
int steps = ceil((sqrt(sqdist)) / resolution);
// add a points in-between
double deltax = (loop_x - last_x) / steps;
double deltay = (loop_y - last_y) / steps;
// TODO: Interpolate orientation
for (int j = 1; j < steps; ++j) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = last_x + j * deltax;
pose.pose.position.y = last_y + j * deltay;
pose.pose.position.z = global_plan_in[i].pose.position.z;
pose.pose.orientation = global_plan_in[i].pose.orientation;
pose.header = global_plan_in[i].header;
global_plan_out.push_back(pose);
}
}
global_plan_out.push_back(global_plan_in[i]);
last_x = loop_x;
last_y = loop_y;
}
}
//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size()) {
ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
MapCell& current = getCell(map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(&current);
started_path = true;
} else if (started_path) {
break;
}
}
if (!started_path) {
ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance(path_dist_queue, costmap);
}
//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
} else {
if (started_path) {
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path) {
ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
if (local_goal_x >= 0 && local_goal_y >= 0) {
MapCell& current = getCell(local_goal_x, local_goal_y);
costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(&current);
}
computeTargetDistance(path_dist_queue, costmap);
}
void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!dist_queue.empty()){
current_cell = dist_queue.front();
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->target_mark){
//mark the cell as visisted
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
}
}
};

131
navigations/base_local_planner/src/map_grid_cost_function.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 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: TKruse
*********************************************************************/
#include <base_local_planner/map_grid_cost_function.h>
namespace base_local_planner {
MapGridCostFunction::MapGridCostFunction(costmap_2d::Costmap2D* costmap,
double xshift,
double yshift,
bool is_local_goal_function,
CostAggregationType aggregationType) :
costmap_(costmap),
map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()),
aggregationType_(aggregationType),
xshift_(xshift),
yshift_(yshift),
is_local_goal_function_(is_local_goal_function),
stop_on_failure_(true) {}
void MapGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
target_poses_ = target_poses;
}
bool MapGridCostFunction::prepare() {
map_.resetPathDist();
if (is_local_goal_function_) {
map_.setLocalGoal(*costmap_, target_poses_);
} else {
map_.setTargetCells(*costmap_, target_poses_);
}
return true;
}
double MapGridCostFunction::getCellCosts(unsigned int px, unsigned int py) {
double grid_dist = map_(px, py).target_dist;
return grid_dist;
}
double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0.0;
if (aggregationType_ == Product) {
cost = 1.0;
}
double px, py, pth;
unsigned int cell_x, cell_y;
double grid_dist;
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
// translate point forward if specified
if (xshift_ != 0.0) {
px = px + xshift_ * cos(pth);
py = py + xshift_ * sin(pth);
}
// translate point sideways if specified
if (yshift_ != 0.0) {
px = px + yshift_ * cos(pth + M_PI_2);
py = py + yshift_ * sin(pth + M_PI_2);
}
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
//we're off the map
ROS_WARN("Off Map %f, %f", px, py);
return -4.0;
}
grid_dist = getCellCosts(cell_x, cell_y);
//if a point on this trajectory has no clear path to the goal... it may be invalid
if (stop_on_failure_) {
if (grid_dist == map_.obstacleCosts()) {
return -3.0;
} else if (grid_dist == map_.unreachableCellCosts()) {
return -2.0;
}
}
switch( aggregationType_ ) {
case Last:
cost = grid_dist;
break;
case Sum:
cost += grid_dist;
break;
case Product:
if (cost > 0) {
cost *= grid_dist;
}
break;
}
}
return cost;
}
} /* namespace base_local_planner */

95
navigations/base_local_planner/src/map_grid_visualizer.cpp Executable file
View File

@@ -0,0 +1,95 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Eric Perko
* 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 Eric Perko 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 <base_local_planner/map_grid_visualizer.h>
#include <base_local_planner/map_cell.h>
#include <vector>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
namespace base_local_planner {
MapGridVisualizer::MapGridVisualizer() {}
void MapGridVisualizer::initialize(const std::string& name, std::string frame_id, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function) {
name_ = name;
frame_id_ = frame_id;
cost_function_ = cost_function;
ns_nh_ = ros::NodeHandle("~/" + name_);
pub_ = ns_nh_.advertise<sensor_msgs::PointCloud2>("cost_cloud", 1);
}
void MapGridVisualizer::publishCostCloud(const costmap_2d::Costmap2D* costmap_p_) {
sensor_msgs::PointCloud2 cost_cloud;
cost_cloud.header.frame_id = frame_id_;
cost_cloud.header.stamp = ros::Time::now();
sensor_msgs::PointCloud2Modifier cloud_mod(cost_cloud);
cloud_mod.setPointCloud2Fields(7, "x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"path_cost", 1, sensor_msgs::PointField::FLOAT32,
"goal_cost", 1, sensor_msgs::PointField::FLOAT32,
"occ_cost", 1, sensor_msgs::PointField::FLOAT32,
"total_cost", 1, sensor_msgs::PointField::FLOAT32);
unsigned int x_size = costmap_p_->getSizeInCellsX();
unsigned int y_size = costmap_p_->getSizeInCellsY();
double z_coord = 0.0;
double x_coord, y_coord;
cloud_mod.resize(x_size * y_size);
sensor_msgs::PointCloud2Iterator<float> iter_x(cost_cloud, "x");
float path_cost, goal_cost, occ_cost, total_cost;
for (unsigned int cx = 0; cx < x_size; cx++) {
for (unsigned int cy = 0; cy < y_size; cy++) {
costmap_p_->mapToWorld(cx, cy, x_coord, y_coord);
if (cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
iter_x[0] = x_coord;
iter_x[1] = y_coord;
iter_x[2] = z_coord;
iter_x[3] = path_cost;
iter_x[4] = goal_cost;
iter_x[5] = occ_cost;
iter_x[6] = total_cost;
++iter_x;
}
}
}
pub_.publish(cost_cloud);
ROS_DEBUG("Cost PointCloud published");
}
};

152
navigations/base_local_planner/src/obstacle_cost_function.cpp Executable file
View File

@@ -0,0 +1,152 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#include <base_local_planner/obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <ros/console.h>
namespace base_local_planner {
ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap)
: costmap_(costmap), sum_scores_(false) {
if (costmap != NULL) {
world_model_ = new base_local_planner::CostmapModel(*costmap_);
}
}
ObstacleCostFunction::~ObstacleCostFunction() {
if (world_model_ != NULL) {
delete world_model_;
}
}
void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
// TODO: move this to prepare if possible
max_trans_vel_ = max_trans_vel;
max_scaling_factor_ = max_scaling_factor;
scaling_speed_ = scaling_speed;
}
void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
footprint_spec_ = footprint_spec;
}
bool ObstacleCostFunction::prepare() {
return true;
}
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0;
double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
double px, py, pth;
if (footprint_spec_.size() == 0) {
// Bug, should never happen
ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
return -9;
}
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
double f_cost = footprintCost(px, py, pth,
scale, footprint_spec_,
costmap_, world_model_);
if(f_cost < 0){
return f_cost;
}
if(sum_scores_)
cost += f_cost;
else
cost = std::max(cost, f_cost);
}
return cost;
}
double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
double vmag = hypot(traj.xv_, traj.yv_);
//if we're over a certain speed threshold, we'll scale the robot's
//footprint to make it either slow down or stay further from walls
double scale = 1.0;
if (vmag > scaling_speed) {
//scale up to the max scaling factor linearly... this could be changed later
double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
scale = max_scaling_factor * ratio + 1.0;
}
return scale;
}
double ObstacleCostFunction::footprintCost (
const double& x,
const double& y,
const double& th,
double scale,
std::vector<geometry_msgs::Point> footprint_spec,
costmap_2d::Costmap2D* costmap,
base_local_planner::WorldModel* world_model) {
std::vector<geometry_msgs::Point> scaled_footprint;
for(unsigned int i = 0; i < footprint_spec.size(); ++i) {
geometry_msgs::Point new_pt;
new_pt.x = scale * footprint_spec[i].x;
new_pt.y = scale * footprint_spec[i].y;
scaled_footprint.push_back(new_pt);
}
//check if the footprint is legal
// TODO: Cache inscribed radius
double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint);
if (footprint_cost < 0) {
return -6.0;
}
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
return -7.0;
}
double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
return occ_cost;
}
} /* namespace base_local_planner */

106
navigations/base_local_planner/src/odometry_helper_ros.cpp Executable file
View File

@@ -0,0 +1,106 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#include <base_local_planner/odometry_helper_ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/convert.h>
namespace base_local_planner {
OdometryHelperRos::OdometryHelperRos(std::string odom_topic) {
setOdomTopic( odom_topic );
}
void OdometryHelperRos::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
ROS_INFO_ONCE("odom received!");
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
base_odom_.child_frame_id = msg->child_frame_id;
// ROS_DEBUG_NAMED("dwa_local_planner", "In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
// base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
}
//copy over the odometry information
void OdometryHelperRos::getOdom(nav_msgs::Odometry& base_odom) {
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom = base_odom_;
}
void OdometryHelperRos::getRobotVel(geometry_msgs::PoseStamped& robot_vel) {
// Set current velocities from odometry
geometry_msgs::Twist global_vel;
{
boost::mutex::scoped_lock lock(odom_mutex_);
global_vel.linear.x = base_odom_.twist.twist.linear.x;
global_vel.linear.y = base_odom_.twist.twist.linear.y;
global_vel.angular.z = base_odom_.twist.twist.angular.z;
robot_vel.header.frame_id = base_odom_.child_frame_id;
}
robot_vel.pose.position.x = global_vel.linear.x;
robot_vel.pose.position.y = global_vel.linear.y;
robot_vel.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, global_vel.angular.z);
tf2::convert(q, robot_vel.pose.orientation);
robot_vel.header.stamp = ros::Time();
}
void OdometryHelperRos::setOdomTopic(std::string odom_topic)
{
if( odom_topic != odom_topic_ )
{
odom_topic_ = odom_topic;
if( odom_topic_ != "" )
{
ros::NodeHandle gn;
odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, [this](auto& msg){ odomCallback(msg); });
}
else
{
odom_sub_.shutdown();
}
}
}
} /* namespace base_local_planner */

178
navigations/base_local_planner/src/oscillation_cost_function.cpp Executable file
View File

@@ -0,0 +1,178 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#include <base_local_planner/oscillation_cost_function.h>
#include <cmath>
namespace base_local_planner {
OscillationCostFunction::OscillationCostFunction() {
}
OscillationCostFunction::~OscillationCostFunction() {
prev_stationary_pos_ = Eigen::Vector3f::Zero();
}
void OscillationCostFunction::setOscillationResetDist(double dist, double angle) {
oscillation_reset_dist_ = dist;
oscillation_reset_angle_ = angle;
}
void OscillationCostFunction::updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory* traj, double min_vel_trans) {
if (traj->cost_ >= 0) {
if (setOscillationFlags(traj, min_vel_trans)) {
prev_stationary_pos_ = pos;
}
//if we've got restrictions... check if we can reset any oscillation flags
if(forward_pos_only_ || forward_neg_only_
|| strafe_pos_only_ || strafe_neg_only_
|| rot_pos_only_ || rot_neg_only_){
resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);
}
}
}
void OscillationCostFunction::resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev) {
double x_diff = pos[0] - prev[0];
double y_diff = pos[1] - prev[1];
double sq_dist = x_diff * x_diff + y_diff * y_diff;
double th_diff = pos[2] - prev[2];
//if we've moved far enough... we can reset our flags
if (sq_dist > oscillation_reset_dist_ * oscillation_reset_dist_ ||
fabs(th_diff) > oscillation_reset_angle_) {
resetOscillationFlags();
}
}
void OscillationCostFunction::resetOscillationFlags() {
strafe_pos_only_ = false;
strafe_neg_only_ = false;
strafing_pos_ = false;
strafing_neg_ = false;
rot_pos_only_ = false;
rot_neg_only_ = false;
rotating_pos_ = false;
rotating_neg_ = false;
forward_pos_only_ = false;
forward_neg_only_ = false;
forward_pos_ = false;
forward_neg_ = false;
}
bool OscillationCostFunction::setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans) {
bool flag_set = false;
//set oscillation flags for moving forward and backward
if (t->xv_ < 0.0) {
if (forward_pos_) {
forward_neg_only_ = true;
flag_set = true;
}
forward_pos_ = false;
forward_neg_ = true;
}
if (t->xv_ > 0.0) {
if (forward_neg_) {
forward_pos_only_ = true;
flag_set = true;
}
forward_neg_ = false;
forward_pos_ = true;
}
//we'll only set flags for strafing and rotating when we're not moving forward at all
if (fabs(t->xv_) <= min_vel_trans) {
//check negative strafe
if (t->yv_ < 0) {
if (strafing_pos_) {
strafe_neg_only_ = true;
flag_set = true;
}
strafing_pos_ = false;
strafing_neg_ = true;
}
//check positive strafe
if (t->yv_ > 0) {
if (strafing_neg_) {
strafe_pos_only_ = true;
flag_set = true;
}
strafing_neg_ = false;
strafing_pos_ = true;
}
//check negative rotation
if (t->thetav_ < 0) {
if (rotating_pos_) {
rot_neg_only_ = true;
flag_set = true;
}
rotating_pos_ = false;
rotating_neg_ = true;
}
//check positive rotation
if (t->thetav_ > 0) {
if (rotating_neg_) {
rot_pos_only_ = true;
flag_set = true;
}
rotating_neg_ = false;
rotating_pos_ = true;
}
}
return flag_set;
}
double OscillationCostFunction::scoreTrajectory(Trajectory &traj) {
if ((forward_pos_only_ && traj.xv_ < 0.0) ||
(forward_neg_only_ && traj.xv_ > 0.0) ||
(strafe_pos_only_ && traj.yv_ < 0.0) ||
(strafe_neg_only_ && traj.yv_ > 0.0) ||
(rot_pos_only_ && traj.thetav_ < 0.0) ||
(rot_neg_only_ && traj.thetav_ > 0.0)) {
return -5.0;
}
return 0.0;
}
} /* namespace base_local_planner */

556
navigations/base_local_planner/src/point_grid.cpp Executable file
View File

@@ -0,0 +1,556 @@
/*********************************************************************
*
* 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 <base_local_planner/point_grid.h>
#include <ros/console.h>
#ifdef HAVE_SYS_TIME_H
#include <sys/time.h>
#endif
#include <math.h>
#include <cstdio>
#include <sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace costmap_2d;
namespace base_local_planner {
PointGrid::PointGrid(double size_x, double size_y, double resolution, geometry_msgs::Point origin, double max_z, double obstacle_range, double min_seperation) :
resolution_(resolution), origin_(origin), max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range), sq_min_separation_(min_seperation * min_seperation)
{
width_ = (int) (size_x / resolution_);
height_ = (int) (size_y / resolution_);
cells_.resize(width_ * height_);
}
double PointGrid::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
//the half-width of the circumscribed sqaure of the robot is equal to the circumscribed radius
double outer_square_radius = circumscribed_radius;
//get all the points inside the circumscribed square of the robot footprint
geometry_msgs::Point c_lower_left, c_upper_right;
c_lower_left.x = position.x - outer_square_radius;
c_lower_left.y = position.y - outer_square_radius;
c_upper_right.x = position.x + outer_square_radius;
c_upper_right.y = position.y + outer_square_radius;
//This may return points that are still outside of the cirumscribed square because it returns the cells
//contained by the range
getPointsInRange(c_lower_left, c_upper_right, points_);
//if there are no points in the circumscribed square... we don't have to check against the footprint
if(points_.empty())
return 1.0;
//compute the half-width of the inner square from the inscribed radius of the robot
double inner_square_radius = sqrt((inscribed_radius * inscribed_radius) / 2.0);
//we'll also check against the inscribed square
geometry_msgs::Point i_lower_left, i_upper_right;
i_lower_left.x = position.x - inner_square_radius;
i_lower_left.y = position.y - inner_square_radius;
i_upper_right.x = position.x + inner_square_radius;
i_upper_right.y = position.y + inner_square_radius;
//if there are points, we have to do a more expensive check
for(unsigned int i = 0; i < points_.size(); ++i){
list<geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
for(list<geometry_msgs::Point32>::iterator it = cell_points->begin(); it != cell_points->end(); ++it){
const geometry_msgs::Point32& pt = *it;
//first, we'll check to make sure we're in the outer square
//printf("(%.2f, %.2f) ... l(%.2f, %.2f) ... u(%.2f, %.2f)\n", pt.x, pt.y, c_lower_left.x, c_lower_left.y, c_upper_right.x, c_upper_right.y);
if(pt.x > c_lower_left.x && pt.x < c_upper_right.x && pt.y > c_lower_left.y && pt.y < c_upper_right.y){
//do a quick check to see if the point lies in the inner square of the robot
if(pt.x > i_lower_left.x && pt.x < i_upper_right.x && pt.y > i_lower_left.y && pt.y < i_upper_right.y)
return -1.0;
//now we really have to do a full footprint check on the point
if(ptInPolygon(pt, footprint))
return -1.0;
}
}
}
}
//if we get through all the points and none of them are in the footprint it's legal
return 1.0;
}
bool PointGrid::ptInPolygon(const geometry_msgs::Point32& pt, const std::vector<geometry_msgs::Point>& poly){
if(poly.size() < 3)
return false;
//a point is in a polygon iff the orientation of the point
//with respect to sides of the polygon is the same for every
//side of the polygon
bool all_left = false;
bool all_right = false;
for(unsigned int i = 0; i < poly.size() - 1; ++i){
//if pt left of a->b
if(orient(poly[i], poly[i + 1], pt) > 0){
if(all_right)
return false;
all_left = true;
}
//if pt right of a->b
else{
if(all_left)
return false;
all_right = true;
}
}
//also need to check the last point with the first point
if(orient(poly[poly.size() - 1], poly[0], pt) > 0){
if(all_right)
return false;
}
else{
if(all_left)
return false;
}
return true;
}
void PointGrid::getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right,
vector< list<geometry_msgs::Point32>* >& points){
points.clear();
//compute the other corners of the box so we can get cells indicies for them
geometry_msgs::Point upper_left, lower_right;
upper_left.x = lower_left.x;
upper_left.y = upper_right.y;
lower_right.x = upper_right.x;
lower_right.y = lower_left.y;
//get the grid coordinates of the cells matching the corners of the range
unsigned int gx, gy;
//if the grid coordinates are outside the bounds of the grid... return
if(!gridCoords(lower_left, gx, gy))
return;
//get the associated index
unsigned int lower_left_index = gridIndex(gx, gy);
if(!gridCoords(lower_right, gx, gy))
return;
unsigned int lower_right_index = gridIndex(gx, gy);
if(!gridCoords(upper_left, gx, gy))
return;
unsigned int upper_left_index = gridIndex(gx, gy);
//compute x_steps and y_steps
unsigned int x_steps = lower_right_index - lower_left_index + 1;
unsigned int y_steps = (upper_left_index - lower_left_index) / width_ + 1;
/*
* (0, 0) ---------------------- (width, 0)
* | |
* | |
* | |
* | |
* | |
* (0, height) ----------------- (width, height)
*/
//get an iterator
vector< list<geometry_msgs::Point32> >::iterator cell_iterator = cells_.begin() + lower_left_index;
//printf("Index: %d, Width: %d, x_steps: %d, y_steps: %d\n", lower_left_index, width_, x_steps, y_steps);
for(unsigned int i = 0; i < y_steps; ++i){
for(unsigned int j = 0; j < x_steps; ++j){
list<geometry_msgs::Point32>& cell = *cell_iterator;
//if the cell contains any points... we need to push them back to our list
if(!cell.empty()){
points.push_back(&cell);
}
if(j < x_steps - 1)
cell_iterator++; //move a cell in the x direction
}
cell_iterator += width_ - (x_steps - 1); //move down a row
}
}
void PointGrid::insert(const geometry_msgs::Point32& pt){
//get the grid coordinates of the point
unsigned int gx, gy;
//if the grid coordinates are outside the bounds of the grid... return
if(!gridCoords(pt, gx, gy))
return;
//if the point is too close to its nearest neighbor... return
if(nearestNeighborDistance(pt) < sq_min_separation_)
return;
//get the associated index
unsigned int pt_index = gridIndex(gx, gy);
//insert the point into the grid at the correct location
cells_[pt_index].push_back(pt);
//printf("Index: %d, size: %d\n", pt_index, cells_[pt_index].size());
}
double PointGrid::getNearestInCell(const geometry_msgs::Point32& pt, unsigned int gx, unsigned int gy){
unsigned int index = gridIndex(gx, gy);
double min_sq_dist = DBL_MAX;
//loop through the points in the cell and find the minimum distance to the passed point
for(list<geometry_msgs::Point32>::const_iterator it = cells_[index].begin(); it != cells_[index].end(); ++it){
min_sq_dist = min(min_sq_dist, sq_distance(pt, *it));
}
return min_sq_dist;
}
double PointGrid::nearestNeighborDistance(const geometry_msgs::Point32& pt){
//get the grid coordinates of the point
unsigned int gx, gy;
gridCoords(pt, gx, gy);
//get the bounds of the grid cell in world coords
geometry_msgs::Point lower_left, upper_right;
getCellBounds(gx, gy, lower_left, upper_right);
//now we need to check what cells could contain the nearest neighbor
geometry_msgs::Point32 check_point;
double sq_dist = DBL_MAX;
double neighbor_sq_dist = DBL_MAX;
//left
if(gx > 0){
check_point.x = lower_left.x;
check_point.y = pt.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy));
}
//upper left
if(gx > 0 && gy < height_ - 1){
check_point.x = lower_left.x;
check_point.y = upper_right.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy + 1));
}
//top
if(gy < height_ - 1){
check_point.x = pt.x;
check_point.y = upper_right.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy + 1));
}
//upper right
if(gx < width_ - 1 && gy < height_ - 1){
check_point.x = upper_right.x;
check_point.y = upper_right.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy + 1));
}
//right
if(gx < width_ - 1){
check_point.x = upper_right.x;
check_point.y = pt.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy));
}
//lower right
if(gx < width_ - 1 && gy > 0){
check_point.x = upper_right.x;
check_point.y = lower_left.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy - 1));
}
//bottom
if(gy > 0){
check_point.x = pt.x;
check_point.y = lower_left.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy - 1));
}
//lower left
if(gx > 0 && gy > 0){
check_point.x = lower_left.x;
check_point.y = lower_left.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy - 1));
}
//we must also check within the cell we're in for a nearest neighbor
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy));
return neighbor_sq_dist;
}
void PointGrid::updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
//for our 2D point grid we only remove freespace based on the first laser scan
if(laser_scans.empty())
return;
removePointsInScanBoundry(laser_scans[0]);
//iterate through all observations and update the grid
for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
const Observation& obs = *it;
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
geometry_msgs::Point32 pt;
for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
//filter out points that are too high
if(*iter_z > max_z_)
continue;
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
+ (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
+ (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
if(sq_dist >= sq_obstacle_range_)
continue;
//insert the point
pt.x = *iter_x;
pt.y = *iter_y;
pt.z = *iter_z;
insert(pt);
}
}
//remove the points that are in the footprint of the robot
removePointsInPolygon(footprint);
}
void PointGrid::removePointsInScanBoundry(const PlanarLaserScan& laser_scan){
if(laser_scan.cloud.points.size() == 0)
return;
//compute the containing square of the scan
geometry_msgs::Point lower_left, upper_right;
lower_left.x = laser_scan.origin.x;
lower_left.y = laser_scan.origin.y;
upper_right.x = laser_scan.origin.x;
upper_right.y = laser_scan.origin.y;
for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
lower_left.x = min((double)lower_left.x, (double)laser_scan.cloud.points[i].x);
lower_left.y = min((double)lower_left.y, (double)laser_scan.cloud.points[i].y);
upper_right.x = max((double)upper_right.x, (double)laser_scan.cloud.points[i].x);
upper_right.y = max((double)upper_right.y, (double)laser_scan.cloud.points[i].y);
}
getPointsInRange(lower_left, upper_right, points_);
//if there are no points in the containing square... we don't have to do anything
if(points_.empty())
return;
//if there are points, we have to check them against the scan explicitly to remove them
for(unsigned int i = 0; i < points_.size(); ++i){
list<geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
list<geometry_msgs::Point32>::iterator it = cell_points->begin();
while(it != cell_points->end()){
const geometry_msgs::Point32& pt = *it;
//check if the point is in the polygon and if it is, erase it from the grid
if(ptInScan(pt, laser_scan)){
it = cell_points->erase(it);
}
else
it++;
}
}
}
}
bool PointGrid::ptInScan(const geometry_msgs::Point32& pt, const PlanarLaserScan& laser_scan){
if(!laser_scan.cloud.points.empty()){
//compute the angle of the point relative to that of the scan
double v1_x = laser_scan.cloud.points[0].x - laser_scan.origin.x;
double v1_y = laser_scan.cloud.points[0].y - laser_scan.origin.y;
double v2_x = pt.x - laser_scan.origin.x;
double v2_y = pt.y - laser_scan.origin.y;
double perp_dot = v1_x * v2_y - v1_y * v2_x;
double dot = v1_x * v2_x + v1_y * v2_y;
//get the signed angle
double vector_angle = atan2(perp_dot, dot);
//we want all angles to be between 0 and 2PI
if(vector_angle < 0)
vector_angle = 2 * M_PI + vector_angle;
double total_rads = laser_scan.angle_max - laser_scan.angle_min;
//if this point lies outside of the scan field of view... it is not in the scan
if(vector_angle < 0 || vector_angle >= total_rads)
return false;
//compute the index of the point in the scan
unsigned int index = (unsigned int) (vector_angle / laser_scan.angle_increment);
//make sure we have a legal index... we always should at this point, but just in case
if(index >= laser_scan.cloud.points.size() - 1){
return false;
}
//if the point lies to the left of the line between the two scan points bounding it, it is within the scan
if(orient(laser_scan.cloud.points[index], laser_scan.cloud.points[index + 1], pt) > 0){
return true;
}
//otherwise it is not
return false;
}
else
return false;
}
void PointGrid::getPoints(sensor_msgs::PointCloud2& cloud){
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
size_t n = 0;
for(unsigned int i = 0; i < cells_.size(); ++i){
for(list<geometry_msgs::Point32>::iterator it = cells_[i].begin(); it != cells_[i].end(); ++it){
++n;
}
}
modifier.resize(n);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
for(unsigned int i = 0; i < cells_.size(); ++i){
for(list<geometry_msgs::Point32>::iterator it = cells_[i].begin(); it != cells_[i].end(); ++it, ++iter_x, ++iter_y, ++iter_z){
*iter_x = it->x;
*iter_y = it->y;
*iter_z = it->z;
}
}
}
void PointGrid::removePointsInPolygon(const std::vector<geometry_msgs::Point> poly){
if(poly.size() == 0)
return;
geometry_msgs::Point lower_left, upper_right;
lower_left.x = poly[0].x;
lower_left.y = poly[0].y;
upper_right.x = poly[0].x;
upper_right.y = poly[0].y;
//compute the containing square of the polygon
for(unsigned int i = 1; i < poly.size(); ++i){
lower_left.x = min(lower_left.x, poly[i].x);
lower_left.y = min(lower_left.y, poly[i].y);
upper_right.x = max(upper_right.x, poly[i].x);
upper_right.y = max(upper_right.y, poly[i].y);
}
ROS_DEBUG("Lower: (%.2f, %.2f), Upper: (%.2f, %.2f)\n", lower_left.x, lower_left.y, upper_right.x, upper_right.y);
getPointsInRange(lower_left, upper_right, points_);
//if there are no points in the containing square... we don't have to do anything
if(points_.empty())
return;
//if there are points, we have to check them against the polygon explicitly to remove them
for(unsigned int i = 0; i < points_.size(); ++i){
list<geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
list<geometry_msgs::Point32>::iterator it = cell_points->begin();
while(it != cell_points->end()){
const geometry_msgs::Point32& pt = *it;
//check if the point is in the polygon and if it is, erase it from the grid
if(ptInPolygon(pt, poly)){
it = cell_points->erase(it);
}
else
it++;
}
}
}
}
void PointGrid::intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
const geometry_msgs::Point& u1, const geometry_msgs::Point& u2, geometry_msgs::Point& result){
//generate the equation for line 1
double a1 = v2.y - v1.y;
double b1 = v1.x - v2.x;
double c1 = a1 * v1.x + b1 * v1.y;
//generate the equation for line 2
double a2 = u2.y - u1.y;
double b2 = u1.x - u2.x;
double c2 = a2 * u1.x + b2 * u1.y;
double det = a1 * b2 - a2 * b1;
//the lines are parallel
if(det == 0)
return;
result.x = (b2 * c1 - b1 * c2) / det;
result.y = (a1 * c2 - a2 * c1) / det;
}
};

221
navigations/base_local_planner/src/point_grid_node.cpp Executable file
View File

@@ -0,0 +1,221 @@
/*********************************************************************
*
* 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 <base_local_planner/point_grid.h>
#include <ros/console.h>
#ifdef HAVE_SYS_TIME_H
#include <sys/time.h>
#endif
#include <math.h>
#include <cstdio>
#include <sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace costmap_2d;
void printPoint(const geometry_msgs::Point& pt){
printf("(%.2f, %.2f, %.2f)", pt.x, pt.y, pt.z);
}
void printPSHeader(){
printf("%%!PS\n");
printf("%%%%Creator: Eitan Marder-Eppstein (Willow Garage)\n");
printf("%%%%EndComments\n");
}
void printPSFooter(){
printf("showpage\n%%%%EOF\n");
}
void printPolygonPS(const std::vector<geometry_msgs::Point32>& poly, double line_width){
if(poly.size() < 2)
return;
printf("%.2f setlinewidth\n", line_width);
printf("newpath\n");
printf("%.4f\t%.4f\tmoveto\n", poly[0].x * 10, poly[0].y * 10);
for(unsigned int i = 1; i < poly.size(); ++i)
printf("%.4f\t%.4f\tlineto\n", poly[i].x * 10, poly[i].y * 10);
printf("%.4f\t%.4f\tlineto\n", poly[0].x * 10, poly[0].y * 10);
printf("closepath stroke\n");
}
using namespace base_local_planner;
int main(int argc, char** argv){
geometry_msgs::Point origin;
origin.x = 0.0;
origin.y = 0.0;
PointGrid pg(50.0, 50.0, 0.2, origin, 2.0, 3.0, 0.0);
/*
double x = 10.0;
double y = 10.0;
for(int i = 0; i < 100; ++i){
for(int j = 0; j < 100; ++j){
pcl::PointXYZ pt;
pt.x = x;
pt.y = y;
pt.z = 1.0;
pg.insert(pt);
x += .03;
}
y += .03;
x = 10.0;
}
*/
std::vector<geometry_msgs::Point> footprint, footprint2, footprint3;
geometry_msgs::Point pt;
pt.x = 1.0;
pt.y = 1.0;
footprint.push_back(pt);
pt.x = 1.0;
pt.y = 1.65;
footprint.push_back(pt);
pt.x = 1.325;
pt.y = 1.75;
footprint.push_back(pt);
pt.x = 1.65;
pt.y = 1.65;
footprint.push_back(pt);
pt.x = 1.65;
pt.y = 1.0;
footprint.push_back(pt);
pt.x = 1.325;
pt.y = 1.00;
footprint2.push_back(pt);
pt.x = 1.325;
pt.y = 1.75;
footprint2.push_back(pt);
pt.x = 1.65;
pt.y = 1.75;
footprint2.push_back(pt);
pt.x = 1.65;
pt.y = 1.00;
footprint2.push_back(pt);
pt.x = 0.99;
pt.y = 0.99;
footprint3.push_back(pt);
pt.x = 0.99;
pt.y = 1.66;
footprint3.push_back(pt);
pt.x = 1.3255;
pt.y = 1.85;
footprint3.push_back(pt);
pt.x = 1.66;
pt.y = 1.66;
footprint3.push_back(pt);
pt.x = 1.66;
pt.y = 0.99;
footprint3.push_back(pt);
pt.x = 1.325;
pt.y = 1.325;
geometry_msgs::Point32 point;
point.x = 1.2;
point.y = 1.2;
point.z = 1.0;
#ifdef HAVE_SYS_TIME_H
struct timeval start, end;
double start_t, end_t, t_diff;
#endif
printPSHeader();
#ifdef HAVE_SYS_TIME_H
gettimeofday(&start, NULL);
#endif
for(unsigned int i = 0; i < 2000; ++i){
pg.insert(point);
}
#ifdef HAVE_SYS_TIME_H
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
printf("%%Insertion Time: %.9f \n", t_diff);
#endif
vector<Observation> obs;
vector<PlanarLaserScan> scan;
#ifdef HAVE_SYS_TIME_H
gettimeofday(&start, NULL);
#endif
pg.updateWorld(footprint, obs, scan);
double legal = pg.footprintCost(pt, footprint, 0.0, .95);
pg.updateWorld(footprint, obs, scan);
double legal2 = pg.footprintCost(pt, footprint, 0.0, .95);
#ifdef HAVE_SYS_TIME_H
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
printf("%%Footprint calc: %.9f \n", t_diff);
#endif
if(legal >= 0.0)
printf("%%Legal footprint %.4f, %.4f\n", legal, legal2);
else
printf("%%Illegal footprint\n");
printPSFooter();
return 0;
}

28
navigations/base_local_planner/src/prefer_forward_cost_function.cpp Executable file
View File

@@ -0,0 +1,28 @@
/*
* prefer_forward_cost_function.cpp
*
* Created on: Apr 4, 2012
* Author: tkruse
*/
#include <base_local_planner/prefer_forward_cost_function.h>
#include <math.h>
namespace base_local_planner {
double PreferForwardCostFunction::scoreTrajectory(Trajectory &traj) {
// backward motions bad on a robot without backward sensors
if (traj.xv_ < 0.0) {
return penalty_;
}
// strafing motions also bad on such a robot
if (traj.xv_ < 0.1 && fabs(traj.thetav_) < 0.2) {
return penalty_;
}
// the more we rotate, the less we progress forward
return fabs(traj.thetav_) * 10;
}
} /* namespace base_local_planner */

145
navigations/base_local_planner/src/simple_scored_sampling_planner.cpp Executable file
View File

@@ -0,0 +1,145 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#include <base_local_planner/simple_scored_sampling_planner.h>
#include <ros/console.h>
namespace base_local_planner {
SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples) {
max_samples_ = max_samples;
gen_list_ = gen_list;
critics_ = critics;
}
double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost) {
double traj_cost = 0;
int gen_id = 0;
for(std::vector<TrajectoryCostFunction*>::iterator score_function = critics_.begin(); score_function != critics_.end(); ++score_function) {
TrajectoryCostFunction* score_function_p = *score_function;
if (score_function_p->getScale() == 0) {
continue;
}
double cost = score_function_p->scoreTrajectory(traj);
if (cost < 0) {
ROS_DEBUG("Velocity %.3lf, %.3lf, %.3lf discarded by cost function %d with cost: %f", traj.xv_, traj.yv_, traj.thetav_, gen_id, cost);
traj_cost = cost;
break;
}
if (cost != 0) {
cost *= score_function_p->getScale();
}
traj_cost += cost;
if (best_traj_cost > 0) {
// since we keep adding positives, once we are worse than the best, we will stay worse
if (traj_cost > best_traj_cost) {
break;
}
}
gen_id ++;
}
return traj_cost;
}
bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {
Trajectory loop_traj;
Trajectory best_traj;
double loop_traj_cost, best_traj_cost = -1;
bool gen_success;
int count, count_valid;
for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {
TrajectoryCostFunction* loop_critic_p = *loop_critic;
if (loop_critic_p->prepare() == false) {
ROS_WARN("A scoring function failed to prepare");
return false;
}
}
for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
count = 0;
count_valid = 0;
TrajectorySampleGenerator* gen_ = *loop_gen;
while (gen_->hasMoreTrajectories()) {
gen_success = gen_->nextTrajectory(loop_traj);
if (gen_success == false) {
// TODO use this for debugging
continue;
}
loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
if (all_explored != NULL) {
loop_traj.cost_ = loop_traj_cost;
all_explored->push_back(loop_traj);
}
if (loop_traj_cost >= 0) {
count_valid++;
if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
best_traj_cost = loop_traj_cost;
best_traj = loop_traj;
}
}
count++;
if (max_samples_ > 0 && count >= max_samples_) {
break;
}
}
if (best_traj_cost >= 0) {
traj.xv_ = best_traj.xv_;
traj.yv_ = best_traj.yv_;
traj.thetav_ = best_traj.thetav_;
traj.cost_ = best_traj_cost;
traj.resetPoints();
double px, py, pth;
for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
best_traj.getPoint(i, px, py, pth);
traj.addPoint(px, py, pth);
}
}
ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
if (best_traj_cost >= 0) {
// do not try fallback generators
break;
}
}
return best_traj_cost >= 0;
}
}// namespace

282
navigations/base_local_planner/src/simple_trajectory_generator.cpp Executable file
View File

@@ -0,0 +1,282 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#include <base_local_planner/simple_trajectory_generator.h>
#include <cmath>
#include <base_local_planner/velocity_iterator.h>
namespace base_local_planner {
void SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
std::vector<Eigen::Vector3f> additional_samples,
bool discretize_by_time) {
initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
// add static samples if any
sample_params_.insert(sample_params_.end(), additional_samples.begin(), additional_samples.end());
}
void SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time) {
/*
* We actually generate all velocity sample vectors here, from which to generate trajectories later on
*/
double max_vel_th = limits->max_vel_theta;
double min_vel_th = -1.0 * max_vel_th;
discretize_by_time_ = discretize_by_time;
Eigen::Vector3f acc_lim = limits->getAccLimits();
pos_ = pos;
vel_ = vel;
limits_ = limits;
next_sample_index_ = 0;
sample_params_.clear();
double min_vel_x = limits->min_vel_x;
double max_vel_x = limits->max_vel_x;
double min_vel_y = limits->min_vel_y;
double max_vel_y = limits->max_vel_y;
// if sampling number is zero in any dimension, we don't generate samples generically
if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
//compute the feasible velocity space based on the rate at which we run
Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
if ( ! use_dwa_) {
// there is no point in overshooting the goal, and it also may break the
// robot behavior, so we limit the velocities to those that do not overshoot in sim_time
double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);
// if we use continous acceleration, we can sample the max velocity we can reach in sim_time_
max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
} else {
// with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
}
Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
for(; !x_it.isFinished(); x_it++) {
vel_samp[0] = x_it.getVelocity();
for(; !y_it.isFinished(); y_it++) {
vel_samp[1] = y_it.getVelocity();
for(; !th_it.isFinished(); th_it++) {
vel_samp[2] = th_it.getVelocity();
//ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
sample_params_.push_back(vel_samp);
}
th_it.reset();
}
y_it.reset();
}
}
}
void SimpleTrajectoryGenerator::setParameters(
double sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_dwa,
double sim_period) {
sim_time_ = sim_time;
sim_granularity_ = sim_granularity;
angular_sim_granularity_ = angular_sim_granularity;
use_dwa_ = use_dwa;
continued_acceleration_ = ! use_dwa_;
sim_period_ = sim_period;
}
/**
* Whether this generator can create more trajectories
*/
bool SimpleTrajectoryGenerator::hasMoreTrajectories() {
return next_sample_index_ < sample_params_.size();
}
/**
* Create and return the next sample trajectory
*/
bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj) {
bool result = false;
if (hasMoreTrajectories()) {
if (generateTrajectory(
pos_,
vel_,
sample_params_[next_sample_index_],
comp_traj)) {
result = true;
}
}
next_sample_index_++;
return result;
}
/**
* @param pos current position of robot
* @param vel desired velocity for sampling
*/
bool SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj) {
double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
double eps = 1e-4;
traj.cost_ = -1.0; // placed here in case we return early
//trajectory might be reused so we'll make sure to reset it
traj.resetPoints();
// make sure that the robot would at least be moving with one of
// the required minimum velocities for translation and rotation (if set)
if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) &&
(limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_vel_theta)) {
return false;
}
// make sure we do not exceed max diagonal (x+y) translational velocity (if set)
if (limits_->max_vel_trans >=0 && vmag - eps > limits_->max_vel_trans) {
return false;
}
int num_steps;
if (discretize_by_time_) {
num_steps = ceil(sim_time_ / sim_granularity_);
} else {
//compute the number of steps we must take along this trajectory to be "safe"
double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
num_steps =
ceil(std::max(sim_time_distance / sim_granularity_,
sim_time_angle / angular_sim_granularity_));
}
if (num_steps == 0) {
return false;
}
//compute a timestep
double dt = sim_time_ / num_steps;
traj.time_delta_ = dt;
Eigen::Vector3f loop_vel;
if (continued_acceleration_) {
// assuming the velocity of the first cycle is the one we want to store in the trajectory object
loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
traj.xv_ = loop_vel[0];
traj.yv_ = loop_vel[1];
traj.thetav_ = loop_vel[2];
} else {
// assuming sample_vel is our target velocity within acc limits for one timestep
loop_vel = sample_target_vel;
traj.xv_ = sample_target_vel[0];
traj.yv_ = sample_target_vel[1];
traj.thetav_ = sample_target_vel[2];
}
//simulate the trajectory and check for collisions, updating costs along the way
for (int i = 0; i < num_steps; ++i) {
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);
if (continued_acceleration_) {
//calculate velocities
loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
//ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
}
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, loop_vel, dt);
} // end for simulation steps
return true; // trajectory has at least one point
}
Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel, double dt) {
Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
new_pos[2] = pos[2] + vel[2] * dt;
return new_pos;
}
/**
* change vel using acceleration limits to converge towards sample_target-vel
*/
Eigen::Vector3f SimpleTrajectoryGenerator::computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt) {
Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
for (int i = 0; i < 3; ++i) {
if (vel[i] < sample_target_vel[i]) {
new_vel[i] = std::min(double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
} else {
new_vel[i] = std::max(double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
}
}
return new_vel;
}
} /* namespace base_local_planner */

80
navigations/base_local_planner/src/trajectory.cpp Executable file
View File

@@ -0,0 +1,80 @@
/*********************************************************************
* 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.
*********************************************************************/
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
Trajectory::Trajectory()
: xv_(0.0), yv_(0.0), thetav_(0.0), cost_(-1.0)
{
}
Trajectory::Trajectory(double xv, double yv, double thetav, double time_delta, unsigned int num_pts)
: xv_(xv), yv_(yv), thetav_(thetav), cost_(-1.0), time_delta_(time_delta), x_pts_(num_pts), y_pts_(num_pts), th_pts_(num_pts)
{
}
void Trajectory::getPoint(unsigned int index, double& x, double& y, double& th) const {
x = x_pts_[index];
y = y_pts_[index];
th = th_pts_[index];
}
void Trajectory::setPoint(unsigned int index, double x, double y, double th){
x_pts_[index] = x;
y_pts_[index] = y;
th_pts_[index] = th;
}
void Trajectory::addPoint(double x, double y, double th){
x_pts_.push_back(x);
y_pts_.push_back(y);
th_pts_.push_back(th);
}
void Trajectory::resetPoints(){
x_pts_.clear();
y_pts_.clear();
th_pts_.clear();
}
void Trajectory::getEndpoint(double& x, double& y, double& th) const {
x = x_pts_.back();
y = y_pts_.back();
th = th_pts_.back();
}
unsigned int Trajectory::getPointsSize() const {
return x_pts_.size();
}
};

1001
navigations/base_local_planner/src/trajectory_planner.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

621
navigations/base_local_planner/src/trajectory_planner_ros.cpp Executable file
View File

@@ -0,0 +1,621 @@
/*********************************************************************
*
* 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 <base_local_planner/trajectory_planner_ros.h>
#ifdef HAVE_SYS_TIME_H
#include <sys/time.h>
#endif
#include <boost/tokenizer.hpp>
#include <Eigen/Core>
#include <cmath>
#include <ros/console.h>
#include <pluginlib/class_list_macros.hpp>
#include <base_local_planner/goal_functions.h>
#include <nav_msgs/Path.h>
#include <nav_core/parameter_magic.h>
#include <tf2/utils.h>
//register this planner as a BaseLocalPlanner plugin
PLUGINLIB_EXPORT_CLASS(base_local_planner::TrajectoryPlannerROS, nav_core::BaseLocalPlanner)
namespace base_local_planner {
void TrajectoryPlannerROS::reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level) {
if (setup_ && config.restore_defaults) {
config = default_config_;
//Avoid looping
config.restore_defaults = false;
}
if ( ! setup_) {
default_config_ = config;
setup_ = true;
}
tc_->reconfigure(config);
reached_goal_ = false;
}
TrajectoryPlannerROS::TrajectoryPlannerROS() :
world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false), odom_helper_("odom") {}
TrajectoryPlannerROS::TrajectoryPlannerROS(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) :
world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), setup_(false), initialized_(false), odom_helper_("odom") {
//initialize the planner
initialize(name, tf, costmap_ros);
}
void TrajectoryPlannerROS::initialize(
std::string name,
tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros){
if (! isInitialized()) {
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
rot_stopped_velocity_ = 1e-2;
trans_stopped_velocity_ = 1e-2;
double sim_time, sim_granularity, angular_sim_granularity;
int vx_samples, vtheta_samples;
double path_distance_bias, goal_distance_bias, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta;
bool holonomic_robot, dwa, simple_attractor, heading_scoring;
double heading_scoring_timestep;
double max_vel_x, min_vel_x;
double backup_vel;
double stop_time_buffer;
std::string world_model_type;
rotating_to_goal_ = false;
//initialize the copy of the costmap the controller will use
costmap_ = costmap_ros_->getCostmap();
global_frame_ = costmap_ros_->getGlobalFrameID();
robot_base_frame_ = costmap_ros_->getBaseFrameID();
private_nh.param("prune_plan", prune_plan_, true);
private_nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
private_nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);
private_nh.param("acc_lim_x", acc_lim_x_, 2.5);
private_nh.param("acc_lim_y", acc_lim_y_, 2.5);
private_nh.param("acc_lim_theta", acc_lim_theta_, 3.2);
private_nh.param("stop_time_buffer", stop_time_buffer, 0.2);
private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
//Since I screwed up nicely in my documentation, I'm going to add errors
//informing the user if they've set one of the wrong parameters
if(private_nh.hasParam("acc_limit_x"))
ROS_ERROR("You are using acc_limit_x where you should be using acc_lim_x. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
if(private_nh.hasParam("acc_limit_y"))
ROS_ERROR("You are using acc_limit_y where you should be using acc_lim_y. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
if(private_nh.hasParam("acc_limit_th"))
ROS_ERROR("You are using acc_limit_th where you should be using acc_lim_th. Please change your configuration files appropriately. The documentation used to be wrong on this, sorry for any confusion.");
//Assuming this planner is being run within the navigation stack, we can
//just do an upward search for the frequency at which its being run. This
//also allows the frequency to be overwritten locally.
std::string controller_frequency_param_name;
if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name))
sim_period_ = 0.05;
else
{
double controller_frequency = 0;
private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
if(controller_frequency > 0)
sim_period_ = 1.0 / controller_frequency;
else
{
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
ROS_INFO("Sim period is set to %.2f", sim_period_);
private_nh.param("sim_time", sim_time, 1.0);
private_nh.param("sim_granularity", sim_granularity, 0.025);
private_nh.param("angular_sim_granularity", angular_sim_granularity, sim_granularity);
private_nh.param("vx_samples", vx_samples, 3);
private_nh.param("vtheta_samples", vtheta_samples, 20);
path_distance_bias = nav_core::loadParameterWithDeprecation(private_nh,
"path_distance_bias",
"pdist_scale",
0.6);
goal_distance_bias = nav_core::loadParameterWithDeprecation(private_nh,
"goal_distance_bias",
"gdist_scale",
0.6);
// values of the deprecated params need to be applied to the current params, as defaults
// of defined for dynamic reconfigure will override them otherwise.
if (private_nh.hasParam("pdist_scale") & !private_nh.hasParam("path_distance_bias"))
{
private_nh.setParam("path_distance_bias", path_distance_bias);
}
if (private_nh.hasParam("gdist_scale") & !private_nh.hasParam("goal_distance_bias"))
{
private_nh.setParam("goal_distance_bias", goal_distance_bias);
}
private_nh.param("occdist_scale", occdist_scale, 0.01);
bool meter_scoring;
if ( ! private_nh.hasParam("meter_scoring")) {
ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring not set. Set it to true to make your settings robust against changes of costmap resolution.");
} else {
private_nh.param("meter_scoring", meter_scoring, false);
if(meter_scoring) {
//if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
double resolution = costmap_->getResolution();
goal_distance_bias *= resolution;
path_distance_bias *= resolution;
} else {
ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring set to false. Set it to true to make your settings robust against changes of costmap resolution.");
}
}
private_nh.param("heading_lookahead", heading_lookahead, 0.325);
private_nh.param("oscillation_reset_dist", oscillation_reset_dist, 0.05);
private_nh.param("escape_reset_dist", escape_reset_dist, 0.10);
private_nh.param("escape_reset_theta", escape_reset_theta, M_PI_4);
private_nh.param("holonomic_robot", holonomic_robot, true);
private_nh.param("max_vel_x", max_vel_x, 0.5);
private_nh.param("min_vel_x", min_vel_x, 0.1);
double max_rotational_vel;
private_nh.param("max_rotational_vel", max_rotational_vel, 1.0);
max_vel_th_ = max_rotational_vel;
min_vel_th_ = -1.0 * max_rotational_vel;
min_in_place_vel_th_ = nav_core::loadParameterWithDeprecation(private_nh,
"min_in_place_vel_theta",
"min_in_place_rotational_vel", 0.4);
reached_goal_ = false;
backup_vel = -0.1;
if(private_nh.getParam("backup_vel", backup_vel))
ROS_WARN("The backup_vel parameter has been deprecated in favor of the escape_vel parameter. To switch, just change the parameter name in your configuration files.");
//if both backup_vel and escape_vel are set... we'll use escape_vel
private_nh.getParam("escape_vel", backup_vel);
if(backup_vel >= 0.0)
ROS_WARN("You've specified a positive escape velocity. This is probably not what you want and will cause the robot to move forward instead of backward. You should probably change your escape_vel parameter to be negative");
private_nh.param("world_model", world_model_type, std::string("costmap"));
private_nh.param("dwa", dwa, true);
private_nh.param("heading_scoring", heading_scoring, false);
private_nh.param("heading_scoring_timestep", heading_scoring_timestep, 0.8);
simple_attractor = false;
//parameters for using the freespace controller
double min_pt_separation, max_obstacle_height, grid_resolution;
private_nh.param("point_grid/max_sensor_range", max_sensor_range_, 2.0);
private_nh.param("point_grid/min_pt_separation", min_pt_separation, 0.01);
private_nh.param("point_grid/max_obstacle_height", max_obstacle_height, 2.0);
private_nh.param("point_grid/grid_resolution", grid_resolution, 0.2);
ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
world_model_ = new CostmapModel(*costmap_);
std::vector<double> y_vels = loadYVels(private_nh);
footprint_spec_ = costmap_ros_->getRobotFootprint();
tc_ = new TrajectoryPlanner(*world_model_, *costmap_, footprint_spec_,
acc_lim_x_, acc_lim_y_, acc_lim_theta_, sim_time, sim_granularity, vx_samples, vtheta_samples, path_distance_bias,
goal_distance_bias, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel,
dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity);
map_viz_.initialize(name,
global_frame_,
[this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost){
return tc_->getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost);
});
initialized_ = true;
dsrv_ = new dynamic_reconfigure::Server<BaseLocalPlannerConfig>(private_nh);
dynamic_reconfigure::Server<BaseLocalPlannerConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
dsrv_->setCallback(cb);
} else {
ROS_WARN("This planner has already been initialized, doing nothing");
}
}
std::vector<double> TrajectoryPlannerROS::loadYVels(ros::NodeHandle node){
std::vector<double> y_vels;
std::string y_vel_list;
if(node.getParam("y_vels", y_vel_list)){
typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
boost::char_separator<char> sep("[], ");
tokenizer tokens(y_vel_list, sep);
for(tokenizer::iterator i = tokens.begin(); i != tokens.end(); i++){
y_vels.push_back(atof((*i).c_str()));
}
}
else{
//if no values are passed in, we'll provide defaults
y_vels.push_back(-0.3);
y_vels.push_back(-0.1);
y_vels.push_back(0.1);
y_vels.push_back(0.3);
}
return y_vels;
}
TrajectoryPlannerROS::~TrajectoryPlannerROS() {
//make sure to clean things up
delete dsrv_;
if(tc_ != NULL)
delete tc_;
if(world_model_ != NULL)
delete world_model_;
}
bool TrajectoryPlannerROS::stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, geometry_msgs::Twist& cmd_vel){
//slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
//but we'll use a tenth of a second to be consistent with the implementation of the local planner.
double vx = sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) - acc_lim_x_ * sim_period_));
double vy = sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) - acc_lim_y_ * sim_period_));
double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim_theta_ * sim_period_));
//we do want to check whether or not the command is valid
double yaw = tf2::getYaw(global_pose.pose.orientation);
bool valid_cmd = tc_->checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw, vx, vy, vth);
//if we have a valid command, we'll pass it on, otherwise we'll command all zeros
if(valid_cmd){
ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
cmd_vel.linear.x = vx;
cmd_vel.linear.y = vy;
cmd_vel.angular.z = vth;
return true;
}
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
return false;
}
bool TrajectoryPlannerROS::rotateToGoal(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel){
double yaw = tf2::getYaw(global_pose.pose.orientation);
double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
double v_theta_samp = ang_diff > 0.0 ? std::min(max_vel_th_,
std::max(min_in_place_vel_th_, ang_diff)) : std::max(min_vel_th_,
std::min(-1.0 * min_in_place_vel_th_, ang_diff));
//take the acceleration limits of the robot into account
double max_acc_vel = fabs(vel_yaw) + acc_lim_theta_ * sim_period_;
double min_acc_vel = fabs(vel_yaw) - acc_lim_theta_ * sim_period_;
v_theta_samp = sign(v_theta_samp) * std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
//we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
double max_speed_to_stop = sqrt(2 * acc_lim_theta_ * fabs(ang_diff));
v_theta_samp = sign(v_theta_samp) * std::min(max_speed_to_stop, fabs(v_theta_samp));
// Re-enforce min_in_place_vel_th_. It is more important than the acceleration limits.
v_theta_samp = v_theta_samp > 0.0
? std::min( max_vel_th_, std::max( min_in_place_vel_th_, v_theta_samp ))
: std::max( min_vel_th_, std::min( -1.0 * min_in_place_vel_th_, v_theta_samp ));
//we still want to lay down the footprint of the robot and check if the action is legal
bool valid_cmd = tc_->checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw, 0.0, 0.0, v_theta_samp);
ROS_DEBUG("Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
if(valid_cmd){
cmd_vel.angular.z = v_theta_samp;
return true;
}
cmd_vel.angular.z = 0.0;
return false;
}
bool TrajectoryPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
//reset the global plan
global_plan_.clear();
global_plan_ = orig_global_plan;
//when we get a new plan, we also want to clear any latch we may have on goal tolerances
xy_tolerance_latch_ = false;
//reset the at goal flag
reached_goal_ = false;
return true;
}
bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
std::vector<geometry_msgs::PoseStamped> local_plan;
geometry_msgs::PoseStamped global_pose;
if (!costmap_ros_->getRobotPose(global_pose)) {
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
//get the global plan in our frame
if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
//now we'll prune the plan based on the position of the robot
if(prune_plan_)
prunePlan(global_pose, transformed_plan, global_plan_);
geometry_msgs::PoseStamped drive_cmds;
drive_cmds.header.frame_id = robot_base_frame_;
geometry_msgs::PoseStamped robot_vel;
odom_helper_.getRobotVel(robot_vel);
/* For timing uncomment
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
*/
//if the global plan passed in is empty... we won't do anything
if(transformed_plan.empty())
return false;
const geometry_msgs::PoseStamped& goal_point = transformed_plan.back();
//we assume the global goal is the last point in the global plan
const double goal_x = goal_point.pose.position.x;
const double goal_y = goal_point.pose.position.y;
const double yaw = tf2::getYaw(goal_point.pose.orientation);
double goal_th = yaw;
//check to see if we've reached the goal position
if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {
//if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
//just rotate in place
if (latch_xy_goal_tolerance_) {
xy_tolerance_latch_ = true;
}
double angle = getGoalOrientationAngleDifference(global_pose, goal_th);
//check to see if the goal orientation has been reached
if (fabs(angle) <= yaw_goal_tolerance_) {
//set the velocity command to zero
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
rotating_to_goal_ = false;
xy_tolerance_latch_ = false;
reached_goal_ = true;
} else {
//we need to call the next two lines to make sure that the trajectory
//planner updates its path distance and goal distance grids
tc_->updatePlan(transformed_plan);
Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
map_viz_.publishCostCloud(costmap_);
//copy over the odometry information
nav_msgs::Odometry base_odom;
odom_helper_.getOdom(base_odom);
//if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, rot_stopped_velocity_, trans_stopped_velocity_)) {
if ( ! stopWithAccLimits(global_pose, robot_vel, cmd_vel)) {
return false;
}
}
//if we're stopped... then we want to rotate to goal
else{
//set this so that we know its OK to be moving
rotating_to_goal_ = true;
if(!rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)) {
return false;
}
}
}
//publish an empty plan because we've reached our goal position
publishPlan(transformed_plan, g_plan_pub_);
publishPlan(local_plan, l_plan_pub_);
//we don't actually want to run the controller when we're just rotating to goal
return true;
}
tc_->updatePlan(transformed_plan);
//compute what trajectory to drive along
Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
map_viz_.publishCostCloud(costmap_);
/* For timing uncomment
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
ROS_INFO("Cycle time: %.9f", t_diff);
*/
//pass along drive commands
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);
//if we cannot move... tell someone
if (path.cost_ < 0) {
ROS_DEBUG_NAMED("trajectory_planner_ros",
"The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
local_plan.clear();
publishPlan(transformed_plan, g_plan_pub_);
publishPlan(local_plan, l_plan_pub_);
return false;
}
ROS_DEBUG_NAMED("trajectory_planner_ros", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
// Fill out the local plan
for (unsigned int i = 0; i < path.getPointsSize(); ++i) {
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
geometry_msgs::PoseStamped pose;
pose.header.frame_id = global_frame_;
pose.header.stamp = ros::Time::now();
pose.pose.position.x = p_x;
pose.pose.position.y = p_y;
pose.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, p_th);
tf2::convert(q, pose.pose.orientation);
local_plan.push_back(pose);
}
//publish information to the visualizer
publishPlan(transformed_plan, g_plan_pub_);
publishPlan(local_plan, l_plan_pub_);
return true;
}
bool TrajectoryPlannerROS::checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
geometry_msgs::PoseStamped global_pose;
if(costmap_ros_->getRobotPose(global_pose)){
if(update_map){
//we need to give the planne some sort of global plan, since we're only checking for legality
//we'll just give the robots current position
std::vector<geometry_msgs::PoseStamped> plan;
plan.push_back(global_pose);
tc_->updatePlan(plan, true);
}
//copy over the odometry information
nav_msgs::Odometry base_odom;
{
boost::recursive_mutex::scoped_lock lock(odom_lock_);
base_odom = base_odom_;
}
return tc_->checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation),
base_odom.twist.twist.linear.x,
base_odom.twist.twist.linear.y,
base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
}
ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
return false;
}
double TrajectoryPlannerROS::scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
// Copy of checkTrajectory that returns a score instead of True / False
geometry_msgs::PoseStamped global_pose;
if(costmap_ros_->getRobotPose(global_pose)){
if(update_map){
//we need to give the planne some sort of global plan, since we're only checking for legality
//we'll just give the robots current position
std::vector<geometry_msgs::PoseStamped> plan;
plan.push_back(global_pose);
tc_->updatePlan(plan, true);
}
//copy over the odometry information
nav_msgs::Odometry base_odom;
{
boost::recursive_mutex::scoped_lock lock(odom_lock_);
base_odom = base_odom_;
}
return tc_->scoreTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation),
base_odom.twist.twist.linear.x,
base_odom.twist.twist.linear.y,
base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
}
ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
return -1.0;
}
bool TrajectoryPlannerROS::isGoalReached() {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
//return flag set in controller
return reached_goal_;
}
};

18
navigations/base_local_planner/src/twirling_cost_function.cpp Executable file
View File

@@ -0,0 +1,18 @@
/*
* twirling_cost_function.cpp
*
* Created on: Apr 20, 2016
* Author: Morgan Quigley
*/
#include <base_local_planner/twirling_cost_function.h>
#include <math.h>
namespace base_local_planner {
double TwirlingCostFunction::scoreTrajectory(Trajectory &traj) {
return fabs(traj.thetav_); // add cost for making the robot spin
}
} /* namespace base_local_planner */

314
navigations/base_local_planner/src/voxel_grid_model.cpp Executable file
View File

@@ -0,0 +1,314 @@
/*********************************************************************
*
* 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 <base_local_planner/voxel_grid_model.h>
#include <sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace costmap_2d;
namespace base_local_planner {
VoxelGridModel::VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range) :
obstacle_grid_(size_x, size_y, size_z), xy_resolution_(xy_resolution), z_resolution_(z_resolution),
origin_x_(origin_x), origin_y_(origin_y), origin_z_(origin_z),
max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range) {}
double VoxelGridModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
if(footprint.size() < 3)
return -1.0;
//now we really have to lay down the footprint in the costmap grid
unsigned int x0, x1, y0, y1;
double line_cost = 0.0;
//we need to rasterize each line in the footprint
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//get the cell coord of the first point
if(!worldToMap2D(footprint[i].x, footprint[i].y, x0, y0))
return -1.0;
//get the cell coord of the second point
if(!worldToMap2D(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
return -1.0;
line_cost = lineCost(x0, x1, y0, y1);
//if there is an obstacle that hits the line... we know that we can return false right away
if(line_cost < 0)
return -1.0;
}
//we also need to connect the first point in the footprint to the last point
//get the cell coord of the last point
if(!worldToMap2D(footprint.back().x, footprint.back().y, x0, y0))
return -1.0;
//get the cell coord of the first point
if(!worldToMap2D(footprint.front().x, footprint.front().y, x1, y1))
return -1.0;
line_cost = lineCost(x0, x1, y0, y1);
if(line_cost < 0)
return -1.0;
//if all line costs are legal... then we can return that the footprint is legal
return 0.0;
}
//calculate the cost of a ray-traced line
double VoxelGridModel::lineCost(int x0, int x1,
int y0, int y1){
//Bresenham Ray-Tracing
int deltax = abs(x1 - x0); // The difference between the x's
int deltay = abs(y1 - y0); // The difference between the y's
int x = x0; // Start x off at the first pixel
int y = y0; // Start y off at the first pixel
int xinc1, xinc2, yinc1, yinc2;
int den, num, numadd, numpixels;
double line_cost = 0.0;
double point_cost = -1.0;
if (x1 >= x0) // The x-values are increasing
{
xinc1 = 1;
xinc2 = 1;
}
else // The x-values are decreasing
{
xinc1 = -1;
xinc2 = -1;
}
if (y1 >= y0) // The y-values are increasing
{
yinc1 = 1;
yinc2 = 1;
}
else // The y-values are decreasing
{
yinc1 = -1;
yinc2 = -1;
}
if (deltax >= deltay) // There is at least one x-value for every y-value
{
xinc1 = 0; // Don't change the x when numerator >= denominator
yinc2 = 0; // Don't change the y for every iteration
den = deltax;
num = deltax / 2;
numadd = deltay;
numpixels = deltax; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2 = 0; // Don't change the x for every iteration
yinc1 = 0; // Don't change the y when numerator >= denominator
den = deltay;
num = deltay / 2;
numadd = deltax;
numpixels = deltay; // There are more y-values than x-values
}
for (int curpixel = 0; curpixel <= numpixels; curpixel++)
{
point_cost = pointCost(x, y); //Score the current point
if(point_cost < 0)
return -1;
if(line_cost < point_cost)
line_cost = point_cost;
num += numadd; // Increase the numerator by the top of the fraction
if (num >= den) // Check if numerator >= denominator
{
num -= den; // Calculate the new numerator value
x += xinc1; // Change the x as appropriate
y += yinc1; // Change the y as appropriate
}
x += xinc2; // Change the x as appropriate
y += yinc2; // Change the y as appropriate
}
return line_cost;
}
double VoxelGridModel::pointCost(int x, int y){
//if the cell is in an obstacle the path is invalid
if(obstacle_grid_.getVoxelColumn(x, y)){
return -1;
}
return 1;
}
void VoxelGridModel::updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
//remove points in the laser scan boundry
for(unsigned int i = 0; i < laser_scans.size(); ++i)
removePointsInScanBoundry(laser_scans[i], 10.0);
//iterate through all observations and update the grid
for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
const Observation& obs = *it;
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
geometry_msgs::Point32 pt;
for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
//filter out points that are too high
if(*iter_z > max_z_)
continue;
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
+ (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
+ (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
if(sq_dist >= sq_obstacle_range_)
continue;
//insert the point
pt.x = *iter_x;
pt.y = *iter_y;
pt.z = *iter_z;
insert(pt);
}
}
//remove the points that are in the footprint of the robot
//removePointsInPolygon(footprint);
}
void VoxelGridModel::removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range){
if(laser_scan.cloud.points.size() == 0)
return;
unsigned int sensor_x, sensor_y, sensor_z;
double ox = laser_scan.origin.x;
double oy = laser_scan.origin.y;
double oz = laser_scan.origin.z;
if(!worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
return;
for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
double wpx = laser_scan.cloud.points[i].x;
double wpy = laser_scan.cloud.points[i].y;
double wpz = laser_scan.cloud.points[i].z;
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = raytrace_range / distance;
scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
//we can only raytrace to a maximum z height
if(wpz >= max_z_){
//we know we want the vector's z value to be max_z
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = (max_z_ - .01 - oz) / c;
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
}
//and we can only raytrace down to the floor
else if(wpz < 0.0){
//we know we want the vector's z value to be 0.0
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = (0.0 - oz) / c;
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
}
unsigned int point_x, point_y, point_z;
if(worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
obstacle_grid_.clearVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
}
}
}
void VoxelGridModel::getPoints(sensor_msgs::PointCloud2& cloud){
size_t n = 0;
for(unsigned int i = 0; i < obstacle_grid_.sizeX(); ++i)
for(unsigned int j = 0; j < obstacle_grid_.sizeY(); ++j)
for(unsigned int k = 0; k < obstacle_grid_.sizeZ(); ++k)
if(obstacle_grid_.getVoxel(i, j, k))
++n;
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(n);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
for(unsigned int i = 0; i < obstacle_grid_.sizeX(); ++i){
for(unsigned int j = 0; j < obstacle_grid_.sizeY(); ++j){
for(unsigned int k = 0; k < obstacle_grid_.sizeZ(); ++k){
if(obstacle_grid_.getVoxel(i, j, k)){
double wx, wy, wz;
mapToWorld3D(i, j, k, wx, wy, wz);
*iter_x = wx;
*iter_y = wy;
*iter_z = wz;
++iter_x;
++iter_y;
++iter_z;
}
}
}
}
}
};

145
navigations/base_local_planner/test/footprint_helper_test.cpp Executable file
View File

@@ -0,0 +1,145 @@
/*
* footprint_helper_test.cpp
*
* Created on: May 2, 2012
* Author: tkruse
*/
#include <gtest/gtest.h>
#include <vector>
#include <base_local_planner/footprint_helper.h>
#include <base_local_planner/map_grid.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/costmap_2d.h>
#include "wavefront_map_accessor.h"
namespace base_local_planner {
class FootprintHelperTest : public testing::Test {
public:
FootprintHelper fh;
FootprintHelperTest() {
}
virtual void TestBody(){}
void correctLineCells() {
std::vector<base_local_planner::Position2DInt> footprint;
fh.getLineCells(0, 10, 0, 10, footprint);
EXPECT_EQ(11, footprint.size());
EXPECT_EQ(footprint[0].x, 0);
EXPECT_EQ(footprint[0].y, 0);
EXPECT_EQ(footprint[5].x, 5);
EXPECT_EQ(footprint[5].y, 5);
EXPECT_EQ(footprint[10].x, 10);
EXPECT_EQ(footprint[10].y, 10);
}
void correctFootprint(){
MapGrid* mg = new MapGrid (10, 10);
WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
const costmap_2d::Costmap2D& map = *wa;
std::vector<geometry_msgs::Point> footprint_spec;
geometry_msgs::Point pt;
//create a square footprint
pt.x = 2;
pt.y = 2;
footprint_spec.push_back(pt);
pt.x = 2;
pt.y = -2;
footprint_spec.push_back(pt);
pt.x = -2;
pt.y = -2;
footprint_spec.push_back(pt);
pt.x = -2;
pt.y = 2;
footprint_spec.push_back(pt);
Eigen::Vector3f pos(4.5, 4.5, 0);
//just create a basic footprint
std::vector<base_local_planner::Position2DInt> footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
EXPECT_EQ(20, footprint.size());
//we expect the front line to be first
EXPECT_EQ(footprint[0].x, 6); EXPECT_EQ(footprint[0].y, 6);
EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);
//next the right line
EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);
//next the back line
EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);
//finally the left line
EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);
pos = Eigen::Vector3f(4.5, 4.5, M_PI_2);
//check that rotation of the footprint works
footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
//first the left line
EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);
//next the front line
EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);
//next the right line
EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);
//next the back line
EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
}
};
TEST(FootprintHelperTest, correctFootprint){
FootprintHelperTest tct;
tct.correctFootprint();
}
TEST(FootprintHelperTest, correctLineCells){
FootprintHelperTest tct;
tct.correctLineCells();
}
}

18
navigations/base_local_planner/test/gtest_main.cpp Executable file
View File

@@ -0,0 +1,18 @@
/*
* gtest_main.cpp
*
* Created on: Apr 6, 2012
* Author: tkruse
*/
#include <iostream>
#include <gtest/gtest.h>
int main(int argc, char **argv) {
std::cout << "Running main() from gtest_main.cc\n";
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

81
navigations/base_local_planner/test/line_iterator_test.cpp Executable file
View File

@@ -0,0 +1,81 @@
/*
* 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 <gtest/gtest.h>
#include "base_local_planner/line_iterator.h"
TEST( LineIterator, south )
{
base_local_planner::LineIterator line( 1, 2, 1, 4 );
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( 1, line.getX() );
EXPECT_EQ( 2, line.getY() );
line.advance();
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( 1, line.getX() );
EXPECT_EQ( 3, line.getY() );
line.advance();
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( 1, line.getX() );
EXPECT_EQ( 4, line.getY() );
line.advance();
EXPECT_FALSE( line.isValid() );
}
TEST( LineIterator, north_north_west )
{
base_local_planner::LineIterator line( 0, 0, -2, -4 );
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( 0, line.getX() );
EXPECT_EQ( 0, line.getY() );
line.advance();
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( -1, line.getX() );
EXPECT_EQ( -1, line.getY() );
line.advance();
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( -1, line.getX() );
EXPECT_EQ( -2, line.getY() );
line.advance();
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( -2, line.getX() );
EXPECT_EQ( -3, line.getY() );
line.advance();
EXPECT_TRUE( line.isValid() );
EXPECT_EQ( -2, line.getX() );
EXPECT_EQ( -4, line.getY() );
line.advance();
EXPECT_FALSE( line.isValid() );
}
int main( int argc, char **argv ) {
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS();
}

206
navigations/base_local_planner/test/map_grid_test.cpp Executable file
View File

@@ -0,0 +1,206 @@
/*
* map_grid_test.cpp
*
* Created on: May 2, 2012
* Author: tkruse
*/
#include <queue>
#include <gtest/gtest.h>
#include <base_local_planner/map_grid.h>
#include <base_local_planner/map_cell.h>
#include "wavefront_map_accessor.h"
namespace base_local_planner {
TEST(MapGridTest, initNull){
MapGrid map_grid;
EXPECT_EQ(0, map_grid.size_x_);
EXPECT_EQ(0, map_grid.size_y_);
}
TEST(MapGridTest, operatorBrackets){
MapGrid map_grid(10, 10);
map_grid(3, 5).target_dist = 5;
EXPECT_EQ(5, map_grid.getCell(3, 5).target_dist);
}
TEST(MapGridTest, copyConstructor){
MapGrid map_grid(10, 10);
map_grid(3, 5).target_dist = 5;
MapGrid map_grid2;
map_grid2 = map_grid;
EXPECT_EQ(5, map_grid(3, 5).target_dist);
}
TEST(MapGridTest, getIndex){
MapGrid map_grid(10, 10);
EXPECT_EQ(53, map_grid.getIndex(3, 5));
}
TEST(MapGridTest, reset){
MapGrid map_grid(10, 10);
map_grid(0, 0).target_dist = 1;
map_grid(0, 0).target_mark = true;
map_grid(0, 0).within_robot = true;
map_grid(3, 5).target_dist = 1;
map_grid(3, 5).target_mark = true;
map_grid(3, 5).within_robot = true;
map_grid(9, 9).target_dist = 1;
map_grid(9, 9).target_mark = true;
map_grid(9, 9).within_robot = true;
EXPECT_EQ(1, map_grid(0, 0).target_dist);
EXPECT_EQ(true, map_grid(0, 0).target_mark);
EXPECT_EQ(true, map_grid(0, 0).within_robot);
EXPECT_EQ(1, map_grid(3, 5).target_dist);
EXPECT_EQ(true, map_grid(3, 5).target_mark);
EXPECT_EQ(true, map_grid(3, 5).within_robot);
EXPECT_EQ(1, map_grid(9, 9).target_dist);
EXPECT_EQ(true, map_grid(9, 9).target_mark);
EXPECT_EQ(true, map_grid(9, 9).within_robot);
map_grid.resetPathDist();
EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(9, 9).target_dist);
EXPECT_EQ(false, map_grid(9, 9).target_mark);
EXPECT_EQ(false, map_grid(9, 9).within_robot);
EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(3, 5).target_dist);
EXPECT_EQ(false, map_grid(3, 5).target_mark);
EXPECT_EQ(false, map_grid(3, 5).within_robot);
EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(0, 0).target_dist);
EXPECT_EQ(false, map_grid(0, 0).target_mark);
EXPECT_EQ(false, map_grid(0, 0).within_robot);
}
TEST(MapGridTest, properGridConstruction){
MapGrid mg(10, 10);
MapCell mc;
for(int i = 0; i < 10; ++i){
for(int j = 0; j < 10; ++j){
EXPECT_FLOAT_EQ(mg(i, j).cx, i);
EXPECT_FLOAT_EQ(mg(i, j).cy, j);
}
}
}
TEST(MapGridTest, sizeCheck){
MapGrid mg(10, 10);
MapCell mc;
mg.sizeCheck(20, 25);
for(int i = 0; i < 20; ++i){
for(int j = 0; j < 25; ++j){
EXPECT_FLOAT_EQ(mg(i, j).cx, i);
EXPECT_FLOAT_EQ(mg(i, j).cy, j);
}
}
}
TEST(MapGridTest, adjustPlanEmpty){
MapGrid mg(10, 10);
const std::vector<geometry_msgs::PoseStamped> global_plan_in;
std::vector<geometry_msgs::PoseStamped> global_plan_out;
double resolution = 0;
mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
EXPECT_EQ(0, global_plan_out.size());
}
TEST(MapGridTest, adjustPlan){
MapGrid mg(10, 10);
std::vector<geometry_msgs::PoseStamped> global_plan_in;
std::vector<geometry_msgs::PoseStamped> global_plan_out;
double resolution = 1;
geometry_msgs::PoseStamped start;
start.pose.position.x = 1;
start.pose.position.y = 1;
geometry_msgs::PoseStamped end;
end.pose.position.x = 5;
end.pose.position.y = 5;
global_plan_in.push_back(start);
global_plan_in.push_back(end);
mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
EXPECT_EQ(1, global_plan_out[0].pose.position.x);
EXPECT_EQ(1, global_plan_out[0].pose.position.y);
EXPECT_EQ(5, global_plan_out.back().pose.position.x);
EXPECT_EQ(5, global_plan_out.back().pose.position.y);
for (unsigned int i = 1; i < global_plan_out.size(); ++i)
{
geometry_msgs::Point& p0 = global_plan_out[i - 1].pose.position;
geometry_msgs::Point& p1 = global_plan_out[i].pose.position;
double d = hypot(p0.x - p1.x, p0.y - p1.y);
EXPECT_LT(d, resolution);
}
}
TEST(MapGridTest, adjustPlan2){
std::vector<geometry_msgs::PoseStamped> base_plan, result;
// Push two points, at (0,0) and (0,1). Gap is 1 meter
base_plan.push_back(geometry_msgs::PoseStamped());
base_plan.push_back(geometry_msgs::PoseStamped());
base_plan.back().pose.position.y = 1.0;
// resolution >= 1, path won't change
MapGrid::adjustPlanResolution(base_plan, result, 2.0);
EXPECT_EQ(2, result.size());
result.clear();
MapGrid::adjustPlanResolution(base_plan, result, 1.0);
EXPECT_EQ(2, result.size());
result.clear();
// 0.5 <= resolution < 1.0, one point should be added in the middle
MapGrid::adjustPlanResolution(base_plan, result, 0.8);
EXPECT_EQ(3, result.size());
result.clear();
MapGrid::adjustPlanResolution(base_plan, result, 0.5);
EXPECT_EQ(3, result.size());
result.clear();
// 0.333 <= resolution < 0.5, two points should be added in the middle
MapGrid::adjustPlanResolution(base_plan, result, 0.34);
EXPECT_EQ(4, result.size());
result.clear();
// 0.25 <= resolution < 0.333, three points should be added in the middle
MapGrid::adjustPlanResolution(base_plan, result, 0.32);
EXPECT_EQ(5, result.size());
result.clear();
MapGrid::adjustPlanResolution(base_plan, result, 0.1);
EXPECT_EQ(11, result.size());
result.clear();
}
TEST(MapGridTest, distancePropagation){
MapGrid mg(10, 10);
WavefrontMapAccessor* wa = new WavefrontMapAccessor(&mg, .25);
std::queue<MapCell*> dist_queue;
mg.computeTargetDistance(dist_queue, *wa);
EXPECT_EQ(false, mg(0, 0).target_mark);
MapCell& mc = mg.getCell(0, 0);
mc.target_dist = 0.0;
mc.target_mark = true;
dist_queue.push(&mc);
mg.computeTargetDistance(dist_queue, *wa);
EXPECT_EQ(true, mg(0, 0).target_mark);
EXPECT_EQ(0.0, mg(0, 0).target_dist);
EXPECT_EQ(true, mg(1, 1).target_mark);
EXPECT_EQ(2.0, mg(1, 1).target_dist);
EXPECT_EQ(true, mg(0, 4).target_mark);
EXPECT_EQ(4.0, mg(0, 4).target_dist);
EXPECT_EQ(true, mg(4, 0).target_mark);
EXPECT_EQ(4.0, mg(4, 0).target_dist);
EXPECT_EQ(true, mg(9, 9).target_mark);
EXPECT_EQ(18.0, mg(9, 9).target_dist);
}
}

26
navigations/base_local_planner/test/trajectory_generator_test.cpp Executable file
View File

@@ -0,0 +1,26 @@
/*
* footprint_helper_test.cpp
*
* Created on: May 2, 2012
* Author: tkruse
*/
#include <gtest/gtest.h>
#include <vector>
#include <base_local_planner/simple_trajectory_generator.h>
namespace base_local_planner {
class TrajectoryGeneratorTest : public testing::Test {
public:
SimpleTrajectoryGenerator tg;
TrajectoryGeneratorTest() {
}
virtual void TestBody(){}
};
}

215
navigations/base_local_planner/test/utest.cpp Executable file
View File

@@ -0,0 +1,215 @@
/*********************************************************************
* 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.
*********************************************************************/
#include <gtest/gtest.h>
#include <iostream>
#include <vector>
#include <utility>
#include <base_local_planner/map_cell.h>
#include <base_local_planner/map_grid.h>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/trajectory_planner.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/costmap_2d.h>
#include <math.h>
#include <geometry_msgs/Point.h>
#include <base_local_planner/Position2DInt.h>
#include "wavefront_map_accessor.h"
using namespace std;
namespace base_local_planner {
class TrajectoryPlannerTest : public testing::Test {
public:
TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec);
void correctFootprint();
void footprintObstacles();
void checkGoalDistance();
void checkPathDistance();
virtual void TestBody(){}
MapGrid* map_;
WavefrontMapAccessor* wa;
CostmapModel cm;
TrajectoryPlanner tc;
};
TrajectoryPlannerTest::TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec)
: map_(g), wa(wave), cm(map), tc(cm, map, footprint_spec, 0.0, 1.0, 1.0, 1.0, 1.0, 2.0)
{}
void TrajectoryPlannerTest::footprintObstacles(){
//place an obstacle
map_->operator ()(4, 6).target_dist = 1;
wa->synchronize();
EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
Trajectory traj(0, 0, 0, 0.1, 30);
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
//we expect this path to hit the obstacle
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
//place a wall next to the footprint of the robot
tc.path_map_(7, 1).target_dist = 1;
tc.path_map_(7, 3).target_dist = 1;
tc.path_map_(7, 4).target_dist = 1;
tc.path_map_(7, 5).target_dist = 1;
tc.path_map_(7, 6).target_dist = 1;
tc.path_map_(7, 7).target_dist = 1;
wa->synchronize();
//try to rotate into it
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
//we expect this path to hit the obstacle
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
}
void TrajectoryPlannerTest::checkGoalDistance(){
//let's box a cell in and make sure that its distance gets set to max
map_->operator ()(1, 2).target_dist = 1;
map_->operator ()(1, 1).target_dist = 1;
map_->operator ()(1, 0).target_dist = 1;
map_->operator ()(2, 0).target_dist = 1;
map_->operator ()(3, 0).target_dist = 1;
map_->operator ()(3, 1).target_dist = 1;
map_->operator ()(3, 2).target_dist = 1;
map_->operator ()(2, 2).target_dist = 1;
wa->synchronize();
//set a goal
tc.path_map_.resetPathDist();
queue<MapCell*> target_dist_queue;
MapCell& current = tc.path_map_(4, 9);
current.target_dist = 0.0;
current.target_mark = true;
target_dist_queue.push(&current);
tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
//check the boxed in cell
EXPECT_FLOAT_EQ(100.0, tc.path_map_(2, 2).target_dist);
}
void TrajectoryPlannerTest::checkPathDistance(){
tc.path_map_.resetPathDist();
queue<MapCell*> target_dist_queue;
MapCell& current = tc.path_map_(4, 9);
current.target_dist = 0.0;
current.target_mark = true;
target_dist_queue.push(&current);
tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
//check the boxed in cell
EXPECT_FLOAT_EQ(tc.path_map_(2, 2).target_dist, 100.0);
}
TrajectoryPlannerTest* tct = NULL;
TrajectoryPlannerTest* setup_testclass_singleton() {
if (tct == NULL) {
MapGrid* mg = new MapGrid (10, 10);
WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
const costmap_2d::Costmap2D& map = *wa;
std::vector<geometry_msgs::Point> footprint_spec;
geometry_msgs::Point pt;
//create a square footprint
pt.x = 2;
pt.y = 2;
footprint_spec.push_back(pt);
pt.x = 2;
pt.y = -2;
footprint_spec.push_back(pt);
pt.x = -2;
pt.y = -2;
footprint_spec.push_back(pt);
pt.x = -2;
pt.y = 2;
footprint_spec.push_back(pt);
tct = new base_local_planner::TrajectoryPlannerTest(mg, wa, map, footprint_spec);
}
return tct;
}
//make sure that trajectories that intersect obstacles are invalidated
TEST(TrajectoryPlannerTest, footprintObstacles){
TrajectoryPlannerTest* tct = setup_testclass_singleton();
tct->footprintObstacles();
}
//make sure that goal distance is being computed as expected
TEST(TrajectoryPlannerTest, checkGoalDistance){
TrajectoryPlannerTest* tct = setup_testclass_singleton();
tct->checkGoalDistance();
}
//make sure that path distance is being computed as expected
TEST(TrajectoryPlannerTest, checkPathDistance){
TrajectoryPlannerTest* tct = setup_testclass_singleton();
tct->checkPathDistance();
}
}; //namespace

178
navigations/base_local_planner/test/velocity_iterator_test.cpp Executable file
View File

@@ -0,0 +1,178 @@
/*********************************************************************
* 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.
*********************************************************************/
#include <gtest/gtest.h>
#include <base_local_planner/velocity_iterator.h>
namespace base_local_planner {
TEST(VelocityIteratorTest, testsingle) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(0.0, 0.0, 1); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(1, i);
EXPECT_EQ(0, result[0]);
}
TEST(VelocityIteratorTest, testsingle_pos) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(2.2, 2.2, 1); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(1, i);
EXPECT_EQ(2.2, result[0]);
}
TEST(VelocityIteratorTest, testsingle_neg) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-3.3, -3.3, 1); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(1, i);
EXPECT_EQ(-3.3, result[0]);
}
TEST(VelocityIteratorTest, test1) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-30, 30, 1); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(3, i);
double expected [3]= {-30.0, 0.0, 30.0};
for (int j = 0; j < 3; ++j) {
EXPECT_EQ(expected[j], result[j]);
}
}
TEST(VelocityIteratorTest, test1_pos) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(10, 30, 1); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(2, i);
double expected [2]= {10.0, 30.0};
for (int j = 0; j < 2; ++j) {
EXPECT_EQ(expected[j], result[j]);
}
}
TEST(VelocityIteratorTest, test1_neg) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-30, -10, 1); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(2, i);
double expected [2]= {-30.0, -10.0};
for (int j = 0; j < 2; ++j) {
EXPECT_EQ(expected[j], result[j]);
}
}
TEST(VelocityIteratorTest, test3) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-30, 30, 3); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(3, i);
double expected [3]= {-30.0, 0.0, 30};
for (int j = 0; j < 3; ++j) {
EXPECT_EQ(expected[j], result[j]);
}
}
TEST(VelocityIteratorTest, test4) {
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-30, 30, 4); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(5, i);
double expected [5]= {-30.0, -10.0, 0.0, 10.0, 30};
for (int j = 0; j < 5; ++j) {
EXPECT_EQ(expected[j], result[j]);
}
}
TEST(VelocityIteratorTest, test_shifted) {
// test where zero is not in the middle
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-10, 50, 4); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(5, i);
double expected [5]= {-10.0, 0.0, 10.0, 30, 50};
for (int j = 0; j < 5; ++j) {
EXPECT_EQ(expected[j], result[j]);
}
}
TEST(VelocityIteratorTest, test_cranky) {
// test where one value is almost zero (nothing to do about that)
double result[5];
int i = 0;
for(base_local_planner::VelocityIterator x_it(-10.00001, 10, 3); !x_it.isFinished(); x_it++) {
result[i] = x_it.getVelocity();
i++;
}
EXPECT_EQ(4, i);
for (int j = 0; j < 5; ++j) {
double expected [5]= {-10.00001, -0.000005, 0.0, 10.0};
EXPECT_FLOAT_EQ(expected[j], result[j]);
}
}
} // namespace

50
navigations/base_local_planner/test/wavefront_map_accessor.h Executable file
View File

@@ -0,0 +1,50 @@
/*
* wavefront_map_accessor.h
*
* Created on: May 2, 2012
* Author: tkruse
*/
#ifndef WAVEFRONT_MAP_ACCESSOR_H_
#define WAVEFRONT_MAP_ACCESSOR_H_
#include <costmap_2d/cost_values.h>
namespace base_local_planner {
/**
* Map_grids rely on costmaps to identify obstacles. We need a costmap that we can easily manipulate for unit tests.
* This class has a grid map where we can set grid cell state, and a synchronize method to make the costmap match.
*/
class WavefrontMapAccessor : public costmap_2d::Costmap2D {
public:
WavefrontMapAccessor(MapGrid* map, double outer_radius)
: costmap_2d::Costmap2D(map->size_x_, map->size_y_, 1, 0, 0),
map_(map), outer_radius_(outer_radius) {
synchronize();
}
virtual ~WavefrontMapAccessor(){};
void synchronize(){
// Write Cost Data from the map
for(unsigned int x = 0; x < size_x_; x++){
for (unsigned int y = 0; y < size_y_; y++){
unsigned int ind = x + (y * size_x_);
if(map_->operator ()(x, y).target_dist == 1) {
costmap_[ind] = costmap_2d::LETHAL_OBSTACLE;
} else {
costmap_[ind] = 0;
}
}
}
}
private:
MapGrid* map_;
double outer_radius_;
};
}
#endif /* WAVEFRONT_MAP_ACCESSOR_H_ */