commit 65edc7a38685cbb0a4c9eb093992fac5a9cde46a Author: duongtd Date: Fri Jan 16 10:53:00 2026 +0700 first commit diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..257807e --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,214 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package base_local_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.17.3 (2023-01-10) +------------------- +* [ROS-O] various patches (`#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 `_) (`#1204 `_) + Co-authored-by: Frank Höller +* 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 `_) +* Contributors: wjwagner + +1.17.0 (2020-04-02) +------------------- +* Merge pull request `#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 `_) +* Contributors: Sam Pfeiffer + +1.16.5 (2020-03-15) +------------------- +* [melodic] updated install for better portability. (`#973 `_) +* Contributors: Sean Yen + +1.16.4 (2020-03-04) +------------------- +* Fixes gdist- and pdist_scale node paramter names (`#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 `_ +* don't include a main() function in base_local_planner library (`#969 `_) +* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#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 `_) +* Merge pull request `#857 `_ from jspricke/add_include + Add missing header +* Add missing header +* [kinetic] Fix for adjusting plan resolution (`#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 `_ 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 `_ +* Switch to TF2 `#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 `_ +* fix param to min_in_place_vel_theta, closes `#487 `_ +* add const to getLocalPlane, fixes `#709 `_ +* Merge pull request `#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 `_ from ros-planning/email_update_lunar + update maintainer email (lunar) +* CostmapModel: Make lineCost and pointCost public (`#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 `_ 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 `_ +* Merge pull request `#596 `_ from ros-planning/lunar_548 +* Add cost function to prevent unnecessary spinning +* Fix CMakeLists + package.xmls (`#548 `_) +* add missing deps on libpcl +* import only PCL common +* pcl proagate -lQt5::Widgets flag so we need to find_package Qt5Widgets (`#578 `_) +* make rostest in CMakeLists optional (`ros/rosdistro#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 `_ 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 `_. +* 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 `_ 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. diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8ec20a9 --- /dev/null +++ b/CMakeLists.txt @@ -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 + $ + $ +) + +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() diff --git a/cfg/BaseLocalPlanner.cfg b/cfg/BaseLocalPlanner.cfg new file mode 100755 index 0000000..fcb7b3d --- /dev/null +++ b/cfg/BaseLocalPlanner.cfg @@ -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")) diff --git a/cfg/LocalPlannerLimits.cfg b/cfg/LocalPlannerLimits.cfg new file mode 100755 index 0000000..954982e --- /dev/null +++ b/cfg/LocalPlannerLimits.cfg @@ -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")) diff --git a/cmake/setup_custom_pythonpath.sh.in b/cmake/setup_custom_pythonpath.sh.in new file mode 100644 index 0000000..62c2ca2 --- /dev/null +++ b/cmake/setup_custom_pythonpath.sh.in @@ -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 diff --git a/include/robot_base_local_planner/Position2DInt.h b/include/robot_base_local_planner/Position2DInt.h new file mode 100644 index 0000000..e0202c4 --- /dev/null +++ b/include/robot_base_local_planner/Position2DInt.h @@ -0,0 +1,68 @@ +#ifndef ROBOT_BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H +#define ROBOT_BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H + + +#include +#include +#include + +namespace robot_base_local_planner +{ +template +struct Position2DInt_ +{ + typedef Position2DInt_ 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_ > Ptr; + typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt_ const> ConstPtr; + +}; // struct Position2DInt_ + +typedef ::robot_base_local_planner::Position2DInt_ > 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 +bool operator==(const ::robot_base_local_planner::Position2DInt_ & lhs, const ::robot_base_local_planner::Position2DInt_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y; +} + +template +bool operator!=(const ::robot_base_local_planner::Position2DInt_ & lhs, const ::robot_base_local_planner::Position2DInt_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_base_local_planner + +#endif // BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H \ No newline at end of file diff --git a/include/robot_base_local_planner/costmap_model.h b/include/robot_base_local_planner/costmap_model.h new file mode 100644 index 0000000..d428bb1 --- /dev/null +++ b/include/robot_base_local_planner/costmap_model.h @@ -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 +// For obstacle data access +#include + +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& 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 diff --git a/include/robot_base_local_planner/footprint_helper.h b/include/robot_base_local_planner/footprint_helper.h new file mode 100644 index 0000000..51bb3eb --- /dev/null +++ b/include/robot_base_local_planner/footprint_helper.h @@ -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 + +#include +#include +#include +#include + +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 getFootprintCells( + Eigen::Vector3f pos, + std::vector 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& 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& footprint); +}; + +} /* namespace robot_base_local_planner */ +#endif /* FOOTPRINT_HELPER_H_ */ diff --git a/include/robot_base_local_planner/goal_functions.h b/include/robot_base_local_planner/goal_functions.h new file mode 100644 index 0000000..5eb93a4 --- /dev/null +++ b/include/robot_base_local_planner/goal_functions.h @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +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& 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& plan, std::vector& 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& global_plan, + const robot_geometry_msgs::PoseStamped& global_robot_pose, + const robot_costmap_2d::Costmap2D& costmap, + const std::string& global_frame, + std::vector& 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& 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& 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 diff --git a/include/robot_base_local_planner/line_iterator.h b/include/robot_base_local_planner/line_iterator.h new file mode 100644 index 0000000..5d845dd --- /dev/null +++ b/include/robot_base_local_planner/line_iterator.h @@ -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 + +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 diff --git a/include/robot_base_local_planner/local_planner_limits.h b/include/robot_base_local_planner/local_planner_limits.h new file mode 100644 index 0000000..a447055 --- /dev/null +++ b/include/robot_base_local_planner/local_planner_limits.h @@ -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 + +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__ diff --git a/include/robot_base_local_planner/map_cell.h b/include/robot_base_local_planner/map_cell.h new file mode 100644 index 0000000..ec2ecb9 --- /dev/null +++ b/include/robot_base_local_planner/map_cell.h @@ -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 + +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 diff --git a/include/robot_base_local_planner/map_grid.h b/include/robot_base_local_planner/map_grid.h new file mode 100644 index 0000000..2d489ac --- /dev/null +++ b/include/robot_base_local_planner/map_grid.h @@ -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 +#include +#include +#include + +#include +#include +#include +#include + +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& global_plan_in, + std::vector& 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& 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& 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& global_plan); + + /** + * @brief Update what cell is considered the next local goal + */ + void setLocalGoal(const robot_costmap_2d::Costmap2D& costmap, + const std::vector& 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 map_; ///< @brief Storage for the MapCells + + }; +} + +#endif diff --git a/include/robot_base_local_planner/map_grid_cost_function.h b/include/robot_base_local_planner/map_grid_cost_function.h new file mode 100644 index 0000000..4bb5ced --- /dev/null +++ b/include/robot_base_local_planner/map_grid_cost_function.h @@ -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 + +#include +#include + +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 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 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_ */ diff --git a/include/robot_base_local_planner/map_grid_visualizer.h b/include/robot_base_local_planner/map_grid_visualizer.h new file mode 100644 index 0000000..ce647f2 --- /dev/null +++ b/include/robot_base_local_planner/map_grid_visualizer.h @@ -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 +#include +#include + +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 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 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 diff --git a/include/robot_base_local_planner/obstacle_cost_function.h b/include/robot_base_local_planner/obstacle_cost_function.h new file mode 100644 index 0000000..e6c4896 --- /dev/null +++ b/include/robot_base_local_planner/obstacle_cost_function.h @@ -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 + +#include +#include + +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 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 footprint_spec, + robot_costmap_2d::Costmap2D* costmap, + robot_base_local_planner::WorldModel* world_model); + +private: + robot_costmap_2d::Costmap2D* costmap_; + std::vector 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_ */ diff --git a/include/robot_base_local_planner/oscillation_cost_function.h b/include/robot_base_local_planner/oscillation_cost_function.h new file mode 100644 index 0000000..e2eb9bc --- /dev/null +++ b/include/robot_base_local_planner/oscillation_cost_function.h @@ -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 +#include + +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_ */ diff --git a/include/robot_base_local_planner/planar_laser_scan.h b/include/robot_base_local_planner/planar_laser_scan.h new file mode 100644 index 0000000..2821f47 --- /dev/null +++ b/include/robot_base_local_planner/planar_laser_scan.h @@ -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 +#include + +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 diff --git a/include/robot_base_local_planner/point_grid.h b/include/robot_base_local_planner/point_grid.h new file mode 100644 index 0000000..190505b --- /dev/null +++ b/include/robot_base_local_planner/point_grid.h @@ -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 +#include +#include +#include +#include +#include + +#include + +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* >& 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& 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& footprint, + const std::vector& observations, const std::vector& 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 + 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& 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 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 > 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* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation + }; +} +#endif diff --git a/include/robot_base_local_planner/prefer_forward_cost_function.h b/include/robot_base_local_planner/prefer_forward_cost_function.h new file mode 100644 index 0000000..731f90d --- /dev/null +++ b/include/robot_base_local_planner/prefer_forward_cost_function.h @@ -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 + +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_ */ diff --git a/include/robot_base_local_planner/simple_scored_sampling_planner.h b/include/robot_base_local_planner/simple_scored_sampling_planner.h new file mode 100644 index 0000000..3a4a24e --- /dev/null +++ b/include/robot_base_local_planner/simple_scored_sampling_planner.h @@ -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 +#include +#include +#include +#include + +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 gen_list, std::vector& 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* all_explored = 0); + + +private: + std::vector gen_list_; + std::vector critics_; + + int max_samples_; +}; + + + + +} // namespace + +#endif /* SIMPLE_SCORED_SAMPLING_PLANNER_H_ */ diff --git a/include/robot_base_local_planner/simple_trajectory_generator.h b/include/robot_base_local_planner/simple_trajectory_generator.h new file mode 100644 index 0000000..a6c375a --- /dev/null +++ b/include/robot_base_local_planner/simple_trajectory_generator.h @@ -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 +#include +#include + +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 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 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_ */ diff --git a/include/robot_base_local_planner/trajectory.h b/include/robot_base_local_planner/trajectory.h new file mode 100644 index 0000000..ab44836 --- /dev/null +++ b/include/robot_base_local_planner/trajectory.h @@ -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 + +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 x_pts_; ///< @brief The x points in the trajectory + std::vector y_pts_; ///< @brief The y points in the trajectory + std::vector th_pts_; ///< @brief The theta points in the trajectory + + }; +} +#endif diff --git a/include/robot_base_local_planner/trajectory_cost_function.h b/include/robot_base_local_planner/trajectory_cost_function.h new file mode 100644 index 0000000..9ecb326 --- /dev/null +++ b/include/robot_base_local_planner/trajectory_cost_function.h @@ -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 + +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_ */ diff --git a/include/robot_base_local_planner/trajectory_inc.h b/include/robot_base_local_planner/trajectory_inc.h new file mode 100644 index 0000000..18435f1 --- /dev/null +++ b/include/robot_base_local_planner/trajectory_inc.h @@ -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 + +#ifndef DBL_MAX /* Max decimal value of a double */ +#define DBL_MAX std::numeric_limits::max() // 1.7976931348623157e+308 +#endif + +#ifndef DBL_MIN //Min decimal value of a double +#define DBL_MIN std::numeric_limits::min() // 2.2250738585072014e-308 +#endif + +#endif diff --git a/include/robot_base_local_planner/trajectory_planner.h b/include/robot_base_local_planner/trajectory_planner.h new file mode 100644 index 0000000..5946f98 --- /dev/null +++ b/include/robot_base_local_planner/trajectory_planner.h @@ -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 +#include + +//for obstacle data access +#include +#include +#include + +#include +#include +#include +#include + +//we'll take in a path as a vector of poses +#include +#include + +//for creating a local cost grid +#include +#include + +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 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 y_vels = std::vector(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& 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 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 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 footprint_spec_; ///< @brief The footprint specification of the robot + + std::vector 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 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 diff --git a/include/robot_base_local_planner/trajectory_sample_generator.h b/include/robot_base_local_planner/trajectory_sample_generator.h new file mode 100644 index 0000000..05b9e33 --- /dev/null +++ b/include/robot_base_local_planner/trajectory_sample_generator.h @@ -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 + +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_ */ diff --git a/include/robot_base_local_planner/trajectory_search.h b/include/robot_base_local_planner/trajectory_search.h new file mode 100644 index 0000000..09b1b95 --- /dev/null +++ b/include/robot_base_local_planner/trajectory_search.h @@ -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 + +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* all_explored) = 0; + + virtual ~TrajectorySearch() {} + +protected: + TrajectorySearch() {} + +}; + + +} + +#endif /* TRAJECTORY_SEARCH_H_ */ diff --git a/include/robot_base_local_planner/twirling_cost_function.h b/include/robot_base_local_planner/twirling_cost_function.h new file mode 100644 index 0000000..09006bf --- /dev/null +++ b/include/robot_base_local_planner/twirling_cost_function.h @@ -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 + +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_ */ diff --git a/include/robot_base_local_planner/velocity_iterator.h b/include/robot_base_local_planner/velocity_iterator.h new file mode 100644 index 0000000..c6197a7 --- /dev/null +++ b/include/robot_base_local_planner/velocity_iterator.h @@ -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 +#include +#include + +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 samples_; + unsigned int current_index; + }; +} +#endif diff --git a/include/robot_base_local_planner/voxel_grid_model.h b/include/robot_base_local_planner/voxel_grid_model.h new file mode 100644 index 0000000..78e4a21 --- /dev/null +++ b/include/robot_base_local_planner/voxel_grid_model.h @@ -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 +#include +#include +#include +#include +#include + +//voxel grid stuff +#include + +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& 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& footprint, + const std::vector& observations, const std::vector& 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 diff --git a/include/robot_base_local_planner/world_model.h b/include/robot_base_local_planner/world_model.h new file mode 100644 index 0000000..fd309df --- /dev/null +++ b/include/robot_base_local_planner/world_model.h @@ -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 +#include +#include +#include +#include + +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& footprint, + double inscribed_radius, double circumscribed_radius) = 0; + + double footprintCost(double x, double y, double theta, const std::vector& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){ + + double cos_th = cos(theta); + double sin_th = sin(theta); + std::vector 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& 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 diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..59b56f7 --- /dev/null +++ b/package.xml @@ -0,0 +1,35 @@ + + + + robot_base_local_planner + 1.17.3 + + + 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 nav_core package. + + + Eitan Marder-Eppstein + Eric Perko + contradict@gmail.com + Michael Ferguson + David V. Lu!! + Aaron Hoy + BSD + http://wiki.ros.org/robot_base_local_planner + + catkin + + robot_tf3_geometry_msgs + robot_angles + robot_costmap_2d + eigen + robot_geometry_msgs + robot_nav_msgs + robot_sensor_msgs + robot_std_msgs + robot_cpp + tf3 + robot_visualization_msgs + robot_voxel_grid + data_convert + diff --git a/src/costmap_model.cpp b/src/costmap_model.cpp new file mode 100644 index 0000000..8f9b22b --- /dev/null +++ b/src/costmap_model.cpp @@ -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 +#include +#include + +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& 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; + } + +} diff --git a/src/footprint_helper.cpp b/src/footprint_helper.cpp new file mode 100644 index 0000000..5c24ab2 --- /dev/null +++ b/src/footprint_helper.cpp @@ -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 + +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& 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& 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 FootprintHelper::getFootprintCells( + Eigen::Vector3f pos, + std::vector 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 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 */ diff --git a/src/goal_functions.cpp b/src/goal_functions.cpp new file mode 100644 index 0000000..808d28d --- /dev/null +++ b/src/goal_functions.cpp @@ -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 +#include +#include +#include +#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& 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& plan, std::vector& global_plan){ + if (static_cast(global_plan.size()) < static_cast(plan.size())) + { + robot::log_error("[FATAL] global_plan.size() < plan.size(): %d < %d", + static_cast(global_plan.size()), static_cast(plan.size()) ); + std::abort(); + } + std::vector::iterator it = plan.begin(); + std::vector::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& global_plan, + const robot_geometry_msgs::PoseStamped& global_pose, + const robot_costmap_2d::Costmap2D& costmap, + const std::string& global_frame, + std::vector& 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& 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& 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; + } +} diff --git a/src/map_cell.cpp b/src/map_cell.cpp new file mode 100644 index 0000000..6c64d6b --- /dev/null +++ b/src/map_cell.cpp @@ -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 + +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) + {} +} diff --git a/src/map_grid.cpp b/src/map_grid.cpp new file mode 100644 index 0000000..9010cb0 --- /dev/null +++ b/src/map_grid.cpp @@ -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 +#include +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& global_plan_in, + std::vector& 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& global_plan) { + sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()); + + bool started_path = false; + + queue path_dist_queue; + + std::vector 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(¤t); + 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& global_plan) { + sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()); + + int local_goal_x = -1; + int local_goal_y = -1; + bool started_path = false; + + std::vector 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 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(¤t); + } + + computeTargetDistance(path_dist_queue, costmap); + } + + + + void MapGrid::computeTargetDistance(queue& 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); + } + } + } + } + } + +} diff --git a/src/map_grid_cost_function.cpp b/src/map_grid_cost_function.cpp new file mode 100644 index 0000000..3b8bf56 --- /dev/null +++ b/src/map_grid_cost_function.cpp @@ -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 + +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 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 */ diff --git a/src/map_grid_visualizer.cpp b/src/map_grid_visualizer.cpp new file mode 100644 index 0000000..0cb3d28 --- /dev/null +++ b/src/map_grid_visualizer.cpp @@ -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 +#include +#include + +#include +#include + +namespace robot_base_local_planner { + MapGridVisualizer::MapGridVisualizer() {} + + + void MapGridVisualizer::initialize(const std::string& name, std::string frame_id, boost::function cost_function) { + name_ = name; + frame_id_ = frame_id; + cost_function_ = cost_function; + + ns_nh_ = robot::NodeHandle("~/" + name_); + // pub_ = ns_nh_.advertise("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 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; + } +}; diff --git a/src/obstacle_cost_function.cpp b/src/obstacle_cost_function.cpp new file mode 100644 index 0000000..8c29a62 --- /dev/null +++ b/src/obstacle_cost_function.cpp @@ -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 +#include +#include +#include + +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 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 footprint_spec, + robot_costmap_2d::Costmap2D* costmap, + robot_base_local_planner::WorldModel* world_model) { + + std::vector 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 */ diff --git a/src/oscillation_cost_function.cpp b/src/oscillation_cost_function.cpp new file mode 100644 index 0000000..8fd38ba --- /dev/null +++ b/src/oscillation_cost_function.cpp @@ -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 + +#include + +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 */ diff --git a/src/point_grid.cpp b/src/point_grid.cpp new file mode 100644 index 0000000..23fda81 --- /dev/null +++ b/src/point_grid.cpp @@ -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 +#include +#ifdef HAVE_SYS_TIME_H +#include +#endif + +#include +#include +#include + +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& 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* cell_points = points_[i]; + if(cell_points != NULL){ + for(list::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& 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* >& 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 >::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& 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::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& footprint, + const vector& observations, const vector& 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::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 iter_x(cloud, "x"); + robot_sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + robot_sensor_msgs::PointCloud2ConstIterator 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* cell_points = points_[i]; + if(cell_points != NULL){ + list::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::iterator it = cells_[i].begin(); it != cells_[i].end(); ++it){ + ++n; + } + } + modifier.resize(n); + + robot_sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + robot_sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + robot_sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + for(unsigned int i = 0; i < cells_.size(); ++i){ + for(list::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 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* cell_points = points_[i]; + if(cell_points != NULL){ + list::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; + } + +} diff --git a/src/prefer_forward_cost_function.cpp b/src/prefer_forward_cost_function.cpp new file mode 100644 index 0000000..02c2e8c --- /dev/null +++ b/src/prefer_forward_cost_function.cpp @@ -0,0 +1,28 @@ +/* + * prefer_forward_cost_function.cpp + * + * Created on: Apr 4, 2012 + * Author: tkruse + */ + +#include + +#include + +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 */ diff --git a/src/simple_scored_sampling_planner.cpp b/src/simple_scored_sampling_planner.cpp new file mode 100644 index 0000000..ff59add --- /dev/null +++ b/src/simple_scored_sampling_planner.cpp @@ -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 + +#include + +namespace robot_base_local_planner { + + SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(std::vector gen_list, std::vector& 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::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* 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::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::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 diff --git a/src/simple_trajectory_generator.cpp b/src/simple_trajectory_generator.cpp new file mode 100644 index 0000000..6fbadc8 --- /dev/null +++ b/src/simple_trajectory_generator.cpp @@ -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 + +#include + +#include + +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 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 */ diff --git a/src/trajectory.cpp b/src/trajectory.cpp new file mode 100644 index 0000000..2b13dfe --- /dev/null +++ b/src/trajectory.cpp @@ -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 + +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(); + } +}; diff --git a/src/trajectory_planner.cpp b/src/trajectory_planner.cpp new file mode 100644 index 0000000..52624f3 --- /dev/null +++ b/src/trajectory_planner.cpp @@ -0,0 +1,1001 @@ +/********************************************************************* +* +* 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 +#include +#include +#include +#include +#include + + + +#include + +#include + +//for computing path distance +#include +#include +#include + +using namespace std; +using namespace robot_costmap_2d; + +namespace robot_base_local_planner{ + + void TrajectoryPlanner::reconfigure(BaseLocalPlannerConfig &cfg) + { + BaseLocalPlannerConfig config(cfg); + + boost::mutex::scoped_lock l(configuration_mutex_); + + acc_lim_x_ = config.acc_lim_x; + acc_lim_y_ = config.acc_lim_y; + acc_lim_theta_ = config.acc_lim_theta; + + max_vel_x_ = config.max_vel_x; + min_vel_x_ = config.min_vel_x; + + max_vel_th_ = config.max_vel_theta; + min_vel_th_ = config.min_vel_theta; + min_in_place_vel_th_ = config.min_in_place_vel_theta; + + sim_time_ = config.sim_time; + sim_granularity_ = config.sim_granularity; + angular_sim_granularity_ = config.angular_sim_granularity; + + path_distance_bias_ = config.path_distance_bias; + goal_distance_bias_ = config.goal_distance_bias; + occdist_scale_ = config.occdist_scale; + + if (meter_scoring_) { + //if we use meter scoring, then we want to multiply the biases by the resolution of the costmap + double resolution = costmap_.getResolution(); + goal_distance_bias_ *= resolution; + path_distance_bias_ *= resolution; + } + + oscillation_reset_dist_ = config.oscillation_reset_dist; + escape_reset_dist_ = config.escape_reset_dist; + escape_reset_theta_ = config.escape_reset_theta; + + vx_samples_ = config.vx_samples; + vtheta_samples_ = config.vtheta_samples; + + if (vx_samples_ <= 0) { + config.vx_samples = 1; + vx_samples_ = config.vx_samples; + robot::log_warning("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead"); + } + if(vtheta_samples_ <= 0) { + config.vtheta_samples = 1; + vtheta_samples_ = config.vtheta_samples; + robot::log_warning("You've specified that you don't want any samples in the theta dimension. We'll at least assume that you want to sample one value... so we're going to set vtheta_samples to 1 instead"); + } + + heading_lookahead_ = config.heading_lookahead; + + holonomic_robot_ = config.holonomic_robot; + + backup_vel_ = config.escape_vel; + + dwa_ = config.dwa; + + heading_scoring_ = config.heading_scoring; + heading_scoring_timestep_ = config.heading_scoring_timestep; + + simple_attractor_ = config.simple_attractor; + + //y-vels + string y_string = config.y_vels; + vector y_strs; + boost::split(y_strs, y_string, boost::is_any_of(", "), boost::token_compress_on); + + vectory_vels; + for(vector::iterator it=y_strs.begin(); it != y_strs.end(); ++it) { + istringstream iss(*it); + double temp; + iss >> temp; + y_vels.push_back(temp); + //ROS_INFO("Adding y_vel: %e", temp); + } + + y_vels_ = y_vels; + + } + + TrajectoryPlanner::TrajectoryPlanner(WorldModel& world_model, + const Costmap2D& costmap, + std::vector footprint_spec, + double acc_lim_x, double acc_lim_y, double acc_lim_theta, + double sim_time, double sim_granularity, + int vx_samples, int vtheta_samples, + double path_distance_bias, double goal_distance_bias, double occdist_scale, + double heading_lookahead, double oscillation_reset_dist, + double escape_reset_dist, double escape_reset_theta, + bool holonomic_robot, + double max_vel_x, double min_vel_x, + double max_vel_th, double min_vel_th, double min_in_place_vel_th, + double backup_vel, + bool dwa, bool heading_scoring, double heading_scoring_timestep, bool meter_scoring, bool simple_attractor, + vector y_vels, double stop_time_buffer, double sim_period, double angular_sim_granularity) + : path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()), + goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()), + costmap_(costmap), + world_model_(world_model), footprint_spec_(footprint_spec), + sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity), + vx_samples_(vx_samples), vtheta_samples_(vtheta_samples), + path_distance_bias_(path_distance_bias), goal_distance_bias_(goal_distance_bias), occdist_scale_(occdist_scale), + acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta), + prev_x_(0), prev_y_(0), escape_x_(0), escape_y_(0), escape_theta_(0), heading_lookahead_(heading_lookahead), + oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist), + escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot), + max_vel_x_(max_vel_x), min_vel_x_(min_vel_x), + max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th), + backup_vel_(backup_vel), + dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep), + simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer), sim_period_(sim_period) + { + //the robot is not stuck to begin with + stuck_left = false; + stuck_right = false; + stuck_left_strafe = false; + stuck_right_strafe = false; + rotating_left = false; + rotating_right = false; + strafe_left = false; + strafe_right = false; + + escaping_ = false; + final_goal_position_valid_ = false; + + + robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_); + } + + TrajectoryPlanner::~TrajectoryPlanner(){} + + bool TrajectoryPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) { + MapCell cell = path_map_(cx, cy); + MapCell goal_cell = goal_map_(cx, cy); + if (cell.within_robot) { + return false; + } + occ_cost = costmap_.getCost(cx, cy); + if (cell.target_dist == path_map_.obstacleCosts() || + cell.target_dist == path_map_.unreachableCellCosts() || + occ_cost >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + return false; + } + path_cost = cell.target_dist; + goal_cost = goal_cell.target_dist; + total_cost = path_distance_bias_ * path_cost + goal_distance_bias_ * goal_cost + occdist_scale_ * occ_cost; + return true; + } + + /** + * create and score a trajectory given the current pose of the robot and selected velocities + */ + void TrajectoryPlanner::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) { + + // make sure the configuration doesn't change mid run + boost::mutex::scoped_lock l(configuration_mutex_); + + double x_i = x; + double y_i = y; + double theta_i = theta; + + double vx_i, vy_i, vtheta_i; + + vx_i = vx; + vy_i = vy; + vtheta_i = vtheta; + + //compute the magnitude of the velocities + double vmag = hypot(vx_samp, vy_samp); + + //compute the number of steps we must take along this trajectory to be "safe" + int num_steps; + if(!heading_scoring_) { + num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5); + } else { + num_steps = int(sim_time_ / sim_granularity_ + 0.5); + } + + //we at least want to take one step... even if we won't move, we want to score our current position + if(num_steps == 0) { + num_steps = 1; + } + + double dt = sim_time_ / num_steps; + double time = 0.0; + + //create a potential trajectory + traj.resetPoints(); + traj.xv_ = vx_samp; + traj.yv_ = vy_samp; + traj.thetav_ = vtheta_samp; + traj.cost_ = -1.0; + + //initialize the costs for the trajectory + double path_dist = 0.0; + double goal_dist = 0.0; + double occ_cost = 0.0; + double heading_diff = 0.0; + + for(int i = 0; i < num_steps; ++i){ + //get map coordinates of a point + unsigned int cell_x, cell_y; + + //we don't want a path that goes off the know map + if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){ + traj.cost_ = -1.0; + return; + } + + //check the point on the trajectory for legality + double footprint_cost = footprintCost(x_i, y_i, theta_i); + + //if the footprint hits an obstacle this trajectory is invalid + if(footprint_cost < 0){ + traj.cost_ = -1.0; + return; + //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits, + //it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to + //come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative, + //but safe. + /* + double max_vel_x, max_vel_y, max_vel_th; + //we want to compute the max allowable speeds to be able to stop + //to be safe... we'll make sure we can stop some time before we actually hit + getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th); + + //check if we can stop in time + if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){ + ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt); + //if we can stop... we'll just break out of the loop here.. no point in checking future points + break; + } + else{ + traj.cost_ = -1.0; + return; + } + */ + } + + occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y))); + + //do we want to follow blindly + if (simple_attractor_) { + goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) * + (x_i - global_plan_[global_plan_.size() -1].pose.position.x) + + (y_i - global_plan_[global_plan_.size() -1].pose.position.y) * + (y_i - global_plan_[global_plan_.size() -1].pose.position.y); + } else { + + bool update_path_and_goal_distances = true; + + // with heading scoring, we take into account heading diff, and also only score + // path and goal distance for one point of the trajectory + if (heading_scoring_) { + if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) { + heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i); + } else { + update_path_and_goal_distances = false; + } + } + + if (update_path_and_goal_distances) { + //update path and goal distances + path_dist = path_map_(cell_x, cell_y).target_dist; + goal_dist = goal_map_(cell_x, cell_y).target_dist; + + //if a point on this trajectory has no clear path to goal it is invalid + if(impossible_cost <= goal_dist || impossible_cost <= path_dist){ +// robot::log_debug("No path to goal with goal distance = %f, path_distance = %f and max cost = %f", +// goal_dist, path_dist, impossible_cost); + traj.cost_ = -2.0; + return; + } + } + } + + + //the point is legal... add it to the trajectory + traj.addPoint(x_i, y_i, theta_i); + + //calculate velocities + vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt); + vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt); + vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt); + + //calculate positions + x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt); + y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt); + theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt); + + //increment time + time += dt; + } // end for i < numsteps + + //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp); + double cost = -1.0; + if (!heading_scoring_) { + cost = path_distance_bias_ * path_dist + goal_dist * goal_distance_bias_ + occdist_scale_ * occ_cost; + } else { + cost = occdist_scale_ * occ_cost + path_distance_bias_ * path_dist + 0.3 * heading_diff + goal_dist * goal_distance_bias_; + } + traj.cost_ = cost; + } + + double TrajectoryPlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading){ + unsigned int goal_cell_x, goal_cell_y; + + // find a clear line of sight from the robot's cell to a farthest point on the path + for (int i = global_plan_.size() - 1; i >=0; --i) { + if (costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)) { + if (lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0) { + double gx, gy; + costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy); + return fabs(angles::shortest_angular_distance(heading, atan2(gy - y, gx - x))); + } + } + } + return DBL_MAX; + } + + //calculate the cost of a ray-traced line + double TrajectoryPlanner::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 TrajectoryPlanner::pointCost(int x, int y){ + unsigned char cost = costmap_.getCost(x, y); + //if the cell is in an obstacle the path is invalid + if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){ + return -1; + } + + return cost; + } + + void TrajectoryPlanner::updatePlan(const vector& new_plan, bool compute_dists){ + global_plan_.resize(new_plan.size()); + for(unsigned int i = 0; i < new_plan.size(); ++i){ + global_plan_[i] = new_plan[i]; + } + + if( global_plan_.size() > 0 ){ + robot_geometry_msgs::PoseStamped& final_goal_pose = global_plan_[ global_plan_.size() - 1 ]; + final_goal_x_ = final_goal_pose.pose.position.x; + final_goal_y_ = final_goal_pose.pose.position.y; + final_goal_position_valid_ = true; + } else { + final_goal_position_valid_ = false; + } + + if (compute_dists) { + //reset the map for new operations + path_map_.resetPathDist(); + goal_map_.resetPathDist(); + + //make sure that we update our path based on the global plan and compute costs + path_map_.setTargetCells(costmap_, global_plan_); + goal_map_.setLocalGoal(costmap_, global_plan_); + robot::log_debug("Path/Goal distance computed"); + } + } + + bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy, + double vtheta, double vx_samp, double vy_samp, double vtheta_samp){ + Trajectory t; + + double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp); + + //if the trajectory is a legal one... the check passes + if(cost >= 0) { + return true; + } + robot::log_warning("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost); + + //otherwise the check fails + return false; + } + + double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy, + double vtheta, double vx_samp, double vy_samp, double vtheta_samp) { + Trajectory t; + double impossible_cost = path_map_.obstacleCosts(); + generateTrajectory(x, y, theta, + vx, vy, vtheta, + vx_samp, vy_samp, vtheta_samp, + acc_lim_x_, acc_lim_y_, acc_lim_theta_, + impossible_cost, t); + + // return the cost. + return double( t.cost_ ); + } + + /* + * create the trajectories we wish to score + */ + Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta, + double vx, double vy, double vtheta, + double acc_x, double acc_y, double acc_theta) { + //compute feasible velocity limits in robot space + double max_vel_x = max_vel_x_, max_vel_theta; + double min_vel_x, min_vel_theta; + + if( final_goal_position_valid_ ){ + double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y ); + max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ ); + } + + //should we use the dynamic window approach? + if (dwa_) { + max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_); + min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_); + + max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_); + min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_); + } else { + max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_); + min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_); + + max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_); + min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_); + } + + + //we want to sample the velocity space regularly + double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1); + double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1); + + double vx_samp = min_vel_x; + double vtheta_samp = min_vel_theta; + double vy_samp = 0.0; + + //keep track of the best trajectory seen so far + Trajectory* best_traj = &traj_one; + best_traj->cost_ = -1.0; + + Trajectory* comp_traj = &traj_two; + comp_traj->cost_ = -1.0; + + Trajectory* swap = NULL; + + //any cell with a cost greater than the size of the map is impossible + double impossible_cost = path_map_.obstacleCosts(); + + //if we're performing an escape we won't allow moving forward + if (!escaping_) { + //loop through all x velocities + for(int i = 0; i < vx_samples_; ++i) { + vtheta_samp = 0; + //first sample the straight trajectory + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it + if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + } + + vtheta_samp = min_vel_theta; + //next sample all theta trajectories + for(int j = 0; j < vtheta_samples_ - 1; ++j){ + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it + if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + } + vtheta_samp += dvtheta; + } + vx_samp += dvx; + } + + //only explore y velocities with holonomic robots + if (holonomic_robot_) { + //explore trajectories that move forward but also strafe slightly + vx_samp = 0.1; + vy_samp = 0.1; + vtheta_samp = 0.0; + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it + if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + } + + vx_samp = 0.1; + vy_samp = -0.1; + vtheta_samp = 0.0; + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it + if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + } + } + } // end if not escaping + + //next we want to generate trajectories for rotating in place + vtheta_samp = min_vel_theta; + vx_samp = 0.0; + vy_samp = 0.0; + + //let's try to rotate toward open space + double heading_dist = DBL_MAX; + + for(int i = 0; i < vtheta_samples_; ++i) { + //enforce a minimum rotational velocity because the base can't handle small in-place rotations + double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_) + : min(vtheta_samp, -1.0 * min_in_place_vel_th_); + + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it... + //note if we can legally rotate in place we prefer to do that rather than move with y velocity + if(comp_traj->cost_ >= 0 + && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0) + && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){ + double x_r, y_r, th_r; + comp_traj->getEndpoint(x_r, y_r, th_r); + x_r += heading_lookahead_ * cos(th_r); + y_r += heading_lookahead_ * sin(th_r); + unsigned int cell_x, cell_y; + + //make sure that we'll be looking at a legal cell + if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) { + double ahead_gdist = goal_map_(cell_x, cell_y).target_dist; + if (ahead_gdist < heading_dist) { + //if we haven't already tried rotating left since we've moved forward + if (vtheta_samp < 0 && !stuck_left) { + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + heading_dist = ahead_gdist; + } + //if we haven't already tried rotating right since we've moved forward + else if(vtheta_samp > 0 && !stuck_right) { + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + heading_dist = ahead_gdist; + } + } + } + } + + vtheta_samp += dvtheta; + } + + //do we have a legal trajectory + if (best_traj->cost_ >= 0) { + // avoid oscillations of in place rotation and in place strafing + if ( ! (best_traj->xv_ > 0)) { + if (best_traj->thetav_ < 0) { + if (rotating_right) { + stuck_right = true; + } + rotating_right = true; + } else if (best_traj->thetav_ > 0) { + if (rotating_left){ + stuck_left = true; + } + rotating_left = true; + } else if(best_traj->yv_ > 0) { + if (strafe_right) { + stuck_right_strafe = true; + } + strafe_right = true; + } else if(best_traj->yv_ < 0){ + if (strafe_left) { + stuck_left_strafe = true; + } + strafe_left = true; + } + + //set the position we must move a certain distance away from + prev_x_ = x; + prev_y_ = y; + } + + double dist = hypot(x - prev_x_, y - prev_y_); + if (dist > oscillation_reset_dist_) { + rotating_left = false; + rotating_right = false; + strafe_left = false; + strafe_right = false; + stuck_left = false; + stuck_right = false; + stuck_left_strafe = false; + stuck_right_strafe = false; + } + + dist = hypot(x - escape_x_, y - escape_y_); + if(dist > escape_reset_dist_ || + fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){ + escaping_ = false; + } + + return *best_traj; + } + + //only explore y velocities with holonomic robots + if (holonomic_robot_) { + //if we can't rotate in place or move forward... maybe we can move sideways and rotate + vtheta_samp = min_vel_theta; + vx_samp = 0.0; + + //loop through all y velocities + for(unsigned int i = 0; i < y_vels_.size(); ++i){ + vtheta_samp = 0; + vy_samp = y_vels_[i]; + //sample completely horizontal trajectories + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it + if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){ + double x_r, y_r, th_r; + comp_traj->getEndpoint(x_r, y_r, th_r); + x_r += heading_lookahead_ * cos(th_r); + y_r += heading_lookahead_ * sin(th_r); + unsigned int cell_x, cell_y; + + //make sure that we'll be looking at a legal cell + if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) { + double ahead_gdist = goal_map_(cell_x, cell_y).target_dist; + if (ahead_gdist < heading_dist) { + //if we haven't already tried strafing left since we've moved forward + if (vy_samp > 0 && !stuck_left_strafe) { + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + heading_dist = ahead_gdist; + } + //if we haven't already tried rotating right since we've moved forward + else if(vy_samp < 0 && !stuck_right_strafe) { + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + heading_dist = ahead_gdist; + } + } + } + } + } + } + + //do we have a legal trajectory + if (best_traj->cost_ >= 0) { + if (!(best_traj->xv_ > 0)) { + if (best_traj->thetav_ < 0) { + if (rotating_right){ + stuck_right = true; + } + rotating_left = true; + } else if(best_traj->thetav_ > 0) { + if(rotating_left){ + stuck_left = true; + } + rotating_right = true; + } else if(best_traj->yv_ > 0) { + if(strafe_right){ + stuck_right_strafe = true; + } + strafe_left = true; + } else if(best_traj->yv_ < 0) { + if(strafe_left){ + stuck_left_strafe = true; + } + strafe_right = true; + } + + //set the position we must move a certain distance away from + prev_x_ = x; + prev_y_ = y; + + } + + double dist = hypot(x - prev_x_, y - prev_y_); + if(dist > oscillation_reset_dist_) { + rotating_left = false; + rotating_right = false; + strafe_left = false; + strafe_right = false; + stuck_left = false; + stuck_right = false; + stuck_left_strafe = false; + stuck_right_strafe = false; + } + + dist = hypot(x - escape_x_, y - escape_y_); + if(dist > escape_reset_dist_ || fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) { + escaping_ = false; + } + + return *best_traj; + } + + //and finally, if we can't do anything else, we want to generate trajectories that move backwards slowly + vtheta_samp = 0.0; + vx_samp = backup_vel_; + vy_samp = 0.0; + generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, + acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); + + //if the new trajectory is better... let's take it + /* + if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){ + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + } + */ + + //we'll allow moving backwards slowly even when the static map shows it as blocked + swap = best_traj; + best_traj = comp_traj; + comp_traj = swap; + + double dist = hypot(x - prev_x_, y - prev_y_); + if (dist > oscillation_reset_dist_) { + rotating_left = false; + rotating_right = false; + strafe_left = false; + strafe_right = false; + stuck_left = false; + stuck_right = false; + stuck_left_strafe = false; + stuck_right_strafe = false; + } + + //only enter escape mode when the planner has given a valid goal point + if (!escaping_ && best_traj->cost_ > -2.0) { + escape_x_ = x; + escape_y_ = y; + escape_theta_ = theta; + escaping_ = true; + } + + dist = hypot(x - escape_x_, y - escape_y_); + + if (dist > escape_reset_dist_ || + fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) { + escaping_ = false; + } + + + //if the trajectory failed because the footprint hits something, we're still going to back up + if(best_traj->cost_ == -1.0) + best_traj->cost_ = 1.0; + + return *best_traj; + + } + + //given the current state of the robot, find a good trajectory + Trajectory TrajectoryPlanner::findBestPath(const robot_geometry_msgs::PoseStamped& global_pose, + robot_geometry_msgs::PoseStamped& global_vel, robot_geometry_msgs::PoseStamped& drive_velocities) { + + Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation)); + Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y, tf2::getYaw(global_vel.pose.orientation)); + + //reset the map for new operations + path_map_.resetPathDist(); + goal_map_.resetPathDist(); + + //temporarily remove obstacles that are within the footprint of the robot + std::vector footprint_list = + footprint_helper_.getFootprintCells( + pos, + footprint_spec_, + costmap_, + true); + + //mark cells within the initial footprint of the robot + for (unsigned int i = 0; i < footprint_list.size(); ++i) { + path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true; + } + + //make sure that we update our path based on the global plan and compute costs + path_map_.setTargetCells(costmap_, global_plan_); + goal_map_.setLocalGoal(costmap_, global_plan_); + robot::log_debug("Path/Goal distance computed"); + + //rollout trajectories and find the minimum cost one + Trajectory best = createTrajectories(pos[0], pos[1], pos[2], + vel[0], vel[1], vel[2], + acc_lim_x_, acc_lim_y_, acc_lim_theta_); + robot::log_debug("Trajectories created"); + + /* + //If we want to print a ppm file to draw goal dist + char buf[4096]; + sprintf(buf, "robot_base_local_planner.ppm"); + FILE *fp = fopen(buf, "w"); + if(fp){ + fprintf(fp, "P3\n"); + fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_); + fprintf(fp, "255\n"); + for(int j = map_.size_y_ - 1; j >= 0; --j){ + for(unsigned int i = 0; i < map_.size_x_; ++i){ + int g_dist = 255 - int(map_(i, j).goal_dist); + int p_dist = 255 - int(map_(i, j).path_dist); + if(g_dist < 0) + g_dist = 0; + if(p_dist < 0) + p_dist = 0; + fprintf(fp, "%d 0 %d ", g_dist, 0); + } + fprintf(fp, "\n"); + } + fclose(fp); + } + */ + + if(best.cost_ < 0){ + drive_velocities.pose.position.x = 0; + drive_velocities.pose.position.y = 0; + drive_velocities.pose.position.z = 0; + drive_velocities.pose.orientation.w = 1; + drive_velocities.pose.orientation.x = 0; + drive_velocities.pose.orientation.y = 0; + drive_velocities.pose.orientation.z = 0; + } + else{ + drive_velocities.pose.position.x = best.xv_; + drive_velocities.pose.position.y = best.yv_; + drive_velocities.pose.position.z = 0; + tf2::Quaternion q; + q.setRPY(0, 0, best.thetav_); + tf2::convert(q, drive_velocities.pose.orientation); + } + + return best; + } + + //we need to take the footprint of the robot into account when we calculate cost to obstacles + double TrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i){ + //check if the footprint is legal + return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_); + } + + + void TrajectoryPlanner::getLocalGoal(double& x, double& y){ + x = path_map_.goal_x_; + y = path_map_.goal_y_; + } + +}; + + diff --git a/src/twirling_cost_function.cpp b/src/twirling_cost_function.cpp new file mode 100644 index 0000000..f4209cd --- /dev/null +++ b/src/twirling_cost_function.cpp @@ -0,0 +1,18 @@ +/* + * twirling_cost_function.cpp + * + * Created on: Apr 20, 2016 + * Author: Morgan Quigley + */ + +#include + +#include + +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 */ diff --git a/src/voxel_grid_model.cpp b/src/voxel_grid_model.cpp new file mode 100644 index 0000000..527a028 --- /dev/null +++ b/src/voxel_grid_model.cpp @@ -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 +#include + +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& 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& footprint, + const vector& observations, const vector& 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::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 iter_x(cloud, "x"); + robot_sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + robot_sensor_msgs::PointCloud2ConstIterator 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 iter_x(cloud, "x"); + robot_sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + robot_sensor_msgs::PointCloud2Iterator 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; + } + } + } + } + } +};