├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── mpepc_costmap_params.yaml ├── mpepc_nav.rviz └── obstacle_costmap_params.yaml ├── include └── model_predictive_navigation │ └── control_law.h ├── launch └── mpepc_turtlebot_nav.launch ├── literature ├── Park-icra-11.pdf └── Park-iros-12.pdf ├── msg └── EgoGoal.msg ├── package.xml └── src ├── control_law.cpp ├── costmap_server.cpp ├── costmap_translator.cpp ├── mpepc_plan.cpp └── mpepc_trajectory.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(model_predictive_navigation) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | costmap_2d 6 | geometry_msgs 7 | nav_msgs 8 | navfn 9 | roscpp 10 | rospy 11 | std_msgs 12 | nav_core 13 | tf 14 | sensor_msgs 15 | roslib 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | find_package(Boost REQUIRED COMPONENTS system thread) 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## Generate messages in the 'msg' folder 26 | add_message_files( 27 | FILES 28 | EgoGoal.msg 29 | ) 30 | 31 | ## Generate added messages and services with any dependencies listed here 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | ) 36 | 37 | ################################### 38 | ## catkin specific configuration ## 39 | ################################### 40 | ## The catkin_package macro generates cmake config files for your package 41 | ## Declare things to be passed to dependent projects 42 | ## INCLUDE_DIRS: uncomment this if you package contains header files 43 | ## LIBRARIES: libraries you create in this project that dependent projects also need 44 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 45 | ## DEPENDS: system dependencies of this project that dependent projects also need 46 | catkin_package( 47 | INCLUDE_DIRS include /opt/ros/indigo/include/navfn/ 48 | LIBRARIES control_law 49 | CATKIN_DEPENDS costmap_2d geometry_msgs nav_msgs navfn roscpp std_msgs nav_core tf sensor_msgs roslib rospy 50 | DEPENDS system_lib 51 | ) 52 | 53 | ########### 54 | ## Build ## 55 | ########### 56 | 57 | ## Specify additional locations of header files 58 | ## Your package locations should be listed before other locations 59 | include_directories( 60 | include 61 | /opt/ros/indigo/include/navfn/ 62 | ${catkin_INCLUDE_DIRS} 63 | ${Boost_INCLUDE_DIRS} 64 | ) 65 | 66 | ## Declare a cpp library 67 | add_library(control_law src/control_law.cpp) 68 | 69 | ## Declare a cpp executable 70 | add_executable(mpepc_trajectory src/mpepc_trajectory.cpp) 71 | add_executable(mpepc_plan src/mpepc_plan.cpp) 72 | add_executable(costmap_translator src/costmap_translator.cpp) 73 | add_executable(costmap_server src/costmap_server.cpp) 74 | 75 | ## Add cmake target dependencies of the executable/library 76 | ## as an example, message headers may need to be generated before nodes 77 | add_dependencies(mpepc_trajectory ${catkin_EXPORTED_TARGETS}) 78 | add_dependencies(mpepc_plan ${catkin_EXPORTED_TARGETS}) 79 | add_dependencies(costmap_translator ${catkin_EXPORTED_TARGETS}) 80 | add_dependencies(costmap_server ${catkin_EXPORTED_TARGETS}) 81 | 82 | ## Specify libraries to link a library or executable target against 83 | target_link_libraries(mpepc_trajectory 84 | ${catkin_LIBRARIES} 85 | control_law 86 | ) 87 | 88 | target_link_libraries(mpepc_plan 89 | ${catkin_LIBRARIES} 90 | ${OpenCV_LIBRARIES} 91 | control_law 92 | nlopt 93 | ) 94 | 95 | target_link_libraries(costmap_translator 96 | ${catkin_LIBRARIES} 97 | ) 98 | 99 | target_link_libraries(costmap_server 100 | ${catkin_LIBRARIES} 101 | ) 102 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | 341 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # model_predictive_navigation 2 | A sample ROS package that provides a model predictive equilibrium point control motion planner for the turtlebot simulator. 3 | 4 | This work is an implementation of the model predictive equilibrium point control published by Jong Jin Park and Ben Kuipers of the University of Michigan. The relevant publications can be found in the literature folder of this repository. 5 | 6 | ## Dependencies 7 | 1) ROS Indigo (http://wiki.ros.org/indigo/Installation/Ubuntu) 8 | 9 | 2) ROS Turtlebot Simulator 10 | ``` 11 | sudo apt-get install ros-indigo-turtlebot-simulator 12 | ``` 13 | 3) ROS Turtlebot Apps 14 | ``` 15 | sudo apt-get install ros-indigo-turtlebot-apps 16 | ``` 17 | 4) FLANN - library for fast approximate nearest neighbors search 18 | ``` 19 | sudo apt-get install libflann-dev 20 | ``` 21 | 5) NLopt - non-linear optimization library (http://ab-initio.mit.edu/wiki/index.php/NLopt) 22 | 23 | ## Install and build package 24 | 1) Clone model_predictive_navigation in to a ROS Indigo workspace 25 | 26 | 2) Build the workspace (planner works best if Release flag is used during build) 27 | ``` 28 | cd ~/ 29 | catkin_make -DCMAKE_BUILD_TYPE=Release 30 | ``` 31 | 3) Source the workspace 32 | ``` 33 | cd ~/ 34 | source devel/setup.bash 35 | ``` 36 | 37 | ## To Run 38 | 1) Run the included launch file: 39 | ``` 40 | roslaunch model_predictive_navigation mpepc_turtlebot_nav.launch 41 | ``` 42 | 2) Rviz will automatically open, select "2D Nav Goal" button and place a goal pose in the scene 43 | 44 | 3) Turtlebot will begin navigating to desire goal pose 45 | 46 | ## Notes About Planner 47 | The planner consists of two nodes `mpepc_trajectory` and `mpepc_plan`. The goal request goes to `mpepc_trajectory` which then initiates the planner `mpepc_plan` to find subgoals online using a cost function optimization. The subgoals are published back to `mpepc_trajectory` which then uses a graceful control law to drive to the subgoal. The planning time horizon is further than the replan rate, so the subgoal is not achieved until it is placed on the final goal. 48 | -------------------------------------------------------------------------------- /config/mpepc_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: odom 2 | robot_base_frame: /base_footprint 3 | map_type: costmap 4 | plugins: 5 | - {name: obstacles, type: "costmap_2d::ObstacleLayer"} 6 | - {name: inflated, type: "costmap_2d::InflationLayer"} 7 | 8 | update_frequency: 10.0 9 | publish_frequency: 5.0 10 | static_map: false 11 | rolling_window: true 12 | width: 20.0 13 | height: 20.0 14 | resolution: 0.025 15 | origin_x: -10.0 16 | origin_y: -10.0 17 | origin_z: 0.0 18 | 19 | transform_tolerance: 0.1 20 | obstacle_range: 5.0 21 | raytrace_range: 10.0 22 | inflation_radius: 0.38 23 | robot_radius: 0.20 24 | footprint_padding: 0.01 25 | 26 | obstacles: 27 | observation_sources: scan 28 | scan: {topic: scan, data_type: LaserScan, expected_update_rate: 0.3, obstacle_range: 5.5, raytrace_range: 10.0, observation_persistence: 0, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35} 29 | 30 | track_unknown_space: false 31 | global_frame: odom 32 | robot_base_frame: /base_footprint 33 | -------------------------------------------------------------------------------- /config/mpepc_nav.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 811 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.03 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: odom 48 | Value: true 49 | - Alpha: 1 50 | Class: rviz/RobotModel 51 | Collision Enabled: false 52 | Enabled: true 53 | Links: 54 | All Links Enabled: true 55 | Expand Joint Details: false 56 | Expand Link Details: false 57 | Expand Tree: false 58 | Link Tree Style: Links in Alphabetic Order 59 | base_footprint: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | base_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | camera_depth_frame: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | camera_depth_optical_frame: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | camera_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | camera_rgb_frame: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | camera_rgb_optical_frame: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | caster_back_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | caster_front_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | cliff_sensor_front_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | cliff_sensor_left_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | cliff_sensor_right_link: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | gyro_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | plate_bottom_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | plate_middle_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | plate_top_link: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | pole_bottom_0_link: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | pole_bottom_1_link: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | Value: true 140 | pole_bottom_2_link: 141 | Alpha: 1 142 | Show Axes: false 143 | Show Trail: false 144 | Value: true 145 | pole_bottom_3_link: 146 | Alpha: 1 147 | Show Axes: false 148 | Show Trail: false 149 | Value: true 150 | pole_bottom_4_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | pole_bottom_5_link: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | pole_kinect_0_link: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | pole_kinect_1_link: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | Value: true 170 | pole_middle_0_link: 171 | Alpha: 1 172 | Show Axes: false 173 | Show Trail: false 174 | Value: true 175 | pole_middle_1_link: 176 | Alpha: 1 177 | Show Axes: false 178 | Show Trail: false 179 | Value: true 180 | pole_middle_2_link: 181 | Alpha: 1 182 | Show Axes: false 183 | Show Trail: false 184 | Value: true 185 | pole_middle_3_link: 186 | Alpha: 1 187 | Show Axes: false 188 | Show Trail: false 189 | Value: true 190 | pole_top_0_link: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | Value: true 195 | pole_top_1_link: 196 | Alpha: 1 197 | Show Axes: false 198 | Show Trail: false 199 | Value: true 200 | pole_top_2_link: 201 | Alpha: 1 202 | Show Axes: false 203 | Show Trail: false 204 | Value: true 205 | pole_top_3_link: 206 | Alpha: 1 207 | Show Axes: false 208 | Show Trail: false 209 | Value: true 210 | wheel_left_link: 211 | Alpha: 1 212 | Show Axes: false 213 | Show Trail: false 214 | Value: true 215 | wheel_right_link: 216 | Alpha: 1 217 | Show Axes: false 218 | Show Trail: false 219 | Value: true 220 | Name: RobotModel 221 | Robot Description: robot_description 222 | TF Prefix: "" 223 | Update Interval: 0 224 | Value: true 225 | Visual Enabled: true 226 | - Alpha: 1 227 | Axes Length: 1 228 | Axes Radius: 0.1 229 | Class: rviz/Pose 230 | Color: 0; 0; 255 231 | Enabled: true 232 | Head Length: 0.3 233 | Head Radius: 0.1 234 | Name: Nav_Goal 235 | Shaft Length: 1 236 | Shaft Radius: 0.05 237 | Shape: Arrow 238 | Topic: /mpepc_goal_request 239 | Value: true 240 | - Alpha: 1 241 | Class: rviz/GridCells 242 | Color: 25; 255; 0 243 | Enabled: true 244 | Name: Obstacles 245 | Topic: /costmap_translator/obstacles 246 | Value: true 247 | - Angle Tolerance: 0.1 248 | Class: rviz/Odometry 249 | Color: 255; 25; 0 250 | Enabled: true 251 | Keep: 100 252 | Length: 0.1 253 | Name: Odometry 254 | Position Tolerance: 0.1 255 | Topic: /odom 256 | Value: true 257 | - Alpha: 0.7 258 | Class: rviz/Map 259 | Color Scheme: costmap 260 | Draw Behind: false 261 | Enabled: true 262 | Name: Nav_FN_Costmap 263 | Topic: /mpepc_plan/mpepc_costmap/costmap 264 | Value: true 265 | - Alpha: 1 266 | Buffer Length: 1 267 | Class: rviz/Path 268 | Color: 25; 255; 0 269 | Enabled: true 270 | Name: Nav_FN_Path 271 | Topic: /mpepc_plan/mpepc_navfn/plan 272 | Value: true 273 | - Arrow Length: 0.3 274 | Class: rviz/PoseArray 275 | Color: 255; 255; 0 276 | Enabled: true 277 | Name: Current_Trajectory 278 | Topic: /traj_plan 279 | Value: true 280 | - Alpha: 1 281 | Autocompute Intensity Bounds: true 282 | Autocompute Value Bounds: 283 | Max Value: 0.2972 284 | Min Value: 0.2972 285 | Value: true 286 | Axis: Z 287 | Channel Name: intensity 288 | Class: rviz/LaserScan 289 | Color: 255; 255; 255 290 | Color Transformer: AxisColor 291 | Decay Time: 0 292 | Enabled: false 293 | Invert Rainbow: false 294 | Max Color: 255; 255; 255 295 | Max Intensity: 4096 296 | Min Color: 0; 0; 0 297 | Min Intensity: 0 298 | Name: LaserScan 299 | Position Transformer: XYZ 300 | Queue Size: 10 301 | Selectable: true 302 | Size (Pixels): 3 303 | Size (m): 0.01 304 | Style: Squares 305 | Topic: /scan 306 | Use Fixed Frame: true 307 | Use rainbow: true 308 | Value: false 309 | - Class: rviz/TF 310 | Enabled: false 311 | Frame Timeout: 15 312 | Frames: 313 | All Enabled: true 314 | Marker Scale: 1 315 | Name: TF 316 | Show Arrows: true 317 | Show Axes: true 318 | Show Names: true 319 | Tree: 320 | {} 321 | Update Interval: 0 322 | Value: false 323 | Enabled: true 324 | Global Options: 325 | Background Color: 48; 48; 48 326 | Fixed Frame: odom 327 | Frame Rate: 30 328 | Name: root 329 | Tools: 330 | - Class: rviz/MoveCamera 331 | - Class: rviz/Interact 332 | Hide Inactive Objects: true 333 | - Class: rviz/Select 334 | - Class: rviz/SetInitialPose 335 | Topic: /initialpose 336 | - Class: rviz/SetGoal 337 | Topic: /mpepc_goal_request 338 | Value: true 339 | Views: 340 | Current: 341 | Class: rviz/Orbit 342 | Distance: 9.17401 343 | Enable Stereo Rendering: 344 | Stereo Eye Separation: 0.06 345 | Stereo Focal Distance: 1 346 | Swap Stereo Eyes: false 347 | Value: false 348 | Focal Point: 349 | X: -0.0344972 350 | Y: 0.065886 351 | Z: 0.148092 352 | Name: Current View 353 | Near Clip Distance: 0.01 354 | Pitch: 1.5698 355 | Target Frame: 356 | Value: Orbit (rviz) 357 | Yaw: 3.05358 358 | Saved: ~ 359 | Window Geometry: 360 | Displays: 361 | collapsed: false 362 | Height: 1024 363 | Hide Left Dock: false 364 | Hide Right Dock: false 365 | QMainWindow State: 000000ff00000000fd000000040000000000000195000003bafc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003ba000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005a4000003ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 366 | Selection: 367 | collapsed: false 368 | Time: 369 | collapsed: false 370 | Tool Properties: 371 | collapsed: false 372 | Views: 373 | collapsed: false 374 | Width: 1855 375 | X: 55 376 | Y: 14 377 | -------------------------------------------------------------------------------- /config/obstacle_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: odom 2 | robot_base_frame: /base_footprint 3 | map_type: costmap 4 | plugins: 5 | - {name: obstacles, type: "costmap_2d::ObstacleLayer"} 6 | 7 | update_frequency: 10.0 8 | publish_frequency: 5.0 9 | static_map: false 10 | rolling_window: true 11 | width: 20.0 12 | height: 20.0 13 | resolution: 0.025 14 | origin_x: -10.0 15 | origin_y: -10.0 16 | origin_z: 0.0 17 | 18 | transform_tolerance: 0.1 19 | obstacle_range: 5.0 20 | raytrace_range: 10.0 21 | inflation_radius: 0.38 22 | robot_radius: 0.20 23 | footprint_padding: 0.01 24 | 25 | obstacles: 26 | observation_sources: scan 27 | scan: {topic: scan, data_type: LaserScan, expected_update_rate: 0.3, obstacle_range: 5.5, raytrace_range: 10.0, observation_persistence: 0, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35} 28 | 29 | track_unknown_space: false 30 | global_frame: odom 31 | robot_base_frame: /base_footprint 32 | -------------------------------------------------------------------------------- /include/model_predictive_navigation/control_law.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLAW_H 2 | #define CONTROLLAW_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define LINEAR_THRESHOLD 0.05 14 | #define ANGULAR_THRESHOLD 0.05 15 | #define R_SPEED_LIMIT 0.5 16 | 17 | using namespace std; 18 | namespace model_predictive_navigation 19 | { 20 | struct EgoPolar 21 | { 22 | double r; 23 | double theta; 24 | double delta; 25 | }; 26 | 27 | struct ControlLawSettings 28 | { 29 | ControlLawSettings() { m_K1 = 2.0; m_K2 = 3.0; m_BETA = 0.4; m_LAMBDA = 2.0; m_R_THRESH = 1.5; m_R_THRESH_TRANS = 1.0; m_TRANS_INTERVAL = 2.3; m_V_MAX = 0.3; m_V_TRANS = 0.2; m_V_MIN = 0.05;} 30 | public: 31 | double m_K1; 32 | double m_K2; // K1 and K2 are parameters that shape the planned manifold 33 | double m_BETA; 34 | double m_LAMBDA; // Beta and Lambda are parameters to shape the linear velocity rule 35 | double m_R_THRESH; 36 | double m_R_THRESH_TRANS; // The thresholds of r to begin slowing down or switch to v_trans 37 | double m_TRANS_INTERVAL; // Time spent to complete the transition 38 | double m_V_MAX; // Max allowable linear velocity 39 | double m_V_TRANS; // Constant velocity used for transition 40 | double m_V_MIN; // Minimum velocity (not included in formulation, but used for practical reasons on real platform) 41 | }; 42 | 43 | class ControlLaw 44 | { 45 | public: 46 | ControlLaw(); 47 | explicit ControlLaw(ControlLawSettings c); 48 | geometry_msgs::Twist get_velocity_command(nav_msgs::Odometry current_position, geometry_msgs::Pose goal, double k1 = 2, double k2 = 3, double vMax = 0.3); 49 | geometry_msgs::Twist get_velocity_command(EgoPolar goal_coords, double k1, double k2, double vMax); 50 | geometry_msgs::Twist get_velocity_command(EgoPolar goal_coords, double vMax = 0.3); 51 | double get_ego_distance(nav_msgs::Odometry current_position, geometry_msgs::Pose goal); 52 | void update_k1_k2(double k1, double k2); 53 | EgoPolar convert_to_egopolar(nav_msgs::Odometry current_pose, geometry_msgs::Pose current_goal_pose); 54 | geometry_msgs::Pose convert_from_egopolar(nav_msgs::Odometry current_pose, EgoPolar current_goal_coords); 55 | double wrap_pos_neg_pi(double angle); 56 | 57 | protected: 58 | double get_kappa(EgoPolar current_ego_goal, double k1, double k2); 59 | double get_linear_vel(double kappa, EgoPolar current_ego_goal, double vMax); 60 | double get_angular_vel(double kappa, double linear_vel); 61 | double calc_sigmoid(double time_tau); 62 | 63 | private: 64 | static const double _PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; 65 | static const double _TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696; 66 | 67 | double mod(double x, double y); 68 | 69 | 70 | ControlLawSettings settings_; 71 | }; 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /launch/mpepc_turtlebot_nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /literature/Park-icra-11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MattDerry/model_predictive_navigation/9f952cee5a8c769a61e8491b10200277aec81dd8/literature/Park-icra-11.pdf -------------------------------------------------------------------------------- /literature/Park-iros-12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MattDerry/model_predictive_navigation/9f952cee5a8c769a61e8491b10200277aec81dd8/literature/Park-iros-12.pdf -------------------------------------------------------------------------------- /msg/EgoGoal.msg: -------------------------------------------------------------------------------- 1 | float32 r 2 | float32 theta 3 | float32 delta 4 | float32 vMax 5 | float32 k1 6 | float32 k2 -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | model_predictive_navigation 4 | 0.0.0 5 | The model_predictive_navigation package 6 | mderry 7 | GPLv3 8 | 9 | Matt Derry --> 10 | 11 | catkin 12 | 13 | message_generation 14 | costmap_2d 15 | geometry_msgs 16 | nav_msgs 17 | navfn 18 | roscpp 19 | rospy 20 | roslib 21 | std_msgs 22 | nav_core 23 | tf 24 | sensor_msgs 25 | 26 | message_runtime 27 | costmap_2d 28 | geometry_msgs 29 | nav_msgs 30 | navfn 31 | roscpp 32 | rospy 33 | roslib 34 | std_msgs 35 | nav_core 36 | tf 37 | sensor_msgs 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/control_law.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace model_predictive_navigation 5 | { 6 | ControlLaw::ControlLaw() 7 | { 8 | } 9 | 10 | ControlLaw::ControlLaw(ControlLawSettings c) 11 | { 12 | settings_ = c; 13 | } 14 | 15 | // Floating-point modulo 16 | // The result (the remainder) has same sign as the divisor. 17 | // Similar to matlab's mod(); Not similar to fmod() - Mod(-3,4)= 1 fmod(-3,4)= -3 18 | double ControlLaw::mod(double x, double y) 19 | { 20 | double m= x - y * floor(x/y); 21 | // handle boundary cases resulted from floating-point cut off: 22 | if (y > 0) // modulo range: [0..y) 23 | { 24 | if (m >= y) // Mod(-1e-16 , 360. ): m= 360. 25 | return 0; 26 | 27 | if (m < 0) 28 | { 29 | if (y+m == y) 30 | return 0; // just in case... 31 | else 32 | return y+m; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14 33 | } 34 | } 35 | else // modulo range: (y..0] 36 | { 37 | if (m <= y) // Mod(1e-16 , -360. ): m= -360. 38 | return 0; 39 | 40 | if (m > 0) 41 | { 42 | if (y+m == y) 43 | return 0; // just in case... 44 | else 45 | return y+m; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14 46 | } 47 | } 48 | return m; 49 | } 50 | 51 | void ControlLaw::update_k1_k2(double k1, double k2) 52 | { 53 | settings_.m_K1 = k1; 54 | settings_.m_K2 = k2; 55 | } 56 | 57 | // wrap [rad] angle to [-PI..PI) 58 | double ControlLaw::wrap_pos_neg_pi(double angle) 59 | { 60 | return mod(angle + _PI, _TWO_PI) - _PI; 61 | } 62 | 63 | double ControlLaw::get_ego_distance(nav_msgs::Odometry current_pose, geometry_msgs::Pose current_goal_pose) 64 | { 65 | double distance; 66 | 67 | double dx = current_goal_pose.position.x - current_pose.pose.pose.position.x; 68 | double dy = current_goal_pose.position.y - current_pose.pose.pose.position.y; 69 | 70 | // calculate distance compenent of egocentric coordinates 71 | distance = sqrt(pow(dx, 2) + pow(dy, 2)); 72 | 73 | return distance; 74 | } 75 | 76 | 77 | EgoPolar ControlLaw::convert_to_egopolar(nav_msgs::Odometry current_pose, geometry_msgs::Pose current_goal_pose) 78 | { 79 | EgoPolar coords; 80 | 81 | double dx = current_goal_pose.position.x - current_pose.pose.pose.position.x; 82 | double dy = current_goal_pose.position.y - current_pose.pose.pose.position.y; 83 | double obs_heading = atan2(dy, dx); 84 | double current_yaw = tf::getYaw(current_pose.pose.pose.orientation); 85 | double goal_yaw = tf::getYaw(current_goal_pose.orientation); 86 | 87 | // calculate r 88 | coords.r = sqrt(pow(dx, 2) + pow(dy, 2)); 89 | // calculate delta 90 | coords.delta = wrap_pos_neg_pi(current_yaw - obs_heading); 91 | // calculate theta 92 | coords.theta = wrap_pos_neg_pi(goal_yaw - obs_heading); 93 | 94 | return coords; 95 | } 96 | 97 | geometry_msgs::Pose ControlLaw::convert_from_egopolar(nav_msgs::Odometry current_pose, EgoPolar current_goal_coords) 98 | { 99 | geometry_msgs::Pose current_goal_pose; 100 | 101 | double current_yaw = tf::getYaw(current_pose.pose.pose.orientation); 102 | 103 | current_goal_pose.position.x = current_pose.pose.pose.position.x + current_goal_coords.r * cos(current_yaw - current_goal_coords.delta); 104 | current_goal_pose.position.y = current_pose.pose.pose.position.y + current_goal_coords.r * sin(current_yaw - current_goal_coords.delta); 105 | current_goal_pose.position.z = 0; 106 | 107 | current_goal_pose.orientation = tf::createQuaternionMsgFromYaw(current_yaw - current_goal_coords.delta + current_goal_coords.theta); 108 | 109 | return current_goal_pose; 110 | } 111 | 112 | double ControlLaw::get_kappa(EgoPolar current_ego_goal, double k1, double k2) 113 | { 114 | double kappa = 0; 115 | kappa = (-1/current_ego_goal.r)*(k2*(current_ego_goal.delta-atan(-1*k1*current_ego_goal.theta)) + (1 + k1/(1+pow(k1*current_ego_goal.theta, 2)))*sin(current_ego_goal.delta)); 116 | return kappa; 117 | } 118 | 119 | double ControlLaw::get_linear_vel(double kappa, EgoPolar current_ego_goal, double vMax) 120 | { 121 | double lin_vel = 0; 122 | lin_vel = min(settings_.m_V_MAX/settings_.m_R_THRESH*current_ego_goal.r, settings_.m_V_MAX/(1 + settings_.m_BETA * pow(abs(kappa), settings_.m_LAMBDA))); 123 | 124 | if (lin_vel < settings_.m_V_MIN && lin_vel > 0.00) 125 | { 126 | lin_vel = settings_.m_V_MIN; 127 | } 128 | return lin_vel; 129 | } 130 | 131 | double ControlLaw::calc_sigmoid(double time_tau) 132 | { 133 | double sigma = 0; 134 | sigma = 1.02040816*(1/(1+exp(-9.2*(time_tau-0.5))) - 0.01); 135 | if (sigma > 1) 136 | sigma = 1; 137 | else if (sigma < 0) 138 | sigma = 0; 139 | return sigma; 140 | } 141 | 142 | geometry_msgs::Twist ControlLaw::get_velocity_command(nav_msgs::Odometry current_position, geometry_msgs::Pose goal, double k1, double k2, double vMax) 143 | { 144 | EgoPolar goal_coords = convert_to_egopolar(current_position, goal); 145 | 146 | return get_velocity_command(goal_coords, k1, k2, vMax); 147 | } 148 | 149 | geometry_msgs::Twist ControlLaw::get_velocity_command(EgoPolar goal_coords, double vMax) 150 | { 151 | return get_velocity_command(goal_coords, settings_.m_K1, settings_.m_K2, vMax); 152 | } 153 | 154 | geometry_msgs::Twist ControlLaw::get_velocity_command(EgoPolar goal_coords, double k1, double k2, double vMax) 155 | { 156 | geometry_msgs::Twist cmd_vel; 157 | double kappa = 0; 158 | 159 | kappa = get_kappa(goal_coords, k1, k2); 160 | 161 | cmd_vel.linear.x = get_linear_vel(kappa, goal_coords, vMax); 162 | cmd_vel.angular.z = kappa*cmd_vel.linear.x; 163 | if (fabs(cmd_vel.angular.z) > R_SPEED_LIMIT) 164 | { 165 | if (cmd_vel.angular.z > 0) 166 | { 167 | cmd_vel.angular.z = R_SPEED_LIMIT; 168 | } 169 | else 170 | { 171 | cmd_vel.angular.z = -1*R_SPEED_LIMIT; 172 | } 173 | cmd_vel.linear.x = cmd_vel.angular.z/kappa; 174 | } 175 | 176 | return cmd_vel; 177 | } 178 | } // end namespace 179 | -------------------------------------------------------------------------------- /src/costmap_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | namespace costmap_2d 4 | { 5 | class Costmap2DNode 6 | { 7 | public: 8 | Costmap2DNode(tf::TransformListener& tf) : costmap_ros_("costmap", tf) {} 9 | private: 10 | Costmap2DROS costmap_ros_; 11 | }; 12 | }; 13 | 14 | int main(int argc, char** argv) 15 | { 16 | ros::init(argc, argv, "costmap_node"); 17 | 18 | tf::TransformListener tf(ros::Duration(10)); 19 | 20 | costmap_2d::Costmap2DNode* costmap_node; 21 | costmap_node = new costmap_2d::Costmap2DNode(tf); 22 | 23 | ros::spin(); 24 | 25 | delete costmap_node; 26 | 27 | return(0); 28 | } 29 | -------------------------------------------------------------------------------- /src/costmap_translator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | ros::Subscriber map_sub; 9 | ros::Publisher obstacle_publisher; 10 | 11 | void map_cb(const nav_msgs::OccupancyGridConstPtr &map) 12 | { 13 | nav_msgs::GridCells obstacles; 14 | obstacles.header.stamp = map->header.stamp; 15 | obstacles.header.frame_id = map->header.frame_id; 16 | obstacles.cell_width = map->info.resolution; 17 | obstacles.cell_height = map->info.resolution; 18 | 19 | for (unsigned int i = 0 ; i < map->info.height; ++i) 20 | { 21 | for(unsigned int j = 0; j < map->info.width; ++j) 22 | { 23 | if(map->data[i*map->info.height+j] == 100) 24 | { 25 | geometry_msgs::Point obstacle_coordinates; 26 | obstacle_coordinates.x = (j * obstacles.cell_height) + map->info.origin.position.x + (map->info.resolution/2.0); 27 | obstacle_coordinates.y = (i * obstacles.cell_width) + map->info.origin.position.y + (map->info.resolution/2.0); 28 | obstacle_coordinates.z = 0; 29 | obstacles.cells.push_back(obstacle_coordinates); 30 | } 31 | } 32 | } 33 | 34 | obstacle_publisher.publish(obstacles); 35 | } 36 | 37 | int main (int argc, char** argv) 38 | { 39 | // Initialize ROS 40 | ros::init (argc, argv, "costmap_translator"); 41 | ros::NodeHandle nh; 42 | 43 | obstacle_publisher = nh.advertise("/costmap_translator/obstacles", 1); 44 | map_sub = nh.subscribe("/costmap_server/costmap/costmap", 1, map_cb); 45 | 46 | ros::spin(); 47 | } 48 | -------------------------------------------------------------------------------- /src/mpepc_plan.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include "flann/flann.hpp" 27 | 28 | using ::model_predictive_navigation::ControlLaw; 29 | using ::model_predictive_navigation::ControlLawSettings; 30 | using ::model_predictive_navigation::EgoPolar; 31 | using ::model_predictive_navigation::EgoGoal; 32 | 33 | #define RATE_FACTOR 0.2 34 | #define DEFAULT_LOOP_RATE 10 35 | 36 | #define RESULT_BEGIN 1 37 | #define RESULT_SUCCESS 2 38 | #define RESULT_CANCEL 3 39 | 40 | // Trajectory Model Params 41 | #define K_1 1.2 // 2 42 | #define K_2 3 // 8 43 | #define BETA 0.4 // 0.5 44 | #define LAMBDA 2 // 3 45 | #define R_THRESH 0.05 46 | #define V_MAX 0.3 // 0.3 47 | #define V_MIN 0.0 48 | 49 | // Trajectory Optimization Params 50 | #define TIME_HORIZON 5.0 51 | #define DELTA_SIM_TIME 0.2 52 | #define SAFETY_ZONE 0.225 53 | #define WAYPOINT_THRESH 1.75 54 | 55 | // Cost function params 56 | static const double C1 = 0.05; 57 | static const double C2 = 2.5; 58 | static const double C3 = 0.05; // 0.15 59 | static const double C4 = 0.05; // 0.2 //turn 60 | static const double PHI_COL = 1.0; // 0.4 61 | static const double SIGMA = 0.2; // 0.10 62 | 63 | static const double PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; 64 | static const double TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696; 65 | static const double minusPI= -3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; 66 | 67 | ros::NodeHandle * nh; 68 | 69 | boost::mutex goals_mutex_; 70 | vector current_goals; 71 | 72 | ros::Publisher ego_goal_pub, plan_pub, min_goal_obstacle_pub, traj_pub; 73 | ros::Subscriber goal_pose_sub, odom_sub, nav_result_sub, cost_map_sub; 74 | 75 | geometry_msgs::PoseStamped global_goal_pose, current_pose_stamp; 76 | nav_msgs::Odometry current_pose; 77 | nav_msgs::GridCells cost_map; 78 | 79 | std_msgs::Int32 current_status; 80 | bool bHasGoal, bHasCostMap, bHasOdom; 81 | bool bUseWaypointFollowing; 82 | boost::mutex pose_mutex_, cost_map_mutex_; 83 | 84 | navfn::NavfnROS nav_fn; 85 | costmap_2d::Costmap2DROS * costmap; 86 | 87 | ControlLaw * cl; 88 | 89 | flann::Index > * obs_tree; 90 | flann::Matrix * data; 91 | 92 | int trajectory_count; 93 | 94 | double map_resolution; 95 | double interp_rotation_factor; 96 | 97 | struct Point { 98 | float a; 99 | float b; 100 | int member; 101 | int p_idx; 102 | Point(float x, float y) : a(x), b(y), member(-1), p_idx(0) {} 103 | Point() : a(0), b(0), member(-1), p_idx(0) {} 104 | inline bool operator==(Point p) { 105 | if (p.a == a && p.b == b) 106 | return true; 107 | else 108 | return false; 109 | } 110 | }; 111 | 112 | struct MinDistResult { 113 | Point p; 114 | double dist; 115 | }; 116 | 117 | double mod(double x, double y) 118 | { 119 | double m= x - y * floor(x/y); 120 | // handle boundary cases resulted from floating-point cut off: 121 | if (y > 0) // modulo range: [0..y) 122 | { 123 | if (m >= y) // Mod(-1e-16 , 360. ): m= 360. 124 | return 0; 125 | 126 | if (m < 0) 127 | { 128 | if (y+m == y) 129 | return 0; // just in case... 130 | else 131 | return y+m; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14 132 | } 133 | } 134 | else // modulo range: (y..0] 135 | { 136 | if (m <= y) // Mod(1e-16 , -360. ): m= -360. 137 | return 0; 138 | 139 | if (m>0 ) 140 | { 141 | if (y+m == y) 142 | return 0; // just in case... 143 | else 144 | return y+m; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14 145 | } 146 | } 147 | 148 | return m; 149 | } 150 | 151 | vector find_points_within_threshold(Point newPoint, double threshold) 152 | { 153 | vector results; 154 | 155 | flann::Matrix query(new float[2], 1, 2); 156 | query[0][0] = newPoint.a; 157 | query[0][1] = newPoint.b; 158 | 159 | std::vector< std::vector > indices; 160 | std::vector< std::vector > dists; 161 | 162 | flann::SearchParams params; 163 | params.checks = 128; 164 | params.max_neighbors = -1; 165 | params.sorted = true; 166 | // ROS_INFO("Do search"); 167 | { 168 | boost::mutex::scoped_lock lock(cost_map_mutex_); 169 | obs_tree->radiusSearch(query, indices, dists, threshold, params); 170 | 171 | // ROS_INFO("Finished search"); 172 | for (int i = 0; i < indices[0].size(); i++) 173 | { 174 | MinDistResult result; 175 | result.p = Point((*data)[indices[0][i]][0], (*data)[indices[0][i]][1]); 176 | result.dist = static_cast(dists[0][i]); 177 | results.push_back(result); 178 | } 179 | } 180 | 181 | delete[] query.ptr(); 182 | indices.clear(); 183 | dists.clear(); 184 | 185 | return results; 186 | } 187 | 188 | MinDistResult find_nearest_neighbor(Point queryPoint) 189 | { 190 | MinDistResult results; 191 | 192 | flann::Matrix query(new float[2], 1, 2); 193 | query[0][0] = queryPoint.a; 194 | query[0][1] = queryPoint.b; 195 | 196 | std::vector< std::vector > indices; 197 | std::vector< std::vector > dists; 198 | 199 | flann::SearchParams params; 200 | params.checks = 128; 201 | params.sorted = true; 202 | 203 | { 204 | boost::mutex::scoped_lock lock(cost_map_mutex_); 205 | obs_tree->knnSearch(query, indices, dists, 1, params); 206 | results.p = Point((*data)[indices[0][0]][0], (*data)[indices[0][0]][1]); 207 | results.dist = static_cast(dists[0][0]); 208 | } 209 | 210 | MinDistResult tempResults; 211 | tempResults.p = Point(cost_map.cells[indices[0][0]].x, cost_map.cells[indices[0][0]].y); 212 | 213 | delete[] query.ptr(); 214 | indices.clear(); 215 | dists.clear(); 216 | 217 | return results; 218 | } 219 | 220 | double distance(double pose_x, double pose_y, double obx, double oby) 221 | { 222 | double diffx = obx - pose_x; 223 | double diffy = oby - pose_y; 224 | double dist = sqrt(diffx*diffx + diffy*diffy); 225 | return dist; 226 | } 227 | 228 | double min_distance_to_obstacle(geometry_msgs::Pose local_current_pose, double *heading) 229 | { 230 | // ROS_INFO("In minDist Function"); 231 | Point global(local_current_pose.position.x, local_current_pose.position.y); 232 | MinDistResult nn_graph_point = find_nearest_neighbor(global); 233 | 234 | double minDist = 100000; 235 | double head = 0; 236 | 237 | double SOME_THRESH = 1.5; 238 | 239 | if(nn_graph_point.dist < SOME_THRESH) 240 | { 241 | int min_i = 0; 242 | vector distResult; 243 | distResult = find_points_within_threshold(global, 1.1*SOME_THRESH); 244 | 245 | //ROS_INFO("Loop through %d points from radius search", distResult.size()); 246 | for (unsigned int i = 0 ; i < distResult.size() && minDist > 0; i++) 247 | { 248 | double dist = distance(local_current_pose.position.x, local_current_pose.position.y, cost_map.cells[i].x, cost_map.cells[i].y); 249 | if (dist < minDist) 250 | { 251 | minDist = dist; 252 | min_i = i; 253 | } 254 | } 255 | 256 | // ROS_INFO("Calculate heading"); 257 | head = tf::getYaw(local_current_pose.orientation) - atan2(cost_map.cells[min_i].y - local_current_pose.position.y, cost_map.cells[min_i].x - local_current_pose.position.x); 258 | head = mod(head + PI, TWO_PI) - PI; 259 | //ROS_INFO("Got nearest radius neighbor, poly dist: %f", minDist); 260 | } 261 | else 262 | { 263 | minDist = distance(local_current_pose.position.x, local_current_pose.position.y, nn_graph_point.p.a, nn_graph_point.p.b); 264 | //ROS_INFO("Got nearest neighbor, poly dist: %f", minDist); 265 | } 266 | 267 | *heading = head; 268 | 269 | return minDist; 270 | } 271 | 272 | float get_obstacle_distance() 273 | { 274 | if (!bHasCostMap || !bHasOdom) 275 | { 276 | return 0; 277 | } 278 | 279 | double heading = 0.0; 280 | 281 | nav_msgs::Odometry cur_pose; 282 | { 283 | boost::mutex::scoped_lock lock(pose_mutex_); 284 | cur_pose = current_pose; 285 | } 286 | 287 | return min_distance_to_obstacle(cur_pose.pose.pose, &heading); 288 | } 289 | 290 | float get_goal_distance() 291 | { 292 | if (!bHasCostMap || !bHasOdom) 293 | { 294 | return 0; 295 | } 296 | 297 | nav_msgs::Odometry cur_pose; 298 | { 299 | boost::mutex::scoped_lock lock(pose_mutex_); 300 | cur_pose = current_pose; 301 | } 302 | 303 | return distance(global_goal_pose.pose.position.x, global_goal_pose.pose.position.y, cur_pose.pose.pose.position.x, cur_pose.pose.pose.position.y); 304 | } 305 | 306 | // Bilinear Interpolation on navigation function 307 | double get_interpolated_point_potential(geometry_msgs::Point position) 308 | { 309 | costmap_2d::Costmap2D * temp_costmap; 310 | temp_costmap = costmap->getCostmap(); 311 | 312 | // find world coords of current cell 313 | double cell_wx, cell_wy; 314 | unsigned int cell_mx, cell_my; 315 | temp_costmap->worldToMap(position.x, position.y, cell_mx, cell_my); 316 | temp_costmap->mapToWorld(cell_mx, cell_my, cell_wx, cell_wy); 317 | 318 | geometry_msgs::Point tempPoint = position; 319 | 320 | tempPoint.x = tempPoint.x + map_resolution; 321 | double cost0 = nav_fn.getPointPotential(tempPoint); 322 | 323 | tempPoint = position; 324 | tempPoint.y = tempPoint.y + map_resolution; 325 | double cost90 = nav_fn.getPointPotential(tempPoint); 326 | 327 | tempPoint = position; 328 | tempPoint.x = tempPoint.x - map_resolution; 329 | double cost180 = nav_fn.getPointPotential(tempPoint); 330 | 331 | // Block at 270 332 | tempPoint = position; 333 | tempPoint.y = tempPoint.y - map_resolution; 334 | double cost270 = nav_fn.getPointPotential(tempPoint); 335 | 336 | geometry_msgs::Point rotPoint; 337 | rotPoint.x = ((position.x - cell_wx)*cos(-1*PI/4) - (position.y - cell_wy)*sin(-1*PI/4)) + interp_rotation_factor; 338 | rotPoint.y = ((position.x - cell_wx)*sin(-1*PI/4) + (position.y - cell_wy)*cos(-1*PI/4)) + interp_rotation_factor; 339 | 340 | double a00 = cost180; 341 | double a10 = cost270 - cost180; 342 | double a01 = cost90 - cost180; 343 | double a11 = cost180 - cost270 - cost90 + cost0; 344 | 345 | return a00 + a10*rotPoint.x + a01*rotPoint.y + a11*rotPoint.x*rotPoint.y; 346 | } 347 | 348 | // This function is used by the optimizer to score different trajectories 349 | double sim_trajectory(double r, double delta, double theta, double vMax, double time_horizon) 350 | { 351 | nav_msgs::Odometry sim_pose; 352 | 353 | { 354 | boost::mutex::scoped_lock lock(pose_mutex_); 355 | sim_pose = current_pose; 356 | } 357 | 358 | EgoPolar sim_goal; 359 | sim_goal.r = r; 360 | sim_goal.delta = delta; 361 | sim_goal.theta = theta; 362 | 363 | geometry_msgs::Pose current_goal = cl->convert_from_egopolar(sim_pose, sim_goal); 364 | 365 | double SIGMA_DENOM = pow(SIGMA, 2); 366 | 367 | double sim_clock = 0.0; 368 | 369 | geometry_msgs::Twist sim_cmd_vel; 370 | double current_yaw = tf::getYaw(sim_pose.pose.pose.orientation); 371 | geometry_msgs::Point collisionPoint; 372 | bool collision_detected = false; 373 | 374 | double expected_progress = 0.0; 375 | double expected_action = 0.0; 376 | double expected_collision = 0.0; 377 | 378 | double nav_fn_t0 = 0; 379 | double nav_fn_t1 = 0; 380 | double collision_prob = 0.0; 381 | double survivability = 1.0; 382 | double obstacle_heading = 0.0; 383 | 384 | while (sim_clock < time_horizon) 385 | { 386 | // Get Velocity Commands 387 | sim_cmd_vel = cl->get_velocity_command(sim_goal, vMax); 388 | 389 | // get navigation function at orig pose 390 | nav_fn_t0 = get_interpolated_point_potential(sim_pose.pose.pose.position); 391 | 392 | // Update pose 393 | current_yaw = current_yaw + (sim_cmd_vel.angular.z * DELTA_SIM_TIME); 394 | sim_pose.pose.pose.position.x = sim_pose.pose.pose.position.x + (sim_cmd_vel.linear.x * DELTA_SIM_TIME * cos(current_yaw)); 395 | sim_pose.pose.pose.position.y = sim_pose.pose.pose.position.y + (sim_cmd_vel.linear.x * DELTA_SIM_TIME * sin(current_yaw)); 396 | sim_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_yaw); 397 | 398 | // Get navigation function at new pose 399 | nav_fn_t1 = get_interpolated_point_potential(sim_pose.pose.pose.position); 400 | 401 | double minDist = min_distance_to_obstacle(sim_pose.pose.pose, &obstacle_heading); 402 | 403 | if (minDist <= SAFETY_ZONE) 404 | { 405 | // ROS_INFO("Collision Detected"); 406 | collision_detected = true; 407 | } 408 | 409 | // Get collision probability 410 | if (!collision_detected) 411 | { 412 | collision_prob = exp(-1*pow(minDist, 2)/SIGMA_DENOM); // sigma^2 413 | } 414 | else 415 | { 416 | collision_prob = 1; 417 | } 418 | 419 | // Get survivability 420 | survivability = survivability*(1 - collision_prob); 421 | 422 | expected_collision = expected_collision + ((1-survivability) * C2); 423 | 424 | // Get progress cost 425 | expected_progress = expected_progress + (survivability * (nav_fn_t1 - nav_fn_t0)); 426 | 427 | // Get action cost 428 | expected_action = expected_action + (C3 * pow(sim_cmd_vel.linear.x, 2) + C4 * pow(sim_cmd_vel.angular.z, 2))*DELTA_SIM_TIME; 429 | 430 | // Calculate new EgoPolar coords for goal 431 | sim_goal = cl->convert_to_egopolar(sim_pose, current_goal); 432 | 433 | sim_clock = sim_clock + DELTA_SIM_TIME; 434 | } 435 | 436 | // Update with angle heuristic - weighted difference between final pose and gradient of navigation function 437 | double gradient_angle = get_interpolated_point_potential(sim_pose.pose.pose.position); 438 | 439 | expected_progress = expected_progress + C1 * abs(tf::getYaw(sim_pose.pose.pose.orientation) - gradient_angle); 440 | 441 | ++trajectory_count; 442 | 443 | // SUM collision cost, progress cost, action cost 444 | return (expected_collision + expected_progress + expected_action); 445 | } 446 | 447 | double score_trajectory(const std::vector &x, std::vector &grad, void* f_data) 448 | { 449 | double time_horizon = TIME_HORIZON; 450 | return sim_trajectory(x[0], x[1], x[2], x[3], time_horizon); 451 | } 452 | 453 | // Use NLOPT to find the next subgoal for the trajectory generator 454 | void find_intermediate_goal_params(EgoGoal *next_step) 455 | { 456 | trajectory_count = 0; 457 | 458 | int max_iter = 250; // 30 459 | nlopt::opt opt = nlopt::opt(nlopt::GN_DIRECT_NOSCAL, 4); 460 | opt.set_min_objective(score_trajectory, NULL); 461 | opt.set_xtol_rel(0.0001); 462 | std::vector lb; 463 | std::vector rb; 464 | lb.push_back(0); 465 | lb.push_back(-1.8); 466 | lb.push_back(-1.8); 467 | lb.push_back(V_MIN); 468 | rb.push_back(5.0); 469 | rb.push_back(1.8); 470 | rb.push_back(1.8); 471 | rb.push_back(V_MAX); 472 | opt.set_lower_bounds(lb); 473 | opt.set_upper_bounds(rb); 474 | opt.set_maxeval(max_iter); 475 | 476 | std::vector k(4); 477 | k[0] = 0.0; 478 | k[1] = 0.0; 479 | k[2] = 0.0; 480 | k[3] = 0.0; 481 | double minf; 482 | 483 | opt.optimize(k, minf); 484 | 485 | ROS_DEBUG("Global Optimization - Trajectories evaluated: %d", trajectory_count); 486 | trajectory_count = 0; 487 | 488 | max_iter = 75; // 200 489 | nlopt::opt opt2 = nlopt::opt(nlopt::LN_BOBYQA, 4); 490 | opt2.set_min_objective(score_trajectory, NULL); 491 | opt2.set_xtol_rel(0.0001); 492 | std::vector lb2; 493 | std::vector rb2; 494 | lb2.push_back(0); 495 | lb2.push_back(-1.8); 496 | lb2.push_back(-3.1); 497 | lb2.push_back(V_MIN); 498 | rb2.push_back(5.0); 499 | rb2.push_back(1.8); 500 | rb2.push_back(3.1); 501 | rb2.push_back(V_MAX); 502 | opt2.set_lower_bounds(lb2); 503 | opt2.set_upper_bounds(rb2); 504 | opt2.set_maxeval(max_iter); 505 | 506 | opt2.optimize(k, minf); 507 | 508 | ROS_DEBUG("Local Optimization - Trajectories evaluated: %d", trajectory_count); 509 | trajectory_count = 0; 510 | 511 | next_step->r = k[0]; 512 | next_step->delta = k[1]; 513 | next_step->theta = k[2]; 514 | next_step->vMax = k[3]; 515 | next_step->k1 = K_1; 516 | next_step->k2 = K_2; 517 | 518 | return; 519 | } 520 | 521 | void nav_status_cb(const std_msgs::Int32::ConstPtr& status) 522 | { 523 | ROS_DEBUG("Recieved Status: %d", status->data); 524 | current_status = *status; 525 | } 526 | 527 | void goal_cb(const geometry_msgs::PoseStamped::ConstPtr& nav_goal) 528 | { 529 | ROS_DEBUG("Recieved Goal Request"); 530 | global_goal_pose = *nav_goal; 531 | 532 | bHasGoal = true; 533 | } 534 | 535 | void odom_cb(const nav_msgs::Odometry::ConstPtr& pose) 536 | { 537 | boost::mutex::scoped_lock lock(pose_mutex_); 538 | current_pose = *pose; 539 | current_pose_stamp.header = current_pose.header; 540 | current_pose_stamp.pose = current_pose.pose.pose; 541 | bHasOdom = true; 542 | } 543 | 544 | // Place new list of occupied cells in to a tree structure that supports fast 545 | // nearest neighbor searching 546 | void nav_cb(const nav_msgs::GridCells::ConstPtr& grid_cells) 547 | { 548 | boost::mutex::scoped_lock lock(cost_map_mutex_); 549 | cost_map = *grid_cells; 550 | 551 | if(cost_map.cells.size() > 0) 552 | { 553 | delete obs_tree; 554 | delete data; 555 | data = new flann::Matrix(new float[grid_cells->cells.size()*2], grid_cells->cells.size(), 2); 556 | 557 | for (size_t i = 0; i < data->rows; ++i) 558 | { 559 | for (size_t j = 0; j < data->cols; ++j) 560 | { 561 | if (j == 0) 562 | (*data)[i][j] = cost_map.cells[i].x; 563 | else 564 | (*data)[i][j] = cost_map.cells[i].y; 565 | } 566 | } 567 | // Obstacle index for fast nearest neighbor search 568 | obs_tree = new flann::Index >(*data, flann::KDTreeIndexParams(4)); 569 | obs_tree->buildIndex(); 570 | bHasCostMap = true; 571 | } 572 | } 573 | 574 | geometry_msgs::PoseArray get_trajectory_viz(EgoGoal new_coords) 575 | { 576 | geometry_msgs::PoseArray viz_plan; 577 | viz_plan.header.stamp = ros::Time::now(); 578 | viz_plan.header.frame_id = "/odom"; 579 | viz_plan.poses.resize(1); 580 | 581 | nav_msgs::Odometry sim_pose; 582 | 583 | { 584 | boost::mutex::scoped_lock lock(pose_mutex_); 585 | sim_pose = current_pose; 586 | } 587 | 588 | EgoPolar sim_goal; 589 | sim_goal.r = new_coords.r; 590 | sim_goal.delta = new_coords.delta; 591 | sim_goal.theta = new_coords.theta; 592 | 593 | geometry_msgs::Pose current_goal = cl->convert_from_egopolar(sim_pose, sim_goal); 594 | 595 | double sim_clock = 0.0; 596 | 597 | geometry_msgs::Twist sim_cmd_vel; 598 | double current_yaw = tf::getYaw(sim_pose.pose.pose.orientation); 599 | 600 | while (sim_clock < TIME_HORIZON) 601 | { 602 | sim_cmd_vel = cl->get_velocity_command(sim_goal, new_coords.k1, new_coords.k2, new_coords.vMax); 603 | 604 | // Update pose 605 | current_yaw = current_yaw + (sim_cmd_vel.angular.z * DELTA_SIM_TIME); 606 | sim_pose.pose.pose.position.x = sim_pose.pose.pose.position.x + (sim_cmd_vel.linear.x * DELTA_SIM_TIME * cos(current_yaw)); 607 | sim_pose.pose.pose.position.y = sim_pose.pose.pose.position.y + (sim_cmd_vel.linear.x * DELTA_SIM_TIME * sin(current_yaw)); 608 | sim_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_yaw); 609 | viz_plan.poses.push_back(sim_pose.pose.pose); 610 | 611 | sim_goal = cl->convert_to_egopolar(sim_pose, current_goal); 612 | 613 | sim_clock = sim_clock + DELTA_SIM_TIME; 614 | } 615 | 616 | return viz_plan; 617 | } 618 | 619 | int main(int argc, char** argv) 620 | { 621 | // Initialize ROS 622 | ros::init(argc, argv, "mpepc_planner"); 623 | ros::NodeHandle n; 624 | nh = &n; 625 | 626 | // Initialize Motion Model 627 | ControlLawSettings settings; 628 | settings.m_K1 = K_1; 629 | settings.m_K2 = K_2; 630 | settings.m_BETA = BETA; 631 | settings.m_LAMBDA = LAMBDA; 632 | settings.m_R_THRESH = R_THRESH; 633 | settings.m_V_MAX = V_MAX; 634 | settings.m_V_MIN = V_MIN; 635 | ControlLaw claw(settings); 636 | cl = &claw; 637 | 638 | current_pose.pose.pose.orientation.x = 0; 639 | current_pose.pose.pose.orientation.y = 0; 640 | current_pose.pose.pose.orientation.z = 0; 641 | current_pose.pose.pose.orientation.w = 1; 642 | 643 | tf::TransformListener tf(ros::Duration(10)); 644 | costmap_2d::Costmap2DROS costmap_init("mpepc_costmap", tf); 645 | costmap = &costmap_init; 646 | nav_fn.initialize("mpepc_navfn", costmap); 647 | 648 | map_resolution = (costmap->getCostmap())->getResolution(); 649 | interp_rotation_factor = (sqrt(2) * map_resolution - map_resolution)/2; 650 | 651 | ROS_DEBUG("map_resolution %f", map_resolution); 652 | 653 | double loop_rate = DEFAULT_LOOP_RATE; 654 | double global_loop_rate; 655 | if (ros::param::has("/global_loop_rate")) 656 | { 657 | nh->getParam("/global_loop_rate", global_loop_rate); 658 | loop_rate= RATE_FACTOR*global_loop_rate; 659 | ROS_DEBUG("mpepc_plan: Loop rate updated using global_loop_rate to : %f", loop_rate); 660 | } 661 | ros::Rate rate_obj(25.0); 662 | 663 | bHasGoal = false; 664 | bHasCostMap = false; 665 | bUseWaypointFollowing = false; 666 | current_status.data = RESULT_CANCEL; 667 | 668 | ego_goal_pub = nh->advertise ("int_goal_pose", 1); 669 | 670 | odom_sub = nh->subscribe("/odom", 1, odom_cb); 671 | nav_result_sub = nh->subscribe ("/nav_result", 1, nav_status_cb); 672 | goal_pose_sub = nh->subscribe("/goal_request", 1, goal_cb); 673 | cost_map_sub = nh->subscribe("/costmap_translator/obstacles", 10, nav_cb); // cost 674 | 675 | plan_pub = nh->advertise("/navfn_plan", 1); 676 | traj_pub = nh->advertise("/traj_plan", 1); 677 | min_goal_obstacle_pub = nh->advertise("min_goal_obstacle_dist", 1); 678 | 679 | vector plan; 680 | std_msgs::Float32MultiArray minDist_goal_obstacle_f; 681 | minDist_goal_obstacle_f.data.resize(2); 682 | 683 | float minObstacleDist = 0.0; 684 | 685 | ros::AsyncSpinner aspin(4); 686 | aspin.start(); 687 | // Control Loop and code 688 | while (ros::ok()) 689 | { 690 | if (current_status.data == RESULT_BEGIN && bHasGoal && bHasCostMap && bHasOdom) 691 | { 692 | plan.clear(); 693 | ROS_DEBUG("Begin Planning"); 694 | EgoGoal new_coords; 695 | 696 | ROS_DEBUG("Compute Nav Function"); 697 | nav_fn.computePotential(global_goal_pose.pose.position); 698 | geometry_msgs::PoseStamped temp_pose; 699 | 700 | { 701 | boost::mutex::scoped_lock lock(pose_mutex_); 702 | temp_pose.header = current_pose.header; 703 | temp_pose.pose = current_pose.pose.pose; 704 | } 705 | 706 | nav_fn.getPlanFromPotential(temp_pose, plan); 707 | 708 | if (!plan.empty()) 709 | { 710 | ROS_DEBUG("Published Plan"); 711 | geometry_msgs::PoseArray viz_plan; 712 | viz_plan.header = plan.front().header; 713 | for (int i = 0; i < plan.size(); i++) 714 | { 715 | viz_plan.poses.push_back(plan[i].pose); 716 | } 717 | plan_pub.publish(viz_plan); 718 | } 719 | else 720 | { 721 | ROS_DEBUG("Plan empty"); 722 | } 723 | 724 | ROS_DEBUG("Optimize"); 725 | double min_goal_dist = get_goal_distance(); 726 | bUseWaypointFollowing = false; 727 | find_intermediate_goal_params(&new_coords); 728 | 729 | traj_pub.publish(get_trajectory_viz(new_coords)); 730 | 731 | ROS_DEBUG("Publish"); 732 | ego_goal_pub.publish(new_coords); 733 | } 734 | 735 | if (bHasGoal) 736 | { 737 | minDist_goal_obstacle_f.data[0] = get_goal_distance(); 738 | minObstacleDist = get_obstacle_distance(); 739 | minDist_goal_obstacle_f.data[1] = minObstacleDist; 740 | min_goal_obstacle_pub.publish(minDist_goal_obstacle_f); 741 | } 742 | 743 | rate_obj.sleep(); 744 | } 745 | } 746 | -------------------------------------------------------------------------------- /src/mpepc_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using ::model_predictive_navigation::ControlLaw; 26 | using ::model_predictive_navigation::ControlLawSettings; 27 | using ::model_predictive_navigation::EgoPolar; 28 | using ::model_predictive_navigation::EgoGoal; 29 | 30 | #define RATE_FACTOR 0.5 31 | #define DEFAULT_LOOP_RATE 25 32 | 33 | #define RESULT_BEGIN 1 34 | #define RESULT_SUCCESS 2 35 | #define RESULT_CANCEL 3 36 | 37 | // Trajectory model parameters 38 | #define K_1 1.2 // 2 39 | #define K_2 3 // 8 40 | #define BETA 0.4 // 0.5 41 | #define LAMBDA 2 // 3 42 | #define R_THRESH 0.15 43 | #define V_MAX 0.3 // 0.3 44 | #define V_MIN 0.00 45 | 46 | #define R_SPEED_LIMIT 0.5 47 | #define V_SPEED_LIMIT 0.3 48 | 49 | // Parameters to determine success 50 | #define GOAL_DIST_UPDATE_THRESH 0.15 // in meters 51 | #define GOAL_ANGLE_UPDATE_THRESH 0.1 // in radians 52 | 53 | #define GOAL_DIST_ID_THRESH 0.1 // in meters 54 | #define GOAL_ANGLE_ID_THRESH 0.3 // in radians 55 | 56 | static const double PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; 57 | static const double TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696; 58 | static const double minusPI= -3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; 59 | 60 | ros::Publisher cmd_pub, current_goal_pub, nav_result_pub, goal_request_pub, rviz_goal_request_pub, min_Obstacle_pub; 61 | ros::Subscriber goal_sub, odom_sub, joy_sub, cost_map_sub, int_goal_pose_sub; 62 | 63 | ros::ServiceServer cancel_goal_service; 64 | ros::NodeHandle * nh; 65 | 66 | geometry_msgs::PoseStamped global_goal_pose, inter_goal_pose; 67 | EgoPolar inter_goal_coords; 68 | double inter_goal_vMax, inter_goal_k1, inter_goal_k2; 69 | nav_msgs::Odometry current_pose; 70 | std::vector plan; 71 | nav_msgs::GridCells cost_map; 72 | 73 | std_msgs::Int32 result; 74 | bool bFollowingTrajectory, bHasGoal, bHasIntGoal, prevButton, bHasCostMap, bHasOdom; 75 | boost::mutex pose_mutex_, cost_map_mutex_, int_goal_mutex_; 76 | 77 | ControlLaw * cl; 78 | 79 | boost::mutex goals_mutex_; 80 | std::vector current_goals; 81 | unsigned int current_nav_goal_idx; 82 | bool bCurrentStepIsTrajectoryEnd; 83 | 84 | double distance(double pose_x, double pose_y, double obx, double oby) 85 | { 86 | double diffx = obx - pose_x; 87 | double diffy = oby - pose_y; 88 | double dist = sqrt(diffx*diffx + diffy*diffy); 89 | return dist; 90 | } 91 | 92 | void cancel_auton_nav(int res) 93 | { 94 | bFollowingTrajectory = false; 95 | bHasIntGoal = false; 96 | result.data = res; 97 | nav_result_pub.publish(result); 98 | } 99 | 100 | void int_goal_cb(const model_predictive_navigation::EgoGoal::ConstPtr& ego_goal) 101 | { 102 | boost::mutex::scoped_lock lock(pose_mutex_); 103 | 104 | inter_goal_coords.r = ego_goal->r; 105 | inter_goal_coords.delta = ego_goal->delta; 106 | inter_goal_coords.theta = ego_goal->theta; 107 | inter_goal_vMax = ego_goal->vMax; 108 | inter_goal_k1 = ego_goal->k1; 109 | inter_goal_k2 = ego_goal->k2; 110 | 111 | inter_goal_pose.header.stamp = ros::Time::now(); 112 | inter_goal_pose.header.frame_id = "/odom"; 113 | inter_goal_pose.pose = cl->convert_from_egopolar(current_pose, inter_goal_coords); 114 | 115 | bHasIntGoal = true; 116 | } 117 | 118 | void odom_cb(const nav_msgs::Odometry::ConstPtr& pose) 119 | { 120 | boost::mutex::scoped_lock lock(pose_mutex_); 121 | current_pose = *pose; 122 | bHasOdom = true; 123 | } 124 | 125 | void nav_cb(const nav_msgs::GridCells::ConstPtr& grid_cells) 126 | { 127 | boost::mutex::scoped_lock lock(cost_map_mutex_); 128 | cost_map = *grid_cells; 129 | bHasCostMap = true; 130 | } 131 | 132 | float goal_dist(geometry_msgs::PoseStamped gp) 133 | { 134 | return sqrt(((current_goals[0].pose.position.x - gp.pose.position.x) 135 | * (current_goals[0].pose.position.x - gp.pose.position.x)) + 136 | ((current_goals[0].pose.position.y - gp.pose.position.y) 137 | * (current_goals[0].pose.position.y - gp.pose.position.y))); 138 | } 139 | 140 | float goal_angle_dist(geometry_msgs::PoseStamped gp) 141 | { 142 | return fabs(tf::getYaw(current_goals[0].pose.orientation) - tf::getYaw(gp.pose.orientation)); 143 | } 144 | 145 | bool same_global_goal(geometry_msgs::PoseStamped new_goal) 146 | { 147 | bool isSameGoal = false; 148 | 149 | if (bHasGoal) 150 | { 151 | if (goal_dist(new_goal) < GOAL_DIST_ID_THRESH) 152 | { 153 | if (goal_angle_dist(new_goal) < GOAL_ANGLE_ID_THRESH) 154 | { 155 | isSameGoal = true; 156 | } 157 | } 158 | } 159 | 160 | return isSameGoal; 161 | } 162 | 163 | void receive_goals(const geometry_msgs::PoseStamped::ConstPtr& goal) 164 | { 165 | boost::mutex::scoped_lock lock(goals_mutex_); 166 | 167 | bool bIsSameGoal = same_global_goal(*goal); 168 | 169 | current_goals.clear(); 170 | current_goals.push_back(*goal); 171 | 172 | if (bHasOdom && bHasCostMap) 173 | { 174 | global_goal_pose = *goal; 175 | } 176 | else 177 | { 178 | ROS_DEBUG("[MPEPC] goal recieved but no odom or cost map"); 179 | return; 180 | } 181 | 182 | if (bFollowingTrajectory && !bIsSameGoal) 183 | { 184 | ROS_DEBUG("cancelling current goal"); 185 | cancel_auton_nav(RESULT_CANCEL); 186 | } 187 | 188 | bHasGoal = true; 189 | 190 | if (!bFollowingTrajectory && bHasGoal) 191 | { 192 | ROS_INFO("Begin path planning and execution"); 193 | rviz_goal_request_pub.publish(global_goal_pose); 194 | bFollowingTrajectory = true; 195 | result.data = RESULT_BEGIN; 196 | goal_request_pub.publish(global_goal_pose); 197 | nav_result_pub.publish(result); 198 | } 199 | } 200 | 201 | int main(int argc, char** argv) 202 | { 203 | // Initialize ROS 204 | ros::init(argc, argv, "mpepc_trajectory"); 205 | ros::NodeHandle n; 206 | nh = &n; 207 | 208 | // Setup control law for trajectory generation 209 | ControlLawSettings settings; 210 | settings.m_K1 = K_1; 211 | settings.m_K2 = K_2; 212 | settings.m_BETA = BETA; 213 | settings.m_LAMBDA = LAMBDA; 214 | settings.m_R_THRESH = R_THRESH; 215 | settings.m_V_MAX = V_MAX; 216 | settings.m_V_MIN = V_MIN; 217 | ControlLaw claw(settings); 218 | cl = &claw; 219 | 220 | current_pose.pose.pose.orientation.x = 0; 221 | current_pose.pose.pose.orientation.y = 0; 222 | current_pose.pose.pose.orientation.z = 0; 223 | current_pose.pose.pose.orientation.w = 1; 224 | 225 | bFollowingTrajectory = false; 226 | bHasGoal = false; 227 | bHasIntGoal = false; 228 | prevButton = false; 229 | bHasOdom = false; 230 | bHasCostMap = true; 231 | 232 | goal_sub = nh->subscribe("/mpepc_goal_request", 1, receive_goals); 233 | cmd_pub = nh->advertise("/cmd_vel_mux/input/navi", 1); 234 | 235 | nav_result_pub = nh->advertise("nav_result", 1); 236 | goal_request_pub = nh->advertise("goal_request", 1); 237 | rviz_goal_request_pub = nh->advertise("rviz_goal_request", 1); 238 | 239 | odom_sub = nh->subscribe("/odom", 1, odom_cb); 240 | int_goal_pose_sub = nh->subscribe("int_goal_pose", 1, int_goal_cb); 241 | cost_map_sub = nh->subscribe("/costmap_translator/obstacles", 100, nav_cb); 242 | 243 | // ros::Rate loop_rate(20.0); 244 | double loop_rate = DEFAULT_LOOP_RATE; 245 | double global_loop_rate; 246 | if (ros::param::has("/global_loop_rate")) 247 | { 248 | nh->getParam("/global_loop_rate", global_loop_rate); 249 | loop_rate= RATE_FACTOR*global_loop_rate; 250 | ROS_DEBUG("Smooth_Traj: Loop rate updated using global_loop_rate to : %f", loop_rate); 251 | } 252 | ros::Rate rate_obj(loop_rate); 253 | 254 | EgoPolar global_goal_coords; 255 | geometry_msgs::Twist cmd_vel; 256 | 257 | // Control Loop and code 258 | while (ros::ok()) 259 | { 260 | if (bFollowingTrajectory && bHasIntGoal) // autonomous trajectory generation and following mode 261 | { 262 | global_goal_coords = cl->convert_to_egopolar(current_pose, global_goal_pose.pose); 263 | if (global_goal_coords.r <= GOAL_DIST_UPDATE_THRESH) 264 | { 265 | double angle_error = tf::getYaw(current_pose.pose.pose.orientation) - tf::getYaw(global_goal_pose.pose.orientation); 266 | angle_error = cl->wrap_pos_neg_pi(angle_error); 267 | if (fabs(angle_error) > GOAL_ANGLE_UPDATE_THRESH) 268 | { 269 | cmd_vel.linear.x = 0; 270 | if (angle_error > 0) 271 | cmd_vel.angular.z = -4 * settings.m_V_MIN; 272 | else 273 | cmd_vel.angular.z = 4 * settings.m_V_MIN; 274 | } 275 | else 276 | { 277 | ROS_DEBUG("[MPEPC] Completed normal trajectory following"); 278 | cancel_auton_nav(RESULT_SUCCESS); 279 | cmd_vel.linear.x = 0; 280 | cmd_vel.angular.z = 0; 281 | } 282 | } 283 | else 284 | { 285 | cmd_vel = cl->get_velocity_command(current_pose, inter_goal_pose.pose, inter_goal_k1, inter_goal_k2, inter_goal_vMax); 286 | } 287 | } 288 | cmd_pub.publish(cmd_vel); 289 | 290 | ros::spinOnce(); 291 | rate_obj.sleep(); 292 | } 293 | } 294 | --------------------------------------------------------------------------------