├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── laser.rviz ├── rviz.rviz └── sim.rviz ├── include └── sdf_slam_2d │ ├── SDFSlam.h │ ├── map │ ├── AbstractMap.h │ ├── GraphMap.h │ ├── OccupancyGrid.h │ └── VectorMap.h │ ├── registration │ ├── AbstractRegistration.h │ └── GaussNewton.h │ └── utility │ ├── Types.h │ └── UtilityFunctions.h ├── launch ├── graphtest.launch ├── graphteststdr.launch ├── local_sdf_slam_laser.launch ├── local_sdf_slam_stdr.launch └── local_stdr_sim.launch ├── package.xml ├── src ├── SDFSlam.cpp └── main.cpp └── stdr_resources ├── maps ├── test.png └── test.yaml └── robots ├── pandora_robot.xml └── pandora_robot.yaml /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | 49 | #clion 50 | .idea -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(sdf_slam_2d) 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED COMPONENTS 9 | sensor_msgs 10 | # cv_bridge 11 | # image_transport 12 | roscpp 13 | tf 14 | tf_conversions 15 | laser_geometry 16 | pcl_conversions 17 | pcl_ros 18 | # opencv2 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | find_package(PCL REQUIRED) 23 | include_directories(${PCL_INCLUDE_DIRS}) 24 | add_definitions(${PCL_DEFINITIONS}) 25 | 26 | # find_package(Eigen REQUIRED) 27 | # include_directories(${EIGEN_INCLUDE_DIRS}) 28 | # add_definitions(${EIGEN_DEFINITIONS}) 29 | 30 | #find_package(OpenCV REQUIRED) 31 | 32 | #find_package(Boost REQUIRED COMPONENTS random) 33 | #find_package(Boost REQUIRED COMPONENTS system) 34 | #include_directories(${Boost_INCLUDE_DIRS}) 35 | 36 | ## Uncomment this if the package has a setup.py. This macro ensures 37 | ## modules and global scripts declared therein get installed 38 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 39 | # catkin_python_setup() 40 | 41 | ################################################ 42 | ## Declare ROS messages, services and actions ## 43 | ################################################ 44 | 45 | ## To declare and build messages, services or actions from within this 46 | ## package, follow these steps: 47 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 48 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 49 | ## * In the file package.xml: 50 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 51 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 52 | ## pulled in transitively but can be declared for certainty nonetheless: 53 | ## * add a build_depend tag for "message_generation" 54 | ## * add a run_depend tag for "message_runtime" 55 | ## * In this file (CMakeLists.txt): 56 | ## * add "message_generation" and every package in MSG_DEP_SET to 57 | ## find_package(catkin REQUIRED COMPONENTS ...) 58 | ## * add "message_runtime" and every package in MSG_DEP_SET to 59 | ## catkin_package(CATKIN_DEPENDS ...) 60 | ## * uncomment the add_*_files sections below as needed 61 | ## and list every .msg/.srv/.action file to be processed 62 | ## * uncomment the generate_messages entry below 63 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 64 | 65 | ## Generate messages in the 'msg' folder 66 | # add_message_files( 67 | # FILES 68 | # Message1.msg 69 | # Message2.msg 70 | # ) 71 | 72 | ## Generate services in the 'srv' folder 73 | # add_service_files( 74 | # FILES 75 | # Service1.srv 76 | # Service2.srv 77 | # ) 78 | 79 | ## Generate actions in the 'action' folder 80 | # add_action_files( 81 | # FILES 82 | # Action1.action 83 | # Action2.action 84 | # ) 85 | 86 | ## Generate added messages and services with any dependencies listed here 87 | # generate_messages( 88 | # DEPENDENCIES 89 | # std_msgs # Or other packages containing msgs 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES sdf_2d 104 | # CATKIN_DEPENDS roscpp 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories(include include/sdf_slam_2d) 115 | include_directories( 116 | ${catkin_INCLUDE_DIRS} 117 | # ${Boost_LIBRARIES} 118 | ) 119 | 120 | ## Declare a cpp library 121 | # add_library(sdf_2d 122 | # src/${PROJECT_NAME}/sdf_2d.cpp 123 | # ) 124 | 125 | ## Declare a cpp executable 126 | add_executable(sdf_slam src/main.cpp src/SDFSlam.cpp include/sdf_slam_2d/SDFSlam.h) 127 | 128 | #add_executable(scanlogger src/utils/scanlogger.cpp) 129 | #add_executable(add_noise src/utils/addNoise.cpp) 130 | 131 | ## Add cmake target dependencies of the executable/library 132 | ## as an example, message headers may need to be generated before nodes 133 | # add_dependencies(sdf_2d_node sdf_2d_generate_messages_cpp) 134 | 135 | ## Specify libraries to link a library or executable target against 136 | target_link_libraries(sdf_slam 137 | ${catkin_LIBRARIES} 138 | # ${OpenCV_LIBS} 139 | ${PCL_LIBRARIES} 140 | ) 141 | 142 | #target_link_libraries(scanlogger 143 | # ${catkin_LIBRARIES} 144 | # ${PCL_LIBRARIES} 145 | #) 146 | 147 | #target_link_libraries(add_noise 148 | # ${catkin_LIBRARIES} 149 | # ${Boost_LIBRARIES} 150 | # ${BOOST_INCLUDE_DIRS} 151 | #) 152 | 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS sdf_2d sdf_2d_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sdf_2d.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /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 | # sdf_slam_2d 2 | Please note that this is experimental code. 3 | Thus, it is neither stable nor does it follow sane programming guidelines. 4 | 5 | Teaser video at https://www.youtube.com/watch?v=j1vs0sUXAQc 6 | 7 | 8 | 9 | If you want to try: 10 | 11 | git clone https://github.com/smARTLab-liv/sdf_slam_2d.git --branch=iros15 12 | 13 | rosdep install sdf_slam 14 | 15 | apt-get install ros-indigo stdr-simulator 16 | 17 | apt-get install ros-indigo-teleop-twist-keyboard 18 | 19 | catkin_make 20 | 21 | roslaunch sdf_slam_2d local_stdr_sim.launch 22 | 23 | roslaunch sdf_slam_2d local_sdf_slam_stdr.launch 24 | 25 | rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=robot0/cmd_vel 26 | 27 | rosrun rviz rviz -d $(rospack find sdf_slam_2d)/cfg/sim.rviz -------------------------------------------------------------------------------- /cfg/laser.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /TF1/Tree1 11 | - /PointCloud21/Status1 12 | - /PointCloud23/Autocompute Value Bounds1 13 | - /Marker1 14 | - /Marker1/Namespaces1 15 | - /Grid1 16 | - /Grid1/Offset1 17 | - /Grid2 18 | - /Map1 19 | - /Map1/Status1 20 | - /Map1/Position1 21 | Splitter Ratio: 0.704641 22 | Tree Height: 1210 23 | - Class: rviz/Selection 24 | Name: Selection 25 | - Class: rviz/Tool Properties 26 | Expanded: 27 | - /2D Pose Estimate1 28 | - /2D Nav Goal1 29 | - /Publish Point1 30 | Name: Tool Properties 31 | Splitter Ratio: 0.588679 32 | - Class: rviz/Views 33 | Expanded: 34 | - /Current View1 35 | - /TopDownOrtho1 36 | Name: Views 37 | Splitter Ratio: 0.5 38 | - Class: rviz/Time 39 | Experimental: false 40 | Name: Time 41 | SyncMode: 0 42 | SyncSource: "" 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 1 47 | Autocompute Intensity Bounds: true 48 | Autocompute Value Bounds: 49 | Max Value: 10 50 | Min Value: -10 51 | Value: true 52 | Axis: Z 53 | Channel Name: intensity 54 | Class: rviz/LaserScan 55 | Color: 170; 0; 0 56 | Color Transformer: FlatColor 57 | Decay Time: 999 58 | Enabled: false 59 | Invert Rainbow: false 60 | Max Color: 255; 255; 255 61 | Max Intensity: 4096 62 | Min Color: 0; 0; 0 63 | Min Intensity: 0 64 | Name: LaserScan 65 | Position Transformer: XYZ 66 | Queue Size: 10 67 | Selectable: true 68 | Size (Pixels): 3 69 | Size (m): 0.01 70 | Style: Spheres 71 | Topic: /scan 72 | Use Fixed Frame: true 73 | Use rainbow: true 74 | Value: false 75 | - Class: rviz/TF 76 | Enabled: true 77 | Frame Timeout: 15 78 | Frames: 79 | All Enabled: false 80 | Robot_1/base_link: 81 | Value: true 82 | sdf_pos: 83 | Value: true 84 | world: 85 | Value: true 86 | Marker Scale: 0.5 87 | Name: TF 88 | Show Arrows: true 89 | Show Axes: true 90 | Show Names: true 91 | Tree: 92 | world: 93 | Robot_1/base_link: 94 | {} 95 | sdf_pos: 96 | {} 97 | Update Interval: 0 98 | Value: true 99 | - Alpha: 1 100 | Autocompute Intensity Bounds: true 101 | Autocompute Value Bounds: 102 | Max Value: 10 103 | Min Value: -10 104 | Value: true 105 | Axis: Z 106 | Channel Name: intensity 107 | Class: rviz/PointCloud2 108 | Color: 255; 255; 255 109 | Color Transformer: FlatColor 110 | Decay Time: 0 111 | Enabled: true 112 | Invert Rainbow: false 113 | Max Color: 255; 255; 255 114 | Max Intensity: 4096 115 | Min Color: 0; 0; 0 116 | Min Intensity: 0 117 | Name: PointCloud2 118 | Position Transformer: XYZ 119 | Queue Size: 10 120 | Selectable: true 121 | Size (Pixels): 3 122 | Size (m): 0.01 123 | Style: Spheres 124 | Topic: /pointcloud_map 125 | Use Fixed Frame: true 126 | Use rainbow: true 127 | Value: true 128 | - Alpha: 1 129 | Autocompute Intensity Bounds: true 130 | Autocompute Value Bounds: 131 | Max Value: 10 132 | Min Value: -10 133 | Value: true 134 | Axis: Z 135 | Channel Name: x 136 | Class: rviz/PointCloud2 137 | Color: 255; 0; 0 138 | Color Transformer: FlatColor 139 | Decay Time: 0 140 | Enabled: true 141 | Invert Rainbow: false 142 | Max Color: 255; 255; 255 143 | Max Intensity: 0.775 144 | Min Color: 0; 0; 0 145 | Min Intensity: -2.625 146 | Name: PointCloud2 147 | Position Transformer: XYZ 148 | Queue Size: 10 149 | Selectable: true 150 | Size (Pixels): 3 151 | Size (m): 0.01 152 | Style: Flat Squares 153 | Topic: /iso_map 154 | Use Fixed Frame: true 155 | Use rainbow: true 156 | Value: true 157 | - Alpha: 1 158 | Autocompute Intensity Bounds: true 159 | Autocompute Value Bounds: 160 | Max Value: 0.420984 161 | Min Value: -0.414726 162 | Value: true 163 | Axis: Z 164 | Channel Name: intensity 165 | Class: rviz/PointCloud2 166 | Color: 170; 0; 0 167 | Color Transformer: AxisColor 168 | Decay Time: 0 169 | Enabled: true 170 | Invert Rainbow: false 171 | Max Color: 255; 255; 255 172 | Max Intensity: 4096 173 | Min Color: 0; 0; 0 174 | Min Intensity: 0 175 | Name: PointCloud2 176 | Position Transformer: XYZ 177 | Queue Size: 10 178 | Selectable: true 179 | Size (Pixels): 3 180 | Size (m): 0.01 181 | Style: Spheres 182 | Topic: /cloudA 183 | Use Fixed Frame: true 184 | Use rainbow: true 185 | Value: true 186 | - Alpha: 1 187 | Autocompute Intensity Bounds: true 188 | Autocompute Value Bounds: 189 | Max Value: 10 190 | Min Value: -10 191 | Value: true 192 | Axis: Z 193 | Channel Name: intensity 194 | Class: rviz/PointCloud2 195 | Color: 50; 73; 223 196 | Color Transformer: FlatColor 197 | Decay Time: 0 198 | Enabled: false 199 | Invert Rainbow: false 200 | Max Color: 255; 255; 255 201 | Max Intensity: 4096 202 | Min Color: 0; 0; 0 203 | Min Intensity: 0 204 | Name: PointCloud2 205 | Position Transformer: XYZ 206 | Queue Size: 10 207 | Selectable: true 208 | Size (Pixels): 3 209 | Size (m): 0.025 210 | Style: Spheres 211 | Topic: /cloudB 212 | Use Fixed Frame: true 213 | Use rainbow: true 214 | Value: false 215 | - Alpha: 1 216 | Axes Length: 1 217 | Axes Radius: 0.1 218 | Class: rviz/Pose 219 | Color: 255; 25; 0 220 | Enabled: true 221 | Head Length: 0.3 222 | Head Radius: 0.1 223 | Name: Pose 224 | Shaft Length: 1 225 | Shaft Radius: 0.05 226 | Shape: Arrow 227 | Topic: /Robot_1/pose_stamped 228 | Value: true 229 | - Class: rviz/Axes 230 | Enabled: false 231 | Length: 1 232 | Name: Axes 233 | Radius: 0.1 234 | Reference Frame: 235 | Value: false 236 | - Class: rviz/Marker 237 | Enabled: true 238 | Marker Topic: /sdf_marker_map 239 | Name: Marker 240 | Namespaces: 241 | sdf_marker_map: true 242 | Queue Size: 100 243 | Value: true 244 | - Alpha: 0.5 245 | Cell Size: 1 246 | Class: rviz/Grid 247 | Color: 160; 160; 164 248 | Enabled: false 249 | Line Style: 250 | Line Width: 0.03 251 | Value: Lines 252 | Name: Grid 253 | Normal Cell Count: 0 254 | Offset: 255 | X: 0 256 | Y: 0 257 | Z: 0 258 | Plane: XY 259 | Plane Cell Count: 15 260 | Reference Frame: 261 | Value: false 262 | - Alpha: 0.5 263 | Cell Size: 0.05 264 | Class: rviz/Grid 265 | Color: 160; 160; 164 266 | Enabled: false 267 | Line Style: 268 | Line Width: 0.03 269 | Value: Lines 270 | Name: Grid 271 | Normal Cell Count: 0 272 | Offset: 273 | X: 0 274 | Y: 0 275 | Z: 0 276 | Plane: XY 277 | Plane Cell Count: 500 278 | Reference Frame: 279 | Value: false 280 | - Alpha: 0.7 281 | Class: rviz/Map 282 | Color Scheme: map 283 | Draw Behind: false 284 | Enabled: true 285 | Name: Map 286 | Topic: /occ_map 287 | Value: true 288 | Enabled: true 289 | Global Options: 290 | Background Color: 48; 48; 48 291 | Fixed Frame: world 292 | Frame Rate: 30 293 | Name: root 294 | Tools: 295 | - Class: rviz/Interact 296 | Hide Inactive Objects: true 297 | - Class: rviz/MoveCamera 298 | - Class: rviz/Select 299 | - Class: rviz/FocusCamera 300 | - Class: rviz/Measure 301 | - Class: rviz/SetInitialPose 302 | Topic: /initialpose 303 | - Class: rviz/SetGoal 304 | Topic: /move_base_simple/goal 305 | - Class: rviz/PublishPoint 306 | Single click: true 307 | Topic: /clicked_point 308 | Value: true 309 | Views: 310 | Current: 311 | Angle: 5.65127e-06 312 | Class: rviz/TopDownOrtho 313 | Enable Stereo Rendering: 314 | Stereo Eye Separation: 0.06 315 | Stereo Focal Distance: 1 316 | Swap Stereo Eyes: false 317 | Value: false 318 | Name: Current View 319 | Near Clip Distance: 0.01 320 | Scale: 68.5587 321 | Target Frame: 322 | Value: TopDownOrtho (rviz) 323 | X: -0.686358 324 | Y: -1.48915 325 | Saved: 326 | - Angle: -1.33999 327 | Class: rviz/TopDownOrtho 328 | Enable Stereo Rendering: 329 | Stereo Eye Separation: 0.06 330 | Stereo Focal Distance: 1 331 | Swap Stereo Eyes: false 332 | Value: false 333 | Name: rl 334 | Near Clip Distance: 0.01 335 | Scale: 178.063 336 | Target Frame: 337 | Value: TopDownOrtho (rviz) 338 | X: 1.41946 339 | Y: -2.43856 340 | - Angle: 0 341 | Class: rviz/TopDownOrtho 342 | Enable Stereo Rendering: 343 | Stereo Eye Separation: 0.06 344 | Stereo Focal Distance: 1 345 | Swap Stereo Eyes: false 346 | Value: false 347 | Name: TopDownOrtho 348 | Near Clip Distance: 0.01 349 | Scale: 149.601 350 | Target Frame: 351 | Value: TopDownOrtho (rviz) 352 | X: 0 353 | Y: 0 354 | Window Geometry: 355 | Displays: 356 | collapsed: false 357 | Height: 1419 358 | Hide Left Dock: false 359 | Hide Right Dock: false 360 | QMainWindow State: 000000ff00000000fd0000000400000000000000f1000004f7fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d00610067006500000001670000014c0000000000000000fc00000036000001cf0000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000001d80000000000000000fb000000100044006900730070006c0061007900730100000036000004f7000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004f7fc0200000003fb0000000a005600690065007700730100000036000004f70000009b00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000068e000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fe0000003efc0100000002fb0000000800540069006d00650100000000000004fe0000024600fffffffb0000000800540069006d00650100000000000004500000000000000000000002f2000004f700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 361 | Selection: 362 | collapsed: false 363 | Time: 364 | collapsed: false 365 | Tool Properties: 366 | collapsed: false 367 | Views: 368 | collapsed: false 369 | Width: 1278 370 | X: 1 371 | Y: 20 372 | -------------------------------------------------------------------------------- /cfg/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /Free cells1/Status1 11 | - /Free cells1/Position1 12 | - /Smooth Walls1/Namespaces1 13 | - /PCD Window1 14 | Splitter Ratio: 0.704641 15 | Tree Height: 1219 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.588679 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | - /TopDownOrtho1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: Laser PCD 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Class: rviz/TF 40 | Enabled: true 41 | Frame Timeout: 15 42 | Frames: 43 | All Enabled: false 44 | map: 45 | Value: true 46 | map_static: 47 | Value: true 48 | robot0: 49 | Value: true 50 | robot0_laser_0: 51 | Value: true 52 | sdf_pos: 53 | Value: true 54 | world: 55 | Value: true 56 | Marker Scale: 0.5 57 | Name: TF 58 | Show Arrows: true 59 | Show Axes: true 60 | Show Names: true 61 | Tree: 62 | world: 63 | map: 64 | map_static: 65 | robot0: 66 | robot0_laser_0: 67 | {} 68 | sdf_pos: 69 | {} 70 | Update Interval: 0 71 | Value: true 72 | - Alpha: 0.7 73 | Class: rviz/Map 74 | Color Scheme: map 75 | Draw Behind: false 76 | Enabled: true 77 | Name: Free cells 78 | Topic: /occ_map 79 | Value: true 80 | - Class: rviz/Marker 81 | Enabled: true 82 | Marker Topic: /sdf_wall_marker 83 | Name: Smooth Walls 84 | Namespaces: 85 | sdf_wall_marker: true 86 | Queue Size: 100 87 | Value: true 88 | - Alpha: 0.9 89 | Autocompute Intensity Bounds: true 90 | Autocompute Value Bounds: 91 | Max Value: 10 92 | Min Value: -10 93 | Value: true 94 | Axis: Z 95 | Channel Name: intensity 96 | Class: rviz/PointCloud2 97 | Color: 255; 0; 4 98 | Color Transformer: FlatColor 99 | Decay Time: 0 100 | Enabled: true 101 | Invert Rainbow: false 102 | Max Color: 255; 255; 255 103 | Max Intensity: 4096 104 | Min Color: 0; 0; 0 105 | Min Intensity: 0 106 | Name: Laser PCD 107 | Position Transformer: XYZ 108 | Queue Size: 10 109 | Selectable: true 110 | Size (Pixels): 10 111 | Size (m): 0.05 112 | Style: Spheres 113 | Topic: /sdf/scan 114 | Use Fixed Frame: true 115 | Use rainbow: true 116 | Value: true 117 | - Alpha: 0.7 118 | Class: rviz/Map 119 | Color Scheme: map 120 | Draw Behind: false 121 | Enabled: true 122 | Name: SDF Window 123 | Topic: /local_df_map 124 | Value: true 125 | - Alpha: 0.7 126 | Class: rviz/Map 127 | Color Scheme: map 128 | Draw Behind: false 129 | Enabled: true 130 | Name: Occupancy Window 131 | Topic: /occ_map_window 132 | Value: true 133 | - Alpha: 1 134 | Autocompute Intensity Bounds: true 135 | Autocompute Value Bounds: 136 | Max Value: 0.409836 137 | Min Value: -0.41685 138 | Value: true 139 | Axis: Z 140 | Channel Name: intensity 141 | Class: rviz/PointCloud2 142 | Color: 255; 255; 255 143 | Color Transformer: AxisColor 144 | Decay Time: 0 145 | Enabled: true 146 | Invert Rainbow: false 147 | Max Color: 255; 255; 255 148 | Max Intensity: 4096 149 | Min Color: 0; 0; 0 150 | Min Intensity: 0 151 | Name: PCD Window 152 | Position Transformer: XYZ 153 | Queue Size: 10 154 | Selectable: true 155 | Size (Pixels): 3 156 | Size (m): 0.01 157 | Style: Flat Squares 158 | Topic: /local_pcd_map 159 | Use Fixed Frame: true 160 | Use rainbow: true 161 | Value: true 162 | Enabled: true 163 | Global Options: 164 | Background Color: 108; 108; 108 165 | Fixed Frame: map 166 | Frame Rate: 30 167 | Name: root 168 | Tools: 169 | - Class: rviz/Interact 170 | Hide Inactive Objects: true 171 | - Class: rviz/MoveCamera 172 | - Class: rviz/Select 173 | - Class: rviz/FocusCamera 174 | - Class: rviz/Measure 175 | - Class: rviz/SetInitialPose 176 | Topic: /initialpose 177 | - Class: rviz/SetGoal 178 | Topic: /move_base_simple/goal 179 | - Class: rviz/PublishPoint 180 | Single click: true 181 | Topic: /clicked_point 182 | Value: true 183 | Views: 184 | Current: 185 | Class: rviz/XYOrbit 186 | Distance: 7.52258 187 | Enable Stereo Rendering: 188 | Stereo Eye Separation: 0.06 189 | Stereo Focal Distance: 1 190 | Swap Stereo Eyes: false 191 | Value: false 192 | Focal Point: 193 | X: 2.13714 194 | Y: 2.27845 195 | Z: -1.43051e-06 196 | Name: Current View 197 | Near Clip Distance: 0.01 198 | Pitch: 0.354797 199 | Target Frame: 200 | Value: XYOrbit (rviz) 201 | Yaw: 1.0104 202 | Saved: 203 | - Angle: -1.33999 204 | Class: rviz/TopDownOrtho 205 | Enable Stereo Rendering: 206 | Stereo Eye Separation: 0.06 207 | Stereo Focal Distance: 1 208 | Swap Stereo Eyes: false 209 | Value: false 210 | Name: rl 211 | Near Clip Distance: 0.01 212 | Scale: 178.063 213 | Target Frame: 214 | Value: TopDownOrtho (rviz) 215 | X: 1.41946 216 | Y: -2.43856 217 | - Angle: 0 218 | Class: rviz/TopDownOrtho 219 | Enable Stereo Rendering: 220 | Stereo Eye Separation: 0.06 221 | Stereo Focal Distance: 1 222 | Swap Stereo Eyes: false 223 | Value: false 224 | Name: TopDownOrtho 225 | Near Clip Distance: 0.01 226 | Scale: 149.601 227 | Target Frame: 228 | Value: TopDownOrtho (rviz) 229 | X: 0 230 | Y: 0 231 | Window Geometry: 232 | Displays: 233 | collapsed: false 234 | Height: 1419 235 | Hide Left Dock: false 236 | Hide Right Dock: false 237 | QMainWindow State: 000000ff00000000fd00000004000000000000012400000500fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d00610067006500000001670000014c0000000000000000fc00000036000001cf0000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000001d80000000000000000fb000000100044006900730070006c006100790073010000003600000500000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000500fc0200000003fb0000000a005600690065007700730100000036000005000000009b00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000068e000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fe00000035fc0100000002fb0000000800540069006d00650100000000000004fe0000024600fffffffb0000000800540069006d00650100000000000004500000000000000000000002bf0000050000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 238 | Selection: 239 | collapsed: false 240 | Time: 241 | collapsed: false 242 | Tool Properties: 243 | collapsed: false 244 | Views: 245 | collapsed: false 246 | Width: 1278 247 | X: 0 248 | Y: 19 249 | -------------------------------------------------------------------------------- /cfg/sim.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1/Offset1 10 | - /TF1/Frames1 11 | - /TF1/Tree1 12 | - /SDFSLAM Walls1/Namespaces1 13 | Splitter Ratio: 0.704641 14 | Tree Height: 1210 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: LaserScan 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 0.05 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: false 42 | Line Style: 43 | Line Width: 0.03 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0.025 49 | Y: 0.025 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10000 53 | Reference Frame: 54 | Value: false 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 10 59 | Min Value: -10 60 | Value: true 61 | Axis: Z 62 | Channel Name: intensity 63 | Class: rviz/LaserScan 64 | Color: 170; 0; 0 65 | Color Transformer: FlatColor 66 | Decay Time: 0 67 | Enabled: true 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 4096 71 | Min Color: 0; 0; 0 72 | Min Intensity: 0 73 | Name: LaserScan 74 | Position Transformer: XYZ 75 | Queue Size: 10 76 | Selectable: true 77 | Size (Pixels): 3 78 | Size (m): 0.05 79 | Style: Spheres 80 | Topic: /robot0/laser_0 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | - Class: rviz/TF 86 | Enabled: true 87 | Frame Timeout: 15 88 | Frames: 89 | All Enabled: false 90 | map: 91 | Value: true 92 | map_static: 93 | Value: true 94 | robot0: 95 | Value: true 96 | robot0_laser_0: 97 | Value: true 98 | sdf_pos: 99 | Value: true 100 | world: 101 | Value: true 102 | Marker Scale: 0.5 103 | Name: TF 104 | Show Arrows: true 105 | Show Axes: true 106 | Show Names: true 107 | Tree: 108 | map: 109 | map_static: 110 | robot0: 111 | robot0_laser_0: 112 | {} 113 | sdf_pos: 114 | {} 115 | world: 116 | {} 117 | Update Interval: 0 118 | Value: true 119 | - Alpha: 0.7 120 | Class: rviz/Map 121 | Color Scheme: map 122 | Draw Behind: false 123 | Enabled: true 124 | Name: SDFSLAM Map 125 | Topic: /occ_map 126 | Unreliable: false 127 | Value: true 128 | - Alpha: 0.7 129 | Class: rviz/Map 130 | Color Scheme: map 131 | Draw Behind: false 132 | Enabled: false 133 | Name: Simulator Map 134 | Topic: /map 135 | Unreliable: false 136 | Value: false 137 | - Class: rviz/Marker 138 | Enabled: true 139 | Marker Topic: /sdf_wall_marker 140 | Name: SDFSLAM Walls 141 | Namespaces: 142 | sdf_wall_marker: true 143 | Queue Size: 100 144 | Value: true 145 | Enabled: true 146 | Global Options: 147 | Background Color: 48; 48; 48 148 | Fixed Frame: map 149 | Frame Rate: 30 150 | Name: root 151 | Tools: 152 | - Class: rviz/Interact 153 | Hide Inactive Objects: true 154 | - Class: rviz/MoveCamera 155 | - Class: rviz/Select 156 | - Class: rviz/FocusCamera 157 | - Class: rviz/Measure 158 | - Class: rviz/SetInitialPose 159 | Topic: /initialpose 160 | - Class: rviz/SetGoal 161 | Topic: /move_base_simple/goal 162 | - Class: rviz/PublishPoint 163 | Single click: true 164 | Topic: /clicked_point 165 | Value: true 166 | Views: 167 | Current: 168 | Class: rviz/XYOrbit 169 | Distance: 16.4595 170 | Enable Stereo Rendering: 171 | Stereo Eye Separation: 0.06 172 | Stereo Focal Distance: 1 173 | Swap Stereo Eyes: false 174 | Value: false 175 | Focal Point: 176 | X: 0.0401151 177 | Y: -3.29754 178 | Z: 2.98023e-06 179 | Name: Current View 180 | Near Clip Distance: 0.01 181 | Pitch: 1.5398 182 | Target Frame: 183 | Value: XYOrbit (rviz) 184 | Yaw: 4.71038 185 | Saved: ~ 186 | Window Geometry: 187 | Displays: 188 | collapsed: false 189 | Height: 1419 190 | Hide Left Dock: false 191 | Hide Right Dock: false 192 | QMainWindow State: 000000ff00000000fd000000040000000000000142000004f7fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d00610067006500000001670000014c0000000000000000fc00000036000001cf0000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d0061006700650000000000000001d80000000000000000fb000000100044006900730070006c0061007900730100000036000004f7000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004f7fc0200000003fb0000000a005600690065007700730000000036000004f70000009b00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000068e000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fe0000003efc0100000002fb0000000800540069006d00650100000000000004fe0000024600fffffffb0000000800540069006d00650100000000000004500000000000000000000003b6000004f700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 193 | Selection: 194 | collapsed: false 195 | Time: 196 | collapsed: false 197 | Tool Properties: 198 | collapsed: false 199 | Views: 200 | collapsed: false 201 | Width: 1278 202 | X: 1 203 | Y: 20 204 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/SDFSlam.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | 20 | #ifndef SDF_H__ 21 | #define SDF_H__ 22 | 23 | #include "map/AbstractMap.h" 24 | #include "map/VectorMap.h" 25 | #include "map/OccupancyGrid.h" 26 | #include "map/GraphMap.h" 27 | 28 | #include "registration/AbstractRegistration.h" 29 | #include "registration/GaussNewton.h" 30 | 31 | #include "utility/Types.h" 32 | #include "utility/UtilityFunctions.h" 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace sdfslam { 47 | 48 | class SignedDistanceField { 49 | 50 | public: 51 | 52 | SignedDistanceField(); 53 | 54 | ~SignedDistanceField(); 55 | 56 | bool checkTimeout(); 57 | 58 | protected: 59 | void scanCb(const sensor_msgs::LaserScan::ConstPtr& scan); 60 | 61 | bool saveMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res); 62 | 63 | bool publishMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res); 64 | 65 | bool resetService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res); 66 | 67 | bool loadMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res); 68 | 69 | bool updateMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res); 70 | 71 | bool createAndPublishVisualMap(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res); 72 | 73 | void checkTodos(); 74 | 75 | ros::Duration update_map(AbstractMap* aMap, const PCLPointCloud& pc, const Eigen::Vector3d& pose3d); 76 | ros::Duration publish_map(AbstractMap* aMap); 77 | 78 | AbstractMap* map_; 79 | AbstractMap* visualization_map_; 80 | 81 | AbstractRegistration* registration_; 82 | 83 | ros::NodeHandle nh_; 84 | 85 | ros::ServiceServer serviceSaveMap; 86 | ros::ServiceServer serviceLoadMap; 87 | ros::ServiceServer serviceUpdateMap; 88 | ros::ServiceServer serviceCreateAndPublishVisualMap; 89 | ros::ServiceServer servicePublishMap; 90 | ros::ServiceServer serviceReset; 91 | 92 | ros::Subscriber scan_sub_; 93 | ros::Publisher tfpub_; 94 | ros::Publisher scan_cloud_pub_; 95 | 96 | tf::TransformListener tf_; 97 | tf::TransformBroadcaster tfbr_; 98 | 99 | laser_geometry::LaserProjection projector_; 100 | 101 | std::string p_scan_topic_; 102 | std::string p_robot_frame_; 103 | std::string p_tar_frame_; 104 | std::string p_fixed_frame_; 105 | Eigen::Vector3d pos_; 106 | Eigen::Vector3d lastMapPos_; 107 | int p_vanity_; 108 | int p_timeout_ticks_; 109 | int time_out_counter_; 110 | int not_converged_counter_; 111 | int p_map_size_x_; 112 | int p_map_size_y_; 113 | int p_scan_subscriber_queue_size_; 114 | double p_time_warning_threshold; 115 | double p_reg_threshold_; 116 | double p_yaw_threshold_; 117 | double p_dist_threshold_; 118 | double p_grid_res_; 119 | bool p_perma_map_; 120 | bool p_initial_pose_; 121 | bool p_publish_map_; 122 | bool map_empty_; 123 | bool pose_estimation_; 124 | bool p_mapping_; 125 | bool map_flag_; 126 | bool updateMapServiceCalled_; 127 | bool publishMapServiceCalled_; 128 | bool loadMapServiceCalled_; 129 | bool converged_; 130 | 131 | private: 132 | 133 | }; 134 | 135 | } 136 | 137 | #endif 138 | 139 | 140 | 141 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/map/AbstractMap.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | #ifndef SDF_SLAM_2D_ABSTRACTMAP_H 20 | #define SDF_SLAM_2D_ABSTRACTMAP_H 21 | 22 | #include "../utility/Types.h" 23 | 24 | 25 | namespace sdfslam{ 26 | 27 | 28 | class AbstractMap { 29 | 30 | public: 31 | 32 | virtual void save_map(std::string filename){}; 33 | 34 | virtual void load_map(std::string filename){}; 35 | 36 | virtual void reset_map(){}; 37 | 38 | virtual void update_map(const PCLPointCloud& pc, const Eigen::Vector3d& pose3d){}; 39 | 40 | virtual void publish_map(){}; 41 | 42 | virtual int get_map_values(const Eigen::Vector2d& coords, float *mpdxdy, bool fine){}; 43 | 44 | protected: 45 | 46 | }; 47 | 48 | } 49 | 50 | #endif //SDF_SLAM_2D_ABSTRACTMAP_H 51 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/map/GraphMap.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | //todo: Save complete map as a graph, for now, just maintain one snapshot.. 20 | 21 | #ifndef SDF_SLAM_2D_GRAPHMAP_H 22 | #define SDF_SLAM_2D_GRAPHMAP_H 23 | 24 | #include "VectorMap.h" 25 | #include 26 | //#include 27 | #include "nav_msgs/OccupancyGrid.h" 28 | #include "sensor_msgs/PointCloud2.h" 29 | 30 | namespace sdfslam { 31 | 32 | class SDFGraphMap : public SDFVectorMap { 33 | 34 | public: 35 | 36 | SDFGraphMap(){ 37 | //rolling params different than map update params? 38 | window_size_ = 5; 39 | occ_map_pub_df_ = nh_.advertise("local_df_map", 10); 40 | pcd_map_pub_ = nh_.advertise("local_pcd_map", 10); 41 | 42 | } 43 | 44 | void update_map(const PCLPointCloud& pc, const Eigen::Vector3d& pose3d){ 45 | pos_ = pose3d; 46 | SDFVectorMap::update_map(pc, pose3d); 47 | } 48 | 49 | void publish_map_pcd(double *from, double *to){ 50 | pcl::PointCloud::Ptr pcd_map (new pcl::PointCloud); 51 | int insertAt = 0; 52 | pcd_map->width = p_map_size_x_ * p_map_size_y_; 53 | pcd_map->height = 1; 54 | pcd_map->is_dense = false; 55 | pcd_map->points.resize(pcd_map->width * pcd_map->height); 56 | for (int i = from[1]; i< to[1]; i++){ 57 | for (int j = from[0]; j< to[0]; j++) { 58 | if (sdf_[i][j] != 0) { 59 | pcd_map->points[insertAt].z = sdf_[i][j]; 60 | Eigen::Vector2f coord(j,i); 61 | coord = util::toRl(coord, p_grid_res_, p_map_size_x_,p_map_size_y_); 62 | 63 | pcd_map->points[insertAt].x = p_grid_res_/2 + j*p_grid_res_ - (p_map_size_x_/2)*p_grid_res_; 64 | //pcd_map->points[insertAt].x = coord.x()+p_grid_res_/2; 65 | pcd_map->points[insertAt].y = p_grid_res_/2 + i*p_grid_res_ - (p_map_size_y_/2)*p_grid_res_; 66 | //pcd_map->points[insertAt].y = coord.y()+p_grid_res_/2; 67 | insertAt++; 68 | } 69 | } 70 | } 71 | sensor_msgs::PointCloud2 cloudMsg; 72 | pcl::toROSMsg(*pcd_map, cloudMsg); 73 | cloudMsg.header.frame_id = p_fixed_frame_; 74 | cloudMsg.header.stamp = ros::Time::now(); 75 | pcd_map_pub_.publish(cloudMsg); 76 | 77 | } 78 | 79 | void publish_map() { 80 | SDFVectorMap::publish_map(); 81 | 82 | //publish as occ df, pos +- window_size_/2 range 83 | double pos[2] = {pos_.x(), pos_.y()}; 84 | util::toMap(pos, p_grid_res_, p_map_size_x_, p_map_size_y_); 85 | pos[0] = (int) pos[0]; 86 | pos[1] = (int) pos[1]; 87 | 88 | double from[2]; 89 | from[0] = pos[0]-window_size_/2/p_grid_res_; 90 | from[1] = pos[1]-window_size_/2/p_grid_res_; 91 | 92 | double to[2]; 93 | to[0] = pos[0]+window_size_/2/p_grid_res_; 94 | to[1] = pos[1]+window_size_/2/p_grid_res_; 95 | 96 | std::vector omap_df; 97 | 98 | publish_map_pcd(from,to); 99 | 100 | for (int j = (int) from[1]; j < (int)to[1]; j++) { 101 | for (int i = (int) from[0]; i < (int)to[0]; i++) { 102 | if (i<0 || i>p_map_size_x_ || j<0 || j>p_map_size_y_) { 103 | omap_df.push_back(0); //unknown 104 | // omap_df.push_back(-1); //unknown 105 | } 106 | else { 107 | int mapVal = (int) (sdf_[j][i]*100); //to cm 108 | 109 | if (mapVal < -128) 110 | mapVal = -128; 111 | else 112 | if(mapVal > 127) 113 | mapVal = 127; 114 | omap_df.push_back((int8_t) mapVal); 115 | } 116 | } 117 | } 118 | nav_msgs::OccupancyGrid mapGrid; 119 | nav_msgs::MapMetaData metaData; 120 | 121 | metaData.resolution = (float) p_grid_res_; 122 | metaData.width = (unsigned int) (window_size_/p_grid_res_); 123 | metaData.height = (unsigned int) (window_size_/p_grid_res_); 124 | //metaData.width = window_size_/; 125 | //metaData.height = 50; 126 | util::toRl(pos, p_grid_res_,p_map_size_x_,p_map_size_y_); 127 | metaData.origin.position.x = pos[0]-window_size_/2; 128 | metaData.origin.position.y = pos[1]-window_size_/2; 129 | 130 | /* metaData.origin.position.x = pos_.x()-window_size_/2; */ 131 | /* metaData.origin.position.y = pos_.y()-window_size_/2; */ 132 | mapGrid.data = omap_df; 133 | mapGrid.info = metaData; 134 | mapGrid.header.frame_id = p_fixed_frame_; 135 | occ_map_pub_df_.publish(mapGrid); 136 | 137 | 138 | } 139 | 140 | 141 | protected: 142 | 143 | ros::Publisher occ_map_pub_df_; 144 | ros::Publisher pcd_map_pub_; 145 | Eigen::Vector3d pos_; 146 | double window_size_; 147 | 148 | 149 | 150 | }; 151 | 152 | } 153 | 154 | #endif //SDF_SLAM_2D_GRAPHMAP_H 155 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/map/OccupancyGrid.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | #ifndef SDF_SLAM_2D_VISUALIZATIONMAP_H 20 | #define SDF_SLAM_2D_VISUALIZATIONMAP_H 21 | 22 | #include "../utility/Types.h" 23 | #include "nav_msgs/OccupancyGrid.h" 24 | 25 | namespace sdfslam{ 26 | 27 | class OccupancyGrid : public AbstractMap { 28 | 29 | public: 30 | 31 | OccupancyGrid() { 32 | ros::NodeHandle private_nh("~"); 33 | private_nh.param("p_grid_res_", p_grid_res_, 0.05); 34 | private_nh.param("p_map_size_y_", p_map_size_y_, 15); 35 | p_map_size_y_ /= p_grid_res_; 36 | private_nh.param("p_map_size_x_", p_map_size_x_, 15); 37 | p_map_size_x_ /= p_grid_res_; 38 | private_nh.param("p_fixed_frame_", p_fixed_frame_, "/map"); 39 | 40 | occ_map_pub_ = nh_.advertise("occ_map", 10); 41 | 42 | occ_map_pub_hack_ = nh_.advertise("occ_map_window", 10); 43 | 44 | VecMapFloat occ_map((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0.0)); 45 | occ_map_ = occ_map; 46 | } 47 | 48 | void save_map(std::string filename) { } 49 | 50 | void load_map(std::string filename) { }; 51 | 52 | void reset_map() { 53 | VecMapFloat occ_map((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0.0)); 54 | occ_map_ = occ_map; 55 | }; 56 | 57 | void publish_map() { 58 | std::vector omap2; 59 | for (int i = 0; i < p_map_size_x_; i++) { 60 | for (int j = 0; j < p_map_size_y_; j++) { 61 | if (occ_map_[i][j] == 0) 62 | omap2.push_back(-1); 63 | else if (occ_map_[i][j] > 0) 64 | omap2.push_back(0); 65 | else 66 | omap2.push_back(-1); 67 | } 68 | } 69 | nav_msgs::OccupancyGrid mapGrid; 70 | nav_msgs::MapMetaData metaData; 71 | 72 | metaData.resolution = (float) p_grid_res_; 73 | metaData.width = p_map_size_x_; 74 | metaData.height = p_map_size_y_; 75 | metaData.origin.position.x = -p_map_size_x_ / 2 * p_grid_res_; 76 | metaData.origin.position.y = -p_map_size_y_ / 2 * p_grid_res_; 77 | mapGrid.data = omap2; 78 | mapGrid.info = metaData; 79 | mapGrid.header.frame_id = p_fixed_frame_; 80 | occ_map_pub_.publish(mapGrid); 81 | 82 | //QUICK HACK 83 | double window_size_ = 5; 84 | double pos[2] = {pos_.x(), pos_.y()}; 85 | util::toMap(pos, p_grid_res_, p_map_size_x_, p_map_size_y_); 86 | pos[0] = (int) pos[0]; 87 | pos[1] = (int) pos[1]; 88 | 89 | double from[2]; 90 | from[0] = pos[0]-window_size_/2/p_grid_res_; 91 | from[1] = pos[1]-window_size_/2/p_grid_res_; 92 | 93 | double to[2]; 94 | to[0] = pos[0]+window_size_/2/p_grid_res_; 95 | to[1] = pos[1]+window_size_/2/p_grid_res_; 96 | std::vector omap_hack; 97 | for (int j = from[1]; j < to[1]; j++) { 98 | for (int i = from[0]; i < to[0]; i++) { 99 | //if (i < 0 || i > p_map_size_x_ || j < 0 || j > p_map_size_y_) { 100 | // omap_hack.push_back(-1); //unknown 101 | // } 102 | // else 103 | if (occ_map_[j][i] == -100) 104 | omap_hack.push_back(100); 105 | else if (occ_map_[j][i] > 0) 106 | omap_hack.push_back(0); 107 | else 108 | omap_hack.push_back(-1); 109 | } 110 | } 111 | 112 | 113 | metaData.resolution = (float) p_grid_res_; 114 | metaData.width = window_size_/p_grid_res_; 115 | metaData.height = window_size_/p_grid_res_; 116 | util::toRl(pos, p_grid_res_,p_map_size_x_,p_map_size_y_); 117 | metaData.origin.position.x = pos[0]-window_size_/2; 118 | metaData.origin.position.y = pos[1]-window_size_/2; 119 | 120 | //metaData.origin.position.x = pos_.x()-window_size_/2; 121 | //metaData.origin.position.y = pos_.y()-window_size_/2; 122 | mapGrid.data = omap_hack; 123 | mapGrid.info = metaData; 124 | mapGrid.header.frame_id = p_fixed_frame_; 125 | occ_map_pub_hack_.publish(mapGrid); 126 | //end hack 127 | 128 | }; 129 | 130 | void update_map(const PCLPointCloud& pc, const Eigen::Vector3d& pose3d){ 131 | pos_ = pose3d; 132 | PCLPointCloud::const_iterator it = pc.begin(); 133 | while (it < pc.end()) { 134 | Eigen::Vector2d a_point(it->x, it->y); 135 | Eigen::Vector2d cur_pos(pose3d.x(), pose3d.y()); 136 | simple_raytrace(cur_pos, a_point, occ_map_); 137 | it++; 138 | } 139 | }; 140 | 141 | 142 | protected: 143 | 144 | void simple_raytrace(Eigen::Vector2d src, Eigen::Vector2d tar, VecMapFloat &aMap) { 145 | 146 | double x0 = src.x() / p_grid_res_ + p_map_size_x_ / 2; 147 | double y0 = src.y() / p_grid_res_ + p_map_size_y_ / 2; 148 | double x1 = tar.x() / p_grid_res_ + p_map_size_x_ / 2; 149 | double y1 = tar.y() / p_grid_res_ + p_map_size_y_ / 2; 150 | 151 | double dx = fabs(x1 - x0); 152 | double dy = fabs(y1 - y0); 153 | 154 | int x = int(floor(x0)); 155 | int y = int(floor(y0)); 156 | 157 | int n = 1; 158 | int x_inc, y_inc; 159 | double error; 160 | double distance; 161 | 162 | if (dx == 0) { 163 | x_inc = 0; 164 | error = std::numeric_limits::infinity(); 165 | } 166 | else if (x1 > x0) { 167 | x_inc = 1; 168 | n += int(floor(x1)) - x; 169 | error = (floor(x0) + 1 - x0) * dy; 170 | } 171 | else { 172 | x_inc = -1; 173 | n += x - int(floor(x1)); 174 | error = (x0 - floor(x0)) * dy; 175 | } 176 | 177 | if (dy == 0) { 178 | y_inc = 0; 179 | error -= std::numeric_limits::infinity(); 180 | } 181 | else if (y1 > y0) { 182 | y_inc = 1; 183 | n += int(floor(y1)) - y; 184 | error -= (floor(y0) + 1 - y0) * dx; 185 | } 186 | else { 187 | y_inc = -1; 188 | n += y - int(floor(y1)); 189 | error -= (y0 - floor(y0)) * dx; 190 | } 191 | 192 | for (; n >=-1; --n) { //todo hack, before n > 1 193 | if (n>1) 194 | aMap[y][x] = 1; 195 | else 196 | aMap[y][x] = -100; 197 | 198 | if (error > 0) { 199 | y += y_inc; 200 | error -= dx; 201 | } 202 | else { 203 | x += x_inc; 204 | error += dy; 205 | } 206 | } 207 | } 208 | 209 | VecMapFloat occ_map_; 210 | ros::Publisher occ_map_pub_; 211 | double p_grid_res_; 212 | int p_map_size_x_; 213 | int p_map_size_y_; 214 | std::string p_fixed_frame_; 215 | ros::NodeHandle nh_; 216 | 217 | Eigen::Vector3d pos_; 218 | 219 | ros::Publisher occ_map_pub_hack_; 220 | }; 221 | 222 | } 223 | 224 | #endif //SDF_SLAM_2D_VISUALIZATIONMAP_H 225 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/map/VectorMap.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | #ifndef SDF_SLAM_2D_VECTORMAP_H 20 | #define SDF_SLAM_2D_VECTORMAP_H 21 | 22 | #include "AbstractMap.h" 23 | 24 | #include 25 | #include 26 | #include "../utility/UtilityFunctions.h" 27 | #include "ros/ros.h" 28 | #include "visualization_msgs/Marker.h" 29 | 30 | 31 | namespace sdfslam{ 32 | 33 | class SDFVectorMap : public AbstractMap { 34 | 35 | public: 36 | 37 | SDFVectorMap(){ 38 | ros::NodeHandle private_nh("~"); 39 | private_nh.param("p_grid_res_", p_grid_res_, 0.05); 40 | private_nh.param("p_map_size_y_", p_map_size_y_, 15); 41 | p_map_size_y_ /= p_grid_res_; 42 | private_nh.param("p_map_size_x_", p_map_size_x_, 15); 43 | p_map_size_x_ /= p_grid_res_; 44 | private_nh.param("p_min_range_", p_min_range_, 0.15); 45 | private_nh.param("p_max_range_", p_max_range_, 5.6); 46 | private_nh.param("p_avg_range_", p_avg_range_, 0); 47 | private_nh.param("p_update_map_aoe_", p_update_map_aoe_, 5); 48 | private_nh.param("p_avg_mapping_", p_avg_mapping_, false); 49 | private_nh.param("p_fixed_frame_", p_fixed_frame_, "/map"); 50 | 51 | 52 | marker_pub_ = nh_.advertise("sdf_wall_marker", 10); 53 | 54 | VecMapFloat sdf((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0.0)); 55 | sdf_ = sdf; 56 | 57 | VecMapInt sdf_level((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0)); 58 | sdf_level_ = sdf_level; 59 | 60 | VecMapInt sdf_count((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0)); 61 | sdf_count_ = sdf_count; 62 | 63 | epsilon_ = 0.0000001; 64 | std::cout << "Map constructor finished" << std::endl; 65 | } 66 | 67 | 68 | ~SDFVectorMap(){ 69 | } 70 | 71 | void publish_map(){ 72 | visualization_msgs::Marker cube_list, line_list, sphere_list; 73 | sphere_list.header.frame_id = cube_list.header.frame_id = line_list.header.frame_id = p_fixed_frame_; 74 | sphere_list.header.stamp = cube_list.header.stamp = line_list.header.stamp = ros::Time::now(); 75 | sphere_list.ns = cube_list.ns = line_list.ns = "sdf_wall_marker"; 76 | sphere_list.pose.orientation.w = cube_list.pose.orientation.w = line_list.pose.orientation.w = 1.0; 77 | sphere_list.action = cube_list.action = line_list.action = visualization_msgs::Marker::ADD; 78 | 79 | sphere_list.id = 1; 80 | cube_list.id = 1; 81 | line_list.id = 2; 82 | 83 | cube_list.type = visualization_msgs::Marker::CUBE_LIST; 84 | line_list.type = visualization_msgs::Marker::LINE_LIST; 85 | sphere_list.type = visualization_msgs::Marker::SPHERE_LIST; 86 | 87 | cube_list.scale.x = 2 * p_grid_res_; 88 | cube_list.scale.y = 2 * p_grid_res_; 89 | cube_list.scale.z = p_grid_res_ / 2; 90 | 91 | line_list.scale.x = p_grid_res_; 92 | 93 | sphere_list.scale.x = p_grid_res_; 94 | sphere_list.scale.y = p_grid_res_; 95 | 96 | line_list.color.r = 0.0; 97 | line_list.color.g = 0.0; 98 | line_list.color.b = 0.0; 99 | line_list.color.a = 1.0; 100 | 101 | sphere_list.color.r = 0.0; 102 | sphere_list.color.g = 0.0; 103 | sphere_list.color.b = 0.0; 104 | sphere_list.color.a = 1.0; 105 | 106 | cube_list.color.r = 0.5; 107 | cube_list.color.g = 0.5; 108 | cube_list.color.b = 0.5; 109 | cube_list.color.a = 1; 110 | 111 | //todo code copy.. 112 | int indMin[2]; 113 | float intensities[4]; 114 | int indices[2]; 115 | for (int i = 0; i < p_map_size_x_ - 1; i++) 116 | for (int j = 0; j < p_map_size_y_ - 1; j++) { 117 | 118 | indMin[0] = i; 119 | indMin[1] = j; 120 | 121 | for (int k = 0; k < 4; k++) { 122 | indices[0] = indMin[0] + k % 2; 123 | indices[1] = indMin[1] + k / 2; 124 | if (indices[0] >= 0 && indices[0] <= p_map_size_x_ && indices[1] >= 0 && indices[1] <= p_map_size_y_) 125 | intensities[k] = sdf_[indices[1]][indices[0]]; 126 | else { 127 | ROS_ERROR("map too small, inds %d %d map size %d %d", indices[0], indices[1], p_map_size_x_, 128 | p_map_size_x_); 129 | } 130 | } 131 | 132 | bool allZero = true; 133 | for (int count = 0; count < 4; count++) 134 | if (intensities[count] != 0) { 135 | allZero = false; 136 | } 137 | if (allZero) { 138 | continue; 139 | } 140 | //check for sgn changes NESW 141 | //203 142 | //3x1 143 | //021 144 | int numChanges = 0; 145 | float px[4]; 146 | float py[4]; 147 | if ((intensities[2] < 0 && intensities[3] > 0) || (intensities[3] < 0 && intensities[2] > 0)) { 148 | px[numChanges] = -intensities[2] / (intensities[3] - intensities[2]); 149 | py[numChanges] = 1; 150 | numChanges++; 151 | } 152 | if ((intensities[1] < 0 && intensities[3] > 0) || (intensities[3] < 0 && intensities[1] > 0)) { 153 | px[numChanges] = 1; 154 | py[numChanges] = -intensities[1] / (intensities[3] - intensities[1]); 155 | numChanges++; 156 | } 157 | if ((intensities[0] < 0 && intensities[1] > 0) || (intensities[1] < 0 && intensities[0] > 0)) { 158 | px[numChanges] = -intensities[0] / (intensities[1] - intensities[0]); 159 | py[numChanges] = 0; 160 | numChanges++; 161 | } 162 | if ((intensities[0] < 0 && intensities[2] > 0) || (intensities[2] < 0 && intensities[0] > 0)) { 163 | px[numChanges] = 0; 164 | py[numChanges] = -intensities[0] / (intensities[2] - intensities[0]); 165 | numChanges++; 166 | } 167 | 168 | double aPoint[2]; 169 | geometry_msgs::Point p; 170 | if (numChanges == 2) { 171 | for (int j = 0; j < numChanges; j++) { 172 | aPoint[0] = indMin[0] + px[j] + 0.5; 173 | aPoint[1] = indMin[1] + py[j] + 0.5; 174 | util::toRl(aPoint, p_grid_res_, p_map_size_x_, p_map_size_y_); 175 | p.x = aPoint[0]; //(int32_t) 176 | p.y = aPoint[1]; 177 | //p.x = 1; 178 | //p.y = 2; 179 | p.z = 0; 180 | line_list.points.push_back(p); 181 | sphere_list.points.push_back(p); 182 | } 183 | } 184 | else if (numChanges == 0) { 185 | if (intensities[0] > 0) { 186 | aPoint[0] = indMin[0] + 1; 187 | aPoint[1] = indMin[1] + 1; 188 | util::toRl(aPoint,p_grid_res_,p_map_size_x_,p_map_size_y_); 189 | p.x = aPoint[0]; //(int32_t) 190 | p.y = aPoint[1]; 191 | 192 | p.z = -0.1; 193 | cube_list.points.push_back(p); 194 | } 195 | } 196 | else { 197 | } 198 | } 199 | 200 | 201 | marker_pub_.publish(line_list); 202 | marker_pub_.publish(sphere_list); 203 | //marker_pub_.publish(cube_list); 204 | 205 | } 206 | 207 | void update_map(const PCLPointCloud& pc, const Eigen::Vector3d& pose3d){ 208 | //group scan endpoints 209 | PCLPointCloud::const_iterator it = pc.begin(); 210 | int scan_rays = pc.width * pc.height; //todo find/use lower bound instead 211 | float points[scan_rays][2]; 212 | float a_point_in_rl[2]; 213 | float a_point_in_map[2]; 214 | float point_window[2]; 215 | float pos[2] = {pose3d[0], pose3d[1]}; 216 | bool cont = true; 217 | int i = 0; 218 | float next_point_window[2]; 219 | 220 | while (it < pc.end()) { 221 | 222 | //todo max range 223 | a_point_in_rl[0] = it->x; 224 | a_point_in_rl[1] = it->y; 225 | 226 | //todo put this somewhere where dist is calculated anyway 227 | if (util::p2pDist(a_point_in_rl, pos) < p_min_range_) { 228 | it++; 229 | continue; 230 | } 231 | else if (util::p2pDist(a_point_in_rl, pos) > p_max_range_) { 232 | it++; 233 | continue; 234 | } 235 | 236 | a_point_in_map[0] = a_point_in_rl[0]; 237 | a_point_in_map[1] = a_point_in_rl[1]; 238 | util::toMap(a_point_in_map,p_grid_res_,p_map_size_x_,p_map_size_y_); 239 | 240 | if (fmod(a_point_in_map[0], 1) < 0.5) 241 | point_window[0] = floor(a_point_in_map[0]) - 0.5; 242 | else 243 | point_window[0] = floor(a_point_in_map[0]) + 0.5; 244 | if (fmod(a_point_in_map[1], 1) < 0.5) 245 | point_window[1] = floor(a_point_in_map[1]) - 0.5; 246 | else 247 | point_window[1] = floor(a_point_in_map[1]) + 0.5; 248 | 249 | cont = true; 250 | i = 0; 251 | while (cont) { 252 | 253 | if (i >= scan_rays) { 254 | ROS_ERROR("TOO MANY SCAN ENDPOINTS, THIS SHOULD NOT HAVE HAPPENED, VectorMap.h"); 255 | } 256 | 257 | points[i][0] = it->x; 258 | points[i][1] = it->y; 259 | i++; 260 | it++; 261 | if (it < pc.end()) { 262 | a_point_in_rl[0] = it->x; 263 | a_point_in_rl[1] = it->y; 264 | a_point_in_map[0] = a_point_in_rl[0]; 265 | a_point_in_map[1] = a_point_in_rl[1]; 266 | util::toMap(a_point_in_map,p_grid_res_,p_map_size_x_,p_map_size_y_); 267 | 268 | if (fmod(a_point_in_map[0], 1) < 0.5) 269 | next_point_window[0] = floor(a_point_in_map[0]) - 0.5; 270 | else 271 | next_point_window[0] = floor(a_point_in_map[0]) + 0.5; 272 | 273 | if (fmod(a_point_in_map[1], 1) < 0.5) 274 | next_point_window[1] = floor(a_point_in_map[1]) - 0.5; 275 | else 276 | next_point_window[1] = floor(a_point_in_map[1]) + 0.5; 277 | 278 | //todo this is not always correct, e.g. corner case: 279 | //___\_ 280 | //| x|2 281 | //| \ 282 | //| |\ 283 | //|_x__|x\ 284 | // 0 |1 285 | if (!(next_point_window[0] == point_window[0] && next_point_window[1] == point_window[1])) { 286 | cont = false; 287 | } 288 | } 289 | else 290 | cont = false; 291 | } 292 | 293 | //more than one point in window 294 | if (i > 1) 295 | update_cells(pos, points, i, point_window, sdf_); 296 | else { 297 | //todo rm hack, look at neighbour scans (see note above..), also, introduce distance threshold for neigbours, if exceeded num=1... 298 | if ((it - 1) > pc.begin() && (it) < pc.end()) { 299 | if (util::p2pDist((it - 2)->x, (it - 2)->y, points[0]) < util::p2pDist(it->x, it->y, points[0])) { 300 | points[i][0] = (it - 2)->x; 301 | points[i][1] = (it - 2)->y; 302 | } 303 | else { 304 | points[i][0] = it->x; 305 | points[i][1] = it->y; 306 | } 307 | } 308 | else if (it - 1 > pc.begin()) { 309 | points[i][0] = (it - 2)->x; 310 | points[i][1] = (it - 2)->y; 311 | } 312 | else if (it < pc.end()) { 313 | points[i][0] = it->x; 314 | points[i][1] = it->y; 315 | } 316 | update_cells(pos, points, 2, point_window, sdf_); 317 | } 318 | } 319 | } 320 | 321 | void reset_map(){ 322 | VecMapFloat sdf((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0.0)); 323 | sdf_ = sdf; 324 | 325 | VecMapInt sdf_level((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0)); 326 | sdf_level_ = sdf_level; 327 | 328 | VecMapInt sdf_count((unsigned long) p_map_size_x_, std::vector((unsigned long) p_map_size_y_, 0)); 329 | sdf_count_ = sdf_count; 330 | } 331 | 332 | int get_map_values(const Eigen::Vector2d& coords, float *mpdxdy, bool fine) { 333 | mpdxdy[0] = 0.0; 334 | mpdxdy[1] = 0.0; 335 | mpdxdy[2] = 0.0; 336 | 337 | float scaledCoords[2] = {coords[0], coords[1]}; 338 | util::toMap(scaledCoords, p_grid_res_,p_map_size_x_,p_map_size_y_); 339 | scaledCoords[0] = scaledCoords[0] - 0.5; 340 | scaledCoords[1] = scaledCoords[1] - 0.5; 341 | int indMin[2] = {floor(scaledCoords[0]), floor(scaledCoords[1])}; 342 | 343 | float factors[2] = {(scaledCoords[0] - indMin[0]), (scaledCoords[1] - indMin[1])}; 344 | 345 | float intensities[4]; 346 | int indices[2]; 347 | for (int i = 0; i < 4; i++) { 348 | indices[0] = indMin[0] + i % 2; 349 | indices[1] = indMin[1] + i / 2; 350 | if (indices[0] >= 0 && indices[0] < p_map_size_x_ && indices[1] >= 0 && indices[1] < p_map_size_y_) 351 | intensities[i] = sdf_[indices[1]][indices[0]]; 352 | else { 353 | ROS_ERROR("map too small, inds %d %d map size %d %d", indices[0], indices[1], p_map_size_x_, p_map_size_x_); 354 | ROS_ERROR("coords %f %f", coords[0], coords[1]); 355 | } 356 | } 357 | 358 | //check for sgn changes NESW 359 | //203 360 | //3x1 361 | //021 362 | int numChanges = 0; 363 | float px[4]; 364 | float py[4]; 365 | if ((intensities[2] < 0 && intensities[3] > 0) || (intensities[3] < 0 && intensities[2] > 0)) { 366 | px[numChanges] = -intensities[2] / (intensities[3] - intensities[2]); 367 | py[numChanges] = 1; 368 | numChanges++; 369 | } 370 | if ((intensities[1] < 0 && intensities[3] > 0) || (intensities[3] < 0 && intensities[1] > 0)) { 371 | px[numChanges] = 1; 372 | py[numChanges] = -intensities[1] / (intensities[3] - intensities[1]); 373 | numChanges++; 374 | } 375 | if ((intensities[0] < 0 && intensities[1] > 0) || (intensities[1] < 0 && intensities[0] > 0)) { 376 | px[numChanges] = -intensities[0] / (intensities[1] - intensities[0]); 377 | py[numChanges] = 0; 378 | numChanges++; 379 | } 380 | if ((intensities[0] < 0 && intensities[2] > 0) || (intensities[2] < 0 && intensities[0] > 0)) { 381 | px[numChanges] = 0; 382 | py[numChanges] = -intensities[0] / (intensities[2] - intensities[0]); 383 | numChanges++; 384 | } 385 | 386 | 387 | bool allZero = true; 388 | for (int count = 0; count < 4; count++) 389 | if (intensities[count] != 0) { 390 | allZero = false; 391 | } 392 | else 393 | numChanges = 6; 394 | 395 | //unclean tho 396 | if (!fine && numChanges == 0) { 397 | //todo follow gradient anyways 398 | 399 | if (!allZero) { 400 | if (intensities[0] < 0) { 401 | mpdxdy[1] = -((intensities[2] - intensities[3]) * factors[1] + 402 | (1 - factors[1]) * (intensities[0] - intensities[1])); 403 | mpdxdy[2] = -((intensities[0] - intensities[2]) * factors[0] + 404 | (1 - factors[0]) * (intensities[1] - intensities[3])); 405 | } 406 | else { 407 | mpdxdy[1] = ((intensities[2] - intensities[3]) * factors[1] + 408 | (1 - factors[1]) * (intensities[0] - intensities[1])); 409 | mpdxdy[2] = ((intensities[0] - intensities[2]) * factors[0] + 410 | (1 - factors[0]) * (intensities[1] - intensities[3])); 411 | } 412 | mpdxdy[0] = factors[1] * (factors[0] * intensities[3] + (1 - factors[0]) * intensities[2]) 413 | + (1 - factors[1]) * (factors[0] * intensities[1] + (1 - factors[0]) * intensities[0]); 414 | mpdxdy[0] = fabs(mpdxdy[0]); 415 | if (fabs(mpdxdy[1]) > fabs(mpdxdy[2])) { 416 | if (mpdxdy[1] > 0) 417 | mpdxdy[1] = 1; 418 | else 419 | mpdxdy[1] = -1; 420 | } 421 | else { 422 | if (mpdxdy[2] > 0) 423 | mpdxdy[2] = 1; 424 | else 425 | mpdxdy[2] = -1; 426 | } 427 | 428 | //cap the gradient at 1 todo experiment with that 429 | if (mpdxdy[0] > p_grid_res_) 430 | mpdxdy[0] = p_grid_res_; 431 | } 432 | else { 433 | mpdxdy[0] = 0; 434 | mpdxdy[1] = 0; 435 | mpdxdy[2] = 0; 436 | } 437 | } 438 | 439 | //42: 440 | //y = mx + b 441 | // yhit 442 | // p1 | 443 | //yCoord--2-\-|----3 444 | // | \| | 445 | //xhit--------\ | 446 | // | x \ | 447 | // | |\ | 448 | // O----|-\-1 449 | // | p0 450 | // xCoord 451 | // 452 | //todo use vector form ffs 453 | 454 | if (numChanges == 2) { 455 | bool vertical; 456 | float m, b; 457 | float hit[2]; 458 | float yCoord, xCoord; 459 | if (fabs(px[1] - px[0]) < epsilon_) { 460 | vertical = true; 461 | hit[0] = px[0]; 462 | hit[1] = factors[1]; 463 | yCoord = hit[1]; 464 | xCoord = hit[0]; 465 | } 466 | else { 467 | if (fabs(py[1] - py[0]) < epsilon_) { 468 | //horizontal 469 | m = 0; 470 | hit[0] = factors[0]; 471 | hit[1] = py[0]; 472 | b = py[0]; 473 | } 474 | else { 475 | m = (py[1] - py[0]) / (px[1] - px[0]); 476 | b = py[0] - m * px[0]; 477 | hit[0] = (b - factors[1] + 1 / m * factors[0]) / (-1 / m - m); 478 | hit[1] = hit[0] * m + b; 479 | } 480 | } 481 | 482 | if (!vertical) { 483 | mpdxdy[0] = util::p2lDist(m, b, factors); //dunno 484 | yCoord = factors[0] * m + b; 485 | 486 | if (!m == 0) 487 | xCoord = (factors[1] - b) / m; 488 | else 489 | //xCoord = factors[0]; 490 | xCoord = 999999; 491 | //todo! not sure if this makes sense, not needed for vector form anyways though 492 | 493 | if (factors[0] > xCoord) 494 | mpdxdy[1] = -(1-xCoord); 495 | //mpdxdy[1] = -1; 496 | else 497 | mpdxdy[1] = xCoord; 498 | //mpdxdy[1] = 1; 499 | 500 | if (factors[1] > yCoord) 501 | mpdxdy[2] = -(1-yCoord); 502 | //mpdxdy[2] = -1; 503 | else 504 | mpdxdy[2] = yCoord; 505 | //mpdxdy[2] = 1; 506 | 507 | if (yCoord < 0 || yCoord > 1) 508 | mpdxdy[2] = 0; 509 | 510 | if (xCoord < 0 || xCoord > 1) 511 | mpdxdy[1] = 0; 512 | 513 | } 514 | else { 515 | //todo replace cases with 1 solution.. 516 | // ROS_WARN("VERTICAL"); 517 | if (factors[0] < px[0]) 518 | mpdxdy[1] = px[0]; 519 | //mpdxdy[1] = 1; 520 | else 521 | mpdxdy[1] = px[0]-1; 522 | //mpdxdy[1] = -1; 523 | 524 | mpdxdy[0] = fabs(factors[0] - px[0]); 525 | mpdxdy[2] = 0; 526 | } 527 | 528 | mpdxdy[0] = mpdxdy[0] / (1 / p_grid_res_); 529 | 530 | //mpdxdy[0] = util::round(mpdxdy[0], 32); 531 | //mpdxdy[1] = util::round(mpdxdy[1], 32); 532 | //mpdxdy[2] = util::round(mpdxdy[2], 32); 533 | 534 | if (mpdxdy[1] < epsilon_ && mpdxdy[1] > -epsilon_) { 535 | //ROS_ERROR("asxaxaxax"); 536 | mpdxdy[1] = 0; 537 | } 538 | if (mpdxdy[2] < epsilon_ && mpdxdy[2] > -epsilon_) { 539 | //ROS_ERROR("asxaxaxax"); 540 | mpdxdy[2] = 0; 541 | } 542 | 543 | } 544 | 545 | if (numChanges == 4) { 546 | mpdxdy[1] = 0; 547 | mpdxdy[2] = 0; 548 | mpdxdy[0] = 0; 549 | } 550 | 551 | return numChanges / 2; 552 | } 553 | 554 | protected: 555 | 556 | void deming_regression(float tars[][2], int num, double &beta0, double &beta1) { 557 | //todo hackhack, use neighbors 558 | bool revert = false; 559 | if (num == 0) { 560 | num = 2; 561 | revert = true; 562 | } 563 | 564 | //orthogonal deming regression 565 | double xbar = 0; 566 | double ybar = 0; 567 | double sxx = 0; 568 | double sxy = 0; 569 | double syy = 0; 570 | for (int i = 0; i < num; i++) { 571 | xbar += util::toMap(tars[i][0], p_grid_res_, p_map_size_x_); 572 | ybar += util::toMap(tars[i][1], p_grid_res_, p_map_size_y_); 573 | } 574 | xbar /= num; 575 | ybar /= num; 576 | for (int i = 0; i < num; i++) { 577 | sxx += (util::toMap(tars[i][0], p_grid_res_, p_map_size_x_) - xbar) * (util::toMap(tars[i][0], p_grid_res_, p_map_size_x_) - xbar); 578 | sxy += (util::toMap(tars[i][0], p_grid_res_, p_map_size_x_) - xbar) * (util::toMap(tars[i][1], p_grid_res_, p_map_size_y_) - ybar); 579 | syy += (util::toMap(tars[i][1], p_grid_res_, p_map_size_y_) - ybar) * (util::toMap(tars[i][1], p_grid_res_, p_map_size_y_) - ybar); 580 | } 581 | sxx /= num - 1; 582 | sxy /= num - 1; 583 | syy /= num - 1; 584 | 585 | beta1 = (syy - sxx + sqrt((syy - sxx) * (syy - sxx) + 4 * sxy * sxy)) / (2 * sxy); 586 | beta0 = ybar - beta1 * xbar; 587 | //y=beta0+beta1*x 588 | //end deming 589 | 590 | if (revert) 591 | num = 1; 592 | } 593 | 594 | void update_cells(float *src, float tars[][2], int num, float *window, VecMapFloat &aMap) { 595 | { 596 | float srcInMap[2] = {src[0], src[1]}; 597 | util::toMap(srcInMap, p_grid_res_, p_map_size_x_, p_map_size_y_); 598 | float tarInMap[2] = {tars[0][0], tars[0][1]}; 599 | util::toMap(tarInMap, p_grid_res_, p_map_size_x_, p_map_size_y_); 600 | 601 | double beta0, beta1; 602 | 603 | bool horizontal = true; 604 | for (int i = 0; i < num; i++) { 605 | //ROS_INFO("i %d/%d comparing %f %f", i, num,tars[0][1], tars[i][1]); 606 | if (fabs(tars[0][1] - tars[i][1]) > epsilon_) { 607 | horizontal = false; 608 | break; 609 | } 610 | } 611 | 612 | //ROS_ERROR("horizontal %d", horizontal); 613 | 614 | if (!horizontal) 615 | deming_regression(tars, num, beta0, beta1); 616 | else { 617 | beta1 = 0; 618 | beta0 = tarInMap[1]; 619 | } 620 | 621 | if (beta0 != beta0 || beta1 != beta1) { 622 | ROS_ERROR("deming says %f %f (adjust epsilon)", beta0, beta1); 623 | return; 624 | } 625 | 626 | int x_index = floor(window[0]) - p_update_map_aoe_; 627 | int y_index = floor(window[1]) - p_update_map_aoe_; 628 | 629 | float reg_border[2][2]; 630 | int reg_border_counter = 0; 631 | 632 | double x_temp; 633 | double y_temp; 634 | for (int reg_counter = 0; reg_counter < 2; reg_counter++) { 635 | if (beta1 != 0) { 636 | x_temp = (window[1] + reg_counter - beta0) / beta1; 637 | if (x_temp >= window[0] && x_temp <= window[0] + 1) { 638 | reg_border[reg_border_counter][0] = x_temp; 639 | reg_border[reg_border_counter][1] = window[1] + reg_counter; 640 | reg_border_counter++; 641 | } 642 | } 643 | 644 | y_temp = beta0 + beta1 * (window[0] + reg_counter); 645 | if (y_temp >= window[1] && y_temp <= window[1] + 1) { 646 | reg_border[reg_border_counter][0] = window[0] + reg_counter;; 647 | reg_border[reg_border_counter][1] = y_temp; 648 | reg_border_counter++; 649 | } 650 | } 651 | 652 | 653 | bool vertical = false; 654 | if (beta1 > 999999 || beta1 < -999999) { //vertical 655 | reg_border[0][0] = tarInMap[0]; 656 | reg_border[0][1] = window[1]; 657 | reg_border[1][0] = tarInMap[0]; 658 | reg_border[1][1] = window[1] + 1; 659 | vertical = true; 660 | } 661 | 662 | int level = p_update_map_aoe_; 663 | int new_x_index; 664 | int new_y_index; 665 | float cell_coords[2]; 666 | bool invert; 667 | float dist_temp; 668 | float mapVal_temp; 669 | for (int counter_x = 0; counter_x < p_update_map_aoe_ * 2 + 2; counter_x++) { 670 | for (int counter_y = 0; counter_y < p_update_map_aoe_ * 2 + 2; counter_y++) { 671 | level = std::max(abs(trunc(counter_x - p_update_map_aoe_ - 0.5)), 672 | abs(trunc(counter_y - p_update_map_aoe_ - 0.5))); 673 | 674 | new_x_index = x_index + counter_x; 675 | new_y_index = y_index + counter_y; 676 | cell_coords[0] = new_x_index + 0.5; 677 | cell_coords[1] = new_y_index + 0.5; 678 | 679 | if (new_x_index >= p_map_size_x_ || new_y_index >= p_map_size_y_) 680 | ROS_ERROR("out of map bounds"); 681 | 682 | invert = invertCheck(srcInMap, cell_coords, reg_border); 683 | 684 | if (vertical) { 685 | if (cell_coords[1] >= window[1] - epsilon_ && cell_coords[1] <= window[1] + 1 + epsilon_) 686 | dist_temp = ((float) (fabs(cell_coords[0] - reg_border[0][0]) * p_grid_res_)); 687 | else 688 | dist_temp = 0.0; 689 | } 690 | else if (level == 0) 691 | dist_temp = util::p2lsDist(reg_border[0], reg_border[1], cell_coords) * p_grid_res_; 692 | else 693 | dist_temp = util::p2lsDistTwo(p_grid_res_/2, reg_border[0], reg_border[1], cell_coords) * p_grid_res_; 694 | 695 | if (dist_temp != 0.0) { 696 | if (invert) 697 | dist_temp = -dist_temp; 698 | 699 | if (sdf_count_[new_y_index][new_x_index] == 0) { 700 | aMap[new_y_index][new_x_index] = dist_temp; 701 | sdf_level_[new_y_index][new_x_index] = level; 702 | } 703 | else if (aMap[new_y_index][new_x_index] * dist_temp < 704 | 0) { //todo not fast either, also use level for something useful... 705 | //if (dist_temp>0 || level==0){ 706 | //if (sdf_level_[new_y_index][new_x_index] > level || (sdf_level_[new_y_index][new_x_index] == level && dist_temp > 0)) { 707 | //if (sdf_level_[new_y_index][new_x_index] > level || (level > p_update_map_aoe_ && sdf_level_[new_y_index][new_x_index] == level && dist_temp > 0)) { 708 | if (sdf_level_[new_y_index][new_x_index] > level) { 709 | aMap[new_y_index][new_x_index] = dist_temp; 710 | sdf_level_[new_y_index][new_x_index] = level; 711 | } 712 | } 713 | else { //todo might be fishy here... 714 | if (sdf_level_[new_y_index][new_x_index] == level || 715 | sdf_level_[new_y_index][new_x_index] == level - p_avg_range_) { 716 | if (!p_avg_mapping_) { 717 | if (!invert) 718 | aMap[new_y_index][new_x_index] = fmin(aMap[new_y_index][new_x_index], dist_temp); 719 | else 720 | aMap[new_y_index][new_x_index] = fmax(aMap[new_y_index][new_x_index], dist_temp); 721 | } 722 | else { 723 | mapVal_temp = (aMap[new_y_index][new_x_index] * sdf_count_[new_y_index][new_x_index] 724 | + dist_temp) / (sdf_count_[new_y_index][new_x_index] + 1); 725 | } 726 | } 727 | else if (aMap[new_y_index][new_x_index] > level) { 728 | aMap[new_y_index][new_x_index] = dist_temp; 729 | } 730 | sdf_level_[new_y_index][new_x_index] = level; 731 | } 732 | sdf_count_[new_y_index][new_x_index] = sdf_count_[new_y_index][new_x_index] + 1; 733 | } 734 | } 735 | } 736 | } 737 | } 738 | 739 | 740 | void save_map(std::string filename) { 741 | std::fstream os(filename.c_str(), std::ios::out | std::ios::app); 742 | 743 | for (int i = 0; i < p_map_size_y_; ++i) { 744 | for (int j = 0; j < p_map_size_x_; ++j) { 745 | os << sdf_[i][j] << " "; 746 | } 747 | os << "\n"; 748 | } 749 | 750 | os.close(); 751 | } 752 | 753 | 754 | void load_map(std::string filename) { 755 | std::string word; 756 | std::ifstream myfile(filename.c_str()); 757 | if (myfile.is_open()) { 758 | int xC = 0; 759 | int yC = 0; 760 | while (myfile >> word) { 761 | sdf_[xC][yC] = (float) atof(word.c_str()); 762 | if (xC < p_map_size_y_ - 1) 763 | xC++; 764 | else { 765 | xC = 0; 766 | yC++; 767 | } 768 | } 769 | myfile.close(); 770 | } 771 | else 772 | ROS_ERROR("Unable to open file"); 773 | } 774 | 775 | /* 776 | void publish_map_cheap(bool updated) { 777 | if (updated) { 778 | iso_map_.clear(); 779 | //sdf_thresholded_.clear(); 780 | cloud_map_.clear(); 781 | 782 | int counter = 0; 783 | for (int i = 1; i < sdf_thresholded_.size() - 1; i++) 784 | for (int j = 1; j < sdf_thresholded_[0].size() - 1; j++) 785 | if (sdf_[i][j] < -p_isovalue_) 786 | sdf_thresholded_[i][j] = 1; 787 | else 788 | sdf_thresholded_[i][j] = 0; 789 | 790 | 791 | for (int i = 1; i < sdf_thresholded_.size() - 1; i++) { 792 | for (int j = 1; j < sdf_thresholded_[0].size() - 1; j++) { 793 | if (sdf_thresholded_[i][j] == 1) { 794 | Eigen::Vector2f coord(j + 0.5, i + 0.5); 795 | coord = util::toRl(coord, p_grid_res_, p_map_size_x_, p_map_size_y_); 796 | iso_map_.push_back(util::genPoint(coord.x(), coord.y())); 797 | counter++; 798 | } 799 | //}//del 800 | //}//del if unmcomment 801 | //}//del if 802 | 803 | int state = 0; 804 | if (sdf_thresholded_[i][j] == 1) 805 | state += 8; 806 | if (sdf_thresholded_[i][j + 1] == 1) 807 | state += 4; 808 | if (sdf_thresholded_[i - 1][j + 1] == 1) 809 | state += 2; 810 | if (sdf_thresholded_[i - 1][j] == 1) 811 | state += 1; 812 | 813 | // double yPos = (i+0.5-p_map_size_y_/2)*p_grid_res_; 814 | // double xPos = (j+0.5-p_map_size_x_/2)*p_grid_res_; 815 | Eigen::Vector2f coord(j, i); 816 | coord = util::toRl(coord, p_grid_res_, p_map_size_x_, p_map_size_y_); 817 | double yPos = coord.y() + p_grid_res_ / 2;//(i-p_map_size_y_/2)*p_grid_res_; 818 | double xPos = coord.x() + p_grid_res_ / 2;//(j-p_map_size_x_/2)*p_grid_res_; 819 | 820 | // cloud_map_.width = counter * 6; 821 | // cloud_map_.height = 1; 822 | // cloud_map_.is_dense = false; 823 | // // cloud_map_.points.resize(cloud_map_.width * cloud_map_.height); 824 | 825 | switch (state) { 826 | case 0: 827 | break; 828 | 829 | case 1: 830 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 831 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 832 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 4, yPos - 3 * p_grid_res_ / 4)); 833 | break; 834 | 835 | case 2: 836 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 837 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 838 | cloud_map_.push_back(util::genPoint(xPos + 3 * p_grid_res_ / 4, yPos - 3 * p_grid_res_ / 4)); 839 | break; 840 | 841 | case 3: 842 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 843 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_ / 2)); 844 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 845 | break; 846 | 847 | case 4: 848 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 849 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 850 | cloud_map_.push_back(util::genPoint(xPos + 3 * p_grid_res_ / 4, yPos - p_grid_res_ / 4)); 851 | break; 852 | 853 | case 5: 854 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 855 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 856 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 4, yPos - p_grid_res_ / 2)); 857 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 858 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 859 | cloud_map_.push_back(util::genPoint(xPos + 3 * p_grid_res_ / 4, yPos - 3 * p_grid_res_ / 4)); 860 | break; 861 | 862 | case 6: 863 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 864 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_ / 2)); 865 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 866 | break; 867 | 868 | case 7: 869 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 870 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 871 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 4, yPos - p_grid_res_ / 2)); 872 | break; 873 | 874 | case 8: 875 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 876 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 877 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 4, yPos - p_grid_res_ / 2)); 878 | break; 879 | 880 | case 9: 881 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 882 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_ / 2)); 883 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 884 | break; 885 | 886 | case 10: 887 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 888 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 889 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 4, yPos - 3 * p_grid_res_ / 4)); 890 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 891 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 892 | cloud_map_.push_back(util::genPoint(xPos + 3 * p_grid_res_ / 4, yPos - p_grid_res_ / 4)); 893 | break; 894 | 895 | case 11: 896 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos)); 897 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 898 | cloud_map_.push_back(util::genPoint(xPos + 3 * p_grid_res_ / 4, yPos - p_grid_res_ / 4)); 899 | break; 900 | 901 | case 12: 902 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 903 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_ / 2)); 904 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 905 | break; 906 | 907 | case 13: 908 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_, yPos - p_grid_res_ / 2)); 909 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 910 | cloud_map_.push_back(util::genPoint(xPos + 3 * p_grid_res_ / 4, yPos - 3 * p_grid_res_ / 4)); 911 | break; 912 | 913 | case 14: 914 | cloud_map_.push_back(util::genPoint(xPos, yPos - p_grid_res_ / 2)); 915 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 2, yPos - p_grid_res_)); 916 | cloud_map_.push_back(util::genPoint(xPos + p_grid_res_ / 4, yPos - 3 * p_grid_res_ / 4)); 917 | break; 918 | 919 | case 15: 920 | break; 921 | 922 | default: 923 | ROS_WARN("this should not have happened, switch @ publish clud_map_ broken"); 924 | break; 925 | } 926 | } 927 | 928 | } 929 | } 930 | sensor_msgs::PointCloud2 cloudMsg; 931 | pcl::toROSMsg(cloud_map_, cloudMsg); 932 | cloudMsg.header.frame_id = p_fixed_frame_; 933 | cloudMsg.header.stamp = ros::Time::now(); 934 | map_pub_.publish(cloudMsg); 935 | 936 | //rm me 937 | sensor_msgs::PointCloud2 isoMsg; 938 | pcl::toROSMsg(iso_map_, isoMsg); 939 | isoMsg.header.frame_id = p_fixed_frame_; 940 | isoMsg.header.stamp = ros::Time::now(); 941 | iso_pub_.publish(isoMsg); //rm me 942 | 943 | pcl::PointCloud::Ptr monster_cloud(new pcl::PointCloud); 944 | int insertAt = 0; 945 | monster_cloud->width = p_map_size_x_ * p_map_size_y_; 946 | monster_cloud->height = 1; 947 | monster_cloud->is_dense = false; 948 | monster_cloud->points.resize(monster_cloud->width * monster_cloud->height); 949 | for (int i = 0; i < p_map_size_y_; i++) { 950 | for (int j = 0; j < p_map_size_x_; j++) { 951 | if (sdf_[i][j] != 0 && sdf_[i][j] < p_truncation_ && sdf_[i][j] > -p_truncation_) { 952 | monster_cloud->points[insertAt].z = sdf_[i][j]; 953 | Eigen::Vector2f coord(j, i); 954 | coord = util::toRl(coord, p_grid_res_, p_map_size_x_, p_map_size_y_); 955 | monster_cloud->points[insertAt].x = p_grid_res_ / 2 + j * p_grid_res_ - (p_map_size_x_ / 2) * p_grid_res_; 956 | //monster_cloud->points[insertAt].x = coord.x()+p_grid_res_/2; 957 | monster_cloud->points[insertAt].y = p_grid_res_ / 2 + i * p_grid_res_ - (p_map_size_y_ / 2) * p_grid_res_; 958 | //monster_cloud->points[insertAt].y = coord.y()+p_grid_res_/2; 959 | insertAt++; 960 | } 961 | } 962 | } 963 | sensor_msgs::PointCloud2 cloudMsg2; 964 | pcl::toROSMsg(*monster_cloud, cloudMsg2); 965 | cloudMsg2.header.frame_id = p_fixed_frame_; 966 | cloudMsg2.header.stamp = ros::Time::now(); 967 | cloudA_pub_.publish(cloudMsg2); 968 | 969 | } 970 | */ 971 | 972 | bool invertCheck(float *src, float *tar, float border[2][2]) { 973 | float x1 = src[0]; 974 | float y1 = src[1]; 975 | float x2 = tar[0]; 976 | float y2 = tar[1]; 977 | float x3 = border[0][0]; 978 | float y3 = border[0][1]; 979 | float x4 = border[1][0]; 980 | float y4 = border[1][1]; 981 | 982 | float sx = ((x4 - x3) * (x2 * y1 - x1 * y2) - (x2 - x1) * (x4 * y3 - x3 * y4)) / 983 | ((y4 - y3) * (x2 - x1) - (y2 - y1) * (x4 - x3)); 984 | float sy = ((y1 - y2) * (x4 * y3 - x3 * y4) - (y3 - y4) * (x2 * y1 - x1 * y2)) / 985 | ((y4 - y3) * (x2 - x1) - (y2 - y1) * (x4 - x3)); 986 | 987 | float src_tar_dist = util::p2pDist(src, tar); 988 | float src_border_dist = util::p2pDist(sx, sy, src); 989 | 990 | if ((tar[0] - src[0] > 0 && sx - src[0] < 0) || 991 | (tar[0] - src[0] < 0 && sx - src[0] > 0) || 992 | (src_border_dist > src_tar_dist)) 993 | return false; 994 | else 995 | return true; 996 | } 997 | 998 | ros::NodeHandle nh_; 999 | 1000 | VecMapFloat sdf_; 1001 | VecMapInt sdf_level_; 1002 | VecMapInt sdf_count_; 1003 | double p_grid_res_; 1004 | int p_map_size_x_; 1005 | int p_map_size_y_; 1006 | double p_min_range_; 1007 | double p_max_range_; 1008 | double epsilon_; 1009 | int p_avg_range_; 1010 | bool p_avg_mapping_; 1011 | int p_update_map_aoe_; 1012 | ros::Publisher marker_pub_; 1013 | std::string p_fixed_frame_; 1014 | 1015 | /* 1016 | map_pub_ = nh_.advertise("pointcloud_map", 10); 1017 | iso_pub_ = nh_.advertise("iso_map", 10); 1018 | cloudA_pub_ = nh_.advertise("cloudA", 10); 1019 | ros::Publisher map_pub_; 1020 | ros::Publisher iso_pub_; 1021 | ros::Publisher cloudA_pub_; 1022 | */ 1023 | }; 1024 | 1025 | } 1026 | 1027 | #endif //SDF_SLAM_2D_VECTORMAP_H 1028 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/registration/AbstractRegistration.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | #ifndef SDF_SLAM_2D_ABSTRACTREGISTRATION_H 20 | #define SDF_SLAM_2D_ABSTRACTREGISTRATION_H 21 | 22 | #include "../utility/Types.h" 23 | 24 | namespace sdfslam{ 25 | 26 | class AbstractRegistration { 27 | 28 | public: 29 | 30 | virtual Eigen::Vector3d new_match(const PCLPointCloud& scan, AbstractMap* const aMap, int* case_count, Eigen::Vector3d pos){}; 31 | 32 | protected: 33 | 34 | }; 35 | 36 | } 37 | 38 | 39 | #endif //SDF_SLAM_2D_ABSTRACTREGISTRATION_H 40 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/registration/GaussNewton.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //Gauss Newton minimization registration extended from Hector SLAM: 5 | //https://github.com/tu-darmstadt-ros-pkg/hector_slam 6 | //Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 7 | // 8 | //This program is free software; you can redistribute it and/or modify 9 | //it under the terms of the GNU General Public License as published by 10 | //the Free Software Foundation; either version 2 of the License, or 11 | //any later version. 12 | // 13 | //This program is distributed in the hope that it will be useful, 14 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | //GNU General Public License for more details. 17 | // 18 | //You should have received a copy of the GNU General Public License along 19 | //with this program; if not, write to the Free Software Foundation, Inc., 20 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 21 | //================================================================================================= 22 | 23 | #ifndef SDF_SLAM_2D_GAUSSNEWTON_H 24 | #define SDF_SLAM_2D_GAUSSNEWTON_H 25 | 26 | #include "ros/ros.h" 27 | 28 | 29 | namespace sdfslam{ 30 | 31 | class GaussNewtonRegistration : public AbstractRegistration{ 32 | 33 | public: 34 | 35 | GaussNewtonRegistration(){ 36 | ros::NodeHandle private_nh("~"); 37 | private_nh.param("p_min_range_", p_min_range_, 0.15); 38 | private_nh.param("p_max_range_", p_max_range_, 5.6); 39 | private_nh.param("p_fine_pos_", p_fine_pos_, true); 40 | private_nh.param("p_stop_iter_time_", p_stop_iter_time_, 0.09); 41 | private_nh.param("p_num_iter_", p_num_iter_, 30); 42 | } 43 | 44 | Eigen::Vector3d new_match(const PCLPointCloud& scan, AbstractMap* const aMap, int* case_count, const Eigen::Vector3d pos){ 45 | ros::Time start = ros::Time::now(); 46 | ros::Time end; 47 | ros::Duration dur; 48 | 49 | tempYaw_ = 0; 50 | for (int i = 0; i < 3; i++) { 51 | searchdirplus[i] = false; 52 | } 53 | converged_ = false; 54 | float searchfacs[3] = {1, 1, p_num_iter_ / 3}; //todo magic 55 | //float searchfacs[3] = { 1, 1, 1}; 56 | bool interrupted = false; 57 | Eigen::Vector3d pose_estimate(0, 0, 0); 58 | Eigen::Vector3d temp_pose_estimate(0, 0, 0); 59 | bool breaking = true; 60 | for (int i = 0; i < p_num_iter_; ++i) { 61 | end = ros::Time::now(); 62 | dur = end - start; 63 | if (dur.toSec() > p_stop_iter_time_) { 64 | ROS_WARN("match dur %d %d, stopping at iter %d", dur.sec, dur.nsec, i); 65 | break; 66 | } 67 | 68 | case_count[0] = 0; 69 | case_count[1] = 0; 70 | case_count[2] = 0; 71 | case_count[3] = 0; 72 | //ROS_WARN("44 iter %d pose x %f y %f yaw %f", i, pose_estimate[0], pose_estimate[1], pose_estimate[2]); 73 | 74 | if (true && i < p_num_iter_ / 2) 75 | interrupted = estimateTransformationLogLh(pose_estimate, scan, searchfacs, aMap, case_count, false, pos); 76 | else { 77 | estimateTransformationLogLh(pose_estimate, scan, searchfacs, aMap, case_count, p_fine_pos_, pos); 78 | interrupted = false; 79 | } 80 | 81 | breaking = true; 82 | for (int x = 0; x < 3; x++) { 83 | if (fabs(temp_pose_estimate[x] - pose_estimate[x]) > 0.00001) { 84 | breaking = false; 85 | } 86 | } 87 | temp_pose_estimate = Eigen::Vector3d(pose_estimate); 88 | 89 | if (interrupted) { 90 | if (pose_estimate[0] != pose_estimate[0]) 91 | pose_estimate = Eigen::Vector3d(0, 0, 0); 92 | i = p_num_iter_ / 2; 93 | continue; 94 | } 95 | 96 | if (breaking) { 97 | if (i < p_num_iter_ / 2) 98 | i = p_num_iter_ / 2; 99 | else { 100 | converged_ = true; 101 | break; 102 | } 103 | } 104 | } 105 | return pose_estimate; 106 | } 107 | 108 | protected: 109 | bool estimateTransformationLogLh(Eigen::Vector3d &estimate, 110 | const PCLPointCloud &cloud, 111 | float *searchfacs, 112 | AbstractMap* const aMap, 113 | int *case_count, 114 | bool fine, 115 | Eigen::Vector3d pos) { 116 | //getCompleteHessianDerivs(estimate, cloud, H, dTr, aMap, false); 117 | //std::cout << H << std::endl << dTr << std::endl <= 1) 159 | searchfacs[2] = searchfacs[2] - 1; 160 | tempYaw_ = searchDir[2]; 161 | } 162 | // ROS_WARN("serachfacs %f %f %f", searchfacs[0],searchfacs[1],searchfacs[2]); 163 | // if (searchDir[2] > 0.2f) { 164 | // searchDir[2] = 0.2f; 165 | // std::cout << "SearchDir angle change too large\n"; 166 | // } else if (searchDir[2] < -0.2f) { 167 | // searchDir[2] = -0.2f; 168 | // std::cout << "SearchDir angle change too large\n"; 169 | // } 170 | // ROS_WARN("serachfacs %f %f %f", searchfacs[0],searchfacs[1],searchfacs[2]); 171 | // //hickhack 172 | // for (int j=2; j<3; j++){ 173 | // if (searchDir[j] > 0) { 174 | // if (!searchdirplus[j]){ 175 | // searchfacs[j] = searchfacs[j]-0.5; 176 | // if (j==0) { 177 | // searchfacs[1] = searchfacs[1]-0.5;; 178 | // } 179 | // if (j==1) { 180 | // searchfacs[0] = searchfacs[0]-0.5;; 181 | // } 182 | 183 | // } 184 | // searchdirplus[j] = true; 185 | // } 186 | // else{ 187 | // if (searchdirplus[j]){ 188 | // searchfacs[j] = searchfacs[j]-0.5; 189 | // if (j==0) { 190 | // searchfacs[1] = searchfacs[1]-0.5;; 191 | // } 192 | // if (j==1) { 193 | // searchfacs[0] = searchfacs[0]-0.5;; 194 | // } 195 | 196 | // } 197 | // searchdirplus[j] = false; 198 | // } 199 | // if (searchDir[j] < 1) 200 | // searchDir[j] = 1; 201 | // } 202 | // ROS_WARN("serachfacs %f %f %f", searchfacs[0],searchfacs[1],searchfacs[2]); 203 | 204 | //bool abort = false; 205 | // if ( (searchDir[0] < 0.005 && searchDir[0] > -0.005) && (searchDir[1] < 0.005 && searchDir[1] > -0.005) && (searchDir[2] < 0.001 && searchDir[2] > -0.001) ) 206 | //abort = true; 207 | 208 | //hier! 209 | // searchDir[0] = searchDir[0]*searchfacs[0]; 210 | // searchDir[1] = searchDir[1]*searchfacs[1]; 211 | // searchDir[2] = searchDir[2]*searchfacs[2]; 212 | 213 | //magic tresh 0.0 214 | for (int i = 0; i < 3; i++) 215 | if (fabs(searchDir[i]) > 0.1) if (searchDir[i] > 0) 216 | searchDir[i] = 0.1; 217 | else 218 | searchDir[i] = -0.1; 219 | 220 | 221 | estimate[0] += searchDir[0]; 222 | estimate[1] += searchDir[1]; 223 | estimate[2] += searchDir[2]; 224 | 225 | //if (abort && false) 226 | //return true; 227 | 228 | } 229 | else 230 | ROS_WARN("nothing todo :()"); 231 | 232 | return false; 233 | } 234 | void getCompleteHessianDerivs(const Eigen::Vector3d &pose, 235 | const PCLPointCloud &cloud, 236 | Eigen::Matrix3d &H, 237 | Eigen::Vector3d &dTr, 238 | AbstractMap* const aMap, 239 | int *case_count, 240 | bool fine, 241 | Eigen::Vector3d pos) { 242 | float yaw = pose[2] + pos[2]; 243 | float x = pose[0] + pos[0]; 244 | float y = pose[1] + pos[1]; 245 | float sinRot = sin(yaw); 246 | float cosRot = cos(yaw); 247 | 248 | H = Eigen::Matrix3d::Zero(); 249 | dTr = Eigen::Vector3d::Zero(); 250 | 251 | double t_H[3][3] = {{0, 0, 0}, 252 | {0, 0, 0}, 253 | {0, 0, 0}}; 254 | double t_dTr[3] = {0, 0, 0}; 255 | float transformedPointData[3] = {0, 0, 0}; 256 | double currPoint[2]; 257 | float rotDeriv = 0; 258 | float funVal = 0; 259 | 260 | // PCLPointCloud newCloud; 261 | // tf::Transform sensorToWorldTf; 262 | // tf::Quaternion rotation; 263 | // rotation.setRPY(0,0,yaw); 264 | // sensorToWorldTf.setRotation(rotation); 265 | // sensorToWorldTf.setOrigin(tf::Vector3(x,y,0)); 266 | // Eigen::Matrix4f sensorToWorld; 267 | // pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); 268 | // pcl::transformPointCloud(cloud, newCloud, sensorToWorld); 269 | 270 | for (PCLPointCloud::const_iterator it = cloud.begin(); it != cloud.end(); ++it) { 271 | //for (PCLPointCloud::const_iterator it = newCloud.begin(); it != newCloud.end(); ++it){ 272 | currPoint[0] = it->x; 273 | currPoint[1] = it->y; 274 | 275 | //todo put this somewhere where dist is calculated anyway 276 | if (util::p2pDist(pose[0], pose[1], currPoint[0], currPoint[1]) < p_min_range_) 277 | continue; 278 | else if (util::p2pDist(pose[0], pose[1], currPoint[0], currPoint[1]) > p_max_range_) 279 | continue; 280 | 281 | //ROS_WARN("x %f y %f, yaw %f, point x %f point y %f", x, y, yaw, currPoint[0], currPoint[1]); 282 | //Eigen::Affine2d tfForState = Eigen::Translation2d(x, y) * Eigen::Rotation2Dd(yaw); 283 | 284 | Eigen::Affine2d tfForState = Eigen::Translation2d(x, y) * Eigen::Rotation2Dd(yaw); 285 | Eigen::Vector2d transformedCoords(tfForState * Eigen::Vector2d(currPoint[0], currPoint[1])); 286 | 287 | //if (it == cloud.begin()) { 288 | //Eigen::Vector2f test(tfForState*Eigen::Vector2f(currPoint[0], currPoint[1])); 289 | //ROS_ERROR("affe %f %f",test.x(),test.y()); 290 | //} 291 | // Eigen::Vector2f transformedCoords(Eigen::Vector2f(currPoint[0], currPoint[1])); 292 | 293 | //todo throw out eigen.. 294 | // float currPoint[2] = {it->x, it->y}; 295 | // float rotation[2] = {sin(yaw), cos(yaw)... 296 | // float tfForState[3][3]; 297 | // float transformedCoords[2]; 298 | 299 | //mapValues(transformedCoords.cast(), aMap, transformedPointData); 300 | //case_count[mapValues(transformedCoords.cast(), aMap, transformedPointData, fine)]++; 301 | case_count[aMap->get_map_values(transformedCoords.cast(), transformedPointData, fine)]++; 302 | 303 | //mapValues(transformedCoords.cast(), aMap, transformedPointData, fine); 304 | 305 | 306 | //ROS_ERROR("tfpointdata: %f %f %f", transformedPointData[0],transformedPointData[1],transformedPointData[2]); 307 | 308 | // for (int wtf = 0; wtf <3; wtf++) 309 | // if (transformedPointData[wtf] < epsilon_ && transformedPointData[wtf] > -epsilon_) 310 | // transformedPointData[wtf] = 0.0f; 311 | 312 | //funVal = 1.0f - transformedPointData[0]; 313 | funVal = transformedPointData[0]; 314 | // rotDeriv = ((-sinRot * currPoint[0] - cosRot * currPoint[1]) * transformedPointData[1] + (cosRot * currPoint[0] - sinRot * currPoint[1]) * transformedPointData[2]); 315 | //currPoint[0]/=0.05; 316 | //currPoint[1]/=0.05; 317 | rotDeriv = ((-sinRot * currPoint[0] - cosRot * currPoint[1]) * transformedPointData[1] + 318 | (cosRot * currPoint[0] - sinRot * currPoint[1]) * transformedPointData[2]); 319 | t_dTr[0] += transformedPointData[1] * funVal; 320 | t_dTr[1] += transformedPointData[2] * funVal; 321 | t_dTr[2] += rotDeriv * funVal; 322 | 323 | t_H[0][0] += transformedPointData[1] * transformedPointData[1]; 324 | t_H[1][1] += transformedPointData[2] * transformedPointData[2]; 325 | t_H[2][2] += rotDeriv * rotDeriv; 326 | 327 | t_H[0][1] += transformedPointData[1] * transformedPointData[2]; 328 | // ROS_ERROR("test %f", t_H[0][1]); 329 | t_H[0][2] += transformedPointData[1] * rotDeriv; 330 | t_H[1][2] += transformedPointData[2] * rotDeriv; 331 | } 332 | t_H[1][0] = t_H[0][1]; 333 | t_H[2][0] = t_H[0][2]; 334 | t_H[2][1] = t_H[1][2]; 335 | 336 | for (int i = 0; i < 3; i++) { 337 | dTr(i, 0) = t_dTr[i]; 338 | for (int j = 0; j < 3; j++) 339 | H(i, j) = t_H[i][j]; 340 | } 341 | } 342 | 343 | //look into those 344 | double tempYaw_; 345 | bool converged_; 346 | bool searchdirplus[3]; 347 | Eigen::Matrix3d H; 348 | Eigen::Vector3d dTr; 349 | bool map_flag_; 350 | int not_converged_counter_; 351 | 352 | 353 | //these are fine 354 | int p_num_iter_; 355 | double p_stop_iter_time_; 356 | bool p_fine_pos_; 357 | double p_min_range_; 358 | double p_max_range_; 359 | 360 | }; 361 | } 362 | 363 | #endif //SDF_SLAM_2D_GAUSSNEWTON_H 364 | -------------------------------------------------------------------------------- /include/sdf_slam_2d/utility/Types.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace sdfslam { 6 | typedef std::vector > VecMapFloat; 7 | typedef std::vector > VecMapInt; 8 | typedef pcl::PointXYZ PointType; 9 | typedef pcl::PointCloud PCLPointCloud 10 | ; 11 | } -------------------------------------------------------------------------------- /include/sdf_slam_2d/utility/UtilityFunctions.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | #ifndef SDF_SLAM_2D_UTILITYFUNCTIONS_H 20 | #define SDF_SLAM_2D_UTILITYFUNCTIONS_H 21 | 22 | #include "Types.h" 23 | #include 24 | #include 25 | #include "ros/ros.h" 26 | 27 | namespace util{ 28 | 29 | static pcl::PointXYZ genPoint(double xPos, double yPos){ 30 | sdfslam::PointType point; 31 | point.x = (float) xPos; 32 | point.y = (float) yPos; 33 | point.z = 0; 34 | return point; 35 | } 36 | 37 | static double round(double Zahl, unsigned int Stellen) { 38 | Zahl *= pow(10, Stellen); 39 | if (Zahl >= 0) 40 | floor(Zahl + 0.5); 41 | else 42 | ceil(Zahl - 0.5); 43 | Zahl /= pow(10, Stellen); 44 | return Zahl; 45 | } 46 | 47 | inline float xVal(float y, float m, float b) { 48 | if (m != 0) 49 | return (y - b) / m; 50 | else 51 | ROS_ERROR("div by 0"); 52 | return 0; 53 | } 54 | 55 | inline float yVal(float x, float m, float b) { 56 | return m * x + b; 57 | } 58 | 59 | inline float p2pDist(float p1x, float p1y, float* p2){ 60 | return sqrt( (p1x-p2[0])*(p1x-p2[0])+(p1y-p2[1])*(p1y-p2[1]) ); 61 | } 62 | 63 | inline float p2pDist(float* p1, float* p2){ 64 | return sqrt( (p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1]) ); 65 | } 66 | 67 | inline float p2pDist(float p1x, float p1y, float p2x, float p2y){ 68 | return sqrt( (p1x-p2x)*(p1x-p2x)+(p1y-p2y)*(p1y-p2y) ); 69 | } 70 | 71 | inline float p2lDist(float m, float b, const Eigen::Vector2f& p1){ 72 | return ( fabs(p1.y() - m*p1.x() - b)/sqrt(m*m+1) ); 73 | } 74 | 75 | inline float p2lDist(float m, float b, const float* p1){ 76 | return ( fabs(p1[1] - m*p1[0] - b)/sqrt(m*m+1) ); 77 | } 78 | 79 | inline Eigen::Vector2f toMap(double p_grid_res_, int p_map_size_x_, int p_map_size_y_, const Eigen::Vector2f& x){ 80 | return ( x/p_grid_res_ + Eigen::Vector2f(p_map_size_x_/2, p_map_size_y_/2) ); 81 | } 82 | 83 | inline float toMap(const float& x, const double& p_grid_res_, const int& s){ 84 | return (x/p_grid_res_+s/2); 85 | } 86 | 87 | inline void toMap(float* x, const double& p_grid_res, const int& p_map_size_x, const int& p_map_size_y){ 88 | x[0] = x[0]/p_grid_res + p_map_size_x/2; 89 | x[1] = x[1]/p_grid_res + p_map_size_y/2; 90 | } 91 | 92 | inline void toMap(double* x, const double& p_grid_res, const int& p_map_size_x, const int& p_map_size_y){ 93 | x[0] = x[0]/p_grid_res + p_map_size_x/2; 94 | x[1] = x[1]/p_grid_res + p_map_size_y/2; 95 | } 96 | 97 | 98 | inline Eigen::Vector2f toRl(const Eigen::Vector2f& x, double p_grid_res, int p_map_size_x, int p_map_size_y){ 99 | return ( (x-Eigen::Vector2f(p_map_size_x/2, p_map_size_y/2) ) * p_grid_res); 100 | } 101 | 102 | 103 | inline void toRl(double* x, double p_grid_res, int p_map_size_x, int p_map_size_y){ 104 | x[0] = (x[0]-p_map_size_x/2) * p_grid_res; 105 | x[1] = (x[1]-p_map_size_y/2) * p_grid_res; 106 | } 107 | 108 | inline void toRl(float* x, double p_grid_res, int p_map_size_x, int p_map_size_y){ 109 | x[0] = (float) ((x[0]-p_map_size_x/2) * p_grid_res); 110 | x[1] = (float) ((x[1]-p_map_size_y/2) * p_grid_res); 111 | } 112 | 113 | //return 0 if orthogonal projection not in cell 114 | static float p2lsDistTwo(double p_reg_threshold_, float *v, float *w, float *p) { 115 | // Return minimum distance between line segment vw and point p 116 | const float l2 = (w[0] - v[0]) * (w[0] - v[0]) + (w[1] - v[1]) * (w[1] - v[1]); // i.e. |w-v|^2 - avoid a sqrt 117 | if (l2 == 0.0) return p2pDist(p, v); // v == w case 118 | // Consider the line extending the segment, parameterized as v + t (w - v). 119 | // We find projection of point p onto the line. 120 | // It falls where t = [(p-v) . (w-v)] / |w-v|^2 121 | const float t = ((p[0] - v[0]) * (w[0] - v[0]) + (p[1] - v[1]) * (w[1] - v[1])) / l2; 122 | if (t <= 0.0 - p_reg_threshold_) return 0.0; // Beyond the 'v' end of the segment 123 | else if (t >= 1.0 + p_reg_threshold_) return 0.0; // Beyond the 'w' end of the segment 124 | float projection[2] = {(v[0] + t * (w[0] - v[0])), 125 | (v[1] + t * (w[1] - v[1]))}; // Projection falls on the segment 126 | return p2pDist(p, projection); 127 | } 128 | 129 | static float p2lsDist(float* v, float* w, float* p){ 130 | // Return minimum distance between line segment vw and point p 131 | const float l2 = (w[0]-v[0])*(w[0]-v[0]) + (w[1]-v[1])*(w[1]-v[1]); // i.e. |w-v|^2 - avoid a sqrt 132 | if (l2 == 0.0) return p2pDist(p, v); // v == w case 133 | // Consider the line extending the segment, parameterized as v + t (w - v). 134 | // We find projection of point p onto the line. 135 | // It falls where t = [(p-v) . (w-v)] / |w-v|^2 136 | const float t = ( (p[0] - v[0]) * (w[0] - v[0]) + (p[1] - v[1]) * (w[1] - v[1]) ) / l2; 137 | if (t < 0.0) return p2pDist(p, v); // Beyond the 'v' end of the segment 138 | else if (t > 1.0) return p2pDist(p, w); // Beyond the 'w' end of the segment 139 | float projection[2] = { (v[0] + t * (w[0] - v[0]) ), (v[1] + t * (w[1] - v[1])) }; // Projection falls on the segment 140 | return p2pDist(p, projection); 141 | } 142 | 143 | } 144 | 145 | #endif //SDF_SLAM_2D_UTILITYFUNCTIONS_H 146 | -------------------------------------------------------------------------------- /launch/graphtest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /launch/graphteststdr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /launch/local_sdf_slam_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/local_sdf_slam_stdr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /launch/local_stdr_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sdf_slam_2d 4 | 0.0.0 5 | The sdf_slam_2d package 6 | 7 | 8 | 9 | 10 | Joscha-David Fossel 11 | 12 | 13 | 14 | 15 | 16 | GPLv2 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Joscha-David Fossel 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | 45 | roscpp 46 | roscpp 47 | 48 | cmake_modules 49 | 50 | libpcl-all-dev 51 | libpcl-all 52 | 53 | 54 | 55 | pcl_ros 56 | pcl_ros 57 | 58 | pcl_conversions 59 | pcl_conversions 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /src/SDFSlam.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without 11 | // even the implied warranty of 12 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | //GNU General Public License for more details. 14 | // 15 | //You should have received a copy of the GNU General Public License along 16 | //with this program; if not, write to the Free Software Foundation, Inc., 17 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 18 | //================================================================================================= 19 | 20 | //todo 21 | // a) remove fishy vanity param: 22 | // 0: no visual map maintained 1:on mapping update visual map, 2:on mapping publish visual map, 23 | // 3:update visual map every iteration, 4:publish visual and internal map every iteration 24 | // 25 | // b) fix map save & load 26 | // 27 | // c) registration time limit variable depending on mapping/publishing dur.. 28 | 29 | #include "SDFSlam.h" 30 | 31 | 32 | namespace sdfslam{ 33 | 34 | SignedDistanceField::SignedDistanceField() { 35 | ros::NodeHandle private_nh("~"); 36 | private_nh.param("p_publish_map_", p_publish_map_, false); 37 | private_nh.param("p_initial_pose_", p_initial_pose_, false); 38 | private_nh.param("p_perma_map_", p_perma_map_, false); 39 | private_nh.param("p_mapping_", p_mapping_, true); 40 | private_nh.param("p_grid_res_", p_grid_res_, 0.05); 41 | private_nh.param("p_map_size_x_", p_map_size_x_, 15); 42 | private_nh.param("p_map_size_y_", p_map_size_y_, 15); 43 | private_nh.param("p_timeout_ticks_", p_timeout_ticks_, 0); 44 | private_nh.param("p_yaw_threshold_", p_yaw_threshold_, 0.9); 45 | private_nh.param("p_dist_threshold_", p_dist_threshold_, 0.4); 46 | private_nh.param("p_reg_threshold_", p_reg_threshold_, 0.1); 47 | private_nh.param("p_vanity_", p_vanity_, 0); 48 | private_nh.param("p_time_warning_threshold_", p_time_warning_threshold, 0.1); 49 | private_nh.param("p_scan_subscriber_queue_size_", p_scan_subscriber_queue_size_, 100); 50 | private_nh.param("p_robot_frame_", p_robot_frame_, "/robot0"); 51 | private_nh.param("p_fixed_frame_", p_fixed_frame_, "/map"); 52 | private_nh.param("p_tar_frame_", p_tar_frame_, "/sdf_pos"); 53 | private_nh.param("p_scan_topic_", p_scan_topic_, "/scan"); 54 | p_map_size_y_ /= p_grid_res_; 55 | p_map_size_x_ /= p_grid_res_; 56 | p_time_warning_threshold *= 1000000000; //s to ns 57 | 58 | time_out_counter_ = 0; 59 | not_converged_counter_ = 0; 60 | lastMapPos_ = Eigen::Vector3d(0, 0, 0); 61 | pos_ = Eigen::Vector3d(0, 0, 0); 62 | map_empty_ = true; 63 | publishMapServiceCalled_ = false; 64 | pose_estimation_ = true; 65 | 66 | //map_ = new SDFVectorMap(); 67 | map_ = new SDFGraphMap(); 68 | visualization_map_ = new OccupancyGrid(); 69 | registration_ = new GaussNewtonRegistration(); 70 | 71 | scan_sub_ = nh_.subscribe(p_scan_topic_, (uint32_t) p_scan_subscriber_queue_size_, &SignedDistanceField::scanCb, this); 72 | tfpub_ = nh_.advertise("sdf_pos", 10); 73 | scan_cloud_pub_ = nh_.advertise("sdf/scan", 10); 74 | serviceSaveMap = nh_.advertiseService("sdf/save_map", &SignedDistanceField::saveMapService, this); 75 | serviceUpdateMap = nh_.advertiseService("sdf/update_map", &SignedDistanceField::updateMapService, this); 76 | serviceLoadMap = nh_.advertiseService("sdf/load_map", &SignedDistanceField::loadMapService, this); 77 | serviceReset = nh_.advertiseService("sdf/reset", &SignedDistanceField::resetService, this); 78 | servicePublishMap = nh_.advertiseService("sdf/publish_map", &SignedDistanceField::publishMapService, this); 79 | serviceCreateAndPublishVisualMap = nh_.advertiseService("sdf/createAndPublishVisualMap", &SignedDistanceField::createAndPublishVisualMap, this); 80 | 81 | ROS_INFO("2D SDF SLAM started with res %f, map size x %f, map size y %f\n", 82 | p_grid_res_, p_map_size_x_*p_grid_res_, p_map_size_y_*p_grid_res_); 83 | } 84 | 85 | SignedDistanceField::~SignedDistanceField() { 86 | delete registration_; 87 | delete map_; 88 | serviceSaveMap.shutdown(); //redundant 89 | serviceLoadMap.shutdown(); 90 | serviceUpdateMap.shutdown(); 91 | servicePublishMap.shutdown(); 92 | serviceReset.shutdown(); 93 | scan_sub_.shutdown(); 94 | } 95 | 96 | bool SignedDistanceField::createAndPublishVisualMap(std_srvs::Empty::Request &req, 97 | std_srvs::Empty::Request &res) { 98 | //todo :) 99 | } 100 | 101 | bool SignedDistanceField::resetService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) { 102 | lastMapPos_ = Eigen::Vector3d(0, 0, 0); 103 | pos_ = Eigen::Vector3d(0, 0, 0); 104 | map_empty_ = true; 105 | map_->reset_map(); 106 | visualization_map_->reset_map(); 107 | ROS_INFO("Service called: resetting sdf.."); 108 | return true; 109 | } 110 | 111 | bool SignedDistanceField::updateMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) { 112 | updateMapServiceCalled_ = true; 113 | ROS_INFO("Service called: updating map"); 114 | return true; 115 | } 116 | 117 | bool SignedDistanceField::publishMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) { 118 | publishMapServiceCalled_ = true; 119 | ROS_INFO("Service called: publishing map"); 120 | return true; 121 | } 122 | 123 | bool SignedDistanceField::saveMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) { 124 | map_->save_map("map.txt"); 125 | ROS_INFO("Service called: saving map"); 126 | return true; 127 | } 128 | 129 | bool SignedDistanceField::loadMapService(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) { 130 | loadMapServiceCalled_ = true; 131 | ROS_INFO("Service called: loading map"); 132 | return true; 133 | } 134 | 135 | bool SignedDistanceField::checkTimeout() { 136 | if (p_timeout_ticks_ != 0) { 137 | if (time_out_counter_++ > p_timeout_ticks_) { 138 | ROS_INFO("Time out ticks %d", time_out_counter_); 139 | map_->publish_map(); 140 | visualization_map_->publish_map(); 141 | return false; 142 | } 143 | } 144 | return true; 145 | } 146 | 147 | void SignedDistanceField::checkTodos() { 148 | time_out_counter_ = 0; 149 | 150 | if (loadMapServiceCalled_) { 151 | map_->load_map("map.txt"); 152 | loadMapServiceCalled_ = false; 153 | } 154 | 155 | if ((map_empty_ && p_initial_pose_) || !pose_estimation_) { 156 | tf::StampedTransform transform; 157 | try { 158 | ros::Time now = ros::Time(0); 159 | double roll, pitch, yaw; 160 | ROS_INFO("External pose update: x %f y %f yaw %f", transform.getOrigin().getX(), transform.getOrigin().getY(), yaw); 161 | tf_.waitForTransform(p_fixed_frame_, p_robot_frame_, now, ros::Duration(5.0)); 162 | tf_.lookupTransform(p_fixed_frame_, p_robot_frame_, now, transform); 163 | 164 | tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw); 165 | ROS_INFO("External pose update: x %f y %f yaw %f", transform.getOrigin().getX(), transform.getOrigin().getY(), yaw); 166 | pos_ = Eigen::Vector3d((double) transform.getOrigin().getX(), (double) transform.getOrigin().getY(), yaw); 167 | } catch (tf::TransformException ex) { 168 | ROS_ERROR("No mocap data received. %s", ex.what()); 169 | } 170 | //ROS_INFO("YAFSD"); 171 | } 172 | 173 | 174 | } 175 | 176 | void SignedDistanceField::scanCb(const sensor_msgs::LaserScan::ConstPtr& scan) { 177 | ros::Time tStartTotal = ros::Time::now(); 178 | checkTodos(); 179 | 180 | sensor_msgs::PointCloud2 laser_point_cloud; 181 | projector_.projectLaser(*scan, laser_point_cloud); 182 | PCLPointCloud pc; 183 | pcl::fromROSMsg(laser_point_cloud, pc); 184 | 185 | tf::Transform sensorToWorldTf; 186 | tf::Quaternion rotation; 187 | Eigen::Matrix4f sensorToWorld; 188 | Eigen::Vector3d pos_delta(0, 0, 0); 189 | 190 | //register 191 | ros::Duration tDurMatch; 192 | if (!map_empty_ && pose_estimation_) { 193 | int case_count[4]; 194 | 195 | ros::Time tStart = ros::Time::now(); 196 | pos_delta = registration_->new_match(pc, map_, case_count, pos_); 197 | ros::Time tEnd = ros::Time::now(); 198 | tDurMatch = tEnd-tStart; 199 | 200 | //permanent mapping trigger 201 | if (case_count[0] <= not_converged_counter_ * not_converged_counter_) { 202 | if (p_perma_map_) 203 | map_flag_ = true; //avg? 204 | converged_ = true; 205 | not_converged_counter_ = 0; 206 | } 207 | else { 208 | converged_ = false; 209 | if (map_flag_ && !p_perma_map_) 210 | not_converged_counter_++; 211 | else if (p_perma_map_) 212 | not_converged_counter_++; 213 | } 214 | 215 | pos_ += pos_delta; 216 | } 217 | 218 | //pose threshold update map check 219 | bool update_map_trigger = map_empty_; 220 | if (fabs(lastMapPos_.z() - pos_.z()) > p_yaw_threshold_) { 221 | update_map_trigger = true; 222 | } 223 | else if ((pos_ - lastMapPos_).norm() > p_dist_threshold_) { 224 | update_map_trigger = true; 225 | } 226 | 227 | if (update_map_trigger) map_flag_ = true; 228 | 229 | 230 | //transform cloud 231 | rotation.setRPY(0, 0, pos_.z()); 232 | sensorToWorldTf.setRotation(rotation); 233 | sensorToWorldTf.setOrigin(tf::Vector3(pos_.x(), pos_.y(), 0)); 234 | 235 | // //todo rm me 236 | // if ((((map_flag_ && converged_) || (map_empty_)) && p_mapping_) || updateMapServiceCalled_) { 237 | // Eigen::Vector3d affe(0.05,0.05,0.0); 238 | // affe += pos_; 239 | // sensorToWorldTf.setOrigin(tf::Vector3(affe.x(), affe.y(), 0)); 240 | // } 241 | // //end rm 242 | 243 | pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); 244 | pcl::transformPointCloud(pc, pc, sensorToWorld); 245 | 246 | //publish tf 247 | sensorToWorldTf.setOrigin(tf::Vector3(pos_.x(), pos_.y(), 0)); 248 | sensorToWorldTf.setRotation(rotation); 249 | tfbr_.sendTransform(tf::StampedTransform(sensorToWorldTf, scan->header.stamp, p_fixed_frame_, p_tar_frame_)); 250 | 251 | //publish pose msg 252 | geometry_msgs::PoseStamped pose_msg; 253 | pose_msg.header.stamp = scan->header.stamp; 254 | pose_msg.pose.position.x = pos_.x(); 255 | pose_msg.pose.position.y = pos_.y(); 256 | pose_msg.pose.orientation = tf::createQuaternionMsgFromYaw(pos_.z()); 257 | tfpub_.publish(pose_msg); 258 | 259 | //publish scan 260 | sensor_msgs::PointCloud2 cloudMsg2; 261 | pcl::toROSMsg(pc, cloudMsg2); 262 | cloudMsg2.header.frame_id = p_fixed_frame_; 263 | cloudMsg2.header.stamp = scan->header.stamp; 264 | scan_cloud_pub_.publish(cloudMsg2); 265 | 266 | //publish/update visual map? 267 | ros::Duration tDurMappingVisual; 268 | if (p_vanity_ >= 3) { 269 | update_map(visualization_map_,pc,pos_); 270 | } 271 | if (p_vanity_ >= 4) { 272 | publishMapServiceCalled_ = true; 273 | } 274 | 275 | ros::Duration tDurPublishInternal; 276 | ros::Duration tDurPublishVisual; 277 | if (publishMapServiceCalled_) { 278 | tDurPublishInternal = publish_map(map_); 279 | tDurPublishVisual = publish_map(visualization_map_); 280 | publishMapServiceCalled_ = false; 281 | } 282 | 283 | //update map? 284 | ros::Duration tDurMappingInternal; 285 | if ((((map_flag_ && converged_) || (map_empty_)) && p_mapping_) || updateMapServiceCalled_) { 286 | updateMapServiceCalled_ = false; 287 | lastMapPos_ = pos_; 288 | map_flag_ = false; 289 | if (map_empty_) 290 | map_empty_ = false; 291 | 292 | tDurMappingInternal = update_map(map_, pc, pos_); 293 | if (p_vanity_ >= 1 && p_vanity_ < 3) { 294 | tDurMappingVisual = update_map(visualization_map_, pc, pos_); 295 | } 296 | if (p_vanity_ >= 2) { 297 | publishMapServiceCalled_ = true; 298 | } 299 | } 300 | 301 | ros::Time tEndTotal = ros::Time::now(); 302 | ros::Duration tdur = tEndTotal - tStartTotal; 303 | if (tdur.nsec > p_time_warning_threshold){ 304 | ROS_WARN("total duration %d %d", tdur.sec, tdur.nsec); 305 | ROS_INFO("matching %d %d", tDurMatch.sec, tDurMatch.nsec); 306 | ROS_INFO("maping internal %d %d", tDurMappingInternal.sec, tDurMappingInternal.nsec); 307 | ROS_INFO("maping visual %d %d", tDurMappingVisual.sec, tDurMappingVisual.nsec); 308 | ROS_INFO("publish internal %d %d", tDurPublishInternal.sec, tDurPublishInternal.nsec); 309 | ROS_INFO("publish visual %d %d", tDurPublishVisual.sec, tDurPublishVisual.nsec); 310 | } 311 | } 312 | 313 | ros::Duration SignedDistanceField::update_map(AbstractMap* aMap, const PCLPointCloud& pc, const Eigen::Vector3d& pose3d) { 314 | ros::Time tStart = ros::Time::now(); 315 | aMap->update_map(pc, pose3d); 316 | ros::Time tEnd = ros::Time::now(); 317 | return tEnd-tStart; 318 | } 319 | 320 | ros::Duration SignedDistanceField::publish_map(AbstractMap* aMap) { 321 | ros::Time tStart = ros::Time::now(); 322 | aMap->publish_map(); 323 | ros::Time tEnd = ros::Time::now(); 324 | return tEnd-tStart; 325 | } 326 | } 327 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | //Copyright (C) 2015, Joscha Fossel 3 | // 4 | //This program is free software; you can redistribute it and/or modify 5 | //it under the terms of the GNU General Public License as published by 6 | //the Free Software Foundation; either version 2 of the License, or 7 | //any later version. 8 | // 9 | //This program is distributed in the hope that it will be useful, 10 | //but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | //MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | //GNU General Public License for more details. 13 | // 14 | //You should have received a copy of the GNU General Public License along 15 | //with this program; if not, write to the Free Software Foundation, Inc., 16 | //51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | //================================================================================================= 18 | 19 | #include 20 | #include "SDFSlam.h" 21 | 22 | int main(int argc, char* argv[] ) { 23 | ros::init(argc, argv, "sdf_2d"); 24 | ros::NodeHandle nh("sdf_main"); 25 | 26 | ros::NodeHandle private_nh("~"); 27 | bool p_bag_mode; 28 | private_nh.param("p_bag_mode", p_bag_mode, false); 29 | 30 | sdfslam::SignedDistanceField sdf; 31 | 32 | if (!p_bag_mode) 33 | ros::spin(); 34 | else { 35 | bool cont = true; 36 | unsigned int microseconds = 10000; 37 | 38 | while (ros::ok && cont) { 39 | cont = sdf.checkTimeout(); 40 | ros::spinOnce(); 41 | usleep(microseconds); 42 | } 43 | } 44 | ROS_INFO("shutting down sdf slam.."); 45 | 46 | return 0; 47 | } 48 | 49 | -------------------------------------------------------------------------------- /stdr_resources/maps/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smARTLab-liv/sdf_slam_2d/c9cc45873999de0fda31306bf83f126b8eee54c6/stdr_resources/maps/test.png -------------------------------------------------------------------------------- /stdr_resources/maps/test.yaml: -------------------------------------------------------------------------------- 1 | image: test.png 2 | resolution: 0.002 3 | origin: [-5, -5, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /stdr_resources/robots/pandora_robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.2 7 | 8 | 9 | 10 | 11 | 12 | 0 13 | 14 | 15 | 0 16 | 17 | 18 | 0 19 | 20 | 21 | 22 | 23 | laser_sensors/hokuyo/hokuyo_URG_04LX.xml 24 | 25 | 26 | 27 | 28 | range_sensors/standard_sonar.xml 29 | 30 | 31 | 32 | 33 | 0.1 34 | 35 | 36 | 37 | 38 | 39 | 40 | range_sensors/standard_sonar.xml 41 | 42 | 43 | 44 | 45 | 0.1 46 | 47 | 48 | 1.570795 49 | 50 | 51 | 52 | 53 | 54 | 55 | range_sensors/standard_sonar.xml 56 | 57 | 58 | 59 | 60 | -0.1 61 | 62 | 63 | -1.570795 64 | 65 | 66 | 67 | 68 | 69 | 70 | range_sensors/standard_sonar.xml 71 | 72 | 73 | 74 | 75 | -0.1 76 | 77 | 78 | -0.1 79 | 80 | 81 | 2.79252444444444 82 | 83 | 84 | 85 | 86 | 87 | 88 | range_sensors/standard_sonar.xml 89 | 90 | 91 | 92 | 93 | -0.1 94 | 95 | 96 | 0.1 97 | 98 | 99 | 3.49065555555556 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /stdr_resources/robots/pandora_robot.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | robot_specifications: 3 | - footprint: 4 | footprint_specifications: 5 | radius: 0.100000002980232 6 | points: 7 | [] 8 | - initial_pose: 9 | x: 0 10 | y: 0 11 | theta: 0 12 | - laser: 13 | laser_specifications: 14 | max_angle: 2.09439516067505 15 | #max_angle: -1.9 16 | min_angle: -2.09439516067505 17 | max_range: 5.6123467891011121 18 | min_range: 0.00599999986588955 19 | num_rays: 667 20 | #num_rays: 10 21 | frequency: 10 22 | frame_id: laser_0 23 | pose: 24 | x: 0 25 | y: 0 26 | theta: 0 27 | noise: 28 | noise_specifications: 29 | noise_mean: 0. 30 | noise_std: 0.0 31 | --------------------------------------------------------------------------------