├── CMakeLists.txt ├── CustomCompletePlanner.launch ├── CustomGlobalPlanner.launch ├── CustomLocalPlanner.launch ├── GlobalPlannerTest.launch ├── LICENSE ├── LocalPlannerTest.launch ├── NavigationStack.launch ├── README.md ├── astar_global_planner_plugin.xml ├── dw_local_planner_plugin.xml ├── include └── pathplanner │ └── astar.h ├── package.xml └── src ├── Astar_global_planner.cpp ├── DW_local_planner.cpp ├── costmap_common_params.yaml ├── global_costmap_params.yaml ├── local_costmap_params.yaml └── local_planner_params.yaml /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pathplanner) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | nav_core 12 | roscpp 13 | rospy 14 | std_msgs 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a run_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Node.msg 54 | # Path.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs 76 | # sensor_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES pathplanner 111 | CATKIN_DEPENDS nav_core roscpp rospy std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/pathplanner_node.cpp 129 | #) 130 | 131 | add_library(Astar_global_planner src/Astar_global_planner.cpp) 132 | add_library(DW_local_planner src/DW_local_planner.cpp) 133 | 134 | ## Add cmake target dependencies of the library 135 | ## as an example, code may need to be generated before libraries 136 | ## either from message generation or dynamic reconfigure 137 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Declare a C++ executable 140 | ## With catkin_make all packages are built within a single CMake context 141 | ## The recommended prefix ensures that target names across packages don't collide 142 | #add_executable(${PROJECT_NAME}_node src/DW_local_planner.cpp) 143 | 144 | ## Rename C++ executable without prefix 145 | ## The above recommended prefix causes long target names, the following renames the 146 | ## target back to the shorter version for ease of user use 147 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 148 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 149 | 150 | ## Add cmake target dependencies of the executable 151 | ## same as for the library above 152 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 153 | 154 | ## Specify libraries to link a library or executable target against 155 | # target_link_libraries(${PROJECT_NAME}_node 156 | # ${catkin_LIBRARIES} 157 | # ) 158 | 159 | ############# 160 | ## Install ## 161 | ############# 162 | 163 | # all install targets should use catkin DESTINATION variables 164 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 165 | 166 | ## Mark executable scripts (Python etc.) for installation 167 | ## in contrast to setup.py, you can choose the destination 168 | # install(PROGRAMS 169 | # scripts/my_python_script 170 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark executables and/or libraries for installation 174 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pathplanner.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /CustomCompletePlanner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 44 | 46 | 48 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /CustomGlobalPlanner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 45 | 47 | 49 | 51 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /CustomLocalPlanner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 46 | 48 | 50 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /GlobalPlannerTest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 50 | 52 | 54 | 56 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Elektrotehnički fakultet Sarajevo 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /LocalPlannerTest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 65 | 67 | 69 | 71 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /NavigationStack.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 48 | 50 | 52 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS navigation stack using move_base package 2 | This repository contains a simple implementation of a Global and Local path planner. 3 | 4 | 5 | ## Global path planner 6 | The file **Astar_global_planner.cpp** contains the derived class **Astar** which implementents the base class **BaseGlobalPlanner**. 7 | 8 | The **BaseGlobalPlanner** interface: 9 | - initialize 10 | - makePlan 11 | 12 | 13 | ## Local path planner 14 | The file **DW_local_planner.cpp** contains the derived class **DynamicWindow** which implementents the base class **BaseLocalPlanner**. 15 | 16 | The **BaseLocalPlanner** interface: 17 | - initialize 18 | - computeVelocityCommands 19 | - isGoalReached 20 | - setPlan 21 | 22 | Both base classes come from the * nav_core * ROS package and both derived classes need to implement the respective interfaces. 23 | 24 | Both classes are exported as plugins and included in the * move_base * navigation stack. 25 | 26 | ## Installing 27 | To install this project, you have to have ROS installed alongside the following ROS packages: 28 | - stage_ros 29 | - gmapping 30 | - rqt_gui 31 | - rviz 32 | - move_base 33 | - nav_core 34 | 35 | You also have to have a Workspace set up, then you can simple clone this project into the ` /src ` folder of your Workspace. 36 | 37 | ## Building 38 | Simply run catkin_make in your workspace, then source the setup file in the ` /devel ` folder. 39 | 40 | ## Running 41 | You can run this project by typing ` roslaunch pathplanner CustomCompletePlanner.launch `. 42 | 43 | ## About 44 | Developed by Muhamed Delalić using ROS Melodic Morenia on Ubuntu 18.04. 45 | 46 | ## License 47 | This project is licensed under the terms of the MIT license. 48 | -------------------------------------------------------------------------------- /astar_global_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Astar pathplaner written by Muhamed Delalic for ROS. 4 | 5 | 6 | -------------------------------------------------------------------------------- /dw_local_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Dynamic window path planer written by Muhamed Delalic for ROS. 4 | 5 | 6 | -------------------------------------------------------------------------------- /include/pathplanner/astar.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | template 7 | class Point{ 8 | public: 9 | Tip x,y; 10 | Point():x(0),y(0){} 11 | Point(Tip x,Tip y): x(x), y(y){} 12 | }; 13 | 14 | vector> astar(vector map, Point start, Point goal){ 15 | 16 | 17 | } 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pathplanner 4 | 0.0.0 5 | The pathplanner package 6 | 7 | 8 | 9 | 10 | delmond 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | message_generation 41 | 42 | 43 | 44 | 45 | 46 | message_runtime 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | sensor_msgs 56 | nav_core 57 | 58 | roscpp 59 | rospy 60 | std_msgs 61 | sensor_msgs 62 | nav_core 63 | 64 | roscpp 65 | rospy 66 | std_msgs 67 | sensor_msgs 68 | nav_core 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /src/Astar_global_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | /* ----------------------------*/ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | 40 | #include 41 | 42 | 43 | // Commend out if you dont want analytics logging 44 | #define ANALYTICS 45 | 46 | using namespace std; 47 | using namespace std::chrono; 48 | 49 | const double epsilon = numeric_limits::epsilon(); 50 | const double infinity = numeric_limits::infinity(); 51 | 52 | struct Node{ 53 | float cost; 54 | int index; 55 | }; 56 | 57 | class Astar: public nav_core::BaseGlobalPlanner{ 58 | 59 | double step_size_, min_dist_from_robot_; 60 | costmap_2d::Costmap2DROS* costmapROS; 61 | costmap_2d::Costmap2D* costmap; 62 | bool isInitilized; 63 | int width; 64 | int height; 65 | int mapSize; 66 | vector OGM; 67 | ros::Publisher guiPathPub; 68 | public: 69 | Astar(){ 70 | isInitilized = false; 71 | } 72 | ~Astar(){} 73 | Astar(string name, costmap_2d::Costmap2DROS* costmap_ros){ 74 | isInitilized = false; 75 | initialize(name, costmap_ros); 76 | } 77 | void initialize(string name, costmap_2d::Costmap2DROS* costmap_ros){ 78 | if(isInitilized){ 79 | ROS_WARN("Astar global planner: Already initilized"); 80 | return; 81 | } 82 | costmapROS = costmap_ros; 83 | costmap = costmap_ros->getCostmap(); 84 | ros::NodeHandle nh("~/Astar"); 85 | guiPathPub = nh.advertise("plan", 1); 86 | 87 | isInitilized = true; 88 | ROS_INFO("Astar global planner: Initialized successfully"); 89 | } 90 | const bool isInfinity(float x){ 91 | return fabs(infinity - x) < epsilon; 92 | } 93 | const bool isInBounds(int x, int y){ 94 | if( x < 0 || y < 0 || x >= height || y >= width) 95 | return false; 96 | return true; 97 | } 98 | const double getMoveCost(int firstIndex, int secondIndex) const{ 99 | unsigned int tmp1, tmp2; 100 | costmap->indexToCells(firstIndex, tmp1, tmp2); 101 | int firstXCord = tmp1,firstYCord = tmp2; 102 | costmap->indexToCells(secondIndex, tmp1, tmp2); 103 | int secondXCord = tmp1, secondYCord = tmp2; 104 | 105 | int difference = abs(firstXCord - secondXCord) + abs(firstYCord - secondYCord); 106 | // Error checking 107 | if(difference != 1 && difference != 2){ 108 | ROS_ERROR("Astar global planner: Error in getMoveCost - difference not valid"); 109 | return 1.0; 110 | } 111 | if(difference == 1) 112 | return 1.0; 113 | else 114 | return 1.4; 115 | } 116 | vector getNeighborIndexes(int index){ 117 | vector neighborIndexes; 118 | for(int i = -1; i <= 1; i++) 119 | for(int j = -1; j <= 1; j++){ 120 | unsigned tmp1, tmp2; 121 | costmap->indexToCells(index, tmp1, tmp2); 122 | 123 | int nextX = tmp1 + i; 124 | int nextY = tmp2 + j; 125 | int nextIndex = costmap->getIndex(nextX, nextY); 126 | if(!( i == 0 && j == 0) && isInBounds(nextX, nextY) && OGM[nextIndex]){ 127 | neighborIndexes.push_back(nextIndex); 128 | } 129 | } 130 | return move(neighborIndexes); 131 | } 132 | const double getHeuristic(int startCell, int goalCell) const{ 133 | enum class Type { EUCLID, MANHATTAN }; 134 | Type type = Type::EUCLID; 135 | unsigned int tmp1, tmp2; 136 | costmap->indexToCells(startCell, tmp1, tmp2); 137 | int startX = tmp1, startY = tmp2; 138 | costmap->indexToCells(goalCell, tmp1, tmp2); 139 | int goalX = tmp1, goalY = tmp2; 140 | 141 | if(type == Type::EUCLID){ 142 | return sqrt(pow(goalY - startY, 2) + pow(goalX - startX, 2)); 143 | } 144 | if(type == Type::MANHATTAN){ 145 | return abs(goalY - startY) + abs(goalX - startX); 146 | } 147 | } 148 | 149 | bool makePlan(const geometry_msgs::PoseStamped& start, 150 | const geometry_msgs::PoseStamped& goal, 151 | std::vector& plan) 152 | { 153 | if(isInitilized == false){ 154 | ROS_ERROR("Astart global planner: Initilize must be called first"); 155 | return false; 156 | } 157 | costmap = costmapROS->getCostmap(); 158 | width = costmap->getSizeInCellsX(); 159 | height = costmap->getSizeInCellsY(); 160 | mapSize = width * height; 161 | OGM.resize(mapSize); 162 | cout << "Doslo dovde!"; 163 | for(int i = 0; i < height; i++) 164 | for(int j = 0; j < width; j++){ 165 | int cost = costmap->getCost(j, i); 166 | OGM[i*width + j] = (cost == 0); 167 | } 168 | float startX = start.pose.position.x; 169 | float startY = start.pose.position.y; 170 | 171 | float goalX = goal.pose.position.x; 172 | float goalY = goal.pose.position.y; 173 | 174 | ROS_DEBUG("Astar global planner: Start cords (%.2f,%.2f) and goal cords (%.2f,%.2f)", 175 | startX, startY, goalX, goalY); 176 | plan.clear(); 177 | // A* implementation 178 | vector gCosts(mapSize, infinity); 179 | vector cameFrom(mapSize, -1); 180 | // Acts as a priority queue 181 | multiset priority_costs; 182 | unsigned int tmp1, tmp2; 183 | costmap->worldToMap(startX, startY, tmp1, tmp2); 184 | int startIndex = costmap->getIndex(tmp1, tmp2); 185 | costmap->worldToMap(goalX, goalY, tmp1, tmp2); 186 | int goalIndex = costmap->getIndex(tmp1, tmp2); 187 | gCosts[startIndex] = 0; 188 | #ifdef ANALYTICS 189 | //** ANALYTICS **// 190 | int numberOfCellsVisited = 0; 191 | double pathLength = 0.0; 192 | int numberOfCellsInPath = 0; 193 | high_resolution_clock::time_point t1 = high_resolution_clock::now(); 194 | //** END ANALYTICS **// 195 | #endif // ANALYTICS 196 | Node currentNode; 197 | currentNode.index = startIndex; 198 | currentNode.cost = gCosts[startIndex] + 0; 199 | priority_costs.insert(currentNode); 200 | while (!priority_costs.empty() && cameFrom[goalIndex] == -1){ 201 | // Take the element from the top 202 | currentNode = *priority_costs.begin(); 203 | //Delete the element from the top 204 | priority_costs.erase(priority_costs.begin()); 205 | // Uzmi sve susjedne celije 206 | vector neighborIndexes = getNeighborIndexes(currentNode.index); 207 | 208 | for(int i = 0; i < neighborIndexes.size(); i++){ 209 | //cout << "neighborIndexes[i]: " << neighborIndexes[i] << endl; 210 | #ifdef ANALYTICS 211 | numberOfCellsVisited++; 212 | #endif // ANALYTICS 213 | if(cameFrom[neighborIndexes[i]] == -1){ 214 | gCosts[neighborIndexes[i]] = gCosts[currentNode.index] 215 | + getMoveCost(currentNode.index, neighborIndexes[i]); 216 | Node nextNode; 217 | nextNode.index = neighborIndexes[i]; 218 | nextNode.cost = gCosts[neighborIndexes[i]] + getHeuristic(neighborIndexes[i], goalIndex); 219 | cameFrom[neighborIndexes[i]] = currentNode.index; 220 | priority_costs.insert(nextNode); 221 | } 222 | } 223 | } 224 | if(cameFrom[goalIndex] == -1){ 225 | cout << "Goal not reachable, failed making a global path." << endl; 226 | return false; 227 | } 228 | #ifdef ANALYTICS 229 | //** ANALYTICS **// 230 | high_resolution_clock::time_point t2 = high_resolution_clock::now(); 231 | auto duration = duration_cast( t2 - t1 ).count(); 232 | //** END ANALYTICS **// 233 | #endif // ANALYTICS 234 | 235 | if(startIndex == goalIndex) 236 | return false; 237 | //Finding the best path 238 | vector bestPath; 239 | currentNode.index = goalIndex; 240 | while(currentNode.index != startIndex){ 241 | bestPath.push_back(cameFrom[currentNode.index]); 242 | currentNode.index = cameFrom[currentNode.index]; 243 | } 244 | reverse(bestPath.begin(), bestPath.end()); 245 | #ifdef ANALYTICS 246 | //** ANALYTICS **// 247 | numberOfCellsInPath = bestPath.size(); 248 | //** END ANALYTICS **// 249 | #endif // ANALYTICS 250 | 251 | ros::Time plan_time = ros::Time::now(); 252 | for(int i = 0; i < bestPath.size(); i++){ 253 | unsigned int tmp1, tmp2; 254 | costmap->indexToCells(bestPath[i], tmp1, tmp2); 255 | double x, y; 256 | costmap->mapToWorld(tmp1,tmp2, x, y); 257 | 258 | geometry_msgs::PoseStamped pose; 259 | pose.header.stamp = plan_time; 260 | pose.header.frame_id = costmapROS->getGlobalFrameID(); 261 | pose.pose.position.x = x; 262 | pose.pose.position.y = y; 263 | pose.pose.position.z = 0.0; 264 | 265 | pose.pose.orientation.x = 0.0; 266 | pose.pose.orientation.y = 0.0; 267 | pose.pose.orientation.z = 0.0; 268 | pose.pose.orientation.w = 1.0; 269 | #ifdef ANALYTICS 270 | //** ANALYTICS **// 271 | if(i != 0){ 272 | double dx = (plan[i - 1].pose.position.x - x); 273 | double dy = (plan[i - 1].pose.position.y - y); 274 | double segmentLength = sqrt(dx*dx + dy*dy); 275 | pathLength += segmentLength; 276 | } 277 | //** END ANALYTICS **// 278 | #endif // ANALYTICS 279 | plan.push_back(pose); 280 | } 281 | #ifdef ANALYTICS 282 | //** ANALYTICS **// 283 | cout << "ANALYTICS: " << endl; 284 | cout << "Time elapsed: " << duration << " microseconds" << endl; 285 | cout << "Number of cells visited: " << numberOfCellsVisited << endl; 286 | cout << "Path length: " << pathLength << endl; 287 | cout << "Number of cells in path: " << numberOfCellsInPath << endl; 288 | cout << "END ANALYTICS: " << endl; 289 | //** END ANALYTICS **// 290 | #endif // ANALYTICS 291 | publishGUIPath(plan); 292 | return true; 293 | } 294 | void publishGUIPath(std::vector& plan){ 295 | nav_msgs::Path gui_path; 296 | gui_path.poses.resize(plan.size()); 297 | if(!plan.empty()) 298 | { 299 | gui_path.header.frame_id = plan[0].header.frame_id; 300 | gui_path.header.stamp = plan[0].header.stamp; 301 | } 302 | for(int i = 0; i < plan.size(); i++) 303 | gui_path.poses[i] = plan[i]; 304 | 305 | guiPathPub.publish(gui_path); 306 | } 307 | }; 308 | // Required for multiset sorting 309 | bool operator <(const Node& x, const Node& y) { 310 | return x.cost < y.cost; 311 | } 312 | 313 | PLUGINLIB_EXPORT_CLASS(Astar, nav_core::BaseGlobalPlanner); 314 | -------------------------------------------------------------------------------- /src/DW_local_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "sensor_msgs/LaserScan.h" 22 | #include "sensor_msgs/PointCloud2.h" 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | /* ----------------------------*/ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | 46 | // CONSTANTS 47 | const double PI = 3.141592653589793238463; 48 | const double MAX_LINEAR_VELOCITY = 0.2; 49 | const double MIN_LINEAR_VELOCITY = 0.0; 50 | const double MAX_ANGULAR_VELOCITY = PI*30.0/180.0; 51 | const double MIN_ANGULAR_VELOCITY = -PI*30.0/180.0; 52 | const double MAX_LINEAR_ACCELERATION = 1.0; 53 | const double MIN_LINEAR_ACCELERATION = -1.0; 54 | const double MAX_ANGULAR_ACCELERATION = PI*40.0/180.0; 55 | const double MIN_ANGULAR_ACCELERATION = -PI*40.0/180.0; 56 | const double DT = 0.5; 57 | const double ROBOT_RADIUS = 0.7; 58 | // const double WEIGHT_HEADING = 0.05; 59 | // const double WEIGHT_CLEARANCE = 0.2; 60 | // const double WEIGHT_VELOCITY = 0.1; 61 | const double WEIGHT_HEADING = 0.4; 62 | const double WEIGHT_CLEARANCE = 2.8; 63 | const double WEIGHT_VELOCITY = 0.2; 64 | const double EPSILON = std::numeric_limits::epsilon(); 65 | const int stepsAhead = 20; 66 | 67 | using namespace std; 68 | 69 | class DynamicWindow: public nav_core::BaseLocalPlanner { 70 | 71 | bool initialized; 72 | tf2_ros::Buffer* tf; 73 | costmap_2d::Costmap2DROS* costmap_ros; 74 | costmap_2d::Costmap2D* costmap; 75 | int width; 76 | int height; 77 | int mapSize; 78 | geometry_msgs::PoseStamped currentPose; 79 | geometry_msgs::PoseStamped goalPose; 80 | vector globalPlan; 81 | ros::Subscriber odometry_sub; 82 | double currentV; 83 | double currentW; 84 | ros::Publisher guiPathPub; 85 | ros::Publisher waypointPub; 86 | nav_msgs::Path gui_path; 87 | ros::NodeHandle nh; 88 | ros::NodeHandle oh; 89 | 90 | struct ScoringHelper{ 91 | double clearance; 92 | double heading; 93 | double v; 94 | double w; 95 | double score; 96 | }; 97 | public: 98 | DynamicWindow(): initialized(false), nh("~/DynamicWindow") {} 99 | double getClearnace(geometry_msgs::PoseStamped robotPose){ 100 | double minDistance = 100000; 101 | // costmap->worldToMap(robotPose.pose.position.x, robotPose.pose.position.y, CordX, CordY); 102 | for(int i = 0; i < costmap->getSizeInCellsX(); i++){ 103 | for(int j = 0; j < costmap->getSizeInCellsY(); j++){ 104 | 105 | //int newIndex = costmap->getIndex(CordX + i, CordY + j); 106 | if(DynamicWindow::costmap->getCost(i,j) == 0){ 107 | continue; 108 | } 109 | double obstacleX, obstacleY; 110 | costmap->mapToWorld(i, j, obstacleX, obstacleY); 111 | double newDistance = sqrt(pow(robotPose.pose.position.x - obstacleX, 2) + pow(robotPose.pose.position.y - obstacleY,2)) - ROBOT_RADIUS; 112 | if(newDistance < minDistance){ 113 | minDistance = newDistance; 114 | } 115 | } 116 | } 117 | if(minDistance > 2 * ROBOT_RADIUS){ 118 | minDistance = 2 * ROBOT_RADIUS; 119 | } 120 | return minDistance; 121 | } 122 | bool isImpactAroundLocation(int x, int y){ 123 | for(int i = -4; i <= 4; i++){ 124 | for(int j = -4; j <= 4; j++){ 125 | if(DynamicWindow::costmap->getCost(x + i, y + j) != 0){ 126 | return true; 127 | } 128 | } 129 | } 130 | return false; 131 | } 132 | double getClearnaceAlternative(geometry_msgs::PoseStamped robotPose, double velocity, double rotationalVelocity){ 133 | double minDistance = 100000; 134 | // costmap->worldToMap(robotPose.pose.position.x, robotPose.pose.position.y, CordX, CordY); 135 | double distanceToObstacle = 0.0; 136 | bool obstacleFound = false; 137 | for(int i = 0; i < 10; i++){ 138 | geometry_msgs::PoseStamped newRobotPose = getNewRobotPose(robotPose, velocity, rotationalVelocity); 139 | 140 | double dx = newRobotPose.pose.position.x - robotPose.pose.position.x; 141 | double dy = newRobotPose.pose.position.y - robotPose.pose.position.y; 142 | distanceToObstacle += sqrt(dx*dx + dy*dy); 143 | 144 | unsigned int tmp1, tmp2; 145 | costmap->worldToMap(newRobotPose.pose.position.x, newRobotPose.pose.position.y, tmp1, tmp2); 146 | 147 | if(isImpactAroundLocation(tmp1, tmp2)){ 148 | obstacleFound = true; 149 | break; 150 | } 151 | robotPose = newRobotPose; 152 | } 153 | if(obstacleFound){ 154 | return (distanceToObstacle > 4*ROBOT_RADIUS)? 4*ROBOT_RADIUS : distanceToObstacle; 155 | } else{ 156 | return 4*ROBOT_RADIUS; 157 | } 158 | } 159 | double desiredVelocity(geometry_msgs::PoseStamped robotPose){ 160 | double dx = robotPose.pose.position.x - goalPose.pose.position.x; 161 | double dy = robotPose.pose.position.y - goalPose.pose.position.y; 162 | double distanceToGoal = sqrt(dx*dx + dy*dy); 163 | return distanceToGoal*DT; 164 | } 165 | double getHeading(geometry_msgs::PoseStamped robotPose){ 166 | double angleToGoal = atan2(goalPose.pose.position.y - robotPose.pose.position.y, goalPose.pose.position.x - robotPose.pose.position.x ); 167 | 168 | // cout << "Angle to goal is: " << angleToGoal << " [rad], "; 169 | angleToGoal = angles::normalize_angle((angles::normalize_angle_positive(angleToGoal) - angles::normalize_angle_positive(tf::getYaw(robotPose.pose.orientation)))); 170 | // cout << "difference in angles of robot and angle to goal is: " << angleToGoal <<" [rad], " << 180 - abs(angleToGoal)/PI * 180 < 0){ 178 | breakingDistance += velocity*DT; 179 | velocity = velocity + MIN_LINEAR_ACCELERATION*DT; 180 | } 181 | return breakingDistance; 182 | } 183 | geometry_msgs::PoseStamped getNewRobotPose(geometry_msgs::PoseStamped robotPose, double velocity, double rotationalVelocity){ 184 | geometry_msgs::PoseStamped newRobotPose; 185 | double robotYaw = tf::getYaw(robotPose.pose.orientation); 186 | newRobotPose.pose.position.x = robotPose.pose.position.x + velocity*DT*cos(robotYaw); 187 | newRobotPose.pose.position.y = robotPose.pose.position.y + velocity*DT*sin(robotYaw); 188 | newRobotPose.pose.position.z = 0; 189 | double newRobotYaw = robotYaw + rotationalVelocity * DT; 190 | newRobotPose.pose.orientation = tf::createQuaternionMsgFromYaw(newRobotYaw); 191 | return newRobotPose; 192 | } 193 | void odomCallback(const nav_msgs::Odometry::ConstPtr& odom){ 194 | currentV = odom->twist.twist.linear.x; 195 | currentW = odom->twist.twist.angular.z; 196 | } 197 | void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros){ 198 | if(initialized){ 199 | ROS_WARN("DW local planner: Already initilized"); 200 | return; 201 | } 202 | DynamicWindow::tf = tf; 203 | DynamicWindow::costmap_ros = costmap_ros; 204 | costmap = costmap_ros->getCostmap(); 205 | costmap_ros->getRobotPose(currentPose); 206 | width = costmap->getSizeInCellsX(); 207 | height = costmap->getSizeInCellsY(); 208 | mapSize = width * height; 209 | // ROS_INFO("Width is: %d, Height is: %d", width, height); 210 | 211 | 212 | odometry_sub = oh.subscribe("odom", 1, boost::bind(&DynamicWindow::odomCallback, this, _1)); 213 | guiPathPub = nh.advertise("plan", 1); 214 | 215 | initialized = true; 216 | 217 | } 218 | geometry_msgs::PoseStamped getNewRobotGoal(geometry_msgs::PoseStamped robotPose){ 219 | int closestPointInPath = 0; 220 | double shortestDistance = 1000000; 221 | for(int i = 0; i < globalPlan.size(); i++){ 222 | double dx = globalPlan[i].pose.position.x - robotPose.pose.position.x; 223 | double dy = globalPlan[i].pose.position.y - robotPose.pose.position.y; 224 | double newDistance = sqrt(dx*dx + dy*dy); 225 | if(newDistance < shortestDistance){ 226 | shortestDistance = newDistance; 227 | closestPointInPath = i; 228 | } 229 | } 230 | if(globalPlan.size() - closestPointInPath < stepsAhead){ 231 | return globalPlan.back(); 232 | } 233 | return globalPlan[closestPointInPath + stepsAhead]; 234 | } 235 | bool setPlan(const vector& orig_global_plan){ 236 | globalPlan = orig_global_plan; 237 | goalPose = orig_global_plan.back(); 238 | gui_path.poses.clear(); 239 | return true; 240 | } 241 | const double getLinearVelocity() const{ 242 | return currentV; 243 | } 244 | const double getAngularVelocity() const{ 245 | return currentW; 246 | } 247 | vector getLocalPlan(geometry_msgs::PoseStamped robotPose, double velocity, double rotationalVelocity){ 248 | vector localPlan; 249 | robotPose = getNewRobotPose(robotPose, velocity, rotationalVelocity); 250 | localPlan.push_back(robotPose); 251 | return localPlan; 252 | } 253 | 254 | double clamp(double value, double minimal, double maximal){ 255 | if(value < minimal) 256 | return minimal; 257 | if(value > maximal) 258 | return maximal; 259 | return value; 260 | } 261 | 262 | bool computeVelocityCommands (geometry_msgs::Twist &cmd_vel){ 263 | if(!initialized){ 264 | ROS_WARN("DW local planner: Must be initilized first"); 265 | return false; 266 | } 267 | if(costmap_ros->getRobotPose(currentPose) == false){ 268 | ROS_WARN("DW local planner: Failed to get current local pose"); 269 | return false; 270 | } 271 | costmap = costmap_ros->getCostmap(); 272 | costmap_ros->getRobotPose(currentPose); 273 | width = costmap->getSizeInCellsX(); 274 | height = costmap->getSizeInCellsY(); 275 | mapSize = width * height; 276 | // ROS_INFO("Width is: %d, Height is: %d", width, height); 277 | 278 | 279 | double robotV = getLinearVelocity(); 280 | double robotW = getAngularVelocity(); 281 | // double desiredV = calculateDesiredVelocity(currentPose, goalPose); 282 | 283 | vector scoringhelper; 284 | goalPose = getNewRobotGoal(currentPose); 285 | double minV = clamp(robotV + DT*MIN_LINEAR_ACCELERATION, MIN_LINEAR_VELOCITY, MAX_LINEAR_VELOCITY); 286 | double maxV = clamp(robotV + DT*MAX_LINEAR_ACCELERATION, MIN_LINEAR_VELOCITY, MAX_LINEAR_VELOCITY); 287 | 288 | double minW = clamp(robotW + DT*MIN_ANGULAR_ACCELERATION, MIN_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY); 289 | double maxW = clamp(robotW + DT*MAX_ANGULAR_ACCELERATION, MIN_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY); 290 | // ROS_INFO("RobotW: %4lf, DT: %4lf, MIN_ANGULAR_ACCELERATION: %4lf",robotW,DT, MIN_ANGULAR_ACCELERATION); 291 | double velocityPrecision = 0.01; 292 | double angularPrecision = 5/180.0 * PI; 293 | 294 | // ROS_INFO("DW Velocity range (%4f,%4f), rotational range (%4f, %4f)", minV,maxV, minW,maxW); 295 | double sumHeading = 0.0, sumClearance = 0.0, sumVelocity = 0.0; 296 | for(double v = minV; v <= maxV; v += velocityPrecision){ 297 | for(double w = minW; w <= maxW; w += angularPrecision){ 298 | vector localPlan = getLocalPlan(currentPose, v, w); 299 | // double clearance = getClearnace(localPlan.back()); 300 | double clearance = getClearnaceAlternative(currentPose, v, w); 301 | double breakingDistance = getBreakingDistance(v); 302 | if(clearance <= breakingDistance) 303 | continue; 304 | 305 | double heading = getHeading(localPlan.back()); 306 | sumHeading += heading; 307 | sumClearance += clearance; 308 | sumVelocity += v; 309 | 310 | ScoringHelper sh; 311 | sh.clearance = clearance; 312 | sh.heading = heading; 313 | // sh.v = clamp(v, 0.0, desiredVelocity(localPlan.back()) ); 314 | sh.v = v; 315 | sh.w = w; 316 | scoringhelper.push_back(sh); 317 | } 318 | } 319 | int bestScoreIndex = 0; 320 | for(int i = 0; i < scoringhelper.size(); i++){ 321 | double normalized_heading = scoringhelper[i].heading/sumHeading; 322 | double normalized_clearance = scoringhelper[i].clearance/sumClearance; 323 | double normalized_velocity = (sumVelocity < EPSILON)? 0 : scoringhelper[i].v/sumVelocity; 324 | scoringhelper[i].score = 325 | WEIGHT_HEADING * normalized_heading + 326 | WEIGHT_CLEARANCE * normalized_clearance + 327 | WEIGHT_VELOCITY * normalized_velocity; 328 | // scoringhelper[i].score = 329 | // WEIGHT_HEADING * scoringhelper[i].heading + 330 | // WEIGHT_CLEARANCE * scoringhelper[i].clearance + 331 | // WEIGHT_VELOCITY * scoringhelper[i].v; 332 | // printf("Score for %d with (v: %lf, w: %lf, heading: %lf, clearance: %lf ) is %lf \n", i, scoringhelper[i].v, scoringhelper[i].w, 333 | // scoringhelper[i].heading, scoringhelper[i].clearance, scoringhelper[i].score); 334 | if(scoringhelper[i].score >= scoringhelper[bestScoreIndex].score) 335 | bestScoreIndex = i; 336 | } 337 | if(scoringhelper.size() == 0){ 338 | cout << "Failed finding velocity comands" << endl; 339 | return false; 340 | } 341 | cmd_vel.linear.x = scoringhelper[bestScoreIndex].v; 342 | cmd_vel.linear.y = 0; 343 | cmd_vel.linear.z = 0; 344 | 345 | cmd_vel.angular.x = 0; 346 | cmd_vel.angular.y = 0; 347 | cmd_vel.angular.z = scoringhelper[bestScoreIndex].w; 348 | 349 | vector lplan = getLocalPlan(currentPose,scoringhelper[bestScoreIndex].v,scoringhelper[bestScoreIndex].w); 350 | publishWaypoint(currentPose); 351 | return true; 352 | } 353 | bool isGoalReached(){ 354 | if(!initialized){ 355 | ROS_WARN("DW local planner: Must be initilized first"); 356 | return false; 357 | } 358 | if(costmap_ros->getRobotPose(currentPose) == false){ 359 | ROS_WARN("DW local planner: Failed to get current local pose"); 360 | return false; 361 | } 362 | geometry_msgs::PoseStamped finalGoal = globalPlan.back(); 363 | double currentXCord = currentPose.pose.position.x; 364 | double currentYCord = currentPose.pose.position.y; 365 | double goalXCord = finalGoal.pose.position.x; 366 | double goalYCord = finalGoal.pose.position.y; 367 | 368 | // compute distance from goal position 369 | double positionalDistance = sqrt(pow(currentXCord - goalXCord, 2) + pow(currentYCord - goalYCord,2)); 370 | // compute distance from rotational positions 371 | double currentYaw = angles::normalize_angle_positive(tf2::getYaw(currentPose.pose.orientation)); 372 | double goalYaw = angles::normalize_angle_positive(tf2::getYaw(finalGoal.pose.orientation)); 373 | double rotationalDistance = abs(currentYaw - goalYaw); 374 | 375 | if(positionalDistance < 0.1) 376 | return true; 377 | 378 | // if(positionalDistance < 0.1 && rotationalDistance < 5*PI/180.0) 379 | // return true; 380 | 381 | return false; 382 | } 383 | void publishWaypoint(geometry_msgs::PoseStamped waypoint){ 384 | ros::Time plan_time = ros::Time::now(); 385 | gui_path.poses.resize(gui_path.poses.size() + 1); 386 | gui_path.header.frame_id = costmap_ros->getGlobalFrameID(); 387 | gui_path.header.stamp = plan_time; 388 | 389 | gui_path.poses[gui_path.poses.size() -1] = waypoint; 390 | gui_path.poses[gui_path.poses.size() -1].header = gui_path.header; 391 | 392 | guiPathPub.publish(gui_path); 393 | } 394 | 395 | }; 396 | PLUGINLIB_EXPORT_CLASS(DynamicWindow, nav_core::BaseLocalPlanner); 397 | -------------------------------------------------------------------------------- /src/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 5.0 2 | raytrace_range: 6.0 3 | max_obstacle_height: 1.0 4 | xmin_obstacle_height: 0.05 5 | # footprint: [ [0.3302, -0.0508], [0.254, -0.0508], 6 | # [0.254, -0.254], [-0.254, -0.254], [-0.254, 0.254], 7 | # [0.254, 0.254], [0.254, 0.0508], [0.3302, 0.0508] ] 8 | footprint: [ [-0.2, 0.12], [-0.2, -0.12], 9 | [-0.12, -0.2555], [0.12, -0.2555], [0.2, -0.12], 10 | [0.2, 0.12], [0.12, 0.2555], [-0.12, 0.2555] ] 11 | 12 | # footprint: [ [-0.2, 0.12], [-0.2, -0.12], [-0.12, -0.2555], [0.12, -0.2555], [0.2, -0.12], [0.2, 0.12], [0.12, 0.2555], [-0.12, 0.2555] ] 13 | robot_radius: 0.35 14 | inflation_radius: 0.1 15 | footprint_padding: 0 16 | 17 | transform_tolerance: 1.0 18 | map_type: costmap 19 | cost_scaling_factor: 100 20 | observation_sources: laser_scan_sensor 21 | laser_scan_sensor: { sensor_frame: base_laser_link, data_type: LaserScan, topic: base_scan, marking: true, clearing: true } 22 | -------------------------------------------------------------------------------- /src/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | # robot_base_frame: robot_0/base_link 4 | robot_base_frame: base_link 5 | update_frequency: 1.0 6 | publish_frequency: 1.0 #0 7 | static_map: false 8 | width: 50 #3.4 9 | height: 50 #3.4 10 | origin_x: -25 #-1.20 is the actual position; -0.95 is the old one, for the frond wheel at the marker 11 | origin_y: -25 #-1.91 12 | resolution: 0.5 13 | transform_tolerance: 1.0 14 | -------------------------------------------------------------------------------- /src/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | # global_frame: robot_0/odom 3 | # robot_base_frame: robot_0/base_link 4 | global_frame: odom 5 | robot_base_frame: base_link 6 | update_frequency: 5.0 7 | publish_frequency: 10.0 8 | static_map: false 9 | rolling_window: true 10 | width: 3.0 11 | height: 3.0 12 | resolution: 0.05 13 | transform_tolerance: 1.0 14 | -------------------------------------------------------------------------------- /src/local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | max_vel_x: 0.3 3 | min_vel_x: 0.00 4 | max_rotational_vel: 2 5 | max_vel_theta: 1 6 | min_vel_theta: -1 7 | min_in_place_rotational_vel: 0.5 8 | min_in_place_vel_theta: 0.5 9 | escape_vel: -0.1 10 | acc_lim_theta: 3.2 11 | acc_lim_x: 1 12 | acc_lim_y: 0 13 | holonomic_robot: false 14 | xy_goal_tolerance: 1.0 15 | yaw_goal_tolerance: 0.1 16 | sim_time: 1.7 17 | --------------------------------------------------------------------------------