├── 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 |
--------------------------------------------------------------------------------