DuongTD/base_local_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd 2026-01-16 10:53:00 +07:00
commit 65edc7a386
50 changed files with 7845 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

214
CHANGELOG.rst Normal 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.

244
CMakeLists.txt Normal file
View File

@ -0,0 +1,244 @@
cmake_minimum_required(VERSION 3.0.2)
project(robot_base_local_planner VERSION 1.0.0 LANGUAGES CXX)
# ========================================================
# Detect Catkin or Standalone
# ========================================================
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building robot_base_local_planner with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building robot_base_local_planner with Standalone CMake")
endif()
# ========================================================
# C++ Standard
# ========================================================
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
add_compile_options(-Wall -Wextra -Wpedantic)
# ========================================================
# Dependencies common
# ========================================================
find_package(Boost REQUIRED COMPONENTS thread)
find_package(Eigen3 REQUIRED CONFIG)
if(NOT EIGEN3_INCLUDE_DIRS AND Eigen3_INCLUDE_DIRS)
set(EIGEN3_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
endif()
if(NOT EIGEN3_LIBRARIES AND Eigen3_LIBRARIES)
set(EIGEN3_LIBRARIES ${Eigen3_LIBRARIES})
endif()
# ========================================================
# Catkin
# ========================================================
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
robot_angles
robot_costmap_2d
robot_std_msgs
robot_geometry_msgs
robot_sensor_msgs
robot_nav_msgs
robot_cpp
tf3
robot_tf3_geometry_msgs
voxel_grid
data_convert
)
# generate_messages(
# DEPENDENCIES std_msgs
# )
endif()
# ========================================================
# Dynamic reconfigure (only for Catkin)
# ========================================================
if(BUILDING_WITH_CATKIN)
# set(CATKIN_ENV_HOOK_CMAKE_PATH_SETUP_CUSTOM_PYTHONPATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake/setup_custom_pythonpath.sh.in)
# generate_dynamic_reconfigure_options(
# cfg/BaseLocalPlanner.cfg
# )
catkin_package(
INCLUDE_DIRS include
LIBRARIES robot_base_local_planner
CATKIN_DEPENDS
robot_angles
robot_costmap_2d
robot_std_msgs
robot_geometry_msgs
robot_sensor_msgs
robot_nav_msgs
robot_cpp
tf3
robot_tf3_geometry_msgs
robot_voxel_grid
data_convert
DEPENDS Boost
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
else()
# =========================
# Standalone build
# =========================
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
endif()
# ========================================================
# Check headers
# ========================================================
include(CheckIncludeFile)
check_include_file(sys/time.h HAVE_SYS_TIME_H)
if(HAVE_SYS_TIME_H)
add_definitions(-DHAVE_SYS_TIME_H)
endif()
# ========================================================
# Libraries
# ========================================================
add_library(robot_base_local_planner SHARED
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/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
)
if(BUILDING_WITH_CATKIN)
add_dependencies(robot_base_local_planner
# ${PROJECT_NAME}_gencfg
# ${PROJECT_NAME}_generate_messages_cpp
# nav_msgs_generate_messages_cpp
${catkin_EXPORTED_TARGETS}
)
endif()
target_include_directories(robot_base_local_planner
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(robot_base_local_planner
PUBLIC
${Boost_LIBRARIES}
Eigen3::Eigen
)
if(BUILDING_WITH_CATKIN)
target_link_libraries(robot_base_local_planner PUBLIC ${catkin_LIBRARIES})
endif()
# ========================================================
# Install
# ========================================================
if(BUILDING_WITH_CATKIN)
install(TARGETS
robot_base_local_planner
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
else()
set_target_properties(robot_base_local_planner PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
install(TARGETS
robot_base_local_planner
EXPORT ${PROJECT_NAME}-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake
NAMESPACE ${PROJECT_NAME}::
DESTINATION lib/cmake/${PROJECT_NAME}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include
)
install(FILES blp_plugin.xml
DESTINATION share/${PROJECT_NAME}
)
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Libraries: robot_base_local_planner")
message(STATUS "=================================")
endif()
# ========================================================
# Testing
# ========================================================
# if(BUILDING_WITH_CATKIN AND CATKIN_ENABLE_TESTING)
# find_package(rostest REQUIRED)
# 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
# robot_base_local_planner
# )
# catkin_add_gtest(line_iterator
# test/line_iterator_test.cpp
# )
# endif()

57
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
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"))

8
cmake/setup_custom_pythonpath.sh.in Normal file
View File

@ -0,0 +1,8 @@
#!/bin/bash
# Custom Python path setup for robot_base_local_planner
# This file is auto-generated by dynamic_reconfigure
if [ ! -z "${_CATKIN_SETUP_DIR}" ]; then
export PYTHONPATH="${_CATKIN_SETUP_DIR}/@CMAKE_INSTALL_PREFIX@/@CATKIN_PACKAGE_BIN_DESTINATION@:$PYTHONPATH"
fi

68
include/robot_base_local_planner/Position2DInt.h Normal file
View File

@ -0,0 +1,68 @@
#ifndef ROBOT_BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H
#define ROBOT_BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H
#include <string>
#include <vector>
#include <memory>
namespace robot_base_local_planner
{
template <class ContainerAllocator>
struct Position2DInt_
{
typedef Position2DInt_<ContainerAllocator> Type;
Position2DInt_()
: x(0)
, y(0) {
}
Position2DInt_(const ContainerAllocator& _alloc)
: x(0)
, y(0) {
(void)_alloc;
}
typedef int64_t _x_type;
_x_type x;
typedef int64_t _y_type;
_y_type y;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt_<ContainerAllocator> const> ConstPtr;
}; // struct Position2DInt_
typedef ::robot_base_local_planner::Position2DInt_<std::allocator<void> > Position2DInt;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt > Position2DIntPtr;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt const> Position2DIntConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_base_local_planner::Position2DInt_<ContainerAllocator1> & lhs, const ::robot_base_local_planner::Position2DInt_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_base_local_planner::Position2DInt_<ContainerAllocator1> & lhs, const ::robot_base_local_planner::Position2DInt_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_base_local_planner
#endif // BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H

102
include/robot_base_local_planner/costmap_model.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#define ROBOT_TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#include <robot_base_local_planner/world_model.h>
// For obstacle data access
#include <robot_costmap_2d/costmap_2d.h>
namespace robot_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 robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_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 robot_costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
}
#endif

87
include/robot_base_local_planner/footprint_helper.h Normal 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 ROBOT_FOOTPRINT_HELPER_H_
#define ROBOT_FOOTPRINT_HELPER_H_
#include <vector>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_geometry_msgs/Point.h>
#include <Eigen/Core>
#include <robot_base_local_planner/Position2DInt.h>
namespace robot_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<robot_base_local_planner::Position2DInt> getFootprintCells(
Eigen::Vector3f pos,
std::vector<robot_geometry_msgs::Point> footprint_spec,
const robot_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<robot_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<robot_base_local_planner::Position2DInt>& footprint);
};
} /* namespace robot_base_local_planner */
#endif /* FOOTPRINT_HELPER_H_ */

154
include/robot_base_local_planner/goal_functions.h Normal file
View File

@ -0,0 +1,154 @@
/*********************************************************************
*
* 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 ROBOT_BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#define ROBOT_BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#include <robot/robot.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/Point.h>
#include <tf3/buffer_core.h>
#include <string>
#include <cmath>
#include <robot_angles/angles.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <data_convert/data_convert.h>
namespace robot_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 robot_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 robot_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<robot_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 robot_geometry_msgs::PoseStamped& global_pose, std::vector<robot_geometry_msgs::PoseStamped>& plan, std::vector<robot_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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const robot_geometry_msgs::PoseStamped& global_robot_pose,
const robot_costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<robot_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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame,
robot_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_robot 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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const robot_costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
robot_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

143
include/robot_base_local_planner/line_iterator.h Normal 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 ROBOT_LINE_ITERATOR_H
#define ROBOT_LINE_ITERATOR_H
#include <stdlib.h>
namespace robot_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 robot_base_local_planner
#endif // LINE_ITERATOR_H

121
include/robot_base_local_planner/local_planner_limits.h Normal 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__ROBOT_LOCALPLANNERLIMITS_H__
#define __base_local_planner__ROBOT_LOCALPLANNERLIMITS_H__
#include <Eigen/Core>
namespace robot_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__

67
include/robot_base_local_planner/map_cell.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_MAP_CELL_H_
#define ROBOT_TRAJECTORY_ROLLOUT_MAP_CELL_H_
#include <robot_base_local_planner/trajectory_inc.h>
namespace robot_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
include/robot_base_local_planner/map_grid.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_MAP_GRID_H_
#define ROBOT_TRAJECTORY_ROLLOUT_MAP_GRID_H_
#include <vector>
#include <iostream>
#include <robot_base_local_planner/trajectory_inc.h>
#include <robot/console.h>
#include <robot_base_local_planner/map_cell.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace robot_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 robot_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<robot_geometry_msgs::PoseStamped>& global_plan_in,
std::vector<robot_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 robot_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 robot_costmap_2d::Costmap2D& costmap);
/**
* @brief Update what cells are considered path based on the global plan
*/
void setTargetCells(const robot_costmap_2d::Costmap2D& costmap, const std::vector<robot_geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Update what cell is considered the next local goal
*/
void setLocalGoal(const robot_costmap_2d::Costmap2D& costmap,
const std::vector<robot_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
include/robot_base_local_planner/map_grid_cost_function.h Normal 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 ROBOT_MAP_GRID_COST_FUNCTION_H_
#define ROBOT_MAP_GRID_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_base_local_planner/map_grid.h>
namespace robot_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_robot 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 robot_base_local_planner::TrajectoryCostFunction {
public:
MapGridCostFunction(robot_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<robot_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<robot_geometry_msgs::PoseStamped> target_poses_;
robot_costmap_2d::Costmap2D* costmap_;
robot_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 robot_base_local_planner */
#endif /* MAP_GRID_COST_FUNCTION_H_ */

71
include/robot_base_local_planner/map_grid_visualizer.h Normal 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 ROBOT_MAP_GRID_VISUALIZER_H_
#define ROBOT_MAP_GRID_VISUALIZER_H_
#include <robot/robot.h>
#include <robot_base_local_planner/map_grid.h>
#include <robot_costmap_2d/costmap_2d.h>
namespace robot_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.
*/
robot_sensor_msgs::PointCloud2 publishCostCloud(const robot_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
robot::NodeHandle ns_nh_;
std::string frame_id_;
// ros::Publisher pub_;
};
};
#endif

89
include/robot_base_local_planner/obstacle_cost_function.h Normal 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 ROBOT_OBSTACLE_COST_FUNCTION_H_
#define ROBOT_OBSTACLE_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <robot_base_local_planner/costmap_model.h>
#include <robot_costmap_2d/costmap_2d.h>
namespace robot_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(robot_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<robot_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<robot_geometry_msgs::Point> footprint_spec,
robot_costmap_2d::Costmap2D* costmap,
robot_base_local_planner::WorldModel* world_model);
private:
robot_costmap_2d::Costmap2D* costmap_;
std::vector<robot_geometry_msgs::Point> footprint_spec_;
robot_base_local_planner::WorldModel* world_model_;
double max_trans_vel_;
bool sum_scores_;
//footprint scaling with velocity;
double max_scaling_factor_, scaling_speed_;
};
} /* namespace robot_base_local_planner */
#endif /* OBSTACLE_COST_FUNCTION_H_ */

89
include/robot_base_local_planner/oscillation_cost_function.h Normal 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 ROBOT_OSCILLATION_COST_FUNCTION_H_
#define ROBOT_OSCILLATION_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <Eigen/Core>
namespace robot_base_local_planner {
class OscillationCostFunction: public robot_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, robot_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(robot_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 robot_base_local_planner */
#endif /* OSCILLATION_COST_FUNCTION_H_ */

57
include/robot_base_local_planner/planar_laser_scan.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#define ROBOT_TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#include <robot_geometry_msgs/Point32.h>
#include <robot_sensor_msgs/PointCloud.h>
namespace robot_base_local_planner {
/**
* @class PlanarLaserScan
* @brief Stores a scan from a planar laser that can be used to clear freespace
*/
class PlanarLaserScan {
public:
PlanarLaserScan() {}
robot_geometry_msgs::Point32 origin;
robot_sensor_msgs::PointCloud cloud;
double angle_min, angle_max, angle_increment;
};
}
#endif

326
include/robot_base_local_planner/point_grid.h Normal 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 ROBOT_POINT_GRID_H_
#define ROBOT_POINT_GRID_H_
#include <vector>
#include <list>
#include <cfloat>
#include <robot_geometry_msgs/Point.h>
#include <robot_costmap_2d/observation.h>
#include <robot_base_local_planner/world_model.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace robot_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, robot_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 robot_geometry_msgs::Point& lower_left, const robot_geometry_msgs::Point& upper_right, std::vector< std::list<robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_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<robot_geometry_msgs::Point>& footprint,
const std::vector<robot_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(robot_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, robot_geometry_msgs::Point& lower_left, robot_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 robot_geometry_msgs::Point32& pt1, const robot_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 robot_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 robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_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 robot_geometry_msgs::Point32& v1, const robot_geometry_msgs::Point32& v2,
const robot_geometry_msgs::Point32& u1, const robot_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 robot_geometry_msgs::Point& v1, const robot_geometry_msgs::Point& v2,
const robot_geometry_msgs::Point& u1, const robot_geometry_msgs::Point& u2,
robot_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 robot_geometry_msgs::Point32& pt, const std::vector<robot_geometry_msgs::Point>& poly);
/**
* @brief Insert a point into the point grid
* @param pt The point to be inserted
*/
void insert(const robot_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 robot_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 robot_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<robot_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 robot_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(robot_sensor_msgs::PointCloud2& cloud);
private:
double resolution_; ///< @brief The resolution of the grid in meters/cell
robot_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<robot_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<robot_geometry_msgs::Point32>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
};
}
#endif

64
include/robot_base_local_planner/prefer_forward_cost_function.h Normal 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 ROBOT_PREFER_FORWARD_COST_FUNCTION_H_
#define ROBOT_PREFER_FORWARD_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
namespace robot_base_local_planner {
class PreferForwardCostFunction: public robot_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 robot_base_local_planner */
#endif /* PREFER_FORWARD_COST_FUNCTION_H_ */

109
include/robot_base_local_planner/simple_scored_sampling_planner.h Normal 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 ROBOT_SIMPLE_SCORED_SAMPLING_PLANNER_H_
#define ROBOT_SIMPLE_SCORED_SAMPLING_PLANNER_H_
#include <vector>
#include <robot_base_local_planner/trajectory.h>
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <robot_base_local_planner/trajectory_sample_generator.h>
#include <robot_base_local_planner/trajectory_search.h>
namespace robot_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 robot_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
include/robot_base_local_planner/simple_trajectory_generator.h Normal 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 ROBOT_SIMPLE_TRAJECTORY_GENERATOR_H_
#define ROBOT_SIMPLE_TRAJECTORY_GENERATOR_H_
#include <robot_base_local_planner/trajectory_sample_generator.h>
#include <robot_base_local_planner/local_planner_limits.h>
#include <Eigen/Core>
namespace robot_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 robot_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,
robot_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,
robot_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,
robot_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_;
robot_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 robot_base_local_planner */
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */

118
include/robot_base_local_planner/trajectory.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#define ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#include <vector>
namespace robot_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
include/robot_base_local_planner/trajectory_cost_function.h Normal 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 ROBOT_TRAJECTORYCOSTFUNCTION_H_
#define ROBOT_TRAJECTORYCOSTFUNCTION_H_
#include <robot_base_local_planner/trajectory.h>
namespace robot_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
include/robot_base_local_planner/trajectory_inc.h Normal 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 ROBOT_TRAJECTORY_INC_H_
#define ROBOT_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
include/robot_base_local_planner/trajectory_planner.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#define ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#include <vector>
#include <cmath>
//for obstacle data access
#include <robot_costmap_2d/robot_costmap_2d.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_base_local_planner/footprint_helper.h>
#include <robot_base_local_planner/world_model.h>
#include <robot_base_local_planner/trajectory.h>
#include <robot_base_local_planner/Position2DInt.h>
#include <robot_base_local_planner/BaseLocalPlannerConfig.h>
//we'll take in a path as a vector of poses
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Point.h>
//for creating a local cost grid
#include <robot_base_local_planner/map_cell.h>
#include <robot_base_local_planner/map_grid.h>
namespace robot_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 robot_costmap_2d::Costmap2D& costmap,
std::vector<robot_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 robot_geometry_msgs::PoseStamped& global_pose,
robot_geometry_msgs::PoseStamped& global_vel, robot_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<robot_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<robot_geometry_msgs::Point> footprint ) { footprint_spec_ = footprint; }
/** @brief Return the footprint specification of the robot. */
robot_geometry_msgs::Polygon getFootprintPolygon() const { return robot_costmap_2d::toPolygon(footprint_spec_); }
std::vector<robot_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);
robot_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 robot_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<robot_geometry_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
std::vector<robot_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

74
include/robot_base_local_planner/trajectory_sample_generator.h Normal 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 ROBOT_TRAJECTORY_SAMPLE_GENERATOR_H_
#define ROBOT_TRAJECTORY_SAMPLE_GENERATOR_H_
#include <robot_base_local_planner/trajectory.h>
namespace robot_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
include/robot_base_local_planner/trajectory_search.h Normal 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 ROBOT_TRAJECTORY_SEARCH_H_
#define ROBOT_TRAJECTORY_SEARCH_H_
#include <robot_base_local_planner/trajectory.h>
namespace robot_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
include/robot_base_local_planner/twirling_cost_function.h Normal 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 ROBOT_TWIRLING_COST_FUNCTION_H
#define ROBOT_TWIRLING_COST_FUNCTION_H
#include <robot_base_local_planner/trajectory_cost_function.h>
namespace robot_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 robot_base_local_planner::TrajectoryCostFunction {
public:
TwirlingCostFunction() {}
~TwirlingCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
};
} /* namespace robot_base_local_planner */
#endif /* TWIRLING_COST_FUNCTION_H_ */

99
include/robot_base_local_planner/velocity_iterator.h Normal 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 ROBOT_DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#define ROBOT_DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#include <algorithm>
#include <cmath>
#include <vector>
namespace robot_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
include/robot_base_local_planner/voxel_grid_model.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#define ROBOT_TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#include <vector>
#include <list>
#include <cfloat>
#include <robot_geometry_msgs/Point.h>
#include <robot_costmap_2d/observation.h>
#include <robot_base_local_planner/world_model.h>
//voxel grid stuff
#include <robot_voxel_grid/voxel_grid.h>
namespace robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_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<robot_geometry_msgs::Point>& footprint,
const std::vector<robot_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(robot_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 robot_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);
}
robot_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
include/robot_base_local_planner/world_model.h Normal 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 ROBOT_TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#define ROBOT_TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#include <vector>
#include <robot_costmap_2d/observation.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_base_local_planner/planar_laser_scan.h>
namespace robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius) = 0;
double footprintCost(double x, double y, double theta, const std::vector<robot_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<robot_geometry_msgs::Point> oriented_footprint;
for(unsigned int i = 0; i < footprint_spec.size(); ++i){
robot_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);
}
robot_geometry_msgs::Point robot_position;
robot_position.x = x;
robot_position.y = y;
if(inscribed_radius==0.0){
robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_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

35
package.xml Normal file
View File

@ -0,0 +1,35 @@
<?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>robot_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/robot_base_local_planner</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<depend>robot_tf3_geometry_msgs</depend>
<depend>robot_angles</depend>
<depend>robot_costmap_2d</depend>
<depend>eigen</depend>
<depend>robot_geometry_msgs</depend>
<depend>robot_nav_msgs</depend>
<depend>robot_sensor_msgs</depend>
<depend>robot_std_msgs</depend>
<depend>robot_cpp</depend>
<depend>tf3</depend>
<depend>robot_visualization_msgs</depend>
<depend>robot_voxel_grid</depend>
<depend>data_convert</depend>
</package>

145
src/costmap_model.cpp Normal 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 <robot_base_local_planner/line_iterator.h>
#include <robot_base_local_planner/costmap_model.h>
#include <robot_costmap_2d/cost_values.h>
using namespace std;
using namespace robot_costmap_2d;
namespace robot_base_local_planner {
CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
double CostmapModel::footprintCost(const robot_geometry_msgs::Point& position, const std::vector<robot_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
src/footprint_helper.cpp Normal 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 <robot_base_local_planner/footprint_helper.h>
namespace robot_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<robot_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;
robot_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<robot_base_local_planner::Position2DInt>& footprint){
//quick bubble sort to sort pts by x
robot_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;
robot_base_local_planner::Position2DInt min_pt;
robot_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<robot_base_local_planner::Position2DInt> FootprintHelper::getFootprintCells(
Eigen::Vector3f pos,
std::vector<robot_geometry_msgs::Point> footprint_spec,
const robot_costmap_2d::Costmap2D& costmap,
bool fill){
double x_i = pos[0];
double y_i = pos[1];
double theta_i = pos[2];
std::vector<robot_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 robot_base_local_planner */

255
src/goal_functions.cpp Normal file
View File

@ -0,0 +1,255 @@
/*********************************************************************
*
* 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 <robot_base_local_planner/goal_functions.h>
#include <tf3/LinearMath/Matrix3x3.h>
#include <tf3/utils.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#ifdef _MSC_VER
#define GOAL_ATTRIBUTE_UNUSED
#else
#define GOAL_ATTRIBUTE_UNUSED __attribute__ ((unused))
#endif
namespace robot_base_local_planner {
double getGoalPositionDistance(const robot_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 robot_geometry_msgs::PoseStamped& global_pose, double goal_th) {
double yaw = data_convert::getYaw(global_pose.pose.orientation);
return robot_angles::shortest_angular_distance(yaw, goal_th);
}
// void publishPlan(const std::vector<robot_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 robot_geometry_msgs::PoseStamped& global_pose, std::vector<robot_geometry_msgs::PoseStamped>& plan, std::vector<robot_geometry_msgs::PoseStamped>& global_plan){
if (static_cast<int>(global_plan.size()) < static_cast<int>(plan.size()))
{
robot::log_error("[FATAL] global_plan.size() < plan.size(): %d < %d",
static_cast<int>(global_plan.size()), static_cast<int>(plan.size()) );
std::abort();
}
std::vector<robot_geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<robot_geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const robot_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){
robot::log_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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const robot_geometry_msgs::PoseStamped& global_pose,
const robot_costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<robot_geometry_msgs::PoseStamped>& transformed_plan){
transformed_plan.clear();
if (global_plan.empty()) {
robot::log_error("Received plan with zero length");
return false;
}
const robot_geometry_msgs::PoseStamped& plan_pose = global_plan[0];
try {
// get plan_to_global_transform from plan frame to global_frame
tf3::TransformStampedMsg plan_to_global_transform = tf.lookupTransform(global_frame,
plan_pose.header.frame_id, tf3::Time());
//let's get the pose of the robot in the frame of the plan
robot_geometry_msgs::PoseStamped robot_pose;
// tf3.transform(global_pose, robot_pose, plan_pose.header.frame_id);
tf3::TransformStampedMsg transform = tf.lookupTransform(plan_pose.header.frame_id,
global_pose.header.frame_id, tf3::Time());
tf3::doTransform(global_pose, robot_pose, transform);
//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;
}
robot_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 robot_geometry_msgs::PoseStamped& pose = global_plan[i];
tf3::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(tf3::LookupException& ex) {
robot::log_error("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf3::ConnectivityException& ex) {
robot::log_error("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf3::ExtrapolationException& ex) {
robot::log_error("Extrapolation Error: %s\n", ex.what());
if (!global_plan.empty())
robot::log_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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, robot_geometry_msgs::PoseStamped &goal_pose) {
if (global_plan.empty())
{
robot::log_error("Received plan with zero length");
return false;
}
const robot_geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try{
// tf3::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));
tf3::TransformStampedMsg transform = tf.lookupTransform(global_frame,
plan_goal_pose.header.frame_id, tf3::Time());
tf3::doTransform(plan_goal_pose, goal_pose, transform);
}
catch(tf3::LookupException& ex) {
robot::log_error("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf3::ConnectivityException& ex) {
robot::log_error("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf3::ExtrapolationException& ex) {
robot::log_error("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
robot::log_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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const robot_costmap_2d::Costmap2D& costmap GOAL_ATTRIBUTE_UNUSED,
const std::string& global_frame,
robot_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
robot_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 = data_convert::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;
}
}

52
src/map_cell.cpp Normal 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 <robot_base_local_planner/map_cell.h>
namespace robot_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)
{}
}

313
src/map_grid.cpp Normal file
View File

@ -0,0 +1,313 @@
/*********************************************************************
* 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 <robot_base_local_planner/map_grid.h>
#include <robot_costmap_2d/cost_values.h>
using namespace std;
namespace robot_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
if (size_x_ == 0 || size_y_ == 0)
{
robot::log_error("[FATAL] Invalid grid size: %f x %f", size_x_, size_y_ );
std::abort();
}
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 robot_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 == robot_costmap_2d::LETHAL_OBSTACLE ||
cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == robot_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<robot_geometry_msgs::PoseStamped>& global_plan_in,
std::vector<robot_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) {
robot_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 robot_costmap_2d::Costmap2D& costmap,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<robot_geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size()) {
robot::log_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) != robot_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) {
robot::log_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 robot_costmap_2d::Costmap2D& costmap,
const std::vector<robot_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<robot_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) != robot_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) {
robot::log_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 robot_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
src/map_grid_cost_function.cpp Normal 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 <robot_base_local_planner/map_grid_cost_function.h>
namespace robot_base_local_planner {
MapGridCostFunction::MapGridCostFunction(robot_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<robot_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
// robot::log_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 robot_base_local_planner */

96
src/map_grid_visualizer.cpp Normal file
View File

@ -0,0 +1,96 @@
/*********************************************************************
* 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 <robot_base_local_planner/map_grid_visualizer.h>
#include <robot_base_local_planner/map_cell.h>
#include <vector>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
namespace robot_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_ = robot::NodeHandle("~/" + name_);
// pub_ = ns_nh_.advertise<robot_sensor_msgs::PointCloud2>("cost_cloud", 1);
}
robot_sensor_msgs::PointCloud2 MapGridVisualizer::publishCostCloud(const robot_costmap_2d::Costmap2D* costmap_p_) {
robot_sensor_msgs::PointCloud2 cost_cloud;
cost_cloud.header.frame_id = frame_id_;
cost_cloud.header.stamp = robot::Time::now();
robot_sensor_msgs::PointCloud2Modifier cloud_mod(cost_cloud);
cloud_mod.setPointCloud2Fields(7, "x", 1, robot_sensor_msgs::PointField::FLOAT32,
"y", 1, robot_sensor_msgs::PointField::FLOAT32,
"z", 1, robot_sensor_msgs::PointField::FLOAT32,
"path_cost", 1, robot_sensor_msgs::PointField::FLOAT32,
"goal_cost", 1, robot_sensor_msgs::PointField::FLOAT32,
"occ_cost", 1, robot_sensor_msgs::PointField::FLOAT32,
"total_cost", 1, robot_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);
robot_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);
robot::log_debug("Cost PointCloud published");
return cost_cloud;
}
};

152
src/obstacle_cost_function.cpp Normal 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 <robot_base_local_planner/obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <robot/console.h>
namespace robot_base_local_planner {
ObstacleCostFunction::ObstacleCostFunction(robot_costmap_2d::Costmap2D* costmap)
: costmap_(costmap), sum_scores_(false) {
if (costmap != NULL) {
world_model_ = new robot_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<robot_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
robot::log_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<robot_geometry_msgs::Point> footprint_spec,
robot_costmap_2d::Costmap2D* costmap,
robot_base_local_planner::WorldModel* world_model) {
std::vector<robot_geometry_msgs::Point> scaled_footprint;
for(unsigned int i = 0; i < footprint_spec.size(); ++i) {
robot_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 robot_base_local_planner */

178
src/oscillation_cost_function.cpp Normal 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 <robot_base_local_planner/oscillation_cost_function.h>
#include <cmath>
namespace robot_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, robot_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(robot_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 robot_base_local_planner */

556
src/point_grid.cpp Normal 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 <robot_base_local_planner/point_grid.h>
#include <robot/console.h>
#ifdef HAVE_SYS_TIME_H
#include <sys/time.h>
#endif
#include <math.h>
#include <cstdio>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace robot_costmap_2d;
namespace robot_base_local_planner {
PointGrid::PointGrid(double size_x, double size_y, double resolution, robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_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
robot_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
robot_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<robot_geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
for(list<robot_geometry_msgs::Point32>::iterator it = cell_points->begin(); it != cell_points->end(); ++it){
const robot_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 robot_geometry_msgs::Point32& pt, const std::vector<robot_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 robot_geometry_msgs::Point& lower_left, const robot_geometry_msgs::Point& upper_right,
vector< list<robot_geometry_msgs::Point32>* >& points){
points.clear();
//compute the other corners of the box so we can get cells indicies for them
robot_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<robot_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<robot_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 robot_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 robot_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<robot_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 robot_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
robot_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
robot_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<robot_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 robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
robot_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
robot_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<robot_geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
list<robot_geometry_msgs::Point32>::iterator it = cell_points->begin();
while(it != cell_points->end()){
const robot_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 robot_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(robot_sensor_msgs::PointCloud2& cloud){
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
size_t n = 0;
for(unsigned int i = 0; i < cells_.size(); ++i){
for(list<robot_geometry_msgs::Point32>::iterator it = cells_[i].begin(); it != cells_[i].end(); ++it){
++n;
}
}
modifier.resize(n);
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
for(unsigned int i = 0; i < cells_.size(); ++i){
for(list<robot_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<robot_geometry_msgs::Point> poly){
if(poly.size() == 0)
return;
robot_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);
}
robot::log_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<robot_geometry_msgs::Point32>* cell_points = points_[i];
if(cell_points != NULL){
list<robot_geometry_msgs::Point32>::iterator it = cell_points->begin();
while(it != cell_points->end()){
const robot_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 robot_geometry_msgs::Point& v1, const robot_geometry_msgs::Point& v2,
const robot_geometry_msgs::Point& u1, const robot_geometry_msgs::Point& u2, robot_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;
}
}

28
src/prefer_forward_cost_function.cpp Normal file
View File

@ -0,0 +1,28 @@
/*
* prefer_forward_cost_function.cpp
*
* Created on: Apr 4, 2012
* Author: tkruse
*/
#include <robot_base_local_planner/prefer_forward_cost_function.h>
#include <math.h>
namespace robot_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 robot_base_local_planner */

145
src/simple_scored_sampling_planner.cpp Normal 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 <robot_base_local_planner/simple_scored_sampling_planner.h>
#include <robot/console.h>
namespace robot_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) {
robot::log_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) {
robot::log_warning("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);
}
}
robot::log_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
src/simple_trajectory_generator.cpp Normal 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 <robot_base_local_planner/simple_trajectory_generator.h>
#include <cmath>
#include <robot_base_local_planner/velocity_iterator.h>
namespace robot_base_local_planner {
void SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
robot_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,
robot_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,
robot_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 robot_base_local_planner */

80
src/trajectory.cpp Normal 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 <robot_base_local_planner/trajectory.h>
namespace robot_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
src/trajectory_planner.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

18
src/twirling_cost_function.cpp Normal file
View File

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

314
src/voxel_grid_model.cpp Normal 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 <robot_base_local_planner/voxel_grid_model.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace robot_costmap_2d;
namespace robot_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 robot_geometry_msgs::Point& position, const std::vector<robot_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<robot_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 robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
robot_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(robot_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;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(n);
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_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;
}
}
}
}
}
};