├── .github ├── PULL_REQUEST_TEMPLATE.md ├── dependabot.yml └── workflows │ └── main.yml ├── .gitignore ├── LICENSE ├── README.md ├── cob_base_velocity_smoother ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── params.cfg ├── config │ └── standalone.yaml ├── include │ └── cob_base_velocity_smoother │ │ ├── cob_base_velocity_smoother.h │ │ └── velocity_smoother.h ├── package.xml └── src │ ├── cob_base_velocity_smoother.cpp │ ├── cob_base_velocity_smoother │ └── velocity_smoother.cpp │ └── velocity_smoother_node.cpp ├── cob_collision_velocity_filter ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── CollisionVelocityFilter.cfg ├── include │ ├── cob_collision_velocity_filter.h │ └── velocity_limited_marker.h ├── mainpage.dox ├── package.xml └── src │ ├── cob_collision_velocity_filter.cpp │ └── velocity_limited_marker.cpp ├── cob_control ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── cob_footprint_observer ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── cob_footprint_observer.h ├── mainpage.dox ├── package.xml ├── src │ └── cob_footprint_observer.cpp └── srv │ └── GetFootprint.srv └── cob_hardware_emulation ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── scripts ├── emulation_base.py ├── emulation_clock.py ├── emulation_follow_joint_trajectory.py ├── emulation_nav.py └── emulation_odom_laser.py └── src └── emulation_clock.cpp /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | - fixes # 2 | - new source code files added (proper [APACHE license header](https://github.com/ipa320/setup/tree/master/templates) **must** be used) 3 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | --- 2 | version: 2 3 | updates: 4 | - package-ecosystem: "github-actions" 5 | directory: "/" 6 | schedule: 7 | interval: "weekly" 8 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: GHA CI 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: 7 | - cron: "0 0 * * 0" # every Sunday at midnight 8 | workflow_dispatch: 9 | 10 | jobs: 11 | industrial_ci: 12 | name: GHA CI 13 | runs-on: ubuntu-latest 14 | timeout-minutes: 60 15 | 16 | env: 17 | ADDITIONAL_DEBS: 'apt-utils dialog git' 18 | CATKIN_LINT: pedantic 19 | CATKIN_LINT_ARGS: '--ignore description_boilerplate' 20 | CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release 21 | PYLINT_ARGS: '--output-format=parseable --errors-only' 22 | PYLINT_CHECK: true 23 | ROS_REPO: main 24 | 25 | strategy: 26 | matrix: 27 | include: 28 | - { ROS_DISTRO: noetic } 29 | 30 | steps: 31 | - uses: actions/checkout@v4 32 | with: 33 | submodules: true 34 | lfs: true 35 | ssh-known-hosts: '' 36 | 37 | - uses: ros-industrial/industrial_ci@master 38 | with: 39 | config: ${{toJSON(matrix)}} 40 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # default .gitignore file for ipa320 repositories 2 | # master file can be found in https://github.com/ipa320/care-o-bot/blob/indigo_dev/.gitignore 3 | 4 | build/ 5 | bin/ 6 | lib/ 7 | 8 | # generated docs 9 | *.dox 10 | *.wikidoc 11 | 12 | # eclipse 13 | .project 14 | .cproject 15 | 16 | # qcreator 17 | qtcreator-* 18 | *.user 19 | 20 | # Emacs 21 | .#* 22 | 23 | # VI/VIM 24 | *.swp 25 | 26 | # python files 27 | *.pcd 28 | *.pyc 29 | *.pco 30 | 31 | # temporary files 32 | *~ 33 | 34 | # merge conflict files 35 | *.orig 36 | *BACKUP* 37 | *BASE* 38 | *LOCAL* 39 | *REMOTE* 40 | 41 | # Catkin custom files 42 | CATKIN_IGNORE 43 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | cob_control 2 | =========== 3 | 4 | ## GitHub Actions - Continuous Integration 5 | 6 | CI-Status ```noetic-devel```: [![GHA CI](https://github.com/4am-robotics/cob_control/actions/workflows/main.yml/badge.svg?branch=noetic-devel)](https://github.com/4am-robotics/cob_control/actions/workflows/main.yml?query=branch%3Anoetic-devel) 7 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_base_velocity_smoother 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.25 (2024-08-05) 6 | ------------------- 7 | 8 | 0.9.24 (2024-04-30) 9 | ------------------- 10 | * 0.8.24 11 | * update changelogs 12 | * Contributors: fmessmer 13 | 14 | 0.8.24 (2024-04-18) 15 | ------------------- 16 | 17 | 0.8.23 (2024-02-20) 18 | ------------------- 19 | 20 | 0.8.22 (2023-04-29) 21 | ------------------- 22 | 23 | 0.8.21 (2023-01-04) 24 | ------------------- 25 | 26 | 0.8.20 (2022-11-17) 27 | ------------------- 28 | 29 | 0.8.19 (2022-07-29) 30 | ------------------- 31 | 32 | 0.8.18 (2022-01-12) 33 | ------------------- 34 | 35 | 0.8.17 (2021-12-23) 36 | ------------------- 37 | 38 | 0.8.16 (2021-10-19) 39 | ------------------- 40 | 41 | 0.8.15 (2021-05-17) 42 | ------------------- 43 | 44 | 0.8.14 (2021-05-10) 45 | ------------------- 46 | 47 | 0.8.13 (2021-04-06) 48 | ------------------- 49 | * Merge pull request `#247 `_ from fmessmer/fix_catkin_lint_melodic 50 | [melodic] fix catkin_lint 51 | * fix catkin_lint 52 | * Contributors: Felix Messmer, fmessmer 53 | 54 | 0.8.12 (2020-10-21) 55 | ------------------- 56 | * Merge pull request `#243 `_ from fmessmer/test_noetic 57 | test noetic 58 | * Bump CMake version to avoid CMP0048 warning 59 | * Contributors: Felix Messmer, fmessmer 60 | 61 | 0.8.11 (2020-03-21) 62 | ------------------- 63 | 64 | 0.8.10 (2020-03-18) 65 | ------------------- 66 | * Merge pull request `#226 `_ from fmessmer/ci_updates_melodic 67 | [travis] ci updates - melodic 68 | * catkin_lint fixes 69 | * Contributors: Felix Messmer, fmessmer 70 | 71 | 0.8.1 (2019-11-07) 72 | ------------------ 73 | 74 | 0.8.0 (2019-08-09) 75 | ------------------ 76 | 77 | 0.7.8 (2019-08-09) 78 | ------------------ 79 | 80 | 0.7.7 (2019-08-06) 81 | ------------------ 82 | 83 | 0.7.6 (2019-06-07) 84 | ------------------ 85 | 86 | 0.7.5 (2019-05-20) 87 | ------------------ 88 | 89 | 0.7.4 (2019-04-05) 90 | ------------------ 91 | 92 | 0.7.3 (2019-03-14) 93 | ------------------ 94 | * Merge pull request `#189 `_ from JerrelU/kinetic_dev 95 | Fixed wrong velocity used for linear y 96 | * Fixed wrong velocity used for linear y 97 | * Contributors: Felix Messmer, Jerrel 98 | 99 | 0.7.2 (2018-07-21) 100 | ------------------ 101 | * Merge pull request `#184 `_ from ipa-bnm/hotfix/velocity_smoother_kinetic 102 | [velocity_smoother] fixed ros param name for velocity smoother decel_factor_safe 103 | * fixed ros param name for velocity smoother decel_factor_safe 104 | * Contributors: Benjamin Maidel, Felix Messmer 105 | 106 | 0.7.1 (2018-01-07) 107 | ------------------ 108 | * Merge remote-tracking branch 'origin/kinetic_release_candidate' into kinetic_dev 109 | * Merge pull request `#169 `_ from ipa-fxm/kinetic_updates_indigo 110 | Kinetic updates indigo 111 | * Merge branch 'indigo_dev' of github.com:ipa320/cob_control into kinetic_dev 112 | Conflicts: 113 | .travis.yml 114 | * Merge pull request `#159 `_ from ipa-fxm/APACHE_license 115 | use license apache 2.0 116 | * use license apache 2.0 117 | * Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk 118 | 119 | 0.7.0 (2017-07-18) 120 | ------------------ 121 | 122 | 0.6.15 (2017-07-18) 123 | ------------------- 124 | * Merge pull request `#122 `_ from ipa-fxm/indigo_roslint 125 | roslint some packages 126 | * starting to roslint some packages 127 | * manually fix changelog 128 | * Contributors: Nadia Hammoudeh García, ipa-fxm 129 | 130 | 0.6.14 (2016-10-10) 131 | ------------------- 132 | 133 | 0.6.13 (2016-10-10) 134 | ------------------- 135 | 136 | 0.6.12 (2016-10-10) 137 | ------------------- 138 | 139 | 0.6.11 (2016-04-01) 140 | ------------------- 141 | * fix install tags 142 | * do not install cfg files 143 | * Change author email 144 | * added dep for cfg 145 | * fixed deceleration factor on inactive topic input 146 | * added additional parameter to adjust deceleration to acceleration ratio in safety cases (no topics received, joy deadmean released) 147 | * revert package version 148 | * fixed ros loop 149 | * added new velocity_smoother based on yocs_velocity_smoother 150 | * Contributors: Benjamin Maidel, Florian Weisshardt, ipa-fmw 151 | 152 | 0.6.10 (2015-08-31) 153 | ------------------- 154 | 155 | 0.6.9 (2015-08-25) 156 | ------------------ 157 | * boost revision 158 | * do not install headers in executable-only packages 159 | * explicit dependency to boost 160 | * remove obsolete autogenerated mainpage.dox files 161 | * explicit dependency to boost 162 | * remove trailing whitespaces 163 | * migrate to package format 2 164 | * sort dependencies 165 | * update with ipa320 166 | * review dependencies 167 | * Contributors: ipa-fxm 168 | 169 | 0.6.8 (2015-06-17) 170 | ------------------ 171 | 172 | 0.6.7 (2015-06-17) 173 | ------------------ 174 | 175 | 0.6.6 (2014-12-18) 176 | ------------------ 177 | 178 | 0.6.5 (2014-12-18) 179 | ------------------ 180 | 181 | 0.6.4 (2014-12-16) 182 | ------------------ 183 | 184 | 0.6.3 (2014-12-16) 185 | ------------------ 186 | 187 | 0.6.2 (2014-12-15) 188 | ------------------ 189 | 190 | 0.6.1 (2014-09-22) 191 | ------------------ 192 | 193 | 0.5.3 (2014-03-31) 194 | ------------------ 195 | * install tags 196 | * Contributors: ipa-fxm 197 | 198 | 0.5.2 (2014-03-20) 199 | ------------------ 200 | 201 | 0.5.1 (2014-03-20) 202 | ------------------ 203 | * add definitions to get rid of compiler warning 204 | * cob_undercarriage_ctrl: expose param for watchdog timeout 205 | * cob_base_velocity_smoother: add param to specify minimal rate which is expected for commands. At slower rates, start filling in zeros 206 | * cleaned up CMakeLists and added install directives 207 | * cob_base_velocity_smoother: make robot stop if a zero is commanded 208 | * cob_base_velocity_smoother: stop publishing velocity commands when we don't receive any 209 | * futher include and linkpath modifications 210 | * Second catkinization push 211 | * First catkinization, still need to update some CMakeLists.txt 212 | * bugfix: added missing default value for parameter in velocity smoother 213 | * the cob_base_velocity_smoother now has a loop rate and some additional updates 214 | * cleaned up the code 215 | * some minor modifications on cob_base_velocity_smoother, removed some unnecessary couts 216 | * some minor modifications on cob_base_velocity_smoother 217 | * some minor modifications on cob_base_velocity_smoother 218 | * some further modifications on cob_base_velocity_smoother, to be (parameter-) tested on hw 219 | * some modifications on cob_base_velocity_smoother, to be tested on hw 220 | * some bugfixes on cob_base_velocity_smoother, to be tested on hw. still contains some couts to be removed 221 | * some modifications/bugfixes in cob_base_velocity_smoother.cpp. to be tested, not finished yet 222 | * integration of cob_base_velocity_smoother, moved here from cob_navigation 223 | * Contributors: Alexander Bubeck, Frederik Hegger, abubeck, frm, ipa-frm, ipa-mig 224 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_base_velocity_smoother) 3 | 4 | find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure geometry_msgs nav_msgs roscpp roslint std_msgs) 5 | 6 | find_package(Boost REQUIRED thread) 7 | 8 | generate_dynamic_reconfigure_options(cfg/params.cfg) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX) 11 | add_definitions(-std=gnu++0x) 12 | else() 13 | add_definitions(-std=c++0x) 14 | endif() 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS dynamic_reconfigure geometry_msgs nav_msgs roscpp std_msgs 18 | ) 19 | 20 | ### BUILD ### 21 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 22 | 23 | add_executable(${PROJECT_NAME} src/${PROJECT_NAME}.cpp) 24 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 25 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${BOOST_LIBRARIES}) 26 | 27 | add_executable(velocity_smoother src/${PROJECT_NAME}/velocity_smoother.cpp src/velocity_smoother_node.cpp) 28 | add_dependencies(velocity_smoother ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 29 | target_link_libraries(velocity_smoother ${catkin_LIBRARIES} ${BOOST_LIBRARIES}) 30 | 31 | roslint_cpp() 32 | 33 | ### INSTALL ### 34 | install(TARGETS ${PROJECT_NAME} velocity_smoother 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(DIRECTORY config 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 42 | ) 43 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/cfg/params.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = "cob_base_velocity_smoother" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("speed_lim_vx", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 10.0) 10 | gen.add("speed_lim_vy", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 10.0) 11 | gen.add("speed_lim_w", double_t, 0, "Maximum angular velocity", 1.0, 0.0, 10.0) 12 | 13 | gen.add("accel_lim_vx", double_t, 0, "Maximum linear acceleration", 0.7, 0.0, 10.0) 14 | gen.add("accel_lim_vy", double_t, 0, "Maximum linear acceleration", 0.7, 0.0, 10.0) 15 | gen.add("accel_lim_w", double_t, 0, "Maximum angular acceleration", 0.7, 0.0, 10.0) 16 | 17 | gen.add("decel_factor", double_t, 0, "Deceleration to acceleration ratio", 2.0, 0.0, 10.0) 18 | gen.add("decel_factor_safe", double_t, 0, "Deceleration to acceleration ratio if safety stop is required", 4.0, 0.0, 10.0) 19 | 20 | # Second arg is node name it will run in (doc purposes only), third is generated filename prefix 21 | exit(gen.generate(PACKAGE, "velocity_smoother_configure", "params")) 22 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/config/standalone.yaml: -------------------------------------------------------------------------------- 1 | # Example configuration: 2 | # - velocity limits are around a 10% above the physical limits 3 | # - acceleration limits are just low enough to avoid jerking 4 | 5 | # Mandatory parameters 6 | speed_lim_vx: 0.8 7 | speed_lim_vy: 0.8 8 | speed_lim_w: 5.4 9 | 10 | accel_lim_vx: 0.3 11 | accel_lim_vy: 0.3 12 | accel_lim_w: 3.5 13 | 14 | # Optional parameters 15 | frequency: 20.0 16 | decel_factor: 1.0 17 | decel_factor_safety: 1.0 18 | 19 | # Robot velocity feedback type: 20 | # 0 - none 21 | # 1 - odometry 22 | # 2 - end robot commands 23 | robot_feedback: 2 24 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/include/cob_base_velocity_smoother/cob_base_velocity_smoother.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COB_BASE_VELOCITY_SMOOTHER_H 19 | #define COB_BASE_VELOCITY_SMOOTHER_H 20 | 21 | //**************************** includes ************************/ 22 | 23 | // standard includes 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // ros includes 33 | #include 34 | #include 35 | #include 36 | #include 37 | /**************************************************************** 38 | * the ros navigation doesn't run very smoothly because acceleration is too high 39 | * --> cob has strong base motors and therefore reacts with shaking behavior 40 | * (PR2 has much more mechanical damping) 41 | * solution: the additional node cob_base_velocity_smoother smooths the velocities 42 | * comming from ROS-navigation or teleoperation, by calculating the mean values of a certain number 43 | * of past messages and limiting the acceleration under a given threshold. 44 | * cob_base_velocity_smoother subsribes (input) and publishes (output) geometry_msgs::Twist. 45 | ****************************************************************/ 46 | class cob_base_velocity_smoother 47 | { 48 | private: 49 | //capacity for circular buffers (to be loaded from parameter server, otherwise set to default value 12) 50 | int buffer_capacity_; 51 | //maximal time-delay in seconds for stored messages in Circular Buffer (to be loaded from parameter server, otherwise set to default value 4) 52 | double store_delay_; 53 | // maximal time-delay in seconds to wait until filling the circular buffer with zero messages when the subscriber doesn't hear anything 54 | double stop_delay_after_no_sub_; 55 | //threshhold for maximal allowed acceleration (to be loaded from parameter server, otherwise set to default value 0.02) 56 | double acc_limit_; 57 | // ros loop rate (to be loaded from parameter server, otherwise set to default value 30) 58 | double loop_rate_; 59 | // delay between received commands that is allowed. After that, fill buffer with zeros. 60 | double max_delay_between_commands_; 61 | //geometry message filled with zero values 62 | geometry_msgs::Twist zero_values_; 63 | // subscribed geometry message 64 | geometry_msgs::Twist sub_msg_; 65 | 66 | pthread_mutex_t m_mutex; 67 | bool new_msg_received_; 68 | 69 | // function for checking wether a new msg has been received, triggering publishers accordingly 70 | void set_new_msg_received(bool received); 71 | bool get_new_msg_received(); 72 | 73 | public: 74 | // constructor 75 | cob_base_velocity_smoother(); 76 | 77 | // destructor 78 | ~cob_base_velocity_smoother(); 79 | 80 | //create node handle 81 | ros::NodeHandle nh_, pnh_; 82 | 83 | //circular buffers for velocity, output and time 84 | boost::circular_buffer cb_; 85 | boost::circular_buffer cb_out_; 86 | boost::circular_buffer cb_time_; 87 | 88 | // declaration of ros subscribers 89 | ros::Subscriber geometry_msgs_sub_; 90 | 91 | // decalaration of ros publishers 92 | ros::Publisher pub_; 93 | 94 | //callback function to subsribe to the geometry messages 95 | void geometryCallback(const geometry_msgs::Twist::ConstPtr &cmd_vel); 96 | //calculation function called periodically in main 97 | void calculationStep(); 98 | //function that updates the circular buffer after receiving a new geometry message 99 | void reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel); 100 | //function to limit the acceleration under the given threshhold thresh 101 | void limitAcceleration(ros::Time now, geometry_msgs::Twist& cmd_vel); 102 | 103 | //boolean function that returns true if all messages stored in the circular buffer are older than store_delay, false otherwise 104 | bool circBuffOutOfDate(ros::Time now); 105 | // function to compare two geometry messages 106 | bool IsEqual(geometry_msgs::Twist msg1, geometry_msgs::Twist msg2); 107 | 108 | //boolean function that returns true if the input msg cmd_vel equals zero_values, false otherwise 109 | bool IsZeroMsg(geometry_msgs::Twist cmd_vel); 110 | 111 | //help-function that returns the signum of a double variable 112 | int signum(double var); 113 | 114 | //functions to calculate the mean values for each direction 115 | double meanValueX(); 116 | double meanValueY(); 117 | double meanValueZ(); 118 | 119 | // function to make the loop rate available outside the class 120 | double getLoopRate(); 121 | 122 | //function for the actual computation 123 | //calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh 124 | //returns the resulting geometry message to be published to the base_controller 125 | geometry_msgs::Twist setOutput(ros::Time now, geometry_msgs::Twist cmd_vel); 126 | }; 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/include/cob_base_velocity_smoother/velocity_smoother.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef VELOCITY_SMOOTHER_HPP_ 19 | #define VELOCITY_SMOOTHER_HPP_ 20 | 21 | /***************************************************************************** 22 | ** Includes 23 | *****************************************************************************/ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /***************************************************************************** 31 | ** Namespaces 32 | *****************************************************************************/ 33 | 34 | namespace cob_base_velocity_smoother { 35 | 36 | /***************************************************************************** 37 | ** VelocitySmoother 38 | *****************************************************************************/ 39 | 40 | class VelocitySmoother 41 | { 42 | public: 43 | VelocitySmoother(const std::string &name); 44 | 45 | ~VelocitySmoother() 46 | { 47 | if (dynamic_reconfigure_server != NULL) 48 | delete dynamic_reconfigure_server; 49 | } 50 | 51 | bool init(ros::NodeHandle& nh); 52 | void spin(); 53 | void shutdown() { shutdown_req = true; }; 54 | 55 | private: 56 | enum RobotFeedbackType 57 | { 58 | NONE, 59 | ODOMETRY, 60 | COMMANDS 61 | } robot_feedback; /**< What source to use as robot velocity feedback */ 62 | 63 | std::string name; 64 | double speed_lim_vx, accel_lim_vx, decel_lim_vx, decel_lim_vx_safe; 65 | double speed_lim_vy, accel_lim_vy, decel_lim_vy, decel_lim_vy_safe; 66 | double speed_lim_w, accel_lim_w, decel_lim_w, decel_lim_w_safe; 67 | double decel_factor, decel_factor_safe; 68 | 69 | double frequency; 70 | 71 | geometry_msgs::Twist last_cmd_vel; 72 | geometry_msgs::Twist current_vel; 73 | geometry_msgs::Twist target_vel; 74 | 75 | bool shutdown_req; /**< Shutdown requested by nodelet; kill worker thread */ 76 | bool input_active; 77 | double cb_avg_time; 78 | ros::Time last_cb_time; 79 | std::vector period_record; /**< Historic of latest periods between velocity commands */ 80 | unsigned int pr_next; /**< Next position to fill in the periods record buffer */ 81 | 82 | ros::Subscriber odometry_sub; /**< Current velocity from odometry */ 83 | ros::Subscriber current_vel_sub; /**< Current velocity from commands sent to the robot, not necessarily by this node */ 84 | ros::Subscriber raw_in_vel_sub; /**< Incoming raw velocity commands */ 85 | ros::Publisher smooth_vel_pub; /**< Outgoing smoothed velocity commands */ 86 | 87 | void velocityCB(const geometry_msgs::Twist::ConstPtr& msg); 88 | void robotVelCB(const geometry_msgs::Twist::ConstPtr& msg); 89 | void odometryCB(const nav_msgs::Odometry::ConstPtr& msg); 90 | 91 | double sign(double x) { return x < 0.0 ? -1.0 : +1.0; }; 92 | 93 | double median(std::vector values) { 94 | // Return the median element of an doubles vector 95 | nth_element(values.begin(), values.begin() + values.size()/2, values.end()); 96 | return values[values.size()/2]; 97 | }; 98 | 99 | /********************* 100 | ** Dynamic Reconfigure 101 | **********************/ 102 | dynamic_reconfigure::Server * dynamic_reconfigure_server; 103 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_callback; 104 | void reconfigCB(cob_base_velocity_smoother::paramsConfig &config, uint32_t unused_level); 105 | }; 106 | 107 | } // cob_base_velocity_smoother 108 | 109 | #endif /* VELOCITY_SMOOTHER_HPP_ */ 110 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_base_velocity_smoother 3 | 0.9.25 4 | The 'cob_base_velocity_smoother' reads velocity messages and publishes messages of the same type for "smoothed" velocity to avoid shaking behavior. 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_base_velocity_smoother 9 | 10 | 11 | Benjamin Maidel 12 | Florian Mirus 13 | Benjamin Maidel 14 | 15 | catkin 16 | 17 | boost 18 | dynamic_reconfigure 19 | geometry_msgs 20 | nav_msgs 21 | roscpp 22 | roslint 23 | std_msgs 24 | 25 | 26 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/src/cob_base_velocity_smoother.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | 20 | /**************************************************************** 21 | * the ros navigation doesn't run very smoothly because acceleration is too high 22 | * --> cob has strong base motors and therefore reacts with shaking behavior 23 | * (PR2 has much more mechanical damping) 24 | * solution: the additional node cob_base_velocity_smoother smooths the velocities 25 | * comming from ROS-navigation or teleoperation, by calculating the mean values of a certain number 26 | * of past messages and limiting the acceleration under a given threshold. 27 | * cob_base_velocity_smoother subsribes (input) and publishes (output) geometry_msgs::Twist. 28 | ****************************************************************/ 29 | 30 | // function for checking wether a new msg has been received, triggering publishers accordingly 31 | void cob_base_velocity_smoother::set_new_msg_received(bool received) 32 | { 33 | pthread_mutex_lock(&m_mutex); 34 | new_msg_received_ = received; 35 | pthread_mutex_unlock(&m_mutex); 36 | } 37 | 38 | bool cob_base_velocity_smoother::get_new_msg_received() 39 | { 40 | pthread_mutex_lock(&m_mutex); 41 | bool ret_val = new_msg_received_; 42 | pthread_mutex_unlock(&m_mutex); 43 | return ret_val; 44 | } 45 | 46 | // constructor 47 | cob_base_velocity_smoother::cob_base_velocity_smoother() 48 | { 49 | m_mutex = PTHREAD_MUTEX_INITIALIZER; 50 | new_msg_received_ = false; 51 | 52 | // create node handles 53 | nh_ = ros::NodeHandle(); 54 | pnh_ = ros::NodeHandle("~"); 55 | 56 | // publisher 57 | pub_ = nh_.advertise("output", 1); 58 | 59 | // subscriber 60 | geometry_msgs_sub_ = nh_.subscribe("input", 1, boost::bind(&cob_base_velocity_smoother::geometryCallback, this, _1)); 61 | 62 | // get parameters from parameter server if possible or write default values to variables 63 | if( !pnh_.hasParam("circular_buffer_capacity") ) 64 | { 65 | ROS_WARN("No parameter circular_buffer_capacity on parameter server. Using default [12]"); 66 | } 67 | pnh_.param("circular_buffer_capacity", buffer_capacity_, 12); 68 | 69 | if( !pnh_.hasParam("maximal_time_delay") ) 70 | { 71 | ROS_WARN("No parameter maximal_time_delay on parameter server. Using default [4 in s]"); 72 | } 73 | pnh_.param("maximal_time_delay", store_delay_, 4.0); 74 | 75 | if( !pnh_.hasParam("maximal_time_delay_to_stop") ) 76 | { 77 | ROS_WARN("No parameter maximal_time_delay_to_stop on parameter server. Using default [0.1 in s]"); 78 | } 79 | pnh_.param("maximal_time_delay_to_stop", stop_delay_after_no_sub_, 0.1); 80 | 81 | if( !pnh_.hasParam("thresh_max_acc") ) 82 | { 83 | ROS_WARN("No parameter thresh_max_acc on parameter server. Using default [0.3 in m/s]"); 84 | } 85 | pnh_.param("thresh_max_acc", acc_limit_, 0.3); 86 | 87 | if( !pnh_.hasParam("loop_rate") ) 88 | { 89 | ROS_WARN("No parameter loop_rate on parameter server. Using default [30 in Hz]"); 90 | } 91 | pnh_.param("loop_rate", loop_rate_, 30.0); 92 | 93 | double min_input_rate; 94 | if( !pnh_.hasParam("min_input_rate") ) 95 | { 96 | ROS_WARN("No parameter min_input_rate on parameter server. Using default [9 in Hz]"); 97 | } 98 | pnh_.param("min_input_rate", min_input_rate, 9.0); 99 | 100 | if( min_input_rate > loop_rate_) 101 | { 102 | ROS_WARN("min_input_rate > loop_rate: Setting min_input_rate to loop_rate = %f", loop_rate_); 103 | min_input_rate = loop_rate_; 104 | } 105 | max_delay_between_commands_ = 1/min_input_rate; 106 | 107 | // set a geometry message containing zero-values 108 | zero_values_.linear.x = 0.0; 109 | zero_values_.linear.y = 0.0; 110 | zero_values_.linear.z = 0.0; 111 | 112 | zero_values_.angular.x = 0.0; 113 | zero_values_.angular.y = 0.0; 114 | zero_values_.angular.z = 0.0; 115 | 116 | // initialize circular buffers 117 | cb_.set_capacity(buffer_capacity_); 118 | cb_out_.set_capacity(buffer_capacity_); 119 | cb_time_.set_capacity(buffer_capacity_); 120 | 121 | // set actual ros::Time 122 | ros::Time now = ros::Time::now(); 123 | 124 | // fill circular buffer with zero values 125 | while(cb_.full() == false) 126 | { 127 | cb_.push_front(zero_values_); 128 | cb_time_.push_front(now); 129 | } 130 | }; 131 | 132 | // destructor 133 | cob_base_velocity_smoother::~cob_base_velocity_smoother(){} 134 | 135 | // callback function to subsribe to the geometry messages cmd_vel and save them in a member variable 136 | void cob_base_velocity_smoother::geometryCallback(const geometry_msgs::Twist::ConstPtr &cmd_vel) 137 | { 138 | sub_msg_ = *cmd_vel; 139 | set_new_msg_received(true); 140 | } 141 | 142 | // calculation function called periodically in main 143 | void cob_base_velocity_smoother::calculationStep() 144 | { 145 | // set current ros::Time 146 | ros::Time now = ros::Time::now(); 147 | static ros::Time last = now; 148 | 149 | geometry_msgs::Twist result = geometry_msgs::Twist(); 150 | 151 | // only publish command if we received a msg or the last message was actually zero 152 | if (get_new_msg_received()) 153 | { 154 | // generate Output messages 155 | result = this->setOutput(now, sub_msg_); 156 | pub_.publish(result); 157 | 158 | last = now; 159 | set_new_msg_received(false); 160 | } 161 | // start writing in zeros if we did not receive a new msg within a certain amount of time. 162 | // Do not publish! Otherwise, the output of other nodes will be overwritten! 163 | else if ( fabs((last - now).toSec()) > max_delay_between_commands_) 164 | { 165 | result = this->setOutput(now, geometry_msgs::Twist()); 166 | } 167 | // if last message was a zero msg, fill the buffer with zeros and publish again 168 | else if (IsZeroMsg(sub_msg_)) 169 | { 170 | result = this->setOutput(now, sub_msg_); 171 | pub_.publish(result); 172 | } 173 | } 174 | 175 | // function for the actual computation 176 | // calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh 177 | // returns the resulting geomtry message to be published to the base_controller 178 | geometry_msgs::Twist cob_base_velocity_smoother::setOutput(ros::Time now, geometry_msgs::Twist cmd_vel) 179 | { 180 | geometry_msgs::Twist result = zero_values_; 181 | 182 | // update the circular buffers 183 | this->reviseCircBuff(now, cmd_vel); 184 | 185 | // calculate the mean values for each direction 186 | result.linear.x = meanValueX(); 187 | result.linear.y = meanValueY(); 188 | result.angular.z = meanValueZ(); 189 | 190 | // limit acceleration 191 | this->limitAcceleration(now, result); 192 | 193 | // insert the result-message into the circular-buffer storing the output 194 | cb_out_.push_front(result); 195 | 196 | return result; 197 | } 198 | 199 | // function that updates the circular buffer after receiving a new geometry message 200 | void cob_base_velocity_smoother::reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel) 201 | { 202 | if(this->circBuffOutOfDate(now) == true) 203 | { 204 | // the circular buffer is out of date, so clear and refill with zero messages before adding the new command 205 | 206 | // clear buffers 207 | cb_.clear(); 208 | cb_time_.clear(); 209 | 210 | // fill circular buffer with zero_values_ and time buffer with actual time-stamp 211 | while(cb_.full() == false) 212 | { 213 | cb_.push_front(zero_values_); 214 | cb_time_.push_front(now); 215 | } 216 | 217 | // add new command velocity message to circular buffer 218 | cb_.push_front(cmd_vel); 219 | // add new timestamp for subscribed command velocity message 220 | cb_time_.push_front(now); 221 | } 222 | else 223 | { 224 | // only some elements of the circular buffer are out of date, so only delete those 225 | double delay=(now.toSec() - cb_time_.back().toSec()); 226 | 227 | while( delay >= store_delay_ ) 228 | { 229 | // remove out-dated messages 230 | cb_.pop_back(); 231 | cb_time_.pop_back(); 232 | 233 | delay=(now.toSec() - cb_time_.back().toSec()); 234 | } 235 | // if the circular buffer is empty now, refill with zero values 236 | if(cb_.empty() == true) 237 | { 238 | while(cb_.full() == false) 239 | { 240 | cb_.push_front(zero_values_); 241 | cb_time_.push_front(now); 242 | } 243 | } 244 | if(this->IsZeroMsg(cmd_vel)) 245 | { 246 | // here we subscribed a zero message, so we want to stop the robot 247 | long unsigned int size = floor( cb_.size() / 3 ); 248 | 249 | // to stop the robot faster, fill the circular buffer with more than one, in fact floor (cb_.size() / 3 ), zero messages 250 | for(long unsigned int i=0; i< size; i++) 251 | { 252 | // add new command velocity message to circular buffer 253 | cb_.push_front(cmd_vel); 254 | // add new timestamp for subscribed command velocity message 255 | cb_time_.push_front(now); 256 | } 257 | } 258 | else 259 | { 260 | // add new command velocity message to circular buffer 261 | cb_.push_front(cmd_vel); 262 | // add new timestamp for subscribed command velocity message 263 | cb_time_.push_front(now); 264 | } 265 | } 266 | }; 267 | 268 | // returns true if all messages in cb are out of date in consideration of store_delay 269 | bool cob_base_velocity_smoother::circBuffOutOfDate(ros::Time now) 270 | { 271 | bool result=true; 272 | 273 | long unsigned int count=0; 274 | 275 | while( (count < cb_.size()) && (result == true) ) 276 | { 277 | double delay=(now.toSec() - cb_time_[count].toSec()); 278 | if(delay < store_delay_) 279 | { 280 | result = false; 281 | } 282 | count++; 283 | } 284 | 285 | return result; 286 | }; 287 | 288 | // returns true if the input msg cmd_vel equals zero_values_, false otherwise 289 | bool cob_base_velocity_smoother::IsZeroMsg(geometry_msgs::Twist cmd_vel) 290 | { 291 | bool result = true; 292 | if( (cmd_vel.linear.x) != 0 || (cmd_vel.linear.y != 0) || (cmd_vel.angular.z != 0) ) 293 | { 294 | result = false; 295 | } 296 | 297 | return result; 298 | }; 299 | 300 | int cob_base_velocity_smoother::signum(double var) 301 | { 302 | if(var < 0) 303 | { 304 | return -1; 305 | } 306 | else 307 | { 308 | return 1; 309 | } 310 | }; 311 | 312 | // functions to calculate the mean values for linear/x 313 | double cob_base_velocity_smoother::meanValueX() 314 | { 315 | double result = 0; 316 | long unsigned int size = cb_.size(); 317 | 318 | // calculate sum 319 | for(long unsigned int i=0; i 1) 326 | { 327 | double help_result = 0; 328 | double max = cb_[0].linear.x; 329 | long unsigned int max_ind = 0; 330 | for(long unsigned int i=0; i abs(result-max)) 333 | { 334 | max = cb_[i].linear.x; 335 | max_ind = i; 336 | } 337 | } 338 | 339 | // calculate sum 340 | for(long unsigned int i=0; i 1) 367 | { 368 | double help_result = 0; 369 | double max = cb_[0].linear.y; 370 | long unsigned int max_ind = 0; 371 | for(long unsigned int i=0; i abs(result-max)) 374 | { 375 | max = cb_[i].linear.y; 376 | max_ind = i; 377 | } 378 | } 379 | 380 | // calculate sum 381 | for(long unsigned int i=0; i 1) 408 | { 409 | double help_result = 0; 410 | double max = cb_[0].angular.z; 411 | long unsigned int max_ind = 0; 412 | for(long unsigned int i = 0; i < size; i++) 413 | { 414 | if(abs(result-cb_[i].angular.z) > abs(result-max)) 415 | { 416 | max = cb_[i].angular.z; 417 | max_ind = i; 418 | } 419 | } 420 | 421 | // calculate sum 422 | for(long unsigned int i = 0; i < size; i++) 423 | { 424 | if(i != max_ind) 425 | { 426 | help_result += cb_[i].angular.z; 427 | } 428 | } 429 | result = help_result / (size - 1); 430 | } 431 | 432 | return result; 433 | }; 434 | 435 | // function to make the loop rate availabe outside the class 436 | double cob_base_velocity_smoother::getLoopRate() 437 | { 438 | return loop_rate_; 439 | } 440 | 441 | // function to compare two geometry messages 442 | bool cob_base_velocity_smoother::IsEqual(geometry_msgs::Twist msg1, geometry_msgs::Twist msg2) 443 | { 444 | if( (msg1.linear.x == msg2.linear.x) && (msg1.linear.y == msg2.linear.y) && (msg1.angular.z == msg2.angular.z)) 445 | { 446 | return true; 447 | } 448 | else 449 | { 450 | return false; 451 | } 452 | } 453 | 454 | //function to limit the acceleration under the given threshhold thresh 455 | void cob_base_velocity_smoother::limitAcceleration(ros::Time now, geometry_msgs::Twist& result) 456 | { 457 | // limit the acceleration under thresh 458 | // only if cob_base_velocity_smoother has published a message yet 459 | 460 | double deltaTime = 0; 461 | 462 | if(cb_time_.size() > 1) 463 | { 464 | deltaTime = now.toSec() - cb_time_[2].toSec(); 465 | } 466 | 467 | if( cb_out_.size() > 0) 468 | { 469 | if(deltaTime > 0) 470 | { 471 | // set delta velocity and acceleration values 472 | double deltaX = result.linear.x - cb_out_.front().linear.x; 473 | 474 | double deltaY = result.linear.y - cb_out_.front().linear.y; 475 | 476 | double deltaZ = result.angular.z - cb_out_.front().angular.z; 477 | 478 | if( abs(deltaX) > acc_limit_) 479 | { 480 | result.linear.x = cb_out_.front().linear.x + this->signum(deltaX) * acc_limit_; 481 | } 482 | if( abs(deltaY) > acc_limit_ ) 483 | { 484 | result.linear.y = cb_out_.front().linear.y + this->signum(deltaY) * acc_limit_; 485 | } 486 | if( abs(deltaZ) > acc_limit_ ) 487 | { 488 | result.angular.z = cb_out_.front().angular.z + this->signum(deltaZ) * acc_limit_; 489 | } 490 | } 491 | } 492 | }; 493 | 494 | 495 | int main(int argc, char **argv) 496 | { 497 | // initialize ros and specifiy node name 498 | ros::init(argc, argv, "cob_base_velocity_smoother"); 499 | 500 | // create Node Class 501 | cob_base_velocity_smoother my_velocity_smoother; 502 | // get loop rate from class member 503 | ros::Rate rate(my_velocity_smoother.getLoopRate()); 504 | // actual calculation step with given frequency 505 | while(my_velocity_smoother.nh_.ok()) 506 | { 507 | my_velocity_smoother.calculationStep(); 508 | 509 | ros::spinOnce(); 510 | rate.sleep(); 511 | } 512 | 513 | return 0; 514 | } 515 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/src/cob_base_velocity_smoother/velocity_smoother.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | /***************************************************************************** 18 | ** Includes 19 | *****************************************************************************/ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | /***************************************************************************** 29 | ** Preprocessing 30 | *****************************************************************************/ 31 | 32 | #define PERIOD_RECORD_SIZE 5 33 | #define ZERO_VEL_COMMAND geometry_msgs::Twist(); 34 | #define IS_ZERO_VEOCITY(a) ((a.linear.x == 0.0) && (a.linear.y == 0.0) && (a.angular.z == 0.0)) 35 | 36 | /***************************************************************************** 37 | ** Namespaces 38 | *****************************************************************************/ 39 | 40 | namespace cob_base_velocity_smoother { 41 | 42 | /********************* 43 | ** Implementation 44 | **********************/ 45 | 46 | VelocitySmoother::VelocitySmoother(const std::string &name) 47 | : name(name) 48 | , shutdown_req(false) 49 | , input_active(false) 50 | , pr_next(0) 51 | , dynamic_reconfigure_server(NULL) 52 | { 53 | } 54 | 55 | void VelocitySmoother::reconfigCB(cob_base_velocity_smoother::paramsConfig &config, uint32_t level) 56 | { 57 | ROS_INFO("Reconfigure request : %f %f %f %f %f %f %f %f", 58 | config.speed_lim_vx, config.speed_lim_vy, config.speed_lim_w, config.accel_lim_vx, config.accel_lim_vy, config.accel_lim_w, config.decel_factor, config.decel_factor_safe); 59 | 60 | speed_lim_vx = config.speed_lim_vx; 61 | speed_lim_vy = config.speed_lim_vy; 62 | speed_lim_w = config.speed_lim_w; 63 | accel_lim_vx = config.accel_lim_vx; 64 | accel_lim_vy = config.accel_lim_vy; 65 | accel_lim_w = config.accel_lim_w; 66 | decel_factor = config.decel_factor; 67 | decel_factor_safe = config.decel_factor_safe; 68 | decel_lim_vx = decel_factor*accel_lim_vx; 69 | decel_lim_vy = decel_factor*accel_lim_vy; 70 | decel_lim_w = decel_factor*accel_lim_w; 71 | decel_lim_vx_safe = decel_factor_safe*accel_lim_vx; 72 | decel_lim_vy_safe = decel_factor_safe*accel_lim_vy; 73 | decel_lim_w_safe = decel_factor_safe*accel_lim_w; 74 | } 75 | 76 | void VelocitySmoother::velocityCB(const geometry_msgs::Twist::ConstPtr& msg) 77 | { 78 | // Estimate commands frequency; we do continuously as it can be very different depending on the 79 | // publisher type, and we don't want to impose extra constraints to keep this package flexible 80 | if (period_record.size() < PERIOD_RECORD_SIZE) 81 | { 82 | period_record.push_back((ros::Time::now() - last_cb_time).toSec()); 83 | } 84 | else 85 | { 86 | period_record[pr_next] = (ros::Time::now() - last_cb_time).toSec(); 87 | } 88 | 89 | pr_next++; 90 | pr_next %= period_record.size(); 91 | last_cb_time = ros::Time::now(); 92 | 93 | if (period_record.size() <= PERIOD_RECORD_SIZE/2) 94 | { 95 | // wait until we have some values; make a reasonable assumption (10 Hz) meanwhile 96 | cb_avg_time = 0.1; 97 | } 98 | else 99 | { 100 | // enough; recalculate with the latest input 101 | cb_avg_time = median(period_record); 102 | } 103 | 104 | input_active = true; 105 | 106 | // Bound speed with the maximum values 107 | target_vel.linear.x = 108 | msg->linear.x > 0.0 ? std::min(msg->linear.x, speed_lim_vx) : std::max(msg->linear.x, -speed_lim_vx); 109 | target_vel.linear.y = 110 | msg->linear.y > 0.0 ? std::min(msg->linear.y, speed_lim_vy) : std::max(msg->linear.y, -speed_lim_vy); 111 | target_vel.angular.z = 112 | msg->angular.z > 0.0 ? std::min(msg->angular.z, speed_lim_w) : std::max(msg->angular.z, -speed_lim_w); 113 | } 114 | 115 | void VelocitySmoother::odometryCB(const nav_msgs::Odometry::ConstPtr& msg) 116 | { 117 | if (robot_feedback == ODOMETRY) 118 | current_vel = msg->twist.twist; 119 | 120 | // ignore otherwise 121 | } 122 | 123 | void VelocitySmoother::robotVelCB(const geometry_msgs::Twist::ConstPtr& msg) 124 | { 125 | if (robot_feedback == COMMANDS) 126 | current_vel = *msg; 127 | 128 | // ignore otherwise 129 | } 130 | 131 | void VelocitySmoother::spin() 132 | { 133 | double period = 1.0/frequency; 134 | ros::Rate spin_rate(frequency); 135 | 136 | double decel_vx; 137 | double decel_vy; 138 | double decel_w; 139 | 140 | while (! shutdown_req && ros::ok()) 141 | { 142 | if ((input_active == true) && (cb_avg_time > 0.0) && 143 | ((ros::Time::now() - last_cb_time).toSec() > std::min(3.0*cb_avg_time, 0.5))) 144 | { 145 | // Velocity input no active anymore; normally last command is a zero-velocity one, but reassure 146 | // this, just in case something went wrong with our input, or he just forgot good manners... 147 | // Issue #2, extra check in case cb_avg_time is very big, for example with several atomic commands 148 | // The cb_avg_time > 0 check is required to deal with low-rate simulated time, that can make that 149 | // several messages arrive with the same time and so lead to a zero median 150 | input_active = false; 151 | if (IS_ZERO_VEOCITY(target_vel) == false) 152 | { 153 | ROS_WARN_STREAM("Velocity Smoother : input got inactive leaving us a non-zero target velocity (" 154 | << target_vel.linear.x << ", " << target_vel.linear.y << ", " << target_vel.angular.z << "), zeroing...[" << name << "]"); 155 | target_vel = ZERO_VEL_COMMAND; 156 | } 157 | } 158 | 159 | if(input_active) 160 | { 161 | decel_vx = decel_lim_vx; 162 | decel_vy = decel_lim_vy; 163 | decel_w = decel_lim_w; 164 | } 165 | else 166 | { 167 | //increase decel factor because this is a safty case, no more commands means we should stop as fast as it is safe 168 | decel_vx = decel_lim_vx_safe; 169 | decel_vy = decel_lim_vy_safe; 170 | decel_w = decel_lim_w_safe; 171 | } 172 | 173 | if ((robot_feedback != NONE) && (input_active == true) && (cb_avg_time > 0.0) && 174 | (((ros::Time::now() - last_cb_time).toSec() > 5.0*cb_avg_time) || // 5 missing msgs 175 | (std::abs(current_vel.linear.x - last_cmd_vel.linear.x) > 0.2) || 176 | (std::abs(current_vel.linear.y - last_cmd_vel.linear.y) > 0.2) || 177 | (std::abs(current_vel.angular.z - last_cmd_vel.angular.z) > 2.0))) 178 | { 179 | // If the publisher has been inactive for a while, or if our current commanding differs a lot 180 | // from robot velocity feedback, we cannot trust the former; relay on robot's feedback instead 181 | // This can happen mainly due to preemption of current controller on velocity multiplexer. 182 | // TODO: current command/feedback difference thresholds are 진짜 arbitrary; they should somehow 183 | // be proportional to max v and w... 184 | // The one for angular velocity is very big because is it's less necessary (for example the 185 | // reactive controller will never make the robot spin) and because the gyro has a 15 ms delay 186 | ROS_WARN("Using robot velocity feedback (%s) instead of last command: %f, %f, %f, %f", 187 | robot_feedback == ODOMETRY ? "odometry" : "end commands", 188 | (ros::Time::now() - last_cb_time).toSec(), 189 | current_vel.linear.x - last_cmd_vel.linear.x, 190 | current_vel.linear.y - last_cmd_vel.linear.y, 191 | current_vel.angular.z - last_cmd_vel.angular.z); 192 | last_cmd_vel = current_vel; 193 | } 194 | 195 | geometry_msgs::TwistPtr cmd_vel; 196 | 197 | if ((target_vel.linear.x != last_cmd_vel.linear.x) || 198 | (target_vel.linear.y != last_cmd_vel.linear.y) || 199 | (target_vel.angular.z != last_cmd_vel.angular.z)) 200 | { 201 | // Try to reach target velocity ensuring that we don't exceed the acceleration limits 202 | cmd_vel.reset(new geometry_msgs::Twist(target_vel)); 203 | 204 | double vx_inc, vy_inc, w_inc, max_vx_inc, max_vy_inc, max_w_inc; 205 | 206 | vx_inc = target_vel.linear.x - last_cmd_vel.linear.x; 207 | if ((robot_feedback == ODOMETRY) && (current_vel.linear.x*target_vel.linear.x < 0.0)) 208 | { 209 | // countermarch (on robots with significant inertia; requires odometry feedback to be detected) 210 | max_vx_inc = decel_vx*period; 211 | } 212 | else 213 | { 214 | max_vx_inc = ((vx_inc*target_vel.linear.x > 0.0)?accel_lim_vx:decel_vx)*period; 215 | } 216 | 217 | vy_inc = target_vel.linear.y - last_cmd_vel.linear.y; 218 | if ((robot_feedback == ODOMETRY) && (current_vel.linear.y*target_vel.linear.y < 0.0)) 219 | { 220 | // countermarch (on robots with significant inertia; requires odometry feedback to be detected) 221 | max_vy_inc = decel_vy*period; 222 | } 223 | else 224 | { 225 | max_vy_inc = ((vy_inc*target_vel.linear.y > 0.0)?accel_lim_vy:decel_vy)*period; 226 | } 227 | 228 | w_inc = target_vel.angular.z - last_cmd_vel.angular.z; 229 | if ((robot_feedback == ODOMETRY) && (current_vel.angular.z*target_vel.angular.z < 0.0)) 230 | { 231 | // countermarch (on robots with significant inertia; requires odometry feedback to be detected) 232 | max_w_inc = decel_w*period; 233 | } 234 | else 235 | { 236 | max_w_inc = ((w_inc*target_vel.angular.z > 0.0)?accel_lim_w:decel_w)*period; 237 | } 238 | 239 | /* 240 | // Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment), 241 | // where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines 242 | // which velocity (v or w) must be overconstrained to keep the direction provided as command 243 | double MA = sqrt( vx_inc * vx_inc + w_inc * w_inc); 244 | double MB = sqrt(max_vx_inc * max_vx_inc + max_w_inc * max_w_inc); 245 | 246 | double Av = std::abs(vx_inc) / MA; 247 | double Aw = std::abs(w_inc) / MA; 248 | double Bv = max_vx_inc / MB; 249 | double Bw = max_w_inc / MB; 250 | double theta = atan2(Bw, Bv) - atan2(Aw, Av); 251 | 252 | if (theta < 0) 253 | { 254 | // overconstrain linear velocity 255 | max_vx_inc = (max_w_inc*std::abs(vx_inc))/std::abs(w_inc); 256 | } 257 | else 258 | { 259 | // overconstrain angular velocity 260 | max_w_inc = (max_vx_inc*std::abs(w_inc))/std::abs(vx_inc); 261 | } 262 | */ 263 | if (std::abs(vx_inc) > max_vx_inc) 264 | { 265 | // we must limit linear velocity 266 | cmd_vel->linear.x = last_cmd_vel.linear.x + sign(vx_inc)*max_vx_inc; 267 | } 268 | 269 | if (std::abs(vy_inc) > max_vy_inc) 270 | { 271 | // we must limit linear velocity 272 | cmd_vel->linear.y = last_cmd_vel.linear.y + sign(vy_inc)*max_vy_inc; 273 | } 274 | 275 | if (std::abs(w_inc) > max_w_inc) 276 | { 277 | // we must limit angular velocity 278 | cmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc; 279 | } 280 | 281 | smooth_vel_pub.publish(cmd_vel); 282 | last_cmd_vel = *cmd_vel; 283 | } 284 | else if (input_active == true) 285 | { 286 | // We already reached target velocity; just keep resending last command while input is active 287 | cmd_vel.reset(new geometry_msgs::Twist(last_cmd_vel)); 288 | smooth_vel_pub.publish(cmd_vel); 289 | } 290 | 291 | spin_rate.sleep(); 292 | } 293 | } 294 | 295 | /** 296 | * Initialise from a nodelet's private nodehandle. 297 | * @param nh : private nodehandle 298 | * @return bool : success or failure 299 | */ 300 | bool VelocitySmoother::init(ros::NodeHandle& nh) 301 | { 302 | // Dynamic Reconfigure 303 | dynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2); 304 | 305 | dynamic_reconfigure_server = new dynamic_reconfigure::Server(nh); 306 | dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback); 307 | 308 | // Optional parameters 309 | int feedback; 310 | nh.param("frequency", frequency, 20.0); 311 | nh.param("decel_factor", decel_factor, 1.0); 312 | nh.param("decel_factor_safe", decel_factor_safe, 1.0); 313 | nh.param("robot_feedback", feedback, (int)NONE); 314 | 315 | if ((int(feedback) < NONE) || (int(feedback) > COMMANDS)) 316 | { 317 | ROS_WARN("Invalid robot feedback type (%d). Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)", 318 | feedback); 319 | feedback = NONE; 320 | } 321 | 322 | robot_feedback = static_cast(feedback); 323 | 324 | // Mandatory parameters 325 | if ((nh.getParam("speed_lim_vx", speed_lim_vx) == false) || 326 | (nh.getParam("speed_lim_vy", speed_lim_vy) == false) || 327 | (nh.getParam("speed_lim_w", speed_lim_w) == false)) 328 | { 329 | ROS_ERROR("Missing velocity limit parameter(s)"); 330 | return false; 331 | } 332 | 333 | if ((nh.getParam("accel_lim_vx", accel_lim_vx) == false) || 334 | (nh.getParam("accel_lim_vy", accel_lim_vy) == false) || 335 | (nh.getParam("accel_lim_w", accel_lim_w) == false)) 336 | { 337 | ROS_ERROR("Missing acceleration limit parameter(s)"); 338 | return false; 339 | } 340 | 341 | // Deceleration can be more aggressive, if necessary 342 | decel_lim_vx = decel_factor*accel_lim_vx; 343 | decel_lim_vy = decel_factor*accel_lim_vy; 344 | decel_lim_w = decel_factor*accel_lim_w; 345 | // In safety cases (no topic command anymore), deceleration should be very aggressive 346 | decel_lim_vx_safe = decel_factor_safe*accel_lim_vx; 347 | decel_lim_vy_safe = decel_factor_safe*accel_lim_vy; 348 | decel_lim_w_safe = decel_factor_safe*accel_lim_w; 349 | 350 | // Publishers and subscribers 351 | odometry_sub = nh.subscribe("odometry", 1, &VelocitySmoother::odometryCB, this); 352 | current_vel_sub = nh.subscribe("robot_cmd_vel", 1, &VelocitySmoother::robotVelCB, this); 353 | raw_in_vel_sub = nh.subscribe("raw_cmd_vel", 1, &VelocitySmoother::velocityCB, this); 354 | smooth_vel_pub = nh.advertise ("smooth_cmd_vel", 1); 355 | 356 | return true; 357 | } 358 | } 359 | -------------------------------------------------------------------------------- /cob_base_velocity_smoother/src/velocity_smoother_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | using namespace cob_base_velocity_smoother; 24 | 25 | std::string unresolvedName(const std::string &name){ 26 | size_t pos = name.find_last_of('/'); 27 | return name.substr(pos + 1); 28 | } 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "velocity_smoother"); 33 | ros::NodeHandle nh_p("~"); 34 | std::string resolved_name = nh_p.getUnresolvedNamespace(); // this always returns like /robosem/goo_arm - why not unresolved? 35 | std::string name = unresolvedName(resolved_name); // unresolve it ourselves 36 | 37 | boost::shared_ptr vel_smoother_; 38 | boost::shared_ptr worker_thread_; 39 | 40 | vel_smoother_.reset(new VelocitySmoother(name)); 41 | if (vel_smoother_->init(nh_p)) 42 | { 43 | ROS_DEBUG_STREAM("Velocity Smoother: initialised [" << name << "]"); 44 | worker_thread_.reset(new boost::thread(&VelocitySmoother::spin, vel_smoother_)); 45 | } 46 | else 47 | { 48 | ROS_ERROR_STREAM("Velocity Smoother: initialisation failed [" << name << "]"); 49 | } 50 | 51 | ros::Rate r(100); 52 | while (ros::ok()) 53 | { 54 | ros::spinOnce(); 55 | r.sleep(); 56 | } 57 | 58 | vel_smoother_->shutdown(); 59 | worker_thread_->join(); 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_collision_velocity_filter 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.25 (2024-08-05) 6 | ------------------- 7 | 8 | 0.9.24 (2024-04-30) 9 | ------------------- 10 | * 0.8.24 11 | * update changelogs 12 | * Contributors: fmessmer 13 | 14 | 0.8.24 (2024-04-18) 15 | ------------------- 16 | 17 | 0.8.23 (2024-02-20) 18 | ------------------- 19 | 20 | 0.8.22 (2023-04-29) 21 | ------------------- 22 | 23 | 0.8.21 (2023-01-04) 24 | ------------------- 25 | 26 | 0.8.20 (2022-11-17) 27 | ------------------- 28 | 29 | 0.8.19 (2022-07-29) 30 | ------------------- 31 | 32 | 0.8.18 (2022-01-12) 33 | ------------------- 34 | 35 | 0.8.17 (2021-12-23) 36 | ------------------- 37 | 38 | 0.8.16 (2021-10-19) 39 | ------------------- 40 | 41 | 0.8.15 (2021-05-17) 42 | ------------------- 43 | 44 | 0.8.14 (2021-05-10) 45 | ------------------- 46 | 47 | 0.8.13 (2021-04-06) 48 | ------------------- 49 | * Merge pull request `#247 `_ from fmessmer/fix_catkin_lint_melodic 50 | [melodic] fix catkin_lint 51 | * fix catkin_lint 52 | * Contributors: Felix Messmer, fmessmer 53 | 54 | 0.8.12 (2020-10-21) 55 | ------------------- 56 | * Merge pull request `#243 `_ from fmessmer/test_noetic 57 | test noetic 58 | * Bump CMake version to avoid CMP0048 warning 59 | * Contributors: Felix Messmer, fmessmer 60 | 61 | 0.8.11 (2020-03-21) 62 | ------------------- 63 | 64 | 0.8.10 (2020-03-18) 65 | ------------------- 66 | * Merge pull request `#226 `_ from fmessmer/ci_updates_melodic 67 | [travis] ci updates - melodic 68 | * catkin_lint fixes 69 | * Merge pull request `#224 `_ from HannesBachter/fix/relevant_obstacle 70 | [melodic] fix info of relevant_obstacles message 71 | * publish relevant_obstacles_grid in private namespace 72 | * fix info of relevant_obstacles message 73 | * Contributors: Felix Messmer, fmessmer, hyb 74 | 75 | 0.8.1 (2019-11-07) 76 | ------------------ 77 | 78 | 0.8.0 (2019-08-09) 79 | ------------------ 80 | 81 | 0.7.8 (2019-08-09) 82 | ------------------ 83 | 84 | 0.7.7 (2019-08-06) 85 | ------------------ 86 | * Merge pull request `#208 `_ from ipa-jba/melodic_dev 87 | [Melodic] melodify 88 | * initialize tf buffer properly 89 | * move include to where it is needed 90 | * add missing tf2_ros dependency 91 | * tf2ify/melodify collision_velocity_filter 92 | * Contributors: Felix Messmer, Jannik Abbenseth 93 | 94 | 0.7.6 (2019-06-07) 95 | ------------------ 96 | * Merge pull request `#207 `_ from fmessmer/fix_collision_velocity_filter 97 | fix collision_velocity_filter parameter initialization 98 | * fix collision_velocity_filter parameter initializatin 99 | * Contributors: Felix Messmer, fmessmer 100 | 101 | 0.7.5 (2019-05-20) 102 | ------------------ 103 | 104 | 0.7.4 (2019-04-05) 105 | ------------------ 106 | 107 | 0.7.3 (2019-03-14) 108 | ------------------ 109 | 110 | 0.7.2 (2018-07-21) 111 | ------------------ 112 | 113 | 0.7.1 (2018-01-07) 114 | ------------------ 115 | * Merge remote-tracking branch 'origin/kinetic_release_candidate' into kinetic_dev 116 | * Merge pull request `#169 `_ from ipa-fxm/kinetic_updates_indigo 117 | Kinetic updates indigo 118 | * Merge branch 'indigo_dev' of github.com:ipa320/cob_control into kinetic_dev 119 | Conflicts: 120 | .travis.yml 121 | * Merge pull request `#164 `_ from ipa-fxm/update_maintainer 122 | update maintainer 123 | * update maintainer 124 | * Merge pull request `#159 `_ from ipa-fxm/APACHE_license 125 | use license apache 2.0 126 | * use license apache 2.0 127 | * Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk 128 | 129 | 0.7.0 (2017-07-18) 130 | ------------------ 131 | 132 | 0.6.15 (2017-07-18) 133 | ------------------- 134 | * manually fix changelog 135 | * Contributors: ipa-fxm 136 | 137 | 0.6.14 (2016-10-10) 138 | ------------------- 139 | 140 | 0.6.13 (2016-10-10) 141 | ------------------- 142 | 143 | 0.6.12 (2016-10-10) 144 | ------------------- 145 | * Integrate costmap in velocity filter 146 | * Added public nodehandle 147 | * Renamed incoming command topic to command_in 148 | * Code reformat with ROS Style 149 | * Little fixes 150 | * Removed parameter for topic 151 | * Cleanup 152 | * Integrated costmap in collision_velocity_filter node 153 | * Intermediate state 154 | * Added parameter "teleop_topic" and removed costmap running check for now. 155 | * Added costmap_2d to CMakeLists 156 | * Integrated costmap in cob_collision_velocity_filter node 157 | * Integrate costmap in cob_collision_velocity_filter (first commit) 158 | * Contributors: mig-em 159 | 160 | 0.6.11 (2016-04-01) 161 | ------------------- 162 | 163 | 0.6.10 (2015-08-31) 164 | ------------------- 165 | 166 | 0.6.9 (2015-08-25) 167 | ------------------ 168 | * boost revision 169 | * do not install headers in executable-only packages 170 | * explicit dependency to boost 171 | * explicit dependency to boost 172 | * remove trailing whitespaces 173 | * migrate to package format 2 174 | * sort dependencies 175 | * review dependencies 176 | * Contributors: ipa-fxm 177 | 178 | 0.6.8 (2015-06-17) 179 | ------------------ 180 | 181 | 0.6.7 (2015-06-17) 182 | ------------------ 183 | * beautify CMakeLists 184 | * Contributors: ipa-fxm 185 | 186 | 0.6.6 (2014-12-18) 187 | ------------------ 188 | 189 | 0.6.5 (2014-12-18) 190 | ------------------ 191 | 192 | 0.6.4 (2014-12-16) 193 | ------------------ 194 | 195 | 0.6.3 (2014-12-16) 196 | ------------------ 197 | 198 | 0.6.2 (2014-12-15) 199 | ------------------ 200 | 201 | 0.6.1 (2014-09-22) 202 | ------------------ 203 | 204 | 0.5.3 (2014-03-31) 205 | ------------------ 206 | * install tags 207 | * Contributors: ipa-fxm 208 | 209 | 0.5.2 (2014-03-20) 210 | ------------------ 211 | 212 | 0.5.1 (2014-03-20) 213 | ------------------ 214 | * add definitions to get rid of compiler warning 215 | * Corrected compile error. See pull-request `#80 `_ in original repo. 216 | * merge changes from frederikhegger, `#80 `_ 217 | * change from gencpp (for msgs and srvs) to gencfg (for dyn recfg) 218 | * changes for hydro 219 | * Installation stuff 220 | * cleaned up CMakeLists and added install directives 221 | * further modifications for catkin, now everything is compiling and linking 222 | * compiling but still some linker errors 223 | * Second catkinization push 224 | * First catkinization, still need to update some CMakeLists.txt 225 | * Color interpolation added to the velocity limited marker 226 | * set max vel to 0.2 227 | * set max velocity for marker to 0.3 228 | * A new marker shown when rotation speed is limited 229 | * remove debug output 230 | * add markers to veclocity filter 231 | * reduce max range 232 | * fix divide by zero bug 233 | * make cob_collision_velocity_filter dynamically reconfigureable 234 | * move src files 235 | * move launch and config files to cob_robots 236 | * add laser_scan_top_clearing as additional observation_source 237 | * add scan_top_clearing topic 238 | * add ramp to limit acceleration 239 | * change documentation 240 | * merge launch files and move configs to same folder 241 | * put GetFootprint service into footprint observer 242 | instead of SetFootprint service in collision velocity filter 243 | fix namespace problems 244 | * only use laser data for velocity filter 245 | * integrate safe velocity controller by default 246 | * add missing dependency 247 | * moved safe base movement to driver stack 248 | * Contributors: Alexander Bubeck, Florian Weißhardt, Frederik Hegger, IPR-SR2, abubeck, but-spanel, ipa-fmw, ipa-mig, mig, srs 249 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_collision_velocity_filter) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cob_footprint_observer 6 | costmap_2d 7 | dynamic_reconfigure 8 | geometry_msgs 9 | nav_msgs 10 | roscpp 11 | tf 12 | tf2_ros 13 | visualization_msgs 14 | ) 15 | 16 | find_package(Boost REQUIRED) 17 | 18 | if(CMAKE_COMPILER_IS_GNUCXX) 19 | add_definitions(-std=gnu++0x) 20 | else() 21 | add_definitions(-std=c++0x) 22 | endif() 23 | 24 | ### Dynamic reconfigure ### 25 | generate_dynamic_reconfigure_options(cfg/CollisionVelocityFilter.cfg) 26 | 27 | catkin_package( 28 | CATKIN_DEPENDS cob_footprint_observer costmap_2d dynamic_reconfigure geometry_msgs nav_msgs roscpp tf visualization_msgs 29 | ) 30 | 31 | ### BUILD ### 32 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 33 | 34 | add_executable(collision_velocity_filter src/${PROJECT_NAME}.cpp src/velocity_limited_marker.cpp) 35 | add_dependencies(collision_velocity_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 36 | target_link_libraries(collision_velocity_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 37 | 38 | ### INSTALL ### 39 | install(TARGETS collision_velocity_filter 40 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 41 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 43 | ) 44 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/cfg/CollisionVelocityFilter.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "cob_collision_velocity_filter" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("influence_radius", double_t, 0, 9 | "Max distance of obstacles from robot center that are used for computing relevant obstacles", 10 | 1.5, 0, 5) 11 | gen.add("stop_threshold", double_t, 0, 12 | "Distance to relevant_obstacle at which robot stops to move completely", 13 | .2, 0, 5) 14 | gen.add("obstacle_damping_dist", double_t, 0, 15 | "Distance in driving direction at which potential field like slow down controller starts to work", 16 | 5.0, .1, 5) 17 | 18 | 19 | exit(gen.generate(PACKAGE, "cob_collision_velocity_filter", "CollisionVelocityFilter")) 20 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/include/cob_collision_velocity_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COB_COLLISION_VELOCITY_FILTER_H 19 | #define COB_COLLISION_VELOCITY_FILTER_H 20 | 21 | //################## 22 | //#### includes #### 23 | 24 | // standard includes 25 | //-- 26 | 27 | // ROS includes 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | // ROS message includes 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | // ROS service includes 44 | #include "cob_footprint_observer/GetFootprint.h" 45 | 46 | // dynamic reconfigure includes 47 | #include 48 | #include 49 | 50 | // BUT velocity limited marker 51 | #include "velocity_limited_marker.h" 52 | 53 | // Costmap for obstacle detection 54 | #include 55 | 56 | /// 57 | /// @class CollisionVelocityFilter 58 | /// @brief checks for obstacles in driving direction and stops the robot 59 | /// 60 | /// 61 | class CollisionVelocityFilter 62 | { 63 | public: 64 | 65 | /// 66 | /// @brief Constructor 67 | /// 68 | CollisionVelocityFilter(costmap_2d::Costmap2DROS * costmap); 69 | 70 | /// 71 | /// @brief Destructor 72 | /// 73 | ~CollisionVelocityFilter(); 74 | 75 | /// 76 | /// @brief reads twist command from teleop device (joystick, teleop_keyboard, ...) and calls functions 77 | /// for collision check (obstacleHandler) and driving of the robot (performControllerStep) 78 | /// @param twist - velocity command sent as twist message (twist.linear.x/y/z, twist.angular.x/y/z) 79 | /// 80 | void joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist); 81 | 82 | /// 83 | /// @brief reads obstacles from costmap 84 | /// @param obstacles - 2D occupancy grid in rolling window mode! 85 | /// 86 | void readObstacles(); 87 | 88 | /// 89 | /// @brief Timer callback, calls GetFootprint Service and adjusts footprint 90 | /// 91 | void getFootprint(const ros::TimerEvent&); 92 | 93 | /// 94 | /// @brief Dynamic reconfigure callback 95 | /// @param config - configuration file with dynamic reconfigureable parameters 96 | /// @param level - the result of ORing together all level values of the parameters that have changed, for now unnecessary 97 | /// 98 | void dynamicReconfigureCB(const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, 99 | const uint32_t level); 100 | 101 | /// create a handle for this node, initialize node 102 | //public 103 | ros::NodeHandle nh_; 104 | 105 | //private 106 | ros::NodeHandle pnh_; 107 | 108 | /// Timer for periodically calling GetFootprint Service 109 | ros::Timer get_footprint_timer_; 110 | 111 | /// declaration of publisher 112 | ros::Publisher topic_pub_command_; 113 | ros::Publisher topic_pub_relevant_obstacles_; 114 | 115 | /// declaration of subscriber 116 | ros::Subscriber joystick_velocity_sub_, obstacles_sub_; 117 | 118 | /// dynamic reconfigure 119 | dynamic_reconfigure::Server dyn_server_; 120 | dynamic_reconfigure::Server::CallbackType dynCB_; 121 | 122 | private: 123 | /* core functions */ 124 | 125 | costmap_2d::Costmap2DROS* anti_collision_costmap_; 126 | //costmap_2d::Costmap2D costmap; 127 | /// 128 | /// @brief checks distance to obstacles in driving direction and slows down/stops 129 | /// robot and publishes command velocity to robot 130 | /// 131 | void performControllerStep(); 132 | 133 | /// 134 | /// @brief checks for obstacles in driving direction of the robot (rotation included) 135 | /// and publishes relevant obstacles 136 | /// 137 | void obstacleHandler(); 138 | 139 | /* helper functions */ 140 | 141 | /// 142 | /// @brief returns the sign of x 143 | /// 144 | double sign(double x); 145 | 146 | /// 147 | /// @brief computes distance between two points 148 | /// @param a,b - Points 149 | /// @return distance 150 | /// 151 | double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b); 152 | 153 | /// 154 | /// @brief checks if obstacle lies already within footprint -> this is ignored due to sensor readings of the hull etc 155 | /// @param x_obstacle - x coordinate of obstacle in occupancy grid local costmap 156 | /// @param y_obstacle - y coordinate of obstacle in occupancy grid local costmap 157 | /// @return true if obstacle outside of footprint 158 | /// 159 | bool obstacleValid(double x_obstacle, double y_obstacle); 160 | 161 | /// 162 | /// @brief stops movement of the robot 163 | /// 164 | void stopMovement(); 165 | 166 | pthread_mutex_t m_mutex; 167 | 168 | //obstacle_treshold 169 | int costmap_obstacle_treshold_; 170 | 171 | //frames 172 | std::string global_frame_, robot_frame_; 173 | 174 | //velocity 175 | geometry_msgs::Vector3 robot_twist_linear_, robot_twist_angular_; 176 | double v_max_, vtheta_max_; 177 | double ax_max_, ay_max_, atheta_max_; 178 | 179 | //obstacle avoidance 180 | std::vector robot_footprint_; 181 | double footprint_left_, footprint_right_, footprint_front_, footprint_rear_; 182 | double footprint_left_initial_, footprint_right_initial_, footprint_front_initial_, footprint_rear_initial_; 183 | bool costmap_received_; 184 | nav_msgs::OccupancyGrid last_costmap_received_, relevant_obstacles_; 185 | double influence_radius_, stop_threshold_, obstacle_damping_dist_, use_circumscribed_threshold_; 186 | double closest_obstacle_dist_, closest_obstacle_angle_; 187 | 188 | // variables for slow down behavior 189 | double last_time_; 190 | double kp_, kv_; 191 | double vx_last_, vy_last_, vtheta_last_; 192 | double virt_mass_; 193 | 194 | // BUT velocity limited marker 195 | cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_; 196 | 197 | }; 198 | //CollisionVelocityFilter 199 | 200 | #endif 201 | 202 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/include/velocity_limited_marker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #pragma once 19 | #ifndef COB_VELOCITY_LIMITED_MARKER_H 20 | #define COB_VELOCITY_LIMITED_MARKER_H 21 | 22 | // ROS includes 23 | #include 24 | 25 | // ROS message includes 26 | #include 27 | 28 | namespace cob_collision_velocity_filter 29 | { 30 | 31 | /// 32 | /// @class CollisionVelocityFilter 33 | /// @brief checks for obstacles in driving direction and stops the robot 34 | /// 35 | class VelocityLimitedMarker 36 | { 37 | public: 38 | /// 39 | /// @brief Constructor 40 | /// 41 | VelocityLimitedMarker(); 42 | 43 | /// 44 | /// @brief Destructor 45 | /// 46 | ~VelocityLimitedMarker(); 47 | 48 | /// 49 | /// @brief Creates all directional markers, the method is called from the constructor. 50 | /// 51 | void createDirectionalMarkers(); 52 | 53 | /// 54 | /// @brief Creates all rotational markers, the method is called from the constructor. 55 | /// 56 | void createRotationalMarkers(); 57 | 58 | /// 59 | /// @brief Creates all the markers, the method is called from the constructor. 60 | /// 61 | void publishMarkers( double vel_x_desired, 62 | double vel_x_actual, 63 | double vel_y_desired, 64 | double vel_y_actual, 65 | double vel_theta_desired, 66 | double vel_theta_actual); 67 | 68 | /// 69 | /// @brief Calculates the color for the marker 70 | //// based on the absolute difference of velocities. 71 | /// 72 | void interpolateColor(double velocity, std_msgs::ColorRGBA& color); 73 | 74 | protected: 75 | // Velocity limited markers 76 | visualization_msgs::Marker x_pos_marker_, x_neg_marker_, y_pos_marker_, y_neg_marker_; 77 | visualization_msgs::Marker theta_pos_marker_, theta_neg_marker_; 78 | 79 | // a handle for this node 80 | ros::NodeHandle nh_; 81 | 82 | // Marker publisher 83 | ros::Publisher marker_pub_; 84 | 85 | // Is the marker disabled? 86 | bool disabled_; 87 | 88 | // Robot base frame 89 | std::string base_frame_; 90 | 91 | // Output topic name 92 | std::string topic_name_; 93 | 94 | // Marker lifetime 95 | double lifetime_; 96 | 97 | // Marker z-position 98 | double z_pos_; 99 | 100 | // last velocities 101 | double vx_last_, vy_last_, vtheta_last_; 102 | }; 103 | 104 | 105 | } 106 | 107 | #endif // COB_VELOCITY_LIMITED_MARKER_H 108 | 109 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b cob_collision_velocity_filter is a package for collision avoidance using teleoperation. 6 | 7 | The cob_collision_velocity_filter node subscribes to a geometry_msgs::Twist topic published by the teleop device. 8 | It further subscribes to the obstacles topic of a local costmap and checks, if there are obstacles in the driving direction of the robot. 9 | Those relevant_obstacles are published as well. 10 | 11 | If the robot moves closer to the relevant_obstacles, the robot slows down until it reaches a stop_threshold. 12 | There the robot stops moving if there is a velocity component that would run it into the obstacle. 13 | Driving in directions not leading to collision is still possible. 14 | 15 | The cob_collision_velocity_filter node further calls a service for getting the adjusted footprint (which is initially read from the footprint parameter specified in the costmap node) during runtime thus accomodating for changes in the robot setup. 16 | Note that cob_collision_velocity_filter is at the moment restriced to rectangular footprints. 17 | 18 | To launch the cob_collision_velocity_filter launch the collision_velocity_filter.launch file. 19 | Make sure, that the geometry_msgs::Twist is maped to the collision_velocity_filter teleop_twist input. 20 | 21 | */ 22 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_collision_velocity_filter 3 | 0.9.25 4 | The cob_collision_velocity_filter package is a package for collision avoidance using teleoperation. 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_collision_velocity_filter 9 | 10 | 11 | Felipe Garcia Lopez 12 | Matthias Gruhler 13 | Michal Spanel 14 | 15 | catkin 16 | 17 | boost 18 | cob_footprint_observer 19 | costmap_2d 20 | dynamic_reconfigure 21 | geometry_msgs 22 | nav_msgs 23 | roscpp 24 | tf 25 | tf2_ros 26 | visualization_msgs 27 | 28 | 29 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/src/cob_collision_velocity_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | 20 | // Backwards compatibility of tf2 (tf for pre-kinetic) for costmap 21 | #if ROS_VERSION_MINIMUM(1, 14, 0) 22 | #include 23 | #else 24 | #include 25 | #endif 26 | 27 | #include 28 | 29 | // Constructor 30 | CollisionVelocityFilter::CollisionVelocityFilter(costmap_2d::Costmap2DROS * costmap) 31 | { 32 | // create node handle 33 | nh_ = ros::NodeHandle(""); 34 | pnh_ = ros::NodeHandle("~"); 35 | 36 | m_mutex = PTHREAD_MUTEX_INITIALIZER; 37 | 38 | anti_collision_costmap_ = costmap; 39 | 40 | if (!pnh_.hasParam("costmap_obstacle_treshold")) 41 | ROS_WARN("Used default parameter for 'costmap_obstacle_treshold' [250]."); 42 | pnh_.param("costmap_obstacle_treshold", costmap_obstacle_treshold_, 250); 43 | 44 | // implementation of topics to publish (command for base and list of relevant obstacles) 45 | topic_pub_command_ = nh_.advertise("command", 1); 46 | topic_pub_relevant_obstacles_ = pnh_.advertise("relevant_obstacles_grid", 1); 47 | 48 | // subscribe to twist-movement of teleop 49 | joystick_velocity_sub_ = nh_.subscribe("command_in", 10, 50 | &CollisionVelocityFilter::joystickVelocityCB, this); 51 | 52 | // create Timer and call getFootprint Service periodically 53 | double footprint_update_frequency; 54 | if (!pnh_.hasParam("footprint_update_frequency")) 55 | ROS_WARN("Used default parameter for 'footprint_update_frequency' [1.0 Hz]."); 56 | pnh_.param("footprint_update_frequency", footprint_update_frequency, 1.0); 57 | get_footprint_timer_ = pnh_.createTimer(ros::Duration(1 / footprint_update_frequency), 58 | &CollisionVelocityFilter::getFootprint, this); 59 | // read parameters from parameter server 60 | // parameters from costmap 61 | if (!pnh_.hasParam("global_frame")) 62 | ROS_WARN("Used default parameter for 'global_frame' [/base_link]"); 63 | pnh_.param("global_frame", global_frame_, std::string("/base_link")); 64 | 65 | if (!pnh_.hasParam("robot_base_frame")) 66 | ROS_WARN("Used default parameter for 'robot_base_frame' [/base_link]"); 67 | pnh_.param("robot_base_frame", robot_frame_, std::string("/base_link")); 68 | 69 | if (!pnh_.hasParam("influence_radius")) 70 | ROS_WARN("Used default parameter for 'influence_radius' [1.5 m]"); 71 | pnh_.param("influence_radius", influence_radius_, 1.5); 72 | closest_obstacle_dist_ = influence_radius_; 73 | closest_obstacle_angle_ = 0.0; 74 | 75 | // parameters for obstacle avoidance and velocity adjustment 76 | if (!pnh_.hasParam("stop_threshold")) 77 | ROS_WARN("Used default parameter for 'stop_threshold' [0.1 m]"); 78 | pnh_.param("stop_threshold", stop_threshold_, 0.10); 79 | 80 | if (!pnh_.hasParam("obstacle_damping_dist")) 81 | ROS_WARN("Used default parameter for 'obstacle_damping_dist' [5.0 m]"); 82 | pnh_.param("obstacle_damping_dist", obstacle_damping_dist_, 5.0); 83 | if (obstacle_damping_dist_ <= stop_threshold_) 84 | { 85 | obstacle_damping_dist_ = stop_threshold_ + 0.01; // set to stop_threshold_+0.01 to avoid divide by zero error 86 | ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!"); 87 | } 88 | 89 | if (!pnh_.hasParam("use_circumscribed_threshold")) 90 | ROS_WARN("Used default parameter for 'use_circumscribed_threshold' [0.2 rad/s]"); 91 | pnh_.param("use_circumscribed_threshold", use_circumscribed_threshold_, 0.20); 92 | 93 | if (!pnh_.hasParam("pot_ctrl_vmax")) 94 | ROS_WARN("Used default parameter for 'pot_ctrl_vmax' [0.6]"); 95 | pnh_.param("pot_ctrl_vmax", v_max_, 0.6); 96 | 97 | if (!pnh_.hasParam("pot_ctrl_vtheta_max")) 98 | ROS_WARN("Used default parameter for 'pot_ctrl_vtheta_max' [0.8]"); 99 | pnh_.param("pot_ctrl_vtheta_max", vtheta_max_, 0.8); 100 | 101 | if (!pnh_.hasParam("pot_ctrl_kv")) 102 | ROS_WARN("Used default parameter for 'pot_ctrl_kv' [1.0]"); 103 | pnh_.param("pot_ctrl_kv", kv_, 1.0); 104 | 105 | if (!pnh_.hasParam("pot_ctrl_kp")) 106 | ROS_WARN("Used default parameter for 'pot_ctrl_kp' [2.0]"); 107 | pnh_.param("pot_ctrl_kp", kp_, 2.0); 108 | 109 | if (!pnh_.hasParam("pot_ctrl_virt_mass")) 110 | ROS_WARN("Used default parameter for 'pot_ctrl_virt_mass' [0.8]"); 111 | pnh_.param("pot_ctrl_virt_mass", virt_mass_, 0.8); 112 | 113 | robot_footprint_ = anti_collision_costmap_->getRobotFootprint(); 114 | 115 | if (robot_footprint_.size() > 4) 116 | ROS_WARN( 117 | "You have set more than 4 points as robot_footprint, cob_collision_velocity_filter can deal only with rectangular footprints so far!"); 118 | 119 | // try to get the max_acceleration values from the parameter server 120 | if (!pnh_.hasParam("max_acceleration")) 121 | ROS_WARN("Used default parameter for 'max_acceleration' [0.5, 0.5, 0.7]"); 122 | XmlRpc::XmlRpcValue max_acc; 123 | if (pnh_.getParam("max_acceleration", max_acc)) 124 | { 125 | ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray); 126 | ax_max_ = (double)max_acc[0]; 127 | ay_max_ = (double)max_acc[1]; 128 | atheta_max_ = (double)max_acc[2]; 129 | } 130 | else 131 | { 132 | ax_max_ = 0.5; 133 | ay_max_ = 0.5; 134 | atheta_max_ = 0.7; 135 | } 136 | 137 | last_time_ = ros::Time::now().toSec(); 138 | vx_last_ = 0.0; 139 | vy_last_ = 0.0; 140 | vtheta_last_ = 0.0; 141 | 142 | // dynamic reconfigure 143 | dynCB_ = boost::bind(&CollisionVelocityFilter::dynamicReconfigureCB, this, _1, _2); 144 | dyn_server_.setCallback(dynCB_); 145 | ROS_DEBUG("[cob_collision_velocity_filter] Initialized"); 146 | } 147 | 148 | // Destructor 149 | CollisionVelocityFilter::~CollisionVelocityFilter() 150 | { 151 | // costmap_thread_->interrupt(); 152 | // costmap_thread_->join(); 153 | } 154 | 155 | // joystick_velocityCB reads twist command from joystick 156 | void CollisionVelocityFilter::joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist) 157 | { 158 | //std::cout << "received command" << std::endl; 159 | ROS_DEBUG_NAMED("joystickVelocityCB", "[cob_collision_velocity_filter] Received command"); 160 | pthread_mutex_lock(&m_mutex); 161 | 162 | robot_twist_linear_ = twist->linear; 163 | robot_twist_angular_ = twist->angular; 164 | 165 | pthread_mutex_unlock(&m_mutex); 166 | 167 | // check for relevant obstacles 168 | obstacleHandler(); 169 | // stop if we are about to run in an obstacle 170 | performControllerStep(); 171 | 172 | } 173 | 174 | // timer callback for periodically checking footprint 175 | void CollisionVelocityFilter::getFootprint(const ros::TimerEvent& event) 176 | { 177 | ROS_DEBUG("[cob_collision_velocity_filter] Update footprint"); 178 | // adjust footprint 179 | std::vector footprint; 180 | footprint = anti_collision_costmap_->getRobotFootprint(); 181 | 182 | pthread_mutex_lock(&m_mutex); 183 | 184 | footprint_front_ = footprint_front_initial_; 185 | footprint_rear_ = footprint_rear_initial_; 186 | footprint_left_ = footprint_left_initial_; 187 | footprint_right_ = footprint_right_initial_; 188 | 189 | robot_footprint_ = footprint; 190 | for (unsigned int i = 0; i < footprint.size(); i++) 191 | { 192 | if (footprint[i].x > footprint_front_) 193 | footprint_front_ = footprint[i].x; 194 | if (footprint[i].x < footprint_rear_) 195 | footprint_rear_ = footprint[i].x; 196 | if (footprint[i].y > footprint_left_) 197 | footprint_left_ = footprint[i].y; 198 | if (footprint[i].y < footprint_right_) 199 | footprint_right_ = footprint[i].y; 200 | } 201 | 202 | pthread_mutex_unlock(&m_mutex); 203 | 204 | } 205 | 206 | void CollisionVelocityFilter::dynamicReconfigureCB( 207 | const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level) 208 | { 209 | pthread_mutex_lock(&m_mutex); 210 | 211 | stop_threshold_ = config.stop_threshold; 212 | obstacle_damping_dist_ = config.obstacle_damping_dist; 213 | if (obstacle_damping_dist_ <= stop_threshold_) 214 | { 215 | obstacle_damping_dist_ = stop_threshold_ + 0.01; // set to stop_threshold_+0.01 to avoid divide by zero error 216 | ROS_WARN("obstacle_damping_dist <= stop_threshold -> robot will stop without deceleration!"); 217 | } 218 | 219 | if (obstacle_damping_dist_ > config.influence_radius || stop_threshold_ > config.influence_radius) 220 | { 221 | ROS_WARN("Not changing influence_radius since obstacle_damping_dist and/or stop_threshold is bigger!"); 222 | } 223 | else 224 | { 225 | influence_radius_ = config.influence_radius; 226 | } 227 | 228 | if (stop_threshold_ <= 0.0 || influence_radius_ <= 0.0) 229 | ROS_WARN("Turned off obstacle avoidance!"); 230 | pthread_mutex_unlock(&m_mutex); 231 | } 232 | 233 | // sets corrected velocity of joystick command 234 | void CollisionVelocityFilter::performControllerStep() 235 | { 236 | 237 | double dt; 238 | double vx_max, vy_max; 239 | geometry_msgs::Twist cmd_vel, cmd_vel_in; 240 | 241 | cmd_vel_in.linear = robot_twist_linear_; 242 | cmd_vel_in.angular = robot_twist_angular_; 243 | 244 | cmd_vel.linear = robot_twist_linear_; 245 | cmd_vel.angular = robot_twist_angular_; 246 | dt = ros::Time::now().toSec() - last_time_; 247 | last_time_ = ros::Time::now().toSec(); 248 | 249 | double vel_angle = atan2(cmd_vel.linear.y, cmd_vel.linear.x); 250 | vx_max = v_max_ * fabs(cos(vel_angle)); 251 | if (vx_max > fabs(cmd_vel.linear.x)) 252 | vx_max = fabs(cmd_vel.linear.x); 253 | vy_max = v_max_ * fabs(sin(vel_angle)); 254 | if (vy_max > fabs(cmd_vel.linear.y)) 255 | vy_max = fabs(cmd_vel.linear.y); 256 | 257 | //Slow down in any way while approximating an obstacle: 258 | if (closest_obstacle_dist_ < influence_radius_) 259 | { 260 | double F_x, F_y; 261 | double vx_d, vy_d, vx_factor, vy_factor; 262 | double kv_obst = kv_, vx_max_obst = vx_max, vy_max_obst = vy_max; 263 | 264 | //implementation for linear decrease of v_max: 265 | double obstacle_linear_slope_x = vx_max / (obstacle_damping_dist_ - stop_threshold_); 266 | vx_max_obst = (closest_obstacle_dist_ - stop_threshold_ + stop_threshold_ / 10.0f) * obstacle_linear_slope_x; 267 | if (vx_max_obst > vx_max) 268 | vx_max_obst = vx_max; 269 | else if (vx_max_obst < 0.0f) 270 | vx_max_obst = 0.0f; 271 | 272 | double obstacle_linear_slope_y = vy_max / (obstacle_damping_dist_ - stop_threshold_); 273 | vy_max_obst = (closest_obstacle_dist_ - stop_threshold_ + stop_threshold_ / 10.0f) * obstacle_linear_slope_y; 274 | if (vy_max_obst > vy_max) 275 | vy_max_obst = vy_max; 276 | else if (vy_max_obst < 0.0f) 277 | vy_max_obst = 0.0f; 278 | 279 | //Translational movement 280 | //calculation of v factor to limit maxspeed 281 | double closest_obstacle_dist_x = closest_obstacle_dist_ * cos(closest_obstacle_angle_); 282 | double closest_obstacle_dist_y = closest_obstacle_dist_ * sin(closest_obstacle_angle_); 283 | vx_d = kp_ / kv_obst * closest_obstacle_dist_x; 284 | vy_d = kp_ / kv_obst * closest_obstacle_dist_y; 285 | vx_factor = vx_max_obst / sqrt(vy_d * vy_d + vx_d * vx_d); 286 | vy_factor = vy_max_obst / sqrt(vy_d * vy_d + vx_d * vx_d); 287 | if (vx_factor > 1.0) 288 | vx_factor = 1.0; 289 | if (vy_factor > 1.0) 290 | vy_factor = 1.0; 291 | 292 | F_x = -kv_obst * vx_last_ + vx_factor * kp_ * closest_obstacle_dist_x; 293 | F_y = -kv_obst * vy_last_ + vy_factor * kp_ * closest_obstacle_dist_y; 294 | 295 | cmd_vel.linear.x = vx_last_ + F_x / virt_mass_ * dt; 296 | cmd_vel.linear.y = vy_last_ + F_y / virt_mass_ * dt; 297 | 298 | } 299 | 300 | // make sure, the computed and commanded velocities are not greater than the specified max velocities 301 | if (fabs(cmd_vel.linear.x) > vx_max) 302 | cmd_vel.linear.x = sign(cmd_vel.linear.x) * vx_max; 303 | if (fabs(cmd_vel.linear.y) > vy_max) 304 | cmd_vel.linear.y = sign(cmd_vel.linear.y) * vy_max; 305 | if (fabs(cmd_vel.angular.z) > vtheta_max_) 306 | cmd_vel.angular.z = sign(cmd_vel.angular.z) * vtheta_max_; 307 | 308 | // limit acceleration: 309 | // only acceleration (in terms of speeding up in any direction) is limited, 310 | // deceleration (in terms of slowing down) is handeled either by cob_teleop or the potential field 311 | // like slow-down behaviour above 312 | if (fabs(cmd_vel.linear.x) > fabs(vx_last_)) 313 | { 314 | if ((cmd_vel.linear.x - vx_last_) / dt > ax_max_) 315 | cmd_vel.linear.x = vx_last_ + ax_max_ * dt; 316 | else if ((cmd_vel.linear.x - vx_last_) / dt < -ax_max_) 317 | cmd_vel.linear.x = vx_last_ - ax_max_ * dt; 318 | } 319 | if (fabs(cmd_vel.linear.y) > fabs(vy_last_)) 320 | { 321 | if ((cmd_vel.linear.y - vy_last_) / dt > ay_max_) 322 | cmd_vel.linear.y = vy_last_ + ay_max_ * dt; 323 | else if ((cmd_vel.linear.y - vy_last_) / dt < -ay_max_) 324 | cmd_vel.linear.y = vy_last_ - ay_max_ * dt; 325 | } 326 | if (fabs(cmd_vel.angular.z) > fabs(vtheta_last_)) 327 | { 328 | if ((cmd_vel.angular.z - vtheta_last_) / dt > atheta_max_) 329 | cmd_vel.angular.z = vtheta_last_ + atheta_max_ * dt; 330 | else if ((cmd_vel.angular.z - vtheta_last_) / dt < -atheta_max_) 331 | cmd_vel.angular.z = vtheta_last_ - atheta_max_ * dt; 332 | } 333 | 334 | pthread_mutex_lock(&m_mutex); 335 | vx_last_ = cmd_vel.linear.x; 336 | vy_last_ = cmd_vel.linear.y; 337 | vtheta_last_ = cmd_vel.angular.z; 338 | pthread_mutex_unlock(&m_mutex); 339 | 340 | velocity_limited_marker_.publishMarkers(cmd_vel_in.linear.x, cmd_vel.linear.x, cmd_vel_in.linear.y, cmd_vel.linear.y, 341 | cmd_vel_in.angular.z, cmd_vel.angular.z); 342 | 343 | // if closest obstacle is within stop_threshold, then do not move 344 | if (closest_obstacle_dist_ < stop_threshold_) 345 | { 346 | stopMovement(); 347 | } 348 | else 349 | { 350 | // publish adjusted velocity 351 | topic_pub_command_.publish(cmd_vel); 352 | } 353 | return; 354 | } 355 | 356 | void CollisionVelocityFilter::obstacleHandler() 357 | { 358 | pthread_mutex_lock(&m_mutex); 359 | closest_obstacle_dist_ = influence_radius_; 360 | pthread_mutex_unlock(&m_mutex); 361 | 362 | double cur_distance_to_center, cur_distance_to_border; 363 | double obstacle_theta_robot, obstacle_delta_theta_robot, obstacle_dist_vel_dir; 364 | bool cur_obstacle_relevant; 365 | geometry_msgs::Point cur_obstacle_robot; 366 | geometry_msgs::Point zero_position; 367 | zero_position.x = 0.0f; 368 | zero_position.y = 0.0f; 369 | zero_position.z = 0.0f; 370 | bool use_circumscribed = true, use_tube = true; 371 | 372 | //Calculate corner angles in robot_frame: 373 | double corner_front_left, corner_rear_left, corner_rear_right, corner_front_right; 374 | corner_front_left = atan2(footprint_left_, footprint_front_); 375 | corner_rear_left = atan2(footprint_left_, footprint_rear_); 376 | corner_rear_right = atan2(footprint_right_, footprint_rear_); 377 | corner_front_right = atan2(footprint_right_, footprint_front_); 378 | 379 | //Decide, whether circumscribed or tube argument should be used for filtering: 380 | if (fabs(robot_twist_linear_.x) <= 0.005f && fabs(robot_twist_linear_.y) <= 0.005f) 381 | { 382 | use_tube = false; 383 | //disable tube filter at very slow velocities 384 | } 385 | if (!use_tube) 386 | { 387 | if (fabs(robot_twist_angular_.z) <= 0.01f) 388 | { 389 | use_circumscribed = false; 390 | } //when tube filter inactive, start circumscribed filter at very low rot-velocities 391 | } 392 | else 393 | { 394 | if (fabs(robot_twist_angular_.z) <= use_circumscribed_threshold_) 395 | { 396 | use_circumscribed = false; 397 | } //when tube filter running, disable circum-filter in a wider range of rot-velocities 398 | } 399 | 400 | //Calculation of tube in driving-dir considered for obstacle avoidence 401 | double velocity_angle = 0.0f, velocity_ortho_angle; 402 | double corner_angle, delta_corner_angle; 403 | double ortho_corner_dist; 404 | double tube_left_border = 0.0f, tube_right_border = 0.0f; 405 | double tube_left_origin = 0.0f, tube_right_origin = 0.0f; 406 | double corner_dist, circumscribed_radius = 0.0f; 407 | 408 | for (unsigned i = 0; i < robot_footprint_.size(); i++) 409 | { 410 | corner_dist = sqrt(robot_footprint_[i].x * robot_footprint_[i].x + robot_footprint_[i].y * robot_footprint_[i].y); 411 | if (corner_dist > circumscribed_radius) 412 | circumscribed_radius = corner_dist; 413 | } 414 | 415 | if (use_tube) 416 | { 417 | //use commanded vel-value for vel-vector direction.. ? 418 | velocity_angle = atan2(robot_twist_linear_.y, robot_twist_linear_.x); 419 | velocity_ortho_angle = velocity_angle + M_PI / 2.0f; 420 | 421 | for (unsigned i = 0; i < robot_footprint_.size(); i++) 422 | { 423 | corner_angle = atan2(robot_footprint_[i].y, robot_footprint_[i].x); 424 | delta_corner_angle = velocity_ortho_angle - corner_angle; 425 | corner_dist = sqrt(robot_footprint_[i].x * robot_footprint_[i].x + robot_footprint_[i].y * robot_footprint_[i].y); 426 | ortho_corner_dist = cos(delta_corner_angle) * corner_dist; 427 | 428 | if (ortho_corner_dist < tube_right_border) 429 | { 430 | tube_right_border = ortho_corner_dist; 431 | tube_right_origin = sin(delta_corner_angle) * corner_dist; 432 | } 433 | else if (ortho_corner_dist > tube_left_border) 434 | { 435 | tube_left_border = ortho_corner_dist; 436 | tube_left_origin = sin(delta_corner_angle) * corner_dist; 437 | } 438 | } 439 | } 440 | 441 | //find relevant obstacles 442 | pthread_mutex_lock(&m_mutex); 443 | relevant_obstacles_.header.frame_id = global_frame_; 444 | relevant_obstacles_.header.stamp = ros::Time::now(); 445 | relevant_obstacles_.info.resolution = anti_collision_costmap_->getCostmap()->getResolution(); 446 | relevant_obstacles_.info.width = anti_collision_costmap_->getCostmap()->getSizeInCellsX(); 447 | relevant_obstacles_.info.height = anti_collision_costmap_->getCostmap()->getSizeInCellsY(); 448 | relevant_obstacles_.info.origin.position.x = anti_collision_costmap_->getCostmap()->getOriginX(); 449 | relevant_obstacles_.info.origin.position.y = anti_collision_costmap_->getCostmap()->getOriginY(); 450 | relevant_obstacles_.info.origin.orientation.w = 1.0; 451 | relevant_obstacles_.data.clear(); 452 | for (unsigned int i = 0; 453 | i 454 | < anti_collision_costmap_->getCostmap()->getSizeInCellsX() 455 | * anti_collision_costmap_->getCostmap()->getSizeInCellsY(); i++) 456 | { 457 | if (anti_collision_costmap_->getCostmap()->getCharMap()[i] == -1) 458 | { 459 | relevant_obstacles_.data.push_back(-1); 460 | } 461 | else if (anti_collision_costmap_->getCostmap()->getCharMap()[i] < costmap_obstacle_treshold_) 462 | { // add trshold 463 | relevant_obstacles_.data.push_back(0); 464 | } 465 | else 466 | { 467 | 468 | // calculate cell in 2D space where robot is is point (0, 0) 469 | geometry_msgs::Point cell; 470 | cell.x = (i % (int)(anti_collision_costmap_->getCostmap()->getSizeInCellsX())) 471 | * anti_collision_costmap_->getCostmap()->getResolution() 472 | + anti_collision_costmap_->getCostmap()->getOriginX(); 473 | cell.y = (i / (int)(anti_collision_costmap_->getCostmap()->getSizeInCellsX())) 474 | * anti_collision_costmap_->getCostmap()->getResolution() 475 | + anti_collision_costmap_->getCostmap()->getOriginY(); 476 | cell.z = 0.0f; 477 | 478 | cur_obstacle_relevant = false; 479 | cur_distance_to_center = getDistance2d(zero_position, cell); 480 | //check whether current obstacle lies inside the circumscribed_radius of the robot -> prevent collisions while rotating 481 | if (use_circumscribed && cur_distance_to_center <= circumscribed_radius) 482 | { 483 | cur_obstacle_robot = cell; 484 | 485 | if (obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y)) 486 | { 487 | cur_obstacle_relevant = true; 488 | obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x); 489 | } 490 | 491 | //for each obstacle, now check whether it lies in the tube or not: 492 | } 493 | else if (use_tube && cur_distance_to_center < influence_radius_) 494 | { 495 | cur_obstacle_robot = cell; 496 | 497 | if (obstacleValid(cur_obstacle_robot.x, cur_obstacle_robot.y)) 498 | { 499 | obstacle_theta_robot = atan2(cur_obstacle_robot.y, cur_obstacle_robot.x); 500 | obstacle_delta_theta_robot = obstacle_theta_robot - velocity_angle; 501 | obstacle_dist_vel_dir = sin(obstacle_delta_theta_robot) * cur_distance_to_center; 502 | 503 | if (obstacle_dist_vel_dir <= tube_left_border && obstacle_dist_vel_dir >= tube_right_border) 504 | { 505 | //found obstacle that lies inside of observation tube 506 | 507 | if (sign(obstacle_dist_vel_dir) >= 0) 508 | { 509 | if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_left_origin) 510 | { 511 | //relevant obstacle in tube found 512 | cur_obstacle_relevant = true; 513 | } 514 | } 515 | else 516 | { // obstacle in right part of tube 517 | if (cos(obstacle_delta_theta_robot) * cur_distance_to_center >= tube_right_origin) 518 | { 519 | //relevant obstacle in tube found 520 | cur_obstacle_relevant = true; 521 | } 522 | } 523 | } 524 | } 525 | } 526 | 527 | if (cur_obstacle_relevant) 528 | { 529 | ROS_DEBUG_STREAM_NAMED("obstacleHandler", "[cob_collision_velocity_filter] Detected an obstacle"); 530 | //relevant obstacle in tube found 531 | relevant_obstacles_.data.push_back(100); 532 | 533 | //now calculate distance of current, relevant obstacle to robot 534 | if (obstacle_theta_robot >= corner_front_right && obstacle_theta_robot < corner_front_left) 535 | { 536 | //obstacle in front: 537 | cur_distance_to_border = cur_distance_to_center - fabs(footprint_front_) / fabs(cos(obstacle_theta_robot)); 538 | } 539 | else if (obstacle_theta_robot >= corner_front_left && obstacle_theta_robot < corner_rear_left) 540 | { 541 | //obstacle left: 542 | cur_distance_to_border = cur_distance_to_center - fabs(footprint_left_) / fabs(sin(obstacle_theta_robot)); 543 | } 544 | else if (obstacle_theta_robot >= corner_rear_left || obstacle_theta_robot < corner_rear_right) 545 | { 546 | //obstacle in rear: 547 | cur_distance_to_border = cur_distance_to_center - fabs(footprint_rear_) / fabs(cos(obstacle_theta_robot)); 548 | } 549 | else 550 | { 551 | //obstacle right: 552 | cur_distance_to_border = cur_distance_to_center - fabs(footprint_right_) / fabs(sin(obstacle_theta_robot)); 553 | } 554 | 555 | if (cur_distance_to_border < closest_obstacle_dist_) 556 | { 557 | closest_obstacle_dist_ = cur_distance_to_border; 558 | closest_obstacle_angle_ = obstacle_theta_robot; 559 | } 560 | } 561 | else 562 | { 563 | relevant_obstacles_.data.push_back(0); 564 | } 565 | } 566 | } 567 | pthread_mutex_unlock(&m_mutex); 568 | 569 | topic_pub_relevant_obstacles_.publish(relevant_obstacles_); 570 | ROS_DEBUG_STREAM_NAMED("obstacleHandler", 571 | "[cob_collision_velocity_filter] closest_obstacle_dist_ = " << closest_obstacle_dist_); 572 | } 573 | 574 | double CollisionVelocityFilter::getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b) 575 | { 576 | return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2)); 577 | } 578 | 579 | double CollisionVelocityFilter::sign(double x) 580 | { 581 | if (x >= 0.0f) 582 | return 1.0f; 583 | else 584 | return -1.0f; 585 | } 586 | 587 | bool CollisionVelocityFilter::obstacleValid(double x_obstacle, double y_obstacle) 588 | { 589 | if (x_obstacle < footprint_front_ && x_obstacle > footprint_rear_ && y_obstacle > footprint_right_ 590 | && y_obstacle < footprint_left_) 591 | { 592 | ROS_WARN("Found an obstacle inside robot_footprint: Skip!"); 593 | return false; 594 | } 595 | 596 | return true; 597 | } 598 | 599 | void CollisionVelocityFilter::stopMovement() 600 | { 601 | geometry_msgs::Twist stop_twist; 602 | stop_twist.linear.x = 0.0f; 603 | stop_twist.linear.y = 0.0f; 604 | stop_twist.linear.z = 0.0f; 605 | stop_twist.angular.x = 0.0f; 606 | stop_twist.angular.y = 0.0f; 607 | stop_twist.linear.z = 0.0f; 608 | topic_pub_command_.publish(stop_twist); 609 | vx_last_ = 0.0; 610 | vy_last_ = 0.0; 611 | vtheta_last_ = 0.0; 612 | } 613 | 614 | //####################### 615 | //#### main programm #### 616 | int main(int argc, char** argv) 617 | { 618 | // initialize ROS, spezify name of node 619 | ros::init(argc, argv, "cob_collision_velocity_filter"); 620 | 621 | // create nodeClass 622 | 623 | #if ROS_VERSION_MINIMUM(1, 14, 0) 624 | tf2_ros::Buffer tf(ros::Duration(10)); 625 | tf2_ros::TransformListener tf_listener(tf); 626 | #else 627 | tf::TransformListener tf(ros::Duration(10)); 628 | #endif 629 | costmap_2d::Costmap2DROS* costmap = new costmap_2d::Costmap2DROS("anti_collision_costmap", tf); 630 | CollisionVelocityFilter collisionVelocityFilter(costmap); 631 | 632 | ros::spin(); 633 | 634 | return 0; 635 | } 636 | 637 | -------------------------------------------------------------------------------- /cob_collision_velocity_filter/src/velocity_limited_marker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | 20 | #include 21 | 22 | 23 | namespace cob_collision_velocity_filter 24 | { 25 | 26 | const double DEFAULT_LIFETIME = 1.0; 27 | const double DEFAULT_Z_POSITION = 0.25; 28 | 29 | const double MARKER_SCALE_DIR = 1.0; 30 | const double MARKER_RADIUS_DIR = 0.5; 31 | const double MARKER_WIDTH_DIR = 0.2; 32 | 33 | const double MARKER_SCALE_ROT = 1.0; 34 | const double MARKER_RADIUS_ROT = 0.6; 35 | //const double MARKER_WIDTH_ROT = 0.065; 36 | const double MARKER_WIDTH_ROT = 0.15; 37 | 38 | //const double MAX_VELOCITY = 0.2; 39 | const double MAX_VELOCITY = 0.25; 40 | const double MIN_VELOCITY = 0.01; 41 | const double VELOCITY_COEFF = 1.0 / (MAX_VELOCITY - MIN_VELOCITY); 42 | 43 | const float MAX_COLOR[4] = { 1.0f, 0.0f, 0.0f, 1.0f }; 44 | const float MIN_COLOR[4] = { 1.0f, 0.8f, 0.0f, 0.2f }; 45 | 46 | 47 | VelocityLimitedMarker::VelocityLimitedMarker() 48 | { 49 | // a handle for this node, initialize node 50 | nh_ = ros::NodeHandle("~"); 51 | 52 | // read parameters from parameter server 53 | nh_.param("marker_frame", base_frame_, std::string("/base_link")); 54 | nh_.param("marker_topic_name", topic_name_, std::string("velocity_limited_marker")); 55 | nh_.param("marker_lifetime", lifetime_, DEFAULT_LIFETIME); 56 | nh_.param("z_pos", z_pos_, DEFAULT_Z_POSITION); 57 | nh_.param("marker_disabled", disabled_, false); 58 | 59 | // Create the publisher 60 | marker_pub_ = nh_.advertise(topic_name_, 1); 61 | 62 | vx_last_ = 0.0; 63 | vy_last_ = 0.0; 64 | vtheta_last_ = 0.0; 65 | 66 | // Create the markers 67 | createDirectionalMarkers(); 68 | createRotationalMarkers(); 69 | } 70 | 71 | 72 | VelocityLimitedMarker::~VelocityLimitedMarker() 73 | { 74 | } 75 | 76 | 77 | void VelocityLimitedMarker::createDirectionalMarkers() 78 | { 79 | // Message template 80 | visualization_msgs::Marker marker; 81 | marker.header.frame_id = base_frame_; 82 | marker.header.stamp = ros::Time::now(); 83 | marker.ns = "cob_velocity_limited_marker"; 84 | marker.id = 0; 85 | marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 86 | marker.action = visualization_msgs::Marker::ADD; 87 | marker.lifetime = ros::Duration(lifetime_); 88 | marker.pose.orientation.x = 0; 89 | marker.pose.orientation.y = 0; 90 | marker.pose.orientation.z = 0; 91 | marker.pose.orientation.w = 1; 92 | marker.pose.position.x = 0; 93 | marker.pose.position.y = 0; 94 | marker.pose.position.z = z_pos_; 95 | marker.scale.x = MARKER_SCALE_DIR; 96 | marker.scale.y = MARKER_SCALE_DIR; 97 | marker.scale.z = 1.0; 98 | marker.color.r = 1.0; 99 | marker.color.g = 0.0; 100 | marker.color.b = 0.0; 101 | marker.color.a = 0.5; // adjust according to the velocity? 102 | 103 | // Create the disc like geometry for the markers 104 | std::vector circle1, circle2; 105 | geometry_msgs::Point v1, v2; 106 | 107 | static const int STEPS = 36; 108 | static const int FIRST = -4; 109 | static const int LAST = 4; 110 | for( int i = FIRST; i <= LAST; ++i ) 111 | { 112 | float a = float(i) / float(STEPS) * M_PI * 2.0; 113 | float cosa = cos(a); 114 | float sina = sin(a); 115 | 116 | v1.x = MARKER_RADIUS_DIR * cosa; 117 | v1.y = MARKER_RADIUS_DIR * sina; 118 | v2.x = (MARKER_RADIUS_DIR + MARKER_WIDTH_DIR) * cosa; 119 | v2.y = (MARKER_RADIUS_DIR + MARKER_WIDTH_DIR) * sina; 120 | 121 | circle1.push_back(v1); 122 | circle2.push_back(v2); 123 | } 124 | 125 | marker.points.clear(); 126 | for( std::size_t i = 0; i < (circle1.size() - 1); ++i ) 127 | { 128 | std::size_t i1 = i; 129 | std::size_t i2 = (i + 1) % circle1.size(); 130 | 131 | marker.points.push_back(circle1[i1]); 132 | marker.points.push_back(circle2[i1]); 133 | marker.points.push_back(circle1[i2]); 134 | 135 | marker.points.push_back(circle1[i2]); 136 | marker.points.push_back(circle2[i2]); 137 | marker.points.push_back(circle2[i1]); 138 | } 139 | 140 | // Particular messages for each axis 141 | x_pos_marker_ = marker; 142 | x_pos_marker_.id = 0; 143 | x_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0); 144 | 145 | x_neg_marker_ = marker; 146 | x_neg_marker_.id = 1; 147 | x_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI); 148 | 149 | y_pos_marker_ = marker; 150 | y_pos_marker_.id = 2; 151 | y_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0.5 * M_PI); 152 | 153 | y_neg_marker_ = marker; 154 | y_neg_marker_.id = 3; 155 | y_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -0.5 * M_PI); 156 | } 157 | 158 | 159 | void VelocityLimitedMarker::createRotationalMarkers() 160 | { 161 | // Message template 162 | visualization_msgs::Marker marker; 163 | marker.header.frame_id = base_frame_; 164 | marker.header.stamp = ros::Time::now(); 165 | marker.ns = "cob_velocity_limited_marker"; 166 | marker.id = 0; 167 | marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 168 | marker.action = visualization_msgs::Marker::ADD; 169 | marker.lifetime = ros::Duration(lifetime_); 170 | marker.pose.orientation.x = 0; 171 | marker.pose.orientation.y = 0; 172 | marker.pose.orientation.z = 0; 173 | marker.pose.orientation.w = 1; 174 | marker.pose.position.x = 0; 175 | marker.pose.position.y = 0; 176 | marker.pose.position.z = z_pos_; 177 | marker.scale.x = MARKER_SCALE_ROT; 178 | marker.scale.y = MARKER_SCALE_ROT; 179 | marker.scale.z = 1.0; 180 | marker.color.r = 1.0; 181 | marker.color.g = 0.0; 182 | marker.color.b = 0.0; 183 | marker.color.a = 0.5; // adjust according to the velocity? 184 | 185 | // Create the disc like geometry for the markers 186 | std::vector circle1, circle2, circle3; 187 | geometry_msgs::Point v1, v2, v3; 188 | 189 | static const int STEPS = 48; 190 | static const int FIRST = 0; 191 | static const int LAST = 23; 192 | for( int i = FIRST; i <= LAST; ++i ) 193 | { 194 | float a = float(2*i) / float(STEPS) * M_PI * 2.0; 195 | float cosa = cos(a); 196 | float sina = sin(a); 197 | 198 | v1.x = MARKER_RADIUS_ROT * cosa; 199 | v1.y = MARKER_RADIUS_ROT * sina; 200 | v2.x = (MARKER_RADIUS_ROT + MARKER_WIDTH_ROT) * cosa; 201 | v2.y = (MARKER_RADIUS_ROT + MARKER_WIDTH_ROT) * sina; 202 | 203 | circle1.push_back(v1); 204 | circle2.push_back(v2); 205 | 206 | a = float(2*i+1) / float(STEPS) * M_PI * 2.0; 207 | cosa = cos(a); 208 | sina = sin(a); 209 | 210 | v3.x = (MARKER_RADIUS_ROT + 0.5 * MARKER_WIDTH_ROT) * cosa; 211 | v3.y = (MARKER_RADIUS_ROT + 0.5 * MARKER_WIDTH_ROT) * sina; 212 | 213 | circle3.push_back(v3); 214 | } 215 | 216 | marker.points.clear(); 217 | for( std::size_t i = 0; i < circle1.size(); ++i ) 218 | { 219 | marker.points.push_back(circle1[i]); 220 | marker.points.push_back(circle2[i]); 221 | marker.points.push_back(circle3[i]); 222 | } 223 | 224 | // Particular messages for each axis 225 | theta_pos_marker_ = marker; 226 | theta_pos_marker_.id = 4; 227 | theta_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0); 228 | 229 | theta_neg_marker_ = marker; 230 | theta_neg_marker_.id = 5; 231 | theta_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0, 0); 232 | } 233 | 234 | 235 | void VelocityLimitedMarker::interpolateColor(double velocity, std_msgs::ColorRGBA& color) 236 | { 237 | if( velocity < MIN_VELOCITY ) 238 | { 239 | color.r = color.g = color.b = color.a = 0.0f; 240 | return; 241 | } 242 | 243 | float alpha = float((velocity - MIN_VELOCITY) * VELOCITY_COEFF); 244 | alpha = (alpha > 1.0f) ? 1.0f : alpha; 245 | 246 | color.r = (1.0f - alpha) * MIN_COLOR[0] + alpha * MAX_COLOR[0]; 247 | color.g = (1.0f - alpha) * MIN_COLOR[1] + alpha * MAX_COLOR[1]; 248 | color.b = (1.0f - alpha) * MIN_COLOR[2] + alpha * MAX_COLOR[2]; 249 | color.a = (1.0f - alpha) * MIN_COLOR[3] + alpha * MAX_COLOR[3]; 250 | } 251 | 252 | 253 | void VelocityLimitedMarker::publishMarkers( double vel_x_desired, 254 | double vel_x_actual, 255 | double vel_y_desired, 256 | double vel_y_actual, 257 | double vel_theta_desired, 258 | double vel_theta_actual) 259 | { 260 | if( disabled_ ) 261 | { 262 | return; 263 | } 264 | 265 | // Publish the markers 266 | double epsilon = 0.05; 267 | 268 | // acceleration 269 | double ax,ay,atheta; 270 | ax = vel_x_actual - vx_last_; 271 | ay = vel_y_actual - vy_last_; 272 | atheta = vel_theta_actual - vtheta_last_; 273 | vx_last_ = vel_x_actual; 274 | vy_last_ = vel_y_actual; 275 | vtheta_last_ = vel_theta_actual; 276 | 277 | // for x-axis 278 | double x_vel_diff = fabs(vel_x_desired - vel_x_actual); 279 | if( x_vel_diff >= epsilon ) 280 | { 281 | // double alpha = x_vel_diff * VELOCITY_COEFF; 282 | if (vel_x_desired >= 0.0 && ax <= 0.0) 283 | { 284 | x_pos_marker_.header.stamp = ros::Time::now(); 285 | interpolateColor(x_vel_diff, x_pos_marker_.color); 286 | // x_pos_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 287 | marker_pub_.publish(x_pos_marker_); 288 | } 289 | else if (vel_x_desired <= 0.0 && ax >= 0.0) 290 | { 291 | x_neg_marker_.header.stamp = ros::Time::now(); 292 | interpolateColor(x_vel_diff, x_neg_marker_.color); 293 | // x_neg_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 294 | marker_pub_.publish(x_neg_marker_); 295 | } 296 | } 297 | 298 | // for y-axis 299 | double y_vel_diff = fabs(vel_y_desired - vel_y_actual); 300 | if( y_vel_diff >= epsilon ) 301 | { 302 | // double alpha = y_vel_diff * VELOCITY_COEFF; 303 | if (vel_y_desired >= 0.0 && ay <= 0.0) 304 | { 305 | y_pos_marker_.header.stamp = ros::Time::now(); 306 | interpolateColor(y_vel_diff, y_pos_marker_.color); 307 | // y_pos_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 308 | marker_pub_.publish(y_pos_marker_); 309 | } 310 | else if (vel_y_desired <= 0.0 && ay >= 0.0) 311 | { 312 | y_neg_marker_.header.stamp = ros::Time::now(); 313 | interpolateColor(y_vel_diff, y_neg_marker_.color); 314 | // y_neg_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 315 | marker_pub_.publish(y_neg_marker_); 316 | } 317 | } 318 | 319 | // for theta-axis 320 | double theta_vel_diff = fabs(vel_theta_desired - vel_theta_actual); 321 | if( theta_vel_diff >= epsilon ) 322 | { 323 | // double alpha = theta_vel_diff * VELOCITY_COEFF; 324 | if (vel_theta_desired >= 0.0 && atheta <= 0.0) 325 | { 326 | theta_pos_marker_.header.stamp = ros::Time::now(); 327 | interpolateColor(theta_vel_diff, theta_pos_marker_.color); 328 | // theta_pos_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 329 | marker_pub_.publish(theta_pos_marker_); 330 | } 331 | else if (vel_theta_desired <= 0.0 && atheta >= 0.0) 332 | { 333 | theta_neg_marker_.header.stamp = ros::Time::now(); 334 | interpolateColor(theta_vel_diff, theta_neg_marker_.color); 335 | // theta_neg_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 336 | marker_pub_.publish(theta_neg_marker_); 337 | } 338 | } 339 | } 340 | 341 | 342 | } 343 | 344 | -------------------------------------------------------------------------------- /cob_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.25 (2024-08-05) 6 | ------------------- 7 | 8 | 0.9.24 (2024-04-30) 9 | ------------------- 10 | * 0.8.24 11 | * update changelogs 12 | * Merge pull request `#283 `_ from fmessmer/noetic-devel 13 | cob4 eol cleanup 14 | * remove cob_control_msgs 15 | * remove cob_frame_tracker 16 | * remove cob_twist_controller 17 | * remove cob_obstacle_distance 18 | * remove cob_cartesian_controller 19 | * remove cob_control_mode_adapter 20 | * remove cob_base_controller_utils 21 | * remove cob_omni_drive_controller 22 | * remove cob_tricycle_controller 23 | * remove cob_trajectory_controller 24 | * remove cob_model_identifier 25 | * remove cob_mecanum_controller 26 | * Contributors: Felix Messmer, fmessmer 27 | 28 | 0.8.24 (2024-04-18) 29 | ------------------- 30 | * Merge pull request `#283 `_ from fmessmer/noetic-devel 31 | cob4 eol cleanup 32 | * remove cob_control_msgs 33 | * remove cob_frame_tracker 34 | * remove cob_twist_controller 35 | * remove cob_obstacle_distance 36 | * remove cob_cartesian_controller 37 | * remove cob_control_mode_adapter 38 | * remove cob_base_controller_utils 39 | * remove cob_omni_drive_controller 40 | * remove cob_tricycle_controller 41 | * remove cob_trajectory_controller 42 | * remove cob_model_identifier 43 | * remove cob_mecanum_controller 44 | * Contributors: Felix Messmer, fmessmer 45 | 46 | 0.8.23 (2024-02-20) 47 | ------------------- 48 | 49 | 0.8.22 (2023-04-29) 50 | ------------------- 51 | 52 | 0.8.21 (2023-01-04) 53 | ------------------- 54 | 55 | 0.8.20 (2022-11-17) 56 | ------------------- 57 | 58 | 0.8.19 (2022-07-29) 59 | ------------------- 60 | 61 | 0.8.18 (2022-01-12) 62 | ------------------- 63 | 64 | 0.8.17 (2021-12-23) 65 | ------------------- 66 | 67 | 0.8.16 (2021-10-19) 68 | ------------------- 69 | 70 | 0.8.15 (2021-05-17) 71 | ------------------- 72 | 73 | 0.8.14 (2021-05-10) 74 | ------------------- 75 | 76 | 0.8.13 (2021-04-06) 77 | ------------------- 78 | 79 | 0.8.12 (2020-10-21) 80 | ------------------- 81 | * Merge pull request `#243 `_ from fmessmer/test_noetic 82 | test noetic 83 | * Bump CMake version to avoid CMP0048 warning 84 | * Contributors: Felix Messmer, fmessmer 85 | 86 | 0.8.11 (2020-03-21) 87 | ------------------- 88 | 89 | 0.8.10 (2020-03-18) 90 | ------------------- 91 | * Merge pull request `#230 `_ from ipa-jba/feature/melodic/raw-mini 92 | [melodic] add a mecanum controller 93 | * add mecanum controller to cob_control 94 | * Contributors: Felix Messmer, Jannik Abbenseth 95 | 96 | 0.8.1 (2019-11-07) 97 | ------------------ 98 | * Merge pull request `#213 `_ from fmessmer/emulator 99 | [melodic] add cob_hardware_emulation 100 | * add cob_hardware_emulation to meta package 101 | 102 | 0.8.0 (2019-08-09) 103 | ------------------ 104 | 105 | 0.7.8 (2019-08-09) 106 | ------------------ 107 | 108 | 0.7.7 (2019-08-06) 109 | ------------------ 110 | 111 | 0.7.6 (2019-06-07) 112 | ------------------ 113 | 114 | 0.7.5 (2019-05-20) 115 | ------------------ 116 | 117 | 0.7.4 (2019-04-05) 118 | ------------------ 119 | 120 | 0.7.3 (2019-03-14) 121 | ------------------ 122 | * Merge pull request `#190 `_ from fmessmer/new_tricycle_controller_kinetic 123 | new tricycle controller kinetic 124 | * introduce cob_base_controller_utils package 125 | * initial tricycle_controller, fake odom 126 | * Contributors: Felix Messmer, fmessmer, ipa-fxm 127 | 128 | 0.7.2 (2018-07-21) 129 | ------------------ 130 | * update maintainer 131 | * Contributors: fmessmer 132 | 133 | 0.7.1 (2018-01-07) 134 | ------------------ 135 | * Merge remote-tracking branch 'origin/kinetic_release_candidate' into kinetic_dev 136 | * Merge pull request `#169 `_ from ipa-fxm/kinetic_updates_indigo 137 | Kinetic updates indigo 138 | * Merge branch 'indigo_dev' of github.com:ipa320/cob_control into kinetic_dev 139 | Conflicts: 140 | .travis.yml 141 | * Merge pull request `#164 `_ from ipa-fxm/update_maintainer 142 | update maintainer 143 | * update maintainer 144 | * Merge pull request `#159 `_ from ipa-fxm/APACHE_license 145 | use license apache 2.0 146 | * use license apache 2.0 147 | * Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk 148 | 149 | 0.7.0 (2017-07-18) 150 | ------------------ 151 | 152 | 0.6.15 (2017-07-18) 153 | ------------------- 154 | * manually fix changelog 155 | * Contributors: ipa-fxm 156 | 157 | 0.6.14 (2016-10-10) 158 | ------------------- 159 | 160 | 0.6.13 (2016-10-10) 161 | ------------------- 162 | 163 | 0.6.12 (2016-10-10) 164 | ------------------- 165 | 166 | 0.6.11 (2016-04-01) 167 | ------------------- 168 | * introducing cob_control_msgs 169 | * add cob_undercarriage_ctrl_node to meta-package 170 | * Contributors: Felix Messmer, ipa-fxm 171 | 172 | 0.6.10 (2015-08-31) 173 | ------------------- 174 | 175 | 0.6.9 (2015-08-25) 176 | ------------------ 177 | * migrate to package format 2 178 | * sort dependencies 179 | * review dependencies 180 | * add missing run_depends to meta-package 181 | * Contributors: ipa-fxm 182 | 183 | 0.6.8 (2015-06-17) 184 | ------------------ 185 | * merge with release candidate 186 | * package renaming: cob_path_broadcaster -> cob_cartesian_controller 187 | * Contributors: ipa-fxm 188 | 189 | 0.6.7 (2015-06-17) 190 | ------------------ 191 | * update meta-package 192 | * Contributors: ipa-fxm 193 | 194 | 0.6.6 (2014-12-18) 195 | ------------------ 196 | 197 | 0.6.5 (2014-12-18) 198 | ------------------ 199 | 200 | 0.6.4 (2014-12-16) 201 | ------------------ 202 | 203 | 0.6.3 (2014-12-16) 204 | ------------------ 205 | 206 | 0.6.2 (2014-12-15) 207 | ------------------ 208 | 209 | 0.6.1 (2014-09-22) 210 | ------------------ 211 | 212 | 0.5.4 (2014-08-26) 213 | ------------------ 214 | * added meta-package 215 | * Contributors: ipa-fxm 216 | -------------------------------------------------------------------------------- /cob_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_control) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /cob_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_control 3 | 0.9.25 4 | cob_control meta-package 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_control 9 | 10 | 11 | Felix Messmer 12 | 13 | catkin 14 | 15 | cob_base_velocity_smoother 16 | cob_collision_velocity_filter 17 | cob_footprint_observer 18 | cob_hardware_emulation 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /cob_footprint_observer/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_footprint_observer 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.25 (2024-08-05) 6 | ------------------- 7 | 8 | 0.9.24 (2024-04-30) 9 | ------------------- 10 | * 0.8.24 11 | * update changelogs 12 | * Contributors: fmessmer 13 | 14 | 0.8.24 (2024-04-18) 15 | ------------------- 16 | 17 | 0.8.23 (2024-02-20) 18 | ------------------- 19 | 20 | 0.8.22 (2023-04-29) 21 | ------------------- 22 | 23 | 0.8.21 (2023-01-04) 24 | ------------------- 25 | 26 | 0.8.20 (2022-11-17) 27 | ------------------- 28 | 29 | 0.8.19 (2022-07-29) 30 | ------------------- 31 | 32 | 0.8.18 (2022-01-12) 33 | ------------------- 34 | 35 | 0.8.17 (2021-12-23) 36 | ------------------- 37 | 38 | 0.8.16 (2021-10-19) 39 | ------------------- 40 | 41 | 0.8.15 (2021-05-17) 42 | ------------------- 43 | 44 | 0.8.14 (2021-05-10) 45 | ------------------- 46 | 47 | 0.8.13 (2021-04-06) 48 | ------------------- 49 | * Merge pull request `#247 `_ from fmessmer/fix_catkin_lint_melodic 50 | [melodic] fix catkin_lint 51 | * fix catkin_lint 52 | * Contributors: Felix Messmer, fmessmer 53 | 54 | 0.8.12 (2020-10-21) 55 | ------------------- 56 | * Merge pull request `#243 `_ from fmessmer/test_noetic 57 | test noetic 58 | * Bump CMake version to avoid CMP0048 warning 59 | * Contributors: Felix Messmer, fmessmer 60 | 61 | 0.8.11 (2020-03-21) 62 | ------------------- 63 | 64 | 0.8.10 (2020-03-18) 65 | ------------------- 66 | * Merge pull request `#226 `_ from fmessmer/ci_updates_melodic 67 | [travis] ci updates - melodic 68 | * catkin_lint fixes 69 | * Contributors: Felix Messmer, fmessmer 70 | 71 | 0.8.1 (2019-11-07) 72 | ------------------ 73 | 74 | 0.8.0 (2019-08-09) 75 | ------------------ 76 | 77 | 0.7.8 (2019-08-09) 78 | ------------------ 79 | 80 | 0.7.7 (2019-08-06) 81 | ------------------ 82 | 83 | 0.7.6 (2019-06-07) 84 | ------------------ 85 | 86 | 0.7.5 (2019-05-20) 87 | ------------------ 88 | 89 | 0.7.4 (2019-04-05) 90 | ------------------ 91 | 92 | 0.7.3 (2019-03-14) 93 | ------------------ 94 | 95 | 0.7.2 (2018-07-21) 96 | ------------------ 97 | 98 | 0.7.1 (2018-01-07) 99 | ------------------ 100 | * Merge remote-tracking branch 'origin/kinetic_release_candidate' into kinetic_dev 101 | * Merge pull request `#169 `_ from ipa-fxm/kinetic_updates_indigo 102 | Kinetic updates indigo 103 | * Merge branch 'indigo_dev' of github.com:ipa320/cob_control into kinetic_dev 104 | Conflicts: 105 | .travis.yml 106 | * Merge pull request `#164 `_ from ipa-fxm/update_maintainer 107 | update maintainer 108 | * update maintainer 109 | * Merge pull request `#159 `_ from ipa-fxm/APACHE_license 110 | use license apache 2.0 111 | * use license apache 2.0 112 | * Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk 113 | 114 | 0.7.0 (2017-07-18) 115 | ------------------ 116 | 117 | 0.6.15 (2017-07-18) 118 | ------------------- 119 | * manually fix changelog 120 | * Contributors: ipa-fxm 121 | 122 | 0.6.14 (2016-10-10) 123 | ------------------- 124 | 125 | 0.6.13 (2016-10-10) 126 | ------------------- 127 | 128 | 0.6.12 (2016-10-10) 129 | ------------------- 130 | 131 | 0.6.11 (2016-04-01) 132 | ------------------- 133 | 134 | 0.6.10 (2015-08-31) 135 | ------------------- 136 | 137 | 0.6.9 (2015-08-25) 138 | ------------------ 139 | * boost revision 140 | * do not install headers in executable-only packages 141 | * explicit dependency to boost 142 | * explicit dependency to boost 143 | * remove trailing whitespaces 144 | * migrate to package format 2 145 | * sort dependencies 146 | * review dependencies 147 | * Contributors: ipa-fxm 148 | 149 | 0.6.8 (2015-06-17) 150 | ------------------ 151 | 152 | 0.6.7 (2015-06-17) 153 | ------------------ 154 | * beautify CMakeLists 155 | * Contributors: ipa-fxm 156 | 157 | 0.6.6 (2014-12-18) 158 | ------------------ 159 | 160 | 0.6.5 (2014-12-18) 161 | ------------------ 162 | 163 | 0.6.4 (2014-12-16) 164 | ------------------ 165 | 166 | 0.6.3 (2014-12-16) 167 | ------------------ 168 | 169 | 0.6.2 (2014-12-15) 170 | ------------------ 171 | 172 | 0.6.1 (2014-09-22) 173 | ------------------ 174 | 175 | 0.5.3 (2014-03-31) 176 | ------------------ 177 | * install tags 178 | * Contributors: ipa-fxm 179 | 180 | 0.5.2 (2014-03-20) 181 | ------------------ 182 | 183 | 0.5.1 (2014-03-20) 184 | ------------------ 185 | * add definitions to get rid of compiler warning 186 | * cob_footprint_observer: introduce epsilon for adjustment and get rid of annoying warning message 187 | * changes for hydro 188 | * cleaned up CMakeLists and added install directives 189 | * further modifications for catkin, now everything is compiling and linking 190 | * futher include and linkpath modifications 191 | * add message dependencies 192 | * compiling but still some linker errors 193 | * Second catkinization push 194 | * First catkinization, still need to update some CMakeLists.txt 195 | * remove param farthest_frame from footprint observer 196 | * change rate of footprint_observer 197 | * changes for fuerte compatibility 198 | * remove unnecessary roslaunch check 199 | * remove unnecessary roslaunch check 200 | * change documentation 201 | * merge launch files and move configs to same folder 202 | * put GetFootprint service into footprint observer 203 | instead of SetFootprint service in collision velocity filter 204 | fix namespace problems 205 | * fix namespace problems 206 | * moved safe base movement to driver stack 207 | * Contributors: Alexander Bubeck, Frederik Hegger, abubeck, ipa-fmw, ipa-mig 208 | -------------------------------------------------------------------------------- /cob_footprint_observer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_footprint_observer) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation roscpp std_msgs tf) 5 | 6 | find_package(Boost REQUIRED) 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX) 9 | add_definitions(-std=gnu++0x) 10 | else() 11 | add_definitions(-std=c++0x) 12 | endif() 13 | 14 | ### Message Generation ### 15 | add_service_files( 16 | FILES 17 | GetFootprint.srv 18 | ) 19 | 20 | generate_messages( 21 | DEPENDENCIES geometry_msgs std_msgs 22 | ) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS geometry_msgs message_runtime roscpp std_msgs tf 26 | ) 27 | 28 | ### BUILD ### 29 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 30 | 31 | add_executable(footprint_observer src/${PROJECT_NAME}.cpp) 32 | add_dependencies(footprint_observer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 33 | target_link_libraries(footprint_observer ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 34 | 35 | ### INSTALL ### 36 | install(TARGETS footprint_observer 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | -------------------------------------------------------------------------------- /cob_footprint_observer/include/cob_footprint_observer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COB_FOOTPRINT_OBSERVER_H 19 | #define COB_FOOTPRINT_OBSERVER_H 20 | 21 | //################## 22 | //#### includes #### 23 | 24 | // ROS includes 25 | #include 26 | #include 27 | 28 | // message includes 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | /// 41 | /// @class FootprintObserver 42 | /// @brief checks the footprint of care-o-bot and advertises a service to get the adjusted footprint 43 | /// 44 | /// 45 | class FootprintObserver 46 | { 47 | public: 48 | /// 49 | /// @brief constructor 50 | /// 51 | FootprintObserver(); 52 | /// 53 | /// @brief destructor 54 | /// 55 | ~FootprintObserver(); 56 | 57 | /// 58 | /// @brief checks the footprint of the robot if it needs to be enlarged due to arm or tray 59 | /// 60 | void checkFootprint(); 61 | 62 | /// 63 | /// @brief callback for GetFootprint service 64 | /// @param req - request message to service 65 | /// @param resp - response message from service 66 | /// @return service call successfull 67 | /// 68 | bool getFootprintCB(cob_footprint_observer::GetFootprint::Request &req, cob_footprint_observer::GetFootprint::Response &resp); 69 | 70 | // public members 71 | ros::NodeHandle nh_; 72 | ros::Publisher topic_pub_footprint_; 73 | ros::ServiceServer srv_get_footprint_; 74 | 75 | private: 76 | /// 77 | /// @brief loads the robot footprint from the costmap node 78 | /// @param node - costmap node to check for footprint parameter 79 | /// @return points of a polygon specifying the footprint 80 | /// 81 | std::vector loadRobotFootprint(ros::NodeHandle node); 82 | 83 | /// 84 | /// @brief publishes the adjusted footprint as geometry_msgs::StampedPolygon message 85 | /// 86 | void publishFootprint(); 87 | 88 | /// 89 | /// @brief computes the sign of x 90 | /// @param x - number 91 | /// @return sign of x 92 | /// 93 | double sign(double x); 94 | 95 | // private members 96 | std::vector robot_footprint_; 97 | double epsilon_; 98 | double footprint_front_initial_, footprint_rear_initial_, footprint_left_initial_, footprint_right_initial_; 99 | double footprint_front_, footprint_rear_, footprint_left_, footprint_right_; 100 | tf::TransformListener tf_listener_; 101 | std::string frames_to_check_; 102 | std::string robot_base_frame_; 103 | 104 | pthread_mutex_t m_mutex; 105 | 106 | ros::Time last_tf_missing_; 107 | unsigned int times_warned_; 108 | }; 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /cob_footprint_observer/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b cob_footprint_observer is adjusting the footprint of cob_collision_velocity_filter based on current setup of the robot. 6 | 7 | The cob_footprint_observer node reads an initial footprint from a source to be specified (needs to be the same as for cob_collision_velocity_filter). 8 | It observes the setup of the robot including arm and tray and inflates the footprint to contain the arm and the tray. 9 | It then provides a GetFootprint service and publishes the adjusted footprint to a topic. 10 | 11 | */ 12 | -------------------------------------------------------------------------------- /cob_footprint_observer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_footprint_observer 3 | 0.9.25 4 | The cob_footprint_observer package adjusts the footprint of the robot based on the setup (e.g. arm and/or tray). 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_footprint_observer 9 | 10 | 11 | Felipe Garcia Lopez 12 | Matthias Gruhler 13 | 14 | catkin 15 | 16 | message_generation 17 | message_runtime 18 | 19 | boost 20 | geometry_msgs 21 | roscpp 22 | std_msgs 23 | tf 24 | 25 | 26 | -------------------------------------------------------------------------------- /cob_footprint_observer/src/cob_footprint_observer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | 20 | // Constructor 21 | FootprintObserver::FootprintObserver() : 22 | times_warned_(0) 23 | { 24 | nh_ = ros::NodeHandle("~"); 25 | 26 | m_mutex = PTHREAD_MUTEX_INITIALIZER; 27 | 28 | // publish footprint 29 | topic_pub_footprint_ = nh_.advertise("adjusted_footprint",1); 30 | 31 | // advertise service 32 | srv_get_footprint_ = nh_.advertiseService("/get_footprint", &FootprintObserver::getFootprintCB, this); 33 | 34 | // read footprint_source parameter 35 | std::string footprint_source; 36 | if(!nh_.hasParam("footprint_source")) ROS_WARN("Checking default location (/local_costmap_node/costmap) for initial footprint parameter."); 37 | nh_.param("footprint_source", footprint_source, std::string("/local_costmap_node/costmap")); 38 | 39 | // node handle to get footprint from parameter server 40 | ros::NodeHandle footprint_source_nh_(footprint_source); 41 | 42 | // load the robot footprint from the parameter server if its available in the local costmap namespace 43 | robot_footprint_ = loadRobotFootprint(footprint_source_nh_); 44 | if(robot_footprint_.size() > 4) 45 | ROS_WARN("You have set more than 4 points as robot_footprint, cob_footprint_observer can deal only with rectangular footprints so far!"); 46 | 47 | // get parameter sepcifying minimal changes of footprint that are accepted. (smaller changes are neglected) 48 | if(!nh_.hasParam("epsilon")) 49 | ROS_WARN("No epsilon value specified. Changes in footprint smaller than epsilon are neglected. Using default [0.01m]!"); 50 | nh_.param("epsilon", epsilon_, 0.01); 51 | 52 | // get the frames for which to check the footprint 53 | if(!nh_.hasParam("frames_to_check")) ROS_WARN("No frames to check for footprint observer. Only using initial footprint!"); 54 | nh_.param("frames_to_check", frames_to_check_, std::string("")); 55 | 56 | if(!nh_.hasParam("robot_base_frame")) ROS_WARN("No parameter robot_base_frame on parameter server. Using default [/base_link]."); 57 | nh_.param("robot_base_frame", robot_base_frame_, std::string("/base_link")); 58 | 59 | last_tf_missing_ = ros::Time::now(); 60 | } 61 | 62 | // Destructor 63 | FootprintObserver::~FootprintObserver() 64 | {} 65 | 66 | // GetFootprint Service callback 67 | bool FootprintObserver::getFootprintCB(cob_footprint_observer::GetFootprint::Request &req, cob_footprint_observer::GetFootprint::Response &resp) 68 | { 69 | // create Polygon message for service call 70 | geometry_msgs::PolygonStamped footprint_poly; 71 | footprint_poly.header.frame_id = robot_base_frame_; 72 | footprint_poly.header.stamp = ros::Time::now(); 73 | 74 | footprint_poly.polygon.points.resize(robot_footprint_.size()); 75 | for(unsigned int i=0; i FootprintObserver::loadRobotFootprint(ros::NodeHandle node){ 89 | std::vector footprint; 90 | geometry_msgs::Point pt; 91 | double padding; 92 | 93 | std::string padding_param, footprint_param; 94 | if(!node.searchParam("footprint_padding", padding_param)) 95 | padding = 0.01; 96 | else 97 | node.param(padding_param, padding, 0.01); 98 | 99 | //grab the footprint from the parameter server if possible 100 | XmlRpc::XmlRpcValue footprint_list; 101 | std::string footprint_string; 102 | std::vector footstring_list; 103 | if(node.searchParam("footprint", footprint_param)){ 104 | node.getParam(footprint_param, footprint_list); 105 | if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) { 106 | footprint_string = std::string(footprint_list); 107 | 108 | //if there's just an empty footprint up there, return 109 | if(footprint_string == "[]" || footprint_string == "") 110 | return footprint; 111 | 112 | boost::erase_all(footprint_string, " "); 113 | 114 | boost::char_separator sep("[]"); 115 | boost::tokenizer > tokens(footprint_string, sep); 116 | footstring_list = std::vector(tokens.begin(), tokens.end()); 117 | } 118 | //make sure we have a list of lists 119 | if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) 120 | && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){ 121 | ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", 122 | footprint_param.c_str(), std::string(footprint_list).c_str()); 123 | throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); 124 | } 125 | 126 | if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) { 127 | for(int i = 0; i < footprint_list.size(); ++i){ 128 | //make sure we have a list of lists of size 2 129 | XmlRpc::XmlRpcValue point = footprint_list[i]; 130 | if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){ 131 | ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); 132 | throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); 133 | } 134 | 135 | //make sure that the value we're looking at is either a double or an int 136 | if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){ 137 | ROS_FATAL("Values in the footprint specification must be numbers"); 138 | throw std::runtime_error("Values in the footprint specification must be numbers"); 139 | } 140 | pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]); 141 | pt.x += sign(pt.x) * padding; 142 | 143 | //make sure that the value we're looking at is either a double or an int 144 | if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){ 145 | ROS_FATAL("Values in the footprint specification must be numbers"); 146 | throw std::runtime_error("Values in the footprint specification must be numbers"); 147 | } 148 | pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]); 149 | pt.y += sign(pt.y) * padding; 150 | 151 | footprint.push_back(pt); 152 | 153 | node.deleteParam(footprint_param); 154 | std::ostringstream oss; 155 | bool first = true; 156 | BOOST_FOREACH(geometry_msgs::Point p, footprint) { 157 | if(first) { 158 | oss << "[[" << p.x << "," << p.y << "]"; 159 | first = false; 160 | } 161 | else { 162 | oss << ",[" << p.x << "," << p.y << "]"; 163 | } 164 | } 165 | oss << "]"; 166 | node.setParam(footprint_param, oss.str().c_str()); 167 | node.setParam("footprint", oss.str().c_str()); 168 | } 169 | } 170 | 171 | else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) { 172 | std::vector footprint_spec; 173 | bool valid_foot = true; 174 | BOOST_FOREACH(std::string t, footstring_list) { 175 | if( t != "," ) { 176 | boost::erase_all(t, " "); 177 | boost::char_separator pt_sep(","); 178 | boost::tokenizer > pt_tokens(t, pt_sep); 179 | std::vector point(pt_tokens.begin(), pt_tokens.end()); 180 | 181 | if(point.size() != 2) { 182 | ROS_WARN("Each point must have exactly 2 coordinates"); 183 | valid_foot = false; 184 | break; 185 | } 186 | 187 | std::vectortmp_pt; 188 | BOOST_FOREACH(std::string p, point) { 189 | std::istringstream iss(p); 190 | double temp; 191 | if(iss >> temp) { 192 | tmp_pt.push_back(temp); 193 | } 194 | else { 195 | ROS_WARN("Each coordinate must convert to a double."); 196 | valid_foot = false; 197 | break; 198 | } 199 | } 200 | if(!valid_foot) 201 | break; 202 | 203 | geometry_msgs::Point pt; 204 | pt.x = tmp_pt[0]; 205 | pt.y = tmp_pt[1]; 206 | 207 | footprint_spec.push_back(pt); 208 | } 209 | } 210 | if (valid_foot) { 211 | footprint = footprint_spec; 212 | node.setParam("footprint", footprint_string); 213 | } 214 | else { 215 | ROS_FATAL("This footprint is not vaid it must be specified as a list of lists with at least 3 points, you specified %s", footprint_string.c_str()); 216 | throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); 217 | } 218 | } 219 | } 220 | 221 | footprint_right_ = 0.0f; footprint_left_ = 0.0f; footprint_front_ = 0.0f; footprint_rear_ = 0.0f; 222 | //extract rectangular borders for simplifying: 223 | for(unsigned int i=0; i footprint_front_) footprint_front_ = footprint[i].x; 225 | if(footprint[i].x < footprint_rear_) footprint_rear_ = footprint[i].x; 226 | if(footprint[i].y > footprint_left_) footprint_left_ = footprint[i].y; 227 | if(footprint[i].y < footprint_right_) footprint_right_ = footprint[i].y; 228 | } 229 | ROS_DEBUG("Extracted rectangular footprint for cob_footprint_observer: Front: %f, Rear %f, Left: %f, Right %f", 230 | footprint_front_, footprint_rear_, footprint_left_, footprint_right_); 231 | 232 | if ( fabs(footprint_front_ - footprint_rear_) == 0.0 || fabs(footprint_right_ - footprint_left_) == 0.0){ 233 | ROS_WARN("Footprint has no physical dimension!"); 234 | } 235 | 236 | // save initial footprint 237 | footprint_front_initial_ = footprint_front_; 238 | footprint_rear_initial_ = footprint_rear_; 239 | footprint_left_initial_ = footprint_left_; 240 | footprint_right_initial_ = footprint_right_; 241 | 242 | return footprint; 243 | } 244 | 245 | // checks if footprint has to be adjusted and does so if necessary 246 | void FootprintObserver::checkFootprint(){ 247 | // check each frame 248 | std::string frame; 249 | std::stringstream ss; 250 | ss << frames_to_check_; 251 | 252 | double x_rear, x_front, y_left, y_right; 253 | x_front = footprint_front_initial_; 254 | x_rear = footprint_rear_initial_; 255 | y_left = footprint_left_initial_; 256 | y_right = footprint_right_initial_; 257 | 258 | bool missing_frame_exists = false; 259 | while(ss >> frame){ 260 | // get transform between robot base frame and frame 261 | if(tf_listener_.canTransform(robot_base_frame_, frame, ros::Time(0))) { 262 | tf::StampedTransform transform; 263 | tf_listener_.lookupTransform(robot_base_frame_, frame, ros::Time(0), transform); 264 | 265 | tf::Vector3 frame_position = transform.getOrigin(); 266 | 267 | // check if frame position is outside of current footprint 268 | if(frame_position.x() > x_front) x_front = frame_position.x(); 269 | if(frame_position.x() < x_rear) x_rear = frame_position.x(); 270 | if(frame_position.y() > y_left) y_left = frame_position.y(); 271 | if(frame_position.y() < y_right) y_right = frame_position.y(); 272 | } else if ( (ros::Time::now() - last_tf_missing_).toSec() > 10.0 273 | && times_warned_ < 3 ) { 274 | ++times_warned_; 275 | missing_frame_exists = true; 276 | ROS_WARN("Footprint Observer: Transformation for %s not available! Frame %s not considered in adjusted footprint!", 277 | frame.c_str(), frame.c_str()); 278 | } 279 | } 280 | if (missing_frame_exists) 281 | last_tf_missing_ = ros::Time::now(); 282 | 283 | // check if footprint has changed 284 | if ( fabs( footprint_front_ - x_front ) > epsilon_ 285 | || fabs( footprint_rear_ - x_rear ) > epsilon_ 286 | || fabs( footprint_left_ - y_left ) > epsilon_ 287 | || fabs( footprint_right_ - y_right ) > epsilon_ ) 288 | { 289 | pthread_mutex_lock(&m_mutex); 290 | // adjust footprint 291 | footprint_front_ = x_front; 292 | footprint_rear_ = x_rear; 293 | footprint_left_ = y_left; 294 | footprint_right_ = y_right; 295 | pthread_mutex_unlock(&m_mutex); 296 | 297 | // create new footprint vector 298 | geometry_msgs::Point point; 299 | std::vector points; 300 | 301 | point.x = footprint_front_; 302 | point.y = footprint_left_; 303 | point.z = 0; 304 | points.push_back(point); 305 | point.y = footprint_right_; 306 | points.push_back(point); 307 | point.x = footprint_rear_; 308 | points.push_back(point); 309 | point.y = footprint_left_; 310 | points.push_back(point); 311 | 312 | pthread_mutex_lock(&m_mutex); 313 | robot_footprint_ = points; 314 | pthread_mutex_unlock(&m_mutex); 315 | 316 | // publish the adjusted footprint 317 | publishFootprint(); 318 | } 319 | 320 | } 321 | 322 | // publishes the adjusted footprint 323 | void FootprintObserver::publishFootprint(){ 324 | 325 | // create Polygon message 326 | geometry_msgs::PolygonStamped footprint_poly; 327 | footprint_poly.header.frame_id = robot_base_frame_; 328 | footprint_poly.header.stamp = ros::Time::now(); 329 | 330 | footprint_poly.polygon.points.resize(robot_footprint_.size()); 331 | for(unsigned int i=0; i= 0.0f) return 1.0f; 345 | else return -1.0f; 346 | } 347 | 348 | //####################### 349 | //#### main programm #### 350 | int main(int argc, char** argv) 351 | { 352 | // initialize ROS, specify name of node 353 | ros::init(argc,argv,"footprint_observer"); 354 | 355 | // create observer 356 | FootprintObserver footprintObserver; 357 | 358 | // run footprint observer periodically until node has been shut down 359 | ros::Rate loop_rate(30); // Hz 360 | while(footprintObserver.nh_.ok()){ 361 | footprintObserver.checkFootprint(); 362 | ros::spinOnce(); 363 | loop_rate.sleep(); 364 | } 365 | 366 | return 0; 367 | }; 368 | -------------------------------------------------------------------------------- /cob_footprint_observer/srv/GetFootprint.srv: -------------------------------------------------------------------------------- 1 | --- 2 | geometry_msgs/PolygonStamped footprint 3 | std_msgs/Bool success 4 | -------------------------------------------------------------------------------- /cob_hardware_emulation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_hardware_emulation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.25 (2024-08-05) 6 | ------------------- 7 | 8 | 0.9.24 (2024-04-30) 9 | ------------------- 10 | * 0.8.24 11 | * update changelogs 12 | * Contributors: fmessmer 13 | 14 | 0.8.24 (2024-04-18) 15 | ------------------- 16 | 17 | 0.8.23 (2024-02-20) 18 | ------------------- 19 | * Merge pull request `#279 `_ from fmessmer/fix/emulation 20 | reliable start 21 | * reliable start 22 | * Contributors: Felix Messmer, fmessmer 23 | 24 | 0.8.22 (2023-04-29) 25 | ------------------- 26 | * Merge pull request `#276 `_ from fmessmer/fix/emulation_odom_laser 27 | publish odom_laser tf with current time 28 | * publish odom_laser tf with current time 29 | * Merge pull request `#275 `_ from FrederikPlahl/fix/odom_laser 30 | Fix emulation bug in transform odom -> base_footprint 31 | * remove obsolete import 32 | * Change frequency 33 | * Fix tf publishing bug 34 | * Merge pull request `#274 `_ from HannesBachter/fix/rename_scan_odom 35 | rename scan odom to /scan_odom_node/scan_odom/odometry 36 | * rename scan odom to /scan_odom_node/scan_odom/odometry 37 | * Contributors: Felix Messmer, FrederikPlahl, HannesBachter, fmessmer 38 | 39 | 0.8.21 (2023-01-04) 40 | ------------------- 41 | 42 | 0.8.20 (2022-11-17) 43 | ------------------- 44 | 45 | 0.8.19 (2022-07-29) 46 | ------------------- 47 | 48 | 0.8.18 (2022-01-12) 49 | ------------------- 50 | * Merge pull request `#266 `_ from HannesBachter/fix/emulation 51 | fix initialpose and add laser odom in emulation 52 | * fix some typos 53 | * add emulation_odom_laser to CMakeLists 54 | * generate odom laser from odom (not from twist) 55 | * publish odom->base in emulation_odom_laser 56 | * add emulation for laser odom 57 | * harmonize variables (add leading underscore) 58 | * fix initialpose handling in emulation 59 | * Contributors: Felix Messmer, HannesBachter 60 | 61 | 0.8.17 (2021-12-23) 62 | ------------------- 63 | * Merge pull request `#265 `_ from fmessmer/fix/emulation_argparse 64 | fixup emulation argparse 65 | * fixup emulation argparse 66 | * Merge pull request `#264 `_ from HannesBachter/feature/laser_odometry 67 | [no odom] make odom frame configurable 68 | * make odom frame configurable 69 | * Merge pull request `#263 `_ from fmessmer/emulation_nav 70 | separate emulation_nav from emulation_base 71 | * separate emulation_nav from emulation_base 72 | * Contributors: Felix Messmer, HannesBachter, fmessmer 73 | 74 | 0.8.16 (2021-10-19) 75 | ------------------- 76 | 77 | 0.8.15 (2021-05-17) 78 | ------------------- 79 | * Merge pull request `#257 `_ from fmessmer/fix_emulated_fjt_melodic 80 | [melodic] fix emulated fjt 81 | * never skip last traj point 82 | * Contributors: Felix Messmer, fmessmer 83 | 84 | 0.8.14 (2021-05-10) 85 | ------------------- 86 | 87 | 0.8.13 (2021-04-06) 88 | ------------------- 89 | * Merge pull request `#253 `_ from floweisshardt/feature/emulator_move_base_melodic 90 | melodic/noetic: add optional move_base interface to base emulator 91 | * add optional move_base interface to base emulator 92 | * Merge pull request `#249 `_ from fmessmer/fix/fjt_emulation_melodic 93 | [melodic] fix fjt emulation 94 | * use timer for joint_states 95 | * skip trajectory points faster than sample_rate 96 | * introduce sample_rate_hz parameter 97 | * Merge pull request `#247 `_ from fmessmer/fix_catkin_lint_melodic 98 | [melodic] fix catkin_lint 99 | * fix catkin_lint 100 | * Contributors: Felix Messmer, Florian Weisshardt, floweisshardt, fmessmer 101 | 102 | 0.8.12 (2020-10-21) 103 | ------------------- 104 | * Merge pull request `#243 `_ from fmessmer/test_noetic 105 | test noetic 106 | * Bump CMake version to avoid CMP0048 warning 107 | * Merge pull request `#244 `_ from lindemeier/bugfix/length-bug 108 | [melodic] bugfix/length-bug 109 | * sets correct length and checks for length 110 | * Merge pull request `#240 `_ from benmaidel/feature/base_emu_reset_odom_melodic 111 | reset odometry service for base emulation 112 | * reset odometry service for base emulation 113 | * Contributors: Benjamin Maidel, Felix Messmer, fmessmer, tsl 114 | 115 | 0.8.11 (2020-03-21) 116 | ------------------- 117 | 118 | 0.8.10 (2020-03-18) 119 | ------------------- 120 | * Merge pull request `#236 `_ from fmessmer/melodic/emulation_clock_cpp 121 | [melodic] add emulation_clock publisher - cpp variant 122 | * fix boost timer + use dt_ms 123 | * use boost timer 124 | * configurable emulation dt 125 | * add emulation_clock publisher - cpp variant 126 | * Merge pull request `#234 `_ from fmessmer/emulation_custom_clock_melodic 127 | [melodic] add emulation_clock publisher 128 | * implement emulation_clock with timer callback 129 | * add emulation_clock publisher 130 | * Merge pull request `#228 `_ from fmessmer/feature/python3_compatibility_melodic 131 | [ci_updates] pylint + Python3 compatibility - melodic 132 | * fix pylint errors 133 | * python3 compatibility via 2to3 134 | * Merge pull request `#226 `_ from fmessmer/ci_updates_melodic 135 | [travis] ci updates - melodic 136 | * catkin_lint fixes 137 | * remove outdated script 138 | * Contributors: Felix Messmer, Loy van Beek, fmessmer 139 | 140 | 0.8.1 (2019-11-07) 141 | ------------------ 142 | * Merge branch 'kinetic_dev' of github.com:ipa320/cob_control into melodic_dev 143 | * add CHANGELOG for cob_hardware_emulation 144 | * Merge pull request `#221 `_ from fmessmer/post_vacation_qa 145 | [WIP] post vacation qa 146 | * fix missing dependencies 147 | * Merge pull request `#218 `_ from floweisshardt/fix/emulator 148 | catch zero division if two trajectory points have the same time_from_start 149 | * catch zero division if two trajectory points have the same time_from_start 150 | * Merge pull request `#217 `_ from floweisshardt/emulator 151 | initialpose from yaml file for base emulator 152 | * initialpose from yaml file for base emulator 153 | * Merge pull request `#216 `_ from benmaidel/feature/base_emulation_initialpose 154 | add initialpose to emulation_base 155 | * Merge pull request `#215 `_ from lindemeier/feature/1238-joint-trajectory-controller-emulator-linear-interpolation 156 | Feature/1238 joint trajectory controller emulator linear interpolation 157 | * 1238 Setting joint velocity and effort to zero after eaching final trajectory point 158 | * reset odom on initialpose 159 | * syntax fixes 160 | * 1238 added service reset 161 | * 1238 joint velocities added 162 | * 1238 adding preempt polling 163 | 1238 readme adjusted and small improvements 164 | * add initialpose to emulation_base 165 | * 1238 linear interpolation of joint states sampling the given trajectory 166 | 1238 lerping start and goal works 167 | 1238 Fixed error in lerp 168 | 1238 using only local time segments for computing the interpolation weight 169 | 1238 added more comments 170 | * 1238 replacing timer with rospy loop rate 171 | + publishing joint_states with 10Hz controlled by loop rate instead of timer 172 | * Merge pull request `#214 `_ from floweisshardt/feature/emulator_base 173 | emulator base can be used with real navigation 174 | * migrate tf to tf2 175 | * emulator base can be used with real navigation 176 | * Merge pull request `#212 `_ from floweisshardt/emulator 177 | initial version of move_base emulator 178 | * review comments 179 | * initial version of move_base emulator 180 | * Merge pull request `#211 `_ from ipa320/emulator 181 | add hardware_emulation package 182 | * add hardware_emulation package 183 | * Contributors: Benjamin Maidel, Felix Messmer, Florian Weisshardt, Thomas Lindemeier, floweisshardt, fmessmer 184 | 185 | 0.8.0 (2019-08-09) 186 | ------------------ 187 | 188 | 0.7.8 (2019-08-09) 189 | ------------------ 190 | 191 | 0.7.7 (2019-08-06) 192 | ------------------ 193 | 194 | 0.7.6 (2019-06-07) 195 | ------------------ 196 | 197 | 0.7.5 (2019-05-20) 198 | ------------------ 199 | 200 | 0.7.4 (2019-04-05) 201 | ------------------ 202 | 203 | 0.7.3 (2019-03-14) 204 | ------------------ 205 | 206 | 0.7.2 (2018-07-21) 207 | ------------------ 208 | 209 | 0.7.1 (2018-01-07) 210 | ------------------ 211 | 212 | 0.7.0 (2017-07-18 10:50) 213 | ------------------------ 214 | 215 | 0.6.15 (2017-07-18 10:30) 216 | ------------------------- 217 | 218 | 0.6.14 (2016-10-10 12:20) 219 | ------------------------- 220 | 221 | 0.6.13 (2016-10-10 11:46) 222 | ------------------------- 223 | 224 | 0.6.12 (2016-10-10 11:45) 225 | ------------------------- 226 | 227 | 0.6.11 (2016-04-01) 228 | ------------------- 229 | 230 | 0.6.10 (2015-08-31) 231 | ------------------- 232 | 233 | 0.6.9 (2015-08-25) 234 | ------------------ 235 | 236 | 0.6.8 (2015-06-22) 237 | ------------------ 238 | 239 | 0.6.7 (2015-06-17) 240 | ------------------ 241 | 242 | 0.6.6 (2014-12-18 10:49) 243 | ------------------------ 244 | 245 | 0.6.5 (2014-12-18 09:08) 246 | ------------------------ 247 | 248 | 0.6.4 (2014-12-16 14:10) 249 | ------------------------ 250 | 251 | 0.6.3 (2014-12-16 14:00) 252 | ------------------------ 253 | 254 | 0.6.2 (2014-12-15) 255 | ------------------ 256 | 257 | 0.6.1 (2014-09-22) 258 | ------------------ 259 | 260 | 0.6.0 (2014-09-18) 261 | ------------------ 262 | 263 | 0.5.4 (2014-08-26 10:26) 264 | ------------------------ 265 | 266 | 0.1.0 (2014-08-26 10:23) 267 | ------------------------ 268 | -------------------------------------------------------------------------------- /cob_hardware_emulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_hardware_emulation) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp rosgraph_msgs) 5 | find_package(Boost REQUIRED COMPONENTS thread) 6 | 7 | catkin_package( 8 | CATKIN_DEPENDS roscpp rosgraph_msgs 9 | DEPENDS Boost 10 | ) 11 | 12 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 13 | 14 | add_executable(emulation_clock src/emulation_clock.cpp) 15 | add_dependencies(emulation_clock ${catkin_EXPORTED_TARGETS}) 16 | target_link_libraries(emulation_clock ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 17 | 18 | install(TARGETS emulation_clock 19 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 20 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 21 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 22 | ) 23 | 24 | catkin_install_python(PROGRAMS 25 | scripts/emulation_base.py 26 | scripts/emulation_clock.py 27 | scripts/emulation_follow_joint_trajectory.py 28 | scripts/emulation_nav.py 29 | scripts/emulation_odom_laser.py 30 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 31 | ) 32 | -------------------------------------------------------------------------------- /cob_hardware_emulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cob_hardware_emulation 4 | 0.9.25 5 | The cob_hardware_emulation package provides idealized nodes emulating real robot hardware and/or physics simulation. 6 | 7 | Florian Weisshardt 8 | Florian Weisshardt 9 | 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | roscpp 15 | rosgraph_msgs 16 | 17 | actionlib 18 | actionlib_msgs 19 | control_msgs 20 | move_base_msgs 21 | nav_msgs 22 | rospy 23 | sensor_msgs 24 | std_srvs 25 | tf_conversions 26 | tf2_ros 27 | 28 | 29 | -------------------------------------------------------------------------------- /cob_hardware_emulation/scripts/emulation_base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import copy 5 | import math 6 | 7 | import rospy 8 | import tf 9 | import tf2_ros 10 | from geometry_msgs.msg import Twist, TransformStamped, Pose 11 | from nav_msgs.msg import Odometry 12 | from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse 13 | 14 | class EmulationBase(object): 15 | def __init__(self, odom_frame): 16 | # this node emulates the controllers of a base including twist_controller and odometry_controller 17 | # 18 | # interfaces 19 | # - subscribers: 20 | # - /base/twist_controller/command [geometry_msgs/Twist] 21 | # - publishers: 22 | # - /base/odometry_controller/odometry [nav_msgs/Odometry] 23 | # - tf (odom_frame --> base_footprint) 24 | 25 | # TODO 26 | # - speed factor 27 | 28 | self.odom_frame_ = odom_frame 29 | 30 | self.br = tf2_ros.TransformBroadcaster() 31 | 32 | self.timestamp_last_update = rospy.Time.now() 33 | 34 | self.twist = Twist() 35 | self.timestamp_last_twist = rospy.Time(0) 36 | 37 | self.odom = Odometry() 38 | self.odom.header.frame_id = self.odom_frame_ 39 | self.odom.child_frame_id = "base_footprint" 40 | self.odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion 41 | 42 | self.pub_odom = rospy.Publisher("/base/odometry_controller/odometry", Odometry, queue_size=1) 43 | rospy.Subscriber("/base/twist_controller/command", Twist, self.twist_callback, queue_size=1) 44 | rospy.Service("/base/odometry_controller/reset_odometry", Trigger, self.reset_odometry) 45 | rospy.Timer(rospy.Duration(0.1), self.timer_cb) 46 | 47 | rospy.loginfo("Emulation for base running") 48 | 49 | def reset_odometry(self, req): 50 | self.odom.pose.pose = Pose() 51 | self.odom.pose.pose.orientation.w = 1 52 | 53 | return TriggerResponse(True, "odometry resetted") 54 | 55 | def twist_callback(self, msg): 56 | self.twist = msg 57 | self.timestamp_last_twist = rospy.Time.now() 58 | 59 | def timer_cb(self, event): 60 | # move robot (calculate new pose) 61 | dt = rospy.Time.now() - self.timestamp_last_update 62 | self.timestamp_last_update = rospy.Time.now() 63 | time_since_last_twist = rospy.Time.now() - self.timestamp_last_twist 64 | 65 | #print "dt =", dt.to_sec(), ". duration since last twist =", time_since_last_twist.to_sec() 66 | # we assume we're not moving any more if there is no new twist after 0.1 sec 67 | if time_since_last_twist < rospy.Duration(0.1): 68 | new_pose = copy.deepcopy(self.odom.pose.pose) 69 | yaw = tf.transformations.euler_from_quaternion([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])[2] + self.twist.angular.z * dt.to_sec() 70 | quat = tf.transformations.quaternion_from_euler(0, 0, yaw) 71 | new_pose.orientation.x = quat[0] 72 | new_pose.orientation.y = quat[1] 73 | new_pose.orientation.z = quat[2] 74 | new_pose.orientation.w = quat[3] 75 | new_pose.position.x += self.twist.linear.x * dt.to_sec() * math.cos(yaw) - self.twist.linear.y * dt.to_sec() * math.sin(yaw) 76 | new_pose.position.y += self.twist.linear.x * dt.to_sec() * math.sin(yaw) + self.twist.linear.y * dt.to_sec() * math.cos(yaw) 77 | self.odom.pose.pose = new_pose 78 | 79 | # we're moving, so we set a non-zero twist 80 | self.odom.twist.twist.linear.x = self.twist.linear.x 81 | self.odom.twist.twist.linear.y = self.twist.linear.y 82 | self.odom.twist.twist.angular.z = self.twist.angular.z 83 | else: 84 | # reset twist as we're not moving anymore 85 | self.odom.twist.twist = Twist() 86 | 87 | # publish odometry 88 | odom = copy.deepcopy(self.odom) 89 | odom.header.stamp = rospy.Time.now() # update to current time stamp 90 | self.pub_odom.publish(odom) 91 | 92 | # publish tf 93 | # pub base_footprint --> odom_frame 94 | t_odom = TransformStamped() 95 | t_odom.header.stamp = rospy.Time.now() 96 | t_odom.header.frame_id = self.odom_frame_ 97 | t_odom.child_frame_id = "base_footprint" 98 | t_odom.transform.translation = self.odom.pose.pose.position 99 | t_odom.transform.rotation = self.odom.pose.pose.orientation 100 | 101 | transforms = [t_odom] 102 | 103 | self.br.sendTransform(transforms) 104 | 105 | if __name__ == '__main__': 106 | rospy.init_node('emulation_base') 107 | parser = argparse.ArgumentParser(conflict_handler='resolve', 108 | description="Tool for emulating base by publishing odometry and propagating base_footprint.") 109 | parser.add_argument('-o', '--odom_frame', help='odom frame name (default: \'odom_combined\')', default='odom_combined') 110 | args, unknown = parser.parse_known_args() 111 | EmulationBase(args.odom_frame) 112 | rospy.spin() 113 | -------------------------------------------------------------------------------- /cob_hardware_emulation/scripts/emulation_clock.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import time 4 | import rospy 5 | from rosgraph_msgs.msg import Clock 6 | 7 | class EmulationClock(object): 8 | def __init__(self): 9 | self.t = time.time() 10 | self.dt_ms = rospy.get_param('/emulation_dt_ms', 10) 11 | self.time_factor = rospy.get_param('/emulation_time_factor', 1.0) 12 | if not self.time_factor > 0.0: 13 | rospy.logerr("emulation_time_factor must be >0.0, but is {}. exiting...".format(self.time_factor)) 14 | exit(-1) 15 | self.pub = rospy.Publisher('/clock', Clock, queue_size=1) 16 | rospy.Timer(rospy.Duration(self.dt_ms/1000.0), self.timer_cb) 17 | 18 | def timer_cb(self, event): 19 | self.t+=self.time_factor*self.dt_ms/1000 20 | msg = Clock() 21 | msg.clock = rospy.Time(self.t) 22 | self.pub.publish(msg) 23 | 24 | 25 | if __name__ == '__main__': 26 | rospy.init_node('emulation_clock', disable_rostime=True) 27 | EmulationClock() 28 | rospy.spin() 29 | -------------------------------------------------------------------------------- /cob_hardware_emulation/scripts/emulation_follow_joint_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import copy 4 | 5 | import rospy 6 | import actionlib 7 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryResult 8 | from sensor_msgs.msg import JointState 9 | from std_srvs.srv import Trigger, TriggerResponse 10 | 11 | class EmulationFollowJointTrajectory(object): 12 | def __init__(self): 13 | # TODO 14 | # - speed factor 15 | 16 | params = rospy.get_param('~') 17 | self.joint_names = params['joint_names'] 18 | # fixed frequency to control the granularity of the sampling resolution 19 | self.sample_rate_hz = rospy.get_param("~sample_rate_hz", 10) 20 | # duration from one sample to the next 21 | self.sample_rate_dur_secs = (1.0 / float(self.sample_rate_hz)) 22 | # rospy loop rate 23 | self.sample_rate = rospy.Rate(self.sample_rate_hz) 24 | 25 | # joint_states publisher 26 | js = JointState() 27 | js.name = copy.deepcopy(self.joint_names) 28 | js.position = [0]*len(js.name) 29 | js.velocity = [0]*len(js.name) 30 | js.effort = [0]*len(js.name) 31 | self.joint_states = js 32 | self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=1) 33 | self.js_timer = rospy.Timer(rospy.Duration(self.sample_rate_dur_secs), self.publish_joint_states) 34 | 35 | # reset service 36 | self.service_reset_fjta = rospy.Service("reset_joint_states", Trigger, self.reset_cb) 37 | 38 | # follow_joint_trajectory action 39 | action_name = "joint_trajectory_controller/follow_joint_trajectory" 40 | self.as_fjta = actionlib.SimpleActionServer(action_name, FollowJointTrajectoryAction, execute_cb=self.fjta_cb, auto_start = False) 41 | self.as_fjta.start() 42 | rospy.loginfo("Emulation running for action %s of type FollowJointTrajectoryAction"%(action_name)) 43 | 44 | def reset_cb(self, req): 45 | self.joint_states.position = [0.0] * len(self.joint_states.position) 46 | self.joint_states.velocity = [0.0] * len(self.joint_states.velocity) 47 | self.joint_states.effort = [0.0] * len(self.joint_states.effort) 48 | 49 | return TriggerResponse( 50 | success = True, 51 | message = "Succesfully reset joint states" 52 | ) 53 | 54 | def fjta_cb(self, goal): 55 | joint_names = copy.deepcopy(self.joint_names) 56 | joint_names.sort() 57 | fjta_joint_names = copy.deepcopy(goal.trajectory.joint_names) 58 | fjta_joint_names.sort() 59 | if joint_names == fjta_joint_names: 60 | rospy.loginfo("got a new joint trajectory goal for %s", joint_names) 61 | # sort goal to fit joint_names order in joint_states 62 | 63 | goal_sorted = copy.deepcopy(goal) 64 | goal_sorted.trajectory.joint_names = self.joint_names 65 | 66 | # keep track of the current time 67 | latest_time_from_start = rospy.Duration(0) 68 | # the point in time of a previous computation. 69 | # This is used to compute the interpolation weight as a function of current and goal time point 70 | time_since_start_of_previous_point = rospy.Duration(0) 71 | 72 | nr_points_dof = len(self.joint_names) 73 | 74 | # for all points in the desired trajectory 75 | for p_idx, point in enumerate(goal_sorted.trajectory.points): 76 | 77 | if len(point.positions) != nr_points_dof: 78 | self.as_fjta.set_aborted() 79 | return 80 | 81 | point_time_delta = point.time_from_start - time_since_start_of_previous_point 82 | if not p_idx == len(goal_sorted.trajectory.points)-1 and point_time_delta.to_sec() < self.sample_rate_dur_secs: 83 | rospy.logwarn("current trajectory point has time_delta smaller than sample_rate: {} < {}! Skipping".format(point_time_delta.to_sec(), self.sample_rate_dur_secs)) 84 | continue 85 | 86 | # we need to resort the positions array because moveit sorts alphabetically but all other ROS components sort in the URDF order 87 | positions_sorted = [] 88 | for joint_name in self.joint_names: 89 | idx = goal.trajectory.joint_names.index(joint_name) 90 | positions_sorted.append(point.positions[idx]) 91 | point.positions = positions_sorted 92 | 93 | joint_states_prev = copy.deepcopy(self.joint_states) 94 | 95 | # linear interpolation of the given trajectory samples is used 96 | # to compute smooth intermediate joints positions at a fixed resolution 97 | 98 | # upper bound of local duration segment 99 | t1 = point.time_from_start - time_since_start_of_previous_point 100 | # compute velocity as the fraction of distance from prev point to next point in trajectory 101 | # and the corresponding time t1 102 | velocities = [0] * nr_points_dof 103 | for i in range(nr_points_dof): 104 | if t1.to_sec() != 0.0: 105 | velocities[i] = (point.positions[i] - joint_states_prev.position[i]) / float(t1.to_sec()) 106 | else: 107 | velocities[i] = 0.0 108 | self.joint_states.velocity = velocities 109 | 110 | # this loop samples the time segment from the current states to the next goal state in "points" 111 | while not rospy.is_shutdown() and ((point.time_from_start - latest_time_from_start) > rospy.Duration(0)): 112 | # check that preempt has not been requested by the client 113 | if self.as_fjta.is_preempt_requested(): 114 | rospy.loginfo("preempt requested") 115 | self.as_fjta.set_preempted() 116 | return 117 | 118 | # current time passed in local duration segment 119 | t0 = latest_time_from_start - time_since_start_of_previous_point 120 | # compute the interpolation weight as a fraction of passed time and upper bound time in this local segment 121 | if t1 != 0.0: 122 | alpha = t0 / t1 123 | else: 124 | alpha = 0.0 125 | 126 | # interpolate linearly (lerp) each component 127 | interpolated_positions = [0] * nr_points_dof 128 | for i in range(nr_points_dof): 129 | interpolated_positions[i] = (1.0 - alpha) * joint_states_prev.position[i] + alpha * point.positions[i] 130 | self.joint_states.position = interpolated_positions 131 | 132 | # sleep until next sample update 133 | self.sample_rate.sleep() 134 | # increment passed time 135 | latest_time_from_start += rospy.Duration(self.sample_rate_dur_secs) 136 | 137 | # ensure that the goal and time point is always exactly reached 138 | latest_time_from_start = point.time_from_start 139 | self.joint_states.position = point.positions 140 | # set lower time bound for the next point 141 | time_since_start_of_previous_point = latest_time_from_start 142 | 143 | # set joint velocities to zero after the robot stopped moving (reaching final point of trajectory) 144 | self.joint_states.velocity = [0.0] * nr_points_dof 145 | self.joint_states.effort = [0.0] * nr_points_dof 146 | 147 | self.as_fjta.set_succeeded(FollowJointTrajectoryResult()) 148 | else: 149 | rospy.logerr("received unexpected joint names in goal") 150 | self.as_fjta.set_aborted() 151 | 152 | def publish_joint_states(self, event): 153 | msg = copy.deepcopy(self.joint_states) 154 | msg.header.stamp = rospy.Time.now() # update to current time stamp 155 | self.pub_joint_states.publish(msg) 156 | 157 | if __name__ == '__main__': 158 | rospy.init_node('emulation_follow_joint_trajectory') 159 | emulation_follow_joint_trajectory = EmulationFollowJointTrajectory() 160 | rospy.spin() -------------------------------------------------------------------------------- /cob_hardware_emulation/scripts/emulation_nav.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import actionlib 4 | import argparse 5 | import numpy 6 | import rospy 7 | import tf 8 | import tf2_ros 9 | from actionlib_msgs.msg import GoalStatus 10 | from geometry_msgs.msg import Transform, TransformStamped, PoseStamped 11 | from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion 12 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult 13 | 14 | class EmulationNav(object): 15 | def __init__(self, odom_frame): 16 | # this node emulates a very basic navigation including move_base action and localization 17 | # 18 | # interfaces 19 | # - subscribers: 20 | # - /initialpose [geometry_msgs/PoseWithCovarianceStamped] 21 | # - publishers: 22 | # - tf (map --> odom_frame) 23 | # - actions: 24 | # - move_base [move_base_msgs/MoveBaseAction] (optional) 25 | 26 | 27 | # TODO 28 | # - speed factor 29 | # - handle cancel on move_base action 30 | 31 | self._odom_frame = odom_frame 32 | 33 | self._buffer = tf2_ros.Buffer() 34 | self._listener = tf2_ros.TransformListener(self._buffer) 35 | self._transform_broadcaster = tf2_ros.TransformBroadcaster() 36 | 37 | initialpose = rospy.get_param("~initialpose", None) 38 | if type(initialpose) == list: 39 | rospy.loginfo("using initialpose from parameter server: %s", str(initialpose)) 40 | elif type(initialpose) == str: 41 | rospy.loginfo("using initialpose from script server: %s", str(initialpose)) 42 | initialpose = rospy.get_param("/script_server/base/" + initialpose) 43 | else: 44 | rospy.loginfo("initialpose not set, using [0, 0, 0] as initialpose") 45 | initialpose = [0, 0, 0] 46 | 47 | self._odom_transform = Transform() 48 | self._odom_transform.translation.x = initialpose[0] 49 | self._odom_transform.translation.y = initialpose[1] 50 | quat = tf.transformations.quaternion_from_euler(0, 0, initialpose[2]) 51 | self._odom_transform.rotation = Quaternion(*quat) 52 | 53 | rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initalpose_callback, queue_size=1) 54 | rospy.Timer(rospy.Duration(0.04), self.timer_cb) 55 | 56 | rospy.loginfo("Emulation for navigation running") 57 | 58 | # Optional move_base action 59 | self._move_base_mode = rospy.get_param("~move_base_mode", None) 60 | if self._move_base_mode == None: 61 | rospy.loginfo("Emulation running without move_base") 62 | elif self._move_base_mode == "beam" or self._move_base_mode == "linear_nav": 63 | self._move_base_action_name = "/move_base" 64 | rospy.Subscriber(self._move_base_action_name + "_simple/goal", PoseStamped, self.move_base_simple_callback, queue_size=1) 65 | self._as_move_base = actionlib.SimpleActionServer(self._move_base_action_name, MoveBaseAction, execute_cb=self.move_base_cb, auto_start = False) 66 | self._as_move_base.start() 67 | rospy.loginfo("Emulation running for action %s of type MoveBaseAction with mode '%s'"%(self._move_base_action_name, self._move_base_mode)) 68 | else: 69 | rospy.logwarn("Emulation running without move_base due to invalid value for parameter move_base_mode: '%s'", self._move_base_mode) 70 | 71 | 72 | def initalpose_callback(self, msg): 73 | rospy.loginfo("Got initialpose, updating %s transformation.", self._odom_frame) 74 | try: 75 | # get pose of base_footprint within odom frame 76 | base_in_odom = self._buffer.lookup_transform("base_footprint", self._odom_frame, rospy.Time(0)) 77 | # convert initialpose to matrix (necessary for matrix multiplication) 78 | trans_ini = tf.transformations.translation_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) 79 | rot_ini = tf.transformations.quaternion_matrix([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) 80 | matrix_ini = numpy.dot(trans_ini, rot_ini) 81 | # convert footprint->odom transform to matrix (necessary for matrix multiplication) 82 | trans_base_odom = tf.transformations.translation_matrix([base_in_odom.transform.translation.x, base_in_odom.transform.translation.y, base_in_odom.transform.translation.z]) 83 | rot_base_odom = tf.transformations.quaternion_matrix([base_in_odom.transform.rotation.x, base_in_odom.transform.rotation.y, base_in_odom.transform.rotation.z, base_in_odom.transform.rotation.w]) 84 | matrix_base_odom = numpy.dot(trans_base_odom, rot_base_odom) 85 | 86 | # matrix multiplication of footprint->odom transform in respect of initialpose 87 | matrix_new_odom = numpy.dot(matrix_ini, matrix_base_odom) 88 | 89 | # translate back into Transform 90 | trans_new_odom = tf.transformations.translation_from_matrix(matrix_new_odom) 91 | rot_new_odom = tf.transformations.quaternion_from_matrix(matrix_new_odom) 92 | self._odom_transform.translation.x = trans_new_odom[0] 93 | self._odom_transform.translation.y = trans_new_odom[1] 94 | self._odom_transform.translation.z = trans_new_odom[2] 95 | self._odom_transform.rotation.x = rot_new_odom[0] 96 | self._odom_transform.rotation.y = rot_new_odom[1] 97 | self._odom_transform.rotation.z = rot_new_odom[2] 98 | self._odom_transform.rotation.w = rot_new_odom[3] 99 | 100 | except Exception as e: 101 | rospy.logerr("Could not calculate new odom transformation:\n%s", e) 102 | return 103 | 104 | def timer_cb(self, event): 105 | # publish tf 106 | # pub odom_frame --> map 107 | t_loc = TransformStamped() 108 | t_loc.header.stamp = rospy.Time.now() 109 | t_loc.header.frame_id = "map" 110 | t_loc.child_frame_id = self._odom_frame 111 | t_loc.transform = self._odom_transform 112 | 113 | transforms = [t_loc] 114 | 115 | self._transform_broadcaster.sendTransform(transforms) 116 | 117 | def move_base_cb(self, goal): 118 | pwcs = PoseWithCovarianceStamped() 119 | pwcs.header = goal.target_pose.header 120 | pwcs.pose.pose = goal.target_pose.pose 121 | if self._move_base_mode == "beam": 122 | rospy.loginfo("move_base: beaming robot to new goal") 123 | self.initalpose_callback(pwcs) 124 | elif self._move_base_mode == "linear_nav": 125 | move_base_linear_action_name = "/move_base_linear" 126 | ac_move_base_linear = actionlib.SimpleActionClient(move_base_linear_action_name, MoveBaseAction) 127 | rospy.loginfo("Waiting for ActionServer: %s", move_base_linear_action_name) 128 | if not ac_move_base_linear.wait_for_server(rospy.Duration(1)): 129 | rospy.logerr("Emulator move_base failed because move_base_linear action server is not available") 130 | self._as_move_base.set_aborted() 131 | return 132 | rospy.loginfo("send goal to %s", move_base_linear_action_name) 133 | ac_move_base_linear.send_goal(goal) 134 | ac_move_base_linear.wait_for_result() 135 | ac_move_base_linear_status = ac_move_base_linear.get_state() 136 | ac_move_base_linear_result = ac_move_base_linear.get_result() 137 | if ac_move_base_linear_status != GoalStatus.SUCCEEDED: 138 | rospy.logerr("Emulator move_base failed because move_base_linear failed") 139 | self._as_move_base.set_aborted() 140 | return 141 | else: 142 | rospy.logerr("Invalid move_base_action_mode") 143 | self._as_move_base.set_aborted() 144 | return 145 | rospy.loginfo("Emulator move_base succeeded") 146 | self._as_move_base.set_succeeded(MoveBaseResult()) 147 | 148 | def move_base_simple_callback(self, msg): 149 | goal = MoveBaseGoal() 150 | goal.target_pose = msg 151 | ac_move_base = actionlib.SimpleActionClient(self._move_base_action_name, MoveBaseAction) 152 | rospy.loginfo("Waiting for ActionServer: %s", self._move_base_action_name) 153 | if not ac_move_base.wait_for_server(rospy.Duration(1)): 154 | rospy.logerr("Emulator move_base simple failed because move_base action server is not available") 155 | return 156 | rospy.loginfo("send goal to %s", self._move_base_action_name) 157 | ac_move_base.send_goal(goal) 158 | # ac_move_base.wait_for_result() # no need to wait for the result as this is the topic interface to move_base without feedback 159 | 160 | if __name__ == '__main__': 161 | rospy.init_node('emulation_nav') 162 | parser = argparse.ArgumentParser(conflict_handler='resolve', 163 | description="Tool for emulating nav by providing localization and move base interface") 164 | parser.add_argument('-o', '--odom_frame', help='odom frame name (default: \'odom_combined\')', default='odom_combined') 165 | args, unknown = parser.parse_known_args() 166 | rospy.loginfo("emulation_nav started!") 167 | EmulationNav(args.odom_frame) 168 | rospy.spin() 169 | -------------------------------------------------------------------------------- /cob_hardware_emulation/scripts/emulation_odom_laser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import rospy 5 | import tf2_ros 6 | from geometry_msgs.msg import TransformStamped 7 | from nav_msgs.msg import Odometry 8 | 9 | class EmulationOdomLaser(object): 10 | def __init__(self, odom_frame): 11 | # this node emulates the laser odom based on the odometry from the base controller 12 | # 13 | # interfaces 14 | # - subscribers: 15 | # - /base/odometry_controller/odometry [nav_msgs/Odometry] 16 | # - publishers: 17 | # - /scan_odom_node/scan_odom/odometry [nav_msgs/Odometry] 18 | # - tf (map --> odom_frame) 19 | 20 | 21 | # TODO 22 | # - speed factor 23 | 24 | self._odom_frame = odom_frame 25 | 26 | self._transform_broadcaster = tf2_ros.TransformBroadcaster() 27 | 28 | self._odom = Odometry() 29 | self._odom.header.frame_id = self._odom_frame 30 | self._odom.child_frame_id = "base_footprint" 31 | self._odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion 32 | 33 | self._odom_publisher = rospy.Publisher("/scan_odom_node/scan_odom/odometry", Odometry, queue_size=1) 34 | rospy.Subscriber("/base/odometry_controller/odometry", Odometry, self.odometry_callback, queue_size=1) 35 | rospy.loginfo("Emulation for laser odometry running") 36 | 37 | def odometry_callback(self, msg): 38 | self._odom = msg 39 | self._odom.header.frame_id = self._odom_frame 40 | self._odom_publisher.publish(self._odom) 41 | self.publish_tf() 42 | 43 | def publish_tf(self): 44 | # publish tf 45 | # pub base_footprint --> odom_frame 46 | t_odom = TransformStamped() 47 | t_odom.header.stamp = rospy.Time.now() 48 | t_odom.header.frame_id = self._odom_frame 49 | t_odom.child_frame_id = "base_footprint" 50 | t_odom.transform.translation = self._odom.pose.pose.position 51 | t_odom.transform.rotation = self._odom.pose.pose.orientation 52 | 53 | transforms = [t_odom] 54 | 55 | self._transform_broadcaster.sendTransform(transforms) 56 | 57 | if __name__ == '__main__': 58 | rospy.init_node('emulation_odom_laser') 59 | parser = argparse.ArgumentParser(conflict_handler='resolve', 60 | description="Tool for emulating laser odom based on the odometry from the base controller") 61 | parser.add_argument('-o', '--odom_frame', help='odom frame name (default: \'odom_combined\')', default='odom_combined') 62 | args, unknown = parser.parse_known_args() 63 | rospy.loginfo("emulation_odom_laser running!") 64 | EmulationOdomLaser(args.odom_frame) 65 | rospy.spin() 66 | -------------------------------------------------------------------------------- /cob_hardware_emulation/src/emulation_clock.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | 10 | class EmulationClock 11 | { 12 | public: 13 | EmulationClock(boost::asio::io_service& io, int dt_ms, double time_factor) 14 | : dt_ms_(dt_ms), 15 | time_factor_(time_factor), 16 | timer_(io, boost::posix_time::milliseconds(dt_ms)) 17 | { 18 | t_ = (double)std::time(NULL); 19 | pub_ = nh_.advertise("/clock", 1); 20 | timer_.async_wait(boost::bind(&EmulationClock::timer_cb, this)); 21 | } 22 | 23 | void timer_cb() 24 | { 25 | t_ += time_factor_*dt_ms_/1000; 26 | ROS_DEBUG_STREAM("WALL: "<<(double)std::time(NULL)<<"; ROS: "<("/emulation_dt_ms", dt_ms, 10); 54 | nh.param("/emulation_time_factor", time_factor, 1.0); 55 | 56 | boost::asio::io_service io; 57 | EmulationClock ec(io, dt_ms, time_factor); 58 | io.run(); 59 | 60 | return 0; 61 | } 62 | --------------------------------------------------------------------------------