├── .codeclimate.yml ├── .github ├── PULL_REQUEST_TEMPLATE.md ├── dependabot.yml └── workflows │ └── main.yml ├── .gitignore ├── LICENSE ├── README.md ├── cob_command_gui ├── CHANGELOG.rst ├── CMakeLists.txt ├── common │ └── files │ │ └── icons │ │ ├── batti-000.png │ │ ├── batti-020.png │ │ ├── batti-040.png │ │ ├── batti-060.png │ │ ├── batti-080.png │ │ ├── batti-100.png │ │ ├── batti-charged.png │ │ ├── batti-charging-000.png │ │ ├── batti-charging-020.png │ │ ├── batti-charging-040.png │ │ ├── batti-charging-060.png │ │ ├── batti-charging-080.png │ │ ├── batti-charging-100.png │ │ ├── batti-empty.png │ │ ├── error.png │ │ └── ok.png ├── package.xml ├── setup.py └── src │ ├── command_gui_buttons │ ├── __init__.py │ └── command_gui_buttons.py │ └── knoeppkes.py ├── cob_command_tools ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── cob_dashboard ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── plugin.xml ├── scripts │ ├── cob_dashboard │ └── cob_dashboard_aggregator.py ├── setup.py └── src │ └── cob_dashboard │ ├── __init__.py │ ├── cob_battery.py │ ├── cob_dashboard.py │ └── cob_runstops.py ├── cob_helper_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── HelperTools.cfg ├── package.xml ├── scripts │ ├── auto_init.py │ ├── auto_recover.py │ ├── auto_tools.py │ ├── fake_diagnostics.py │ ├── fake_driver.py │ └── visualize_navigation_goals.py ├── setup.py └── src │ └── cob_auto_tools │ ├── __init__.py │ ├── auto_init.py │ └── auto_recover.py ├── cob_monitoring ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── setup.py └── src │ ├── battery_monitor.py │ ├── cpu_monitor.py │ ├── ddwrt.py │ ├── emergency_stop_monitor.py │ ├── hd_monitor.py │ ├── hz_monitor.py │ ├── net_monitor.py │ ├── netdata_interface │ ├── __init__.py │ └── netdata_interface.py │ ├── ntp_monitor.py │ ├── topic_status_monitor.cpp │ ├── wifi_monitor.py │ └── wlan_monitor.py ├── cob_script_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ ├── Script.action │ └── State.action ├── launch │ ├── script_server.launch │ └── script_server.test ├── msg │ └── ScriptState.msg ├── package.xml ├── setup.py ├── src │ ├── cob_console │ ├── cob_console_node │ ├── script_server.py │ └── simple_script_server │ │ ├── __init__.py │ │ └── simple_script_server.py ├── srv │ └── ComposeTrajectory.srv └── test │ ├── test_action_interface.py │ ├── test_move.py │ ├── test_python_api.py │ ├── test_say.py │ └── test_trigger.py ├── cob_teleop ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── ros │ ├── launch │ └── teleop_keyboard.launch │ └── src │ ├── cob_teleop.cpp │ ├── cob_teleop_keyboard.cpp │ └── keyboard_publisher.cpp ├── generic_throttle ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── package.xml ├── scripts │ └── generic_throttle_node.py ├── setup.py └── src │ └── generic_throttle │ ├── __init__.py │ └── generic_throttle.py └── scenario_test_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── docs ├── Makefile ├── conf.py ├── index.rst ├── make.bat └── modules.rst ├── package.xml ├── scripts ├── dummy_behavior.py ├── example_move_base.py ├── example_test.py └── move_base_success.py ├── setup.py └── src └── scenario_test_tools ├── __init__.py ├── scriptable_action_server.py ├── scriptable_base.py ├── scriptable_move_base.py ├── scriptable_service_server.py └── util.py /.codeclimate.yml: -------------------------------------------------------------------------------- 1 | engines: 2 | # duplication: 3 | # enabled: true 4 | # config: 5 | # languages: 6 | # - python 7 | # fixme: 8 | # enabled: true 9 | pep8: 10 | enabled: true 11 | # radon: 12 | # enabled: true 13 | ratings: 14 | paths: 15 | - "**.py" 16 | exclude_paths: [] 17 | -------------------------------------------------------------------------------- /.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 --ignored-modules=gtk,mechanize,python_qt_binding' 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | cob_command_tools 2 | =========== 3 | 4 | ## GitHub Actions - Continuous Integration 5 | 6 | CI-Status ```noetic-devel```: [![GHA CI](https://github.com/4am-robotics/cob_command_tools/actions/workflows/main.yml/badge.svg?branch=noetic-devel)](https://github.com/4am-robotics/cob_command_tools/actions/workflows/main.yml?query=branch%3Anoetic-devel) 7 | -------------------------------------------------------------------------------- /cob_command_gui/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_command_gui 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.37 (2024-10-02) 6 | ------------------- 7 | 8 | 0.7.36 (2024-08-05) 9 | ------------------- 10 | 11 | 0.7.35 (2024-04-30) 12 | ------------------- 13 | * 0.6.35 14 | * update changelogs 15 | * Contributors: fmessmer 16 | 17 | 0.6.35 (2024-04-18) 18 | ------------------- 19 | 20 | 0.6.34 (2024-02-18) 21 | ------------------- 22 | 23 | 0.6.33 (2023-11-06) 24 | ------------------- 25 | 26 | 0.6.32 (2023-04-29) 27 | ------------------- 28 | 29 | 0.6.31 (2023-01-04) 30 | ------------------- 31 | 32 | 0.6.30 (2022-11-17) 33 | ------------------- 34 | 35 | 0.6.29 (2022-07-29) 36 | ------------------- 37 | 38 | 0.6.28 (2022-03-15) 39 | ------------------- 40 | 41 | 0.6.27 (2022-01-12) 42 | ------------------- 43 | 44 | 0.6.26 (2021-11-26) 45 | ------------------- 46 | 47 | 0.6.25 (2021-08-02) 48 | ------------------- 49 | 50 | 0.6.24 (2021-07-02) 51 | ------------------- 52 | 53 | 0.6.23 (2021-07-01) 54 | ------------------- 55 | 56 | 0.6.22 (2021-05-10) 57 | ------------------- 58 | 59 | 0.6.21 (2021-04-06) 60 | ------------------- 61 | * Merge pull request `#296 `_ from fmessmer/fix/pylint_segfault 62 | fix pylint segfault 63 | * specify required version 64 | * Contributors: Felix Messmer, fmessmer 65 | 66 | 0.6.20 (2021-01-25) 67 | ------------------- 68 | * Merge pull request `#291 `_ from fmessmer/fix_python3 69 | fix python3 70 | * use keys vs iterkeys for python3-compatibility 71 | * Contributors: Felix Messmer, fmessmer 72 | 73 | 0.6.19 (2020-12-02) 74 | ------------------- 75 | * Merge pull request `#288 `_ from benmaidel/noetic_migration 76 | [noetic] cob_command_gui migration 77 | * switch from pygtk to pygobject's pygtkcompat compatibility layer 78 | * Contributors: Benjamin Maidel, Felix Messmer 79 | 80 | 0.6.18 (2020-10-21) 81 | ------------------- 82 | 83 | 0.6.17 (2020-10-17) 84 | ------------------- 85 | * Merge pull request `#284 `_ from fmessmer/test_noetic 86 | test noetic 87 | * use setuptools instead of distutils 88 | * Bump CMake version to avoid CMP0048 warning 89 | * Contributors: Felix Messmer, fmessmer 90 | 91 | 0.6.16 (2020-03-18) 92 | ------------------- 93 | * Merge pull request `#270 `_ from LoyVanBeek/feature/python3_compatibility 94 | [ci_updates] pylint + Python3 compatibility 95 | * fix modules 96 | * fix more pylint errors 97 | * use threading 98 | * python3 compatibility via 2to3 99 | * Merge pull request `#271 `_ from fmessmer/ci_updates 100 | [travis] ci updates 101 | * catkin_lint fixes 102 | * Contributors: Felix Messmer, Loy van Beek, fmessmer 103 | 104 | 0.6.15 (2019-11-07) 105 | ------------------- 106 | 107 | 0.6.14 (2019-08-07) 108 | ------------------- 109 | 110 | 0.6.13 (2019-07-19) 111 | ------------------ 112 | 113 | 0.6.12 (2019-06-07) 114 | ------------------- 115 | 116 | 0.6.11 (2019-04-05) 117 | ------------------- 118 | 119 | 0.6.10 (2019-03-14) 120 | ------------------- 121 | * Merge pull request `#241 `_ from fmessmer/add_string_action 122 | Add string action 123 | * add SetString action interface 124 | * temp_woz 125 | * Contributors: Felix Messmer, fmessmer, ipa-fmw 126 | 127 | 0.6.9 (2018-07-21) 128 | ------------------ 129 | * update maintainer 130 | * Contributors: ipa-fxm 131 | 132 | 0.6.8 (2018-07-21) 133 | ------------------ 134 | * Merge pull request `#216 `_ from ipa-fxm/missing_dependency_python-gtk2 135 | add missing dependency python-gtk2 136 | * add missing dependency python-gtk2 137 | * Contributors: Felix Messmer, ipa-fxm 138 | 139 | 0.6.7 (2018-01-07) 140 | ------------------ 141 | * Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev 142 | * Merge pull request `#197 `_ from ipa-fxm/APACHE_license 143 | use license apache 2.0 144 | * use license apache 2.0 145 | * Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk 146 | 147 | 0.6.6 (2017-07-17) 148 | ------------------ 149 | * 'trigger_action' is blocking 150 | * add buttons for 'trigger_action' 151 | * fix python import 152 | * add proper print_functions import 153 | * some python3 print fixes 154 | * manually fix changelog 155 | * Contributors: ipa-fxm, robot 156 | 157 | 0.6.5 (2016-10-10) 158 | ------------------ 159 | 160 | 0.6.4 (2016-04-01) 161 | ------------------ 162 | * undo pop-up related changes 163 | * only show MessagDialog for init and recover 164 | * calling sss.commands in thread with MessageDialog 165 | * Contributors: ipa-fxm 166 | 167 | 0.6.3 (2015-08-25) 168 | ------------------ 169 | * remove obsolete autogenerated mainpage.dox files 170 | * remove trailing whitespaces 171 | * migrate to package format 2 172 | * sort dependencies 173 | * critically review dependencies 174 | * Contributors: ipa-fxm 175 | 176 | 0.6.2 (2015-06-17) 177 | ------------------ 178 | * cleanup CMakeLists 179 | * remove anoying command_gui notification popups 180 | * Contributors: ipa-fmw, ipa-fxm 181 | 182 | 0.6.1 (2014-12-15) 183 | ------------------ 184 | * merge 185 | * add nice images to command_gui 186 | * add halt service support 187 | * adapt namespaces to new canopen version 188 | * merge 189 | * Update package.xml 190 | * Contributors: Florian Weisshardt, ipa-fmw 191 | 192 | 0.6.0 (2014-09-18) 193 | ------------------ 194 | 195 | 0.5.2 (2014-08-28) 196 | ------------------ 197 | * move EmergencyStopState.msg to cob_msgs 198 | * Cleanup for indigo and rewrite of dashboard to run without pr2_msgs 199 | * Contributors: Alexander Bubeck, ipa-fxm 200 | 201 | 0.5.1 (2014-03-20) 202 | ------------------ 203 | * merged catkin version 204 | * Initial catkinization. 205 | * command_gui threading bugfix 206 | * thread stability improvements 207 | * confirmation for command_gui 208 | * fixed indentation Error 209 | * added checkbutton to enable command confirmation 210 | * Revert "changed component names to explicitly contain full namespace" 211 | This reverts commit b3cf8a5e500a754d19091aba25a9fe442518556d. 212 | * changed component names to explicitly contain full namespace 213 | * allow individual buttons for command gui 214 | * fix typo 215 | * fix typo 216 | * add stop all 217 | * support for init_all and recover_all based on loaded robot modules in command_gui 218 | * base stop wirking 219 | * added relative motion to script server 220 | * renamed last dashboard lines to command gui 221 | * renamed last dashboard lines to command gui 222 | * fixed some bugs after renaming knoeppkes 223 | * modified manifests 224 | * changed name of cob_dashboard to cob_command_gui and added cob_dashboard overlay 225 | * Contributors: Alexander Bubeck, Florian Weißhardt, Tobias Sing, abubeck, ipa-bnm, ipa-fmw 226 | -------------------------------------------------------------------------------- /cob_command_gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_command_gui) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | ### Install ### 11 | catkin_install_python(PROGRAMS src/knoeppkes.py 12 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 13 | ) 14 | 15 | install(DIRECTORY common 16 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 17 | ) 18 | -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-000.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-020.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-040.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-040.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-060.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-060.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-080.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-080.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-100.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charged.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charged.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charging-000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charging-000.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charging-020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charging-020.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charging-040.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charging-040.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charging-060.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charging-060.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charging-080.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charging-080.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-charging-100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-charging-100.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/batti-empty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/batti-empty.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/error.png -------------------------------------------------------------------------------- /cob_command_gui/common/files/icons/ok.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_command_gui/common/files/icons/ok.png -------------------------------------------------------------------------------- /cob_command_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | cob_command_gui 7 | 0.7.37 8 | This package provides a simple GUI for operating Care-O-bot. 9 | 10 | Apache 2.0 11 | 12 | http://www.ros.org/wiki/cob_command_gui 13 | 14 | 15 | Felix Messmer 16 | Florian Weisshardt 17 | Florian Weisshardt 18 | 19 | catkin 20 | python-setuptools 21 | python3-setuptools 22 | 23 | cob_msgs 24 | cob_script_server 25 | python-gi 26 | python3-gi 27 | roslib 28 | rospy 29 | 30 | 31 | -------------------------------------------------------------------------------- /cob_command_gui/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['command_gui_buttons'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /cob_command_gui/src/command_gui_buttons/__init__.py: -------------------------------------------------------------------------------- 1 | from .command_gui_buttons import * 2 | -------------------------------------------------------------------------------- /cob_command_gui/src/command_gui_buttons/command_gui_buttons.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | from simple_script_server import * 20 | 21 | ## Implements configurable buttons 22 | class command_gui_buttons: 23 | def __init__(self): 24 | self.sss = simple_script_server() 25 | self.panels = [] 26 | self.stop_buttons = [] 27 | self.init_buttons = [] 28 | self.recover_buttons = [] 29 | self.halt_buttons = [] 30 | self.CreateControlPanel() 31 | 32 | ## Creates the control panel out of configuration from ROS parameter server 33 | def CreateControlPanel(self): 34 | param_prefix = "~control_buttons" 35 | if not rospy.has_param(param_prefix): 36 | rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_prefix) 37 | return False 38 | group_param = rospy.get_param(param_prefix) 39 | #print group_param 40 | group_param = self.SortDict(group_param) 41 | #print group_param 42 | 43 | for group in group_param: 44 | group_name = group[1]["group_name"] 45 | component_name = group[1]["component_name"] # \todo check component name with robot_components.yaml files 46 | button_list = group[1]["buttons"] 47 | buttons = [] 48 | for button in button_list: 49 | #print "button = ",button 50 | if button[1] == "move": 51 | buttons.append(self.CreateButton(button[0],self.sss.move,component_name,button[2])) 52 | elif button[1] == "move_base_rel": 53 | buttons.append(self.CreateButton(button[0],self.sss.move_base_rel,component_name,button[2])) 54 | elif button[1] == "trigger_action": 55 | buttons.append(self.CreateButton(button[0],self.sss.trigger_action,component_name,button[2],True)) 56 | elif button[1] == "string_action": 57 | buttons.append(self.CreateButton(button[0],self.sss.string_action,component_name,button[2],True)) 58 | elif button[1] == "trigger": 59 | buttons.append(self.CreateButton(button[0],self.sss.trigger,component_name,button[2])) 60 | if button[2] == "stop": 61 | self.stop_buttons.append(component_name) 62 | if button[2] == "init": 63 | self.init_buttons.append(component_name) 64 | if button[2] == "recover": 65 | self.recover_buttons.append(component_name) 66 | if button[2] == "halt": 67 | self.halt_buttons.append(component_name) 68 | elif button[1] == "stop": 69 | buttons.append(self.CreateButton(button[0],self.sss.stop,component_name)) 70 | self.stop_buttons.append(component_name) 71 | elif button[1] == "init": 72 | buttons.append(self.CreateButton(button[0],self.sss.init,component_name)) 73 | self.init_buttons.append(component_name) 74 | elif button[1] == "recover": 75 | buttons.append(self.CreateButton(button[0],self.sss.recover,component_name)) 76 | self.recover_buttons.append(component_name) 77 | elif button[1] == "halt": 78 | buttons.append(self.CreateButton(button[0],self.sss.halt,component_name)) 79 | self.halt_buttons.append(component_name) 80 | else: 81 | rospy.logerr("Function <<%s>> not known to command gui",button[1]) 82 | return False 83 | group = (group_name,buttons) 84 | 85 | # add nav buttons (optional) 86 | if component_name == "base": # \todo get base name from robot_components.yaml 87 | param_prefix = "~nav_buttons" 88 | if rospy.has_param(param_prefix): 89 | nav_buttons_param = rospy.get_param(param_prefix) 90 | nav_button_list = nav_buttons_param["buttons"] 91 | #print nav_button_list 92 | for button in nav_button_list: 93 | #print "button = ",button 94 | buttons.append(self.CreateButton(button[0],self.sss.move,component_name,button[2])) 95 | else: 96 | rospy.logwarn("parameter %s does not exist on ROS Parameter Server, no nav buttons will be available.",param_prefix) 97 | self.panels.append(group) 98 | 99 | # uniqify lists to not have double entries 100 | self.stop_buttons = self.uniqify_list(self.stop_buttons) 101 | self.init_buttons = self.uniqify_list(self.init_buttons) 102 | self.recover_buttons = self.uniqify_list(self.recover_buttons) 103 | self.halt_buttons = self.uniqify_list(self.halt_buttons) 104 | 105 | 106 | ## Creates one button with functionality 107 | def CreateButton(self,button_name,function,component_name,parameter_name=None,blocking=False): 108 | if parameter_name == None: 109 | button = (button_name,function,(component_name,blocking)) 110 | else: 111 | button = (button_name,function,(component_name,parameter_name,blocking)) 112 | return button 113 | 114 | ## Sorts a dictionary alphabetically 115 | def SortDict(self,dictionary): 116 | keys = sorted(dictionary.keys()) 117 | k=[] 118 | #print "keys = ", keys 119 | #for key in keys: 120 | # print "values = ", dictionary[key] 121 | return [[key,dictionary[key]] for key in keys] 122 | 123 | def idfun(self,x): 124 | return x 125 | 126 | ## Uniqifies a list to not have double entries 127 | def uniqify_list(self,seq, idfun=None): 128 | # order preserving 129 | if idfun is None: 130 | idfun=self.idfun 131 | seen = {} 132 | result = [] 133 | for item in seq: 134 | marker = idfun(item) 135 | # in old Python versions: 136 | # if seen.has_key(marker) 137 | # but in new ones: 138 | if marker in seen: continue 139 | seen[marker] = 1 140 | result.append(item) 141 | return result 142 | -------------------------------------------------------------------------------- /cob_command_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_command_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.37 (2024-10-02) 6 | ------------------- 7 | 8 | 0.7.36 (2024-08-05) 9 | ------------------- 10 | 11 | 0.7.35 (2024-04-30) 12 | ------------------- 13 | * 0.6.35 14 | * update changelogs 15 | * Contributors: fmessmer 16 | 17 | 0.6.35 (2024-04-18) 18 | ------------------- 19 | 20 | 0.6.34 (2024-02-18) 21 | ------------------- 22 | * Merge pull request `#341 `_ from fmessmer/feature/optimize_workspace 23 | [WIP] optimize workspace 24 | * remove cob_interactive_teleop 25 | * Contributors: Felix Messmer, fmessmer 26 | 27 | 0.6.33 (2023-11-06) 28 | ------------------- 29 | 30 | 0.6.32 (2023-04-29) 31 | ------------------- 32 | 33 | 0.6.31 (2023-01-04) 34 | ------------------- 35 | 36 | 0.6.30 (2022-11-17) 37 | ------------------- 38 | 39 | 0.6.29 (2022-07-29) 40 | ------------------- 41 | 42 | 0.6.28 (2022-03-15) 43 | ------------------- 44 | 45 | 0.6.27 (2022-01-12) 46 | ------------------- 47 | 48 | 0.6.26 (2021-11-26) 49 | ------------------- 50 | 51 | 0.6.25 (2021-08-02) 52 | ------------------- 53 | 54 | 0.6.24 (2021-07-02) 55 | ------------------- 56 | 57 | 0.6.23 (2021-07-01) 58 | ------------------- 59 | 60 | 0.6.22 (2021-05-10) 61 | ------------------- 62 | 63 | 0.6.21 (2021-04-06) 64 | ------------------- 65 | 66 | 0.6.20 (2021-01-25) 67 | ------------------- 68 | 69 | 0.6.19 (2020-12-02) 70 | ------------------- 71 | 72 | 0.6.18 (2020-10-21) 73 | ------------------- 74 | 75 | 0.6.17 (2020-10-17) 76 | ------------------- 77 | * Merge pull request `#284 `_ from fmessmer/test_noetic 78 | test noetic 79 | * Bump CMake version to avoid CMP0048 warning 80 | * Contributors: Felix Messmer, fmessmer 81 | 82 | 0.6.16 (2020-03-18) 83 | ------------------- 84 | 85 | 0.6.15 (2019-11-07) 86 | ------------------- 87 | 88 | 0.6.14 (2019-08-07) 89 | ------------------- 90 | 91 | 0.6.13 (2019-07-19) 92 | ------------------ 93 | 94 | 0.6.12 (2019-06-07) 95 | ------------------- 96 | 97 | 0.6.11 (2019-04-05) 98 | ------------------- 99 | 100 | 0.6.10 (2019-03-14) 101 | ------------------- 102 | 103 | 0.6.9 (2018-07-21) 104 | ------------------ 105 | * update maintainer 106 | * Contributors: ipa-fxm 107 | 108 | 0.6.8 (2018-07-21) 109 | ------------------ 110 | 111 | 0.6.7 (2018-01-07) 112 | ------------------ 113 | * Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev 114 | * Merge pull request `#202 `_ from ipa-fxm/update_maintainer 115 | update maintainer 116 | * update maintainer 117 | * Merge pull request `#197 `_ from ipa-fxm/APACHE_license 118 | use license apache 2.0 119 | * use license apache 2.0 120 | * Contributors: Felix Messmer, Richard Bormann, ipa-fxm, ipa-uhr-mk 121 | 122 | 0.6.6 (2017-07-17) 123 | ------------------ 124 | * add auto_recover to new cob_helper_tools pkg 125 | * manually fix changelog 126 | * Contributors: ipa-fxm 127 | 128 | 0.6.5 (2016-10-10) 129 | ------------------ 130 | 131 | 0.6.4 (2016-04-01) 132 | ------------------ 133 | 134 | 0.6.3 (2015-08-25) 135 | ------------------ 136 | * remove trailing whitespaces 137 | * migrate to package format 2 138 | * sort dependencies 139 | * critically review dependencies 140 | * Contributors: ipa-fxm 141 | 142 | 0.6.2 (2015-06-17) 143 | ------------------ 144 | 145 | 0.6.1 (2014-12-15) 146 | ------------------ 147 | * merge 148 | * rename finished 149 | * move cob_monitoring 150 | * missed dependency 151 | * Contributors: Florian Weisshardt, ipa-nhg 152 | 153 | 0.6.0 (2014-09-18) 154 | ------------------ 155 | 156 | 0.5.2 (2014-08-28) 157 | ------------------ 158 | * Update package.xml 159 | * Contributors: Florian Weisshardt 160 | 161 | 0.5.1 (2014-03-20) 162 | ------------------ 163 | * merged catkin version 164 | * Initial catkinization. 165 | * Contributors: abubeck 166 | -------------------------------------------------------------------------------- /cob_command_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_command_tools) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /cob_command_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_command_tools 3 | 0.7.37 4 | The cob_command_tools stack provides tools, for operating Care-O-bot. 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_command_tools 9 | 10 | 11 | Felix Messmer 12 | Florian Weisshardt 13 | 14 | catkin 15 | 16 | cob_command_gui 17 | cob_dashboard 18 | cob_helper_tools 19 | cob_monitoring 20 | cob_script_server 21 | cob_teleop 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /cob_dashboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_dashboard) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | ### Install ### 11 | catkin_install_python(PROGRAMS scripts/${PROJECT_NAME} scripts/${PROJECT_NAME}_aggregator.py 12 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 13 | ) 14 | 15 | install(FILES plugin.xml 16 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 17 | ) 18 | -------------------------------------------------------------------------------- /cob_dashboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | cob_dashboard 7 | 0.7.37 8 | cob_dashboard is a modified version of [[pr2_dashboard]]. 9 | 10 | Apache 2.0 11 | 12 | http://ros.org/wiki/cob_dashboard 13 | 14 | 15 | Felix Messmer 16 | Florian Weisshardt 17 | Alexander Bubeck 18 | 19 | catkin 20 | python-setuptools 21 | python3-setuptools 22 | 23 | cob_msgs 24 | roslib 25 | rospy 26 | rqt_gui 27 | rqt_robot_dashboard 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /cob_dashboard/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A Python GUI plugin for displaying a dashboard that displays and interacts with low level pr2 states. 5 | 6 | 7 | 8 | task-past-due 9 | A Python GUI plugin for displaying a dashboard that displays and interacts with low level pr2 states. 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /cob_dashboard/scripts/cob_dashboard: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | 20 | pkg = 'cob_dashboard' 21 | from rqt_gui.main import Main 22 | 23 | main = Main() 24 | sys.exit(main.main(sys.argv, standalone=pkg)) 25 | -------------------------------------------------------------------------------- /cob_dashboard/scripts/cob_dashboard_aggregator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import time 19 | import rospy 20 | from cob_msgs.msg import EmergencyStopState, PowerState, DashboardState 21 | from diagnostic_msgs.msg import DiagnosticStatus 22 | 23 | class DashboardAggregator: 24 | def __init__(self): 25 | self.msg = DashboardState() 26 | 27 | # Create publisher 28 | self.pub = rospy.Publisher("dashboard_agg", DashboardState, queue_size=1) 29 | 30 | # Create subscribers 31 | # Diagnostics 32 | rospy.Subscriber("diagnostics_toplevel_state", DiagnosticStatus, self.DiagnosticStatusCB) 33 | # Power state 34 | rospy.Subscriber("power_state", PowerState, self.PowerStateCB) 35 | # Emergency stop state 36 | rospy.Subscriber("emergency_stop_state", EmergencyStopState, self.EmergencyStopStateCB) 37 | 38 | def DiagnosticStatusCB(self, msg): 39 | self.msg.diagnostics_toplevel_state = msg 40 | 41 | def PowerStateCB(self, msg): 42 | self.msg.power_state = msg 43 | 44 | def EmergencyStopStateCB(self, msg): 45 | self.msg.emergency_stop_state = msg 46 | 47 | def publish(self): 48 | self.pub.publish(self.msg) 49 | 50 | if __name__ == "__main__": 51 | rospy.init_node("cob_dashboard_aggregator") 52 | rospy.sleep(1) 53 | da = DashboardAggregator() 54 | r = rospy.Rate(1) 55 | while not rospy.is_shutdown(): 56 | da.publish() 57 | try: 58 | r.sleep() 59 | except rospy.exceptions.ROSInterruptException as e: 60 | pass 61 | -------------------------------------------------------------------------------- /cob_dashboard/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['cob_dashboard'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /cob_dashboard/src/cob_dashboard/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_dashboard/src/cob_dashboard/__init__.py -------------------------------------------------------------------------------- /cob_dashboard/src/cob_dashboard/cob_battery.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | 20 | from python_qt_binding.QtCore import QSize 21 | from rqt_robot_dashboard.widgets import BatteryDashWidget 22 | 23 | 24 | class COBBattery(BatteryDashWidget): 25 | """ 26 | Dashboard widget to display COB battery state. 27 | """ 28 | #TODO When nonbutton Dashboard objects are available rebase this widget 29 | def __init__(self, context): 30 | """ 31 | :param context: the plugin context 32 | :type context: qt_gui.plugin.Plugin 33 | """ 34 | super(COBBattery, self).__init__('COB Battery') 35 | self._time_remaining = 0.0 36 | self._charging = False 37 | 38 | self.setFixedSize(self._icons[1].actualSize(QSize(50, 30))) 39 | 40 | self.update_perc(0) 41 | 42 | def set_power_state(self, msg): 43 | """ 44 | Sets button state based on msg 45 | 46 | :param msg: message containing the power state of the COB 47 | :type msg: cob_msgs.PowerState 48 | """ 49 | last_charging = self._charging 50 | last_time_remaining = self._time_remaining 51 | 52 | self._time_remaining = msg.time_remaining 53 | self._charging = msg.charging 54 | if (last_charging != self._charging or last_time_remaining != self._time_remaining): 55 | drain_str = "remaining" 56 | if (self._charging): 57 | drain_str = "to full charge" 58 | self.charging = True 59 | self.setToolTip("Battery: %.2f%% \nTime %s: %.2f Minutes" % (msg.relative_remaining_capacity, drain_str, self._time_remaining * 60.0)) 60 | self.update_perc(msg.relative_remaining_capacity) 61 | 62 | def set_stale(self): 63 | self._charging = 0 64 | self._time_remaining = rospy.rostime.Duration(0) 65 | self.setToolTip("Battery: Stale") 66 | -------------------------------------------------------------------------------- /cob_dashboard/src/cob_dashboard/cob_dashboard.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import roslib 19 | import rospy 20 | 21 | from cob_msgs.msg import DashboardState 22 | 23 | from rqt_robot_dashboard.dashboard import Dashboard 24 | from rqt_robot_dashboard.widgets import MonitorDashWidget, ConsoleDashWidget 25 | 26 | from python_qt_binding.QtCore import QSize 27 | try: 28 | from python_qt_binding.QtWidgets import QMessageBox # Qt 5 29 | except ImportError: 30 | from python_qt_binding.QtGui import QMessageBox # Qt 4 31 | 32 | from .cob_battery import COBBattery 33 | from .cob_runstops import COBRunstops 34 | 35 | 36 | class CobDashboard(Dashboard): 37 | """ 38 | Dashboard for Care-O-bots 39 | 40 | :param context: the plugin context 41 | :type context: qt_gui.plugin.Plugin 42 | """ 43 | def setup(self, context): 44 | self.name = 'CobDashboard' 45 | self.max_icon_size = QSize(50, 30) 46 | self.message = None 47 | 48 | self._dashboard_message = None 49 | self._last_dashboard_message_time = 0.0 50 | 51 | self._raw_byte = None 52 | self.digital_outs = [0, 0, 0] 53 | 54 | self._console = ConsoleDashWidget(self.context, minimal=False) 55 | self._monitor = MonitorDashWidget(self.context) 56 | self._runstop = COBRunstops('RunStops') 57 | self._battery = COBBattery(self.context) 58 | 59 | self._dashboard_agg_sub = rospy.Subscriber("/dashboard_agg", DashboardState, self.db_agg_cb) 60 | 61 | def get_widgets(self): 62 | return [[self._monitor, self._console], [self._runstop], [self._battery]] 63 | 64 | def db_agg_cb(self, msg): 65 | self._last_dashboard_message_time = rospy.get_time() 66 | self._battery.set_power_state(msg.power_state) 67 | 68 | if(msg.emergency_stop_state.emergency_state == 0): 69 | self._runstop.set_ok() 70 | self._runstop.setToolTip(self.tr("Button stop: OK\nScanner stop: OK")) 71 | else: 72 | if msg.emergency_stop_state.emergency_button_stop: 73 | self._runstop.set_button_stop() 74 | elif msg.emergency_stop_state.scanner_stop: 75 | self._runstop.set_scanner_stop() 76 | else: 77 | rospy.logerr("reason for emergency stop not known") 78 | self._runstop.setToolTip(self.tr("Button stop: %s\nScanner stop: %s" %(str(msg.emergency_stop_state.emergency_button_stop), str(msg.emergency_stop_state.scanner_stop)))) 79 | 80 | def shutdown_dashboard(self): 81 | self._dashboard_agg_sub.unregister() 82 | 83 | def save_settings(self, plugin_settings, instance_settings): 84 | self._console.save_settings(plugin_settings, instance_settings) 85 | self._monitor.save_settings(plugin_settings, instance_settings) 86 | 87 | def restore_settings(self, plugin_settings, instance_settings): 88 | self._console.restore_settings(plugin_settings, instance_settings) 89 | self._monitor.restore_settings(plugin_settings, instance_settings) 90 | -------------------------------------------------------------------------------- /cob_dashboard/src/cob_dashboard/cob_runstops.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | from python_qt_binding.QtCore import QSize 19 | 20 | from rqt_robot_dashboard.widgets import IconToolButton 21 | 22 | 23 | class COBRunstops(IconToolButton): 24 | """ 25 | Dashboard widget to display Care-O-bot Runstop state. 26 | """ 27 | def __init__(self, context): 28 | """ 29 | :param context: the plugin context 30 | :type context: qt_gui.plugin.Plugin 31 | """ 32 | 33 | ok_icon = ['bg-green.svg', 'ic-runstop-off.svg'] 34 | button_engaged_icon = ['bg-red.svg', 'ic-runstop-on.svg'] 35 | scanner_engaged_icon = ['bg-red.svg', 'ic-wireless-runstop-on.svg'] 36 | stale_icon = ['bg-grey.svg', 'ic-runstop-off.svg', 'ol-stale-badge.svg'] 37 | 38 | icons = [ok_icon, button_engaged_icon, scanner_engaged_icon, stale_icon] 39 | super(COBRunstops, self).__init__('Runstop', icons, icons) 40 | self.setToolTip('Runstop') 41 | self.set_stale() 42 | self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) 43 | 44 | def set_ok(self): 45 | self.update_state(0) 46 | 47 | def set_button_stop(self): 48 | self.update_state(1) 49 | 50 | def set_scanner_stop(self): 51 | self.update_state(2) 52 | 53 | def set_stale(self): 54 | self.update_state(3) 55 | -------------------------------------------------------------------------------- /cob_helper_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cob_helper_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.37 (2024-10-02) 6 | ------------------- 7 | 8 | 0.7.36 (2024-08-05) 9 | ------------------- 10 | 11 | 0.7.35 (2024-04-30) 12 | ------------------- 13 | * 0.6.35 14 | * update changelogs 15 | * Contributors: fmessmer 16 | 17 | 0.6.35 (2024-04-18) 18 | ------------------- 19 | 20 | 0.6.34 (2024-02-18) 21 | ------------------- 22 | * Merge pull request `#336 `_ from HannesBachter/feature/delay_retry 23 | make delay for retry configurable 24 | * make delay for retry configurable 25 | * Contributors: Felix Messmer, HannesBachter 26 | 27 | 0.6.33 (2023-11-06) 28 | ------------------- 29 | 30 | 0.6.32 (2023-04-29) 31 | ------------------- 32 | 33 | 0.6.31 (2023-01-04) 34 | ------------------- 35 | * Merge pull request `#324 `_ from pgehring/reduce_logging 36 | [kevin] throttle identical loginfo stream 37 | * fix pylint 38 | * only check diagnostics message if parameter true 39 | * throttle identical loginfo stream 40 | * Contributors: Felix Messmer, fmessmer, pgehring 41 | 42 | 0.6.30 (2022-11-17) 43 | ------------------- 44 | 45 | 0.6.29 (2022-07-29) 46 | ------------------- 47 | * Merge pull request `#319 `_ from fmessmer/feature/extend_fake_driver 48 | proper service and diagnostic logic for fake_driver 49 | * proper service and diagnostic logic for fake_driver 50 | * Contributors: Felix Messmer, fmessmer 51 | 52 | 0.6.28 (2022-03-15) 53 | ------------------- 54 | * Merge pull request `#310 `_ from HannesBachter/fix/sleep_init 55 | sleep for 1 second after unsuccessful try to initialize 56 | * sleep for 1 second after unsuccessful try to initialize 57 | * Contributors: Felix Messmer, HannesBachter 58 | 59 | 0.6.27 (2022-01-12) 60 | ------------------- 61 | * Merge pull request `#309 `_ from pgehring/feature/auto_tools 62 | combine auto_init and auto_recover 63 | * check enable state for subscribing and unsubscribing 64 | * fix imports 65 | * fix package name conflict 66 | * Revert "move dynamic reconfigure to python module" 67 | This reverts commit ce13c73a7638b44a8dcf23f75c82d86a83431ad4. 68 | * move dynamic reconfigure to python module 69 | * preserve auto_init and auto_recover nodes 70 | * use params from server to enable init or recover 71 | * setup auto_tools node 72 | * add auto_tools node 73 | * move auto_init and auto_recover to python module 74 | * Contributors: Felix Messmer, pgehring 75 | 76 | 0.6.26 (2021-11-26) 77 | ------------------- 78 | 79 | 0.6.25 (2021-08-02) 80 | ------------------- 81 | 82 | 0.6.24 (2021-07-02) 83 | ------------------- 84 | * Merge pull request `#304 `_ from fmessmer/fixup_auto_tools 85 | fixup condition in auto_recover 86 | * fixup condition in auto_recover 87 | * Contributors: Felix Messmer, fmessmer 88 | 89 | 0.6.23 (2021-07-01) 90 | ------------------- 91 | * Merge pull request `#303 `_ from fmessmer/enhance_auto_tools 92 | enhance auto tools 93 | * tmp: debug logoutput for auto_recover for disabled cases 94 | * introduce em_state_ignore for auto_init 95 | * allow infinite auto_init 96 | * use message enum 97 | * Contributors: Felix Messmer, fmessmer 98 | 99 | 0.6.22 (2021-05-10) 100 | ------------------- 101 | 102 | 0.6.21 (2021-04-06) 103 | ------------------- 104 | * Merge pull request `#301 `_ from HannesBachter/fix/visualize_in_map 105 | remove leading / in frame_id 106 | * remove leading / in frame_id 107 | * Contributors: Felix Messmer, HannesBachter 108 | 109 | 0.6.20 (2021-01-25) 110 | ------------------- 111 | 112 | 0.6.19 (2020-12-02) 113 | ------------------- 114 | * Merge pull request `#287 `_ from fmessmer/fix_catkin_lint 115 | fix catkin_lint 116 | * fix catkin_lint 117 | * Contributors: Felix Messmer, fmessmer 118 | 119 | 0.6.18 (2020-10-21) 120 | ------------------- 121 | 122 | 0.6.17 (2020-10-17) 123 | ------------------- 124 | * Merge pull request `#284 `_ from fmessmer/test_noetic 125 | test noetic 126 | * Bump CMake version to avoid CMP0048 warning 127 | * Contributors: Felix Messmer, fmessmer 128 | 129 | 0.6.16 (2020-03-18) 130 | ------------------- 131 | * Merge pull request `#270 `_ from LoyVanBeek/feature/python3_compatibility 132 | [ci_updates] pylint + Python3 compatibility 133 | * python3 compatibility via 2to3 134 | * Merge pull request `#271 `_ from fmessmer/ci_updates 135 | [travis] ci updates 136 | * catkin_lint fixes 137 | * Merge pull request `#265 `_ from HannesBachter/feature/recover_service 138 | add services to en- and disable auto recover 139 | * add services to en- and disable auto recover 140 | add services to en- and disable auto recover 141 | * Contributors: Felix Messmer, Loy van Beek, fmessmer, hyb 142 | 143 | 0.6.15 (2019-11-07) 144 | ------------------- 145 | * Merge pull request `#253 `_ from benmaidel/fix/visualize_nav_goals 146 | visualize nav_goals improvements 147 | * improvements: 148 | - add every nav_goal to a separate namespace (enable/disable visual) 149 | - fix scaling for Arrow marker 150 | * Contributors: Benjamin Maidel, Florian Weisshardt 151 | 152 | 0.6.14 (2019-08-07) 153 | ------------------- 154 | 155 | 0.6.13 (2019-07-19) 156 | ------------------ 157 | 158 | 0.6.12 (2019-06-07) 159 | ------------------- 160 | 161 | 0.6.11 (2019-04-05) 162 | ------------------- 163 | 164 | 0.6.10 (2019-03-14) 165 | ------------------- 166 | * Merge pull request `#239 `_ from fmessmer/max_retry_auto_init 167 | introduce max_retries for auto_init 168 | * introduce max_retries for auto_init 169 | * Merge pull request `#231 `_ from fmessmer/auto_recover_diagnostics_based 170 | fix typo 171 | * fix typo 172 | * Merge pull request `#230 `_ from fmessmer/auto_recover_diagnostics_based 173 | do not auto-recover based on diagnostics 174 | * do not auto-recover based on diagnostics 175 | * Merge pull request `#229 `_ from HannesBachter/fix/auto_recover 176 | case insensitive comparison 177 | * case insensitive comparison 178 | * Contributors: Felix Messmer, fmessmer, hyb 179 | 180 | 0.6.9 (2018-07-21) 181 | ------------------ 182 | * update maintainer 183 | * Contributors: ipa-fxm 184 | 185 | 0.6.8 (2018-07-21) 186 | ------------------ 187 | * Merge pull request `#217 `_ from HannesBachter/feature/textsize_navgoal 188 | enable setting textsize for visualization of navigation goals 189 | * use dynamic reconfigure for text size 190 | * enable setting textsize of navigation goals 191 | * Contributors: Richard Bormann, hyb, ipa-fxm 192 | 193 | 0.6.7 (2018-01-07) 194 | ------------------ 195 | * Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev 196 | * Merge pull request `#213 `_ from ipa-fxm/no_recover_em_stop 197 | do not recover on em_stop 198 | * do not recover on em_stop 199 | * Merge pull request `#212 `_ from ipa-fxm/enhance_auto_recover_logic 200 | enhance auto_recover logic 201 | * enhance auto_recover logic 202 | * Merge pull request `#197 `_ from ipa-fxm/APACHE_license 203 | use license apache 2.0 204 | * use license apache 2.0 205 | * Contributors: Felix Messmer, Florian Weisshardt, ipa-fxm, ipa-uhr-mk 206 | 207 | 0.6.6 (2017-07-17) 208 | ------------------ 209 | * move visualize navigation goals to cob_helper_tools 210 | * retry init on failure 211 | * only store timestamp for last recover on success 212 | * add fake_diagnostics 213 | * add fake_driver 214 | * added license header 215 | * evaluate handle and better output 216 | * add auto_init 217 | * add auto_recover to new cob_helper_tools pkg 218 | * Contributors: Florian Weisshardt, ipa-fxm, robot 219 | -------------------------------------------------------------------------------- /cob_helper_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_helper_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation) 5 | 6 | catkin_python_setup() 7 | 8 | generate_dynamic_reconfigure_options( 9 | cfg/HelperTools.cfg 10 | ) 11 | 12 | catkin_package( 13 | CATKIN_DEPENDS dynamic_reconfigure 14 | ) 15 | 16 | ############# 17 | ## Install ## 18 | ############# 19 | 20 | catkin_install_python(PROGRAMS 21 | scripts/auto_tools.py 22 | scripts/auto_init.py 23 | scripts/auto_recover.py 24 | scripts/fake_diagnostics.py 25 | scripts/fake_driver.py 26 | scripts/visualize_navigation_goals.py 27 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 28 | ) 29 | -------------------------------------------------------------------------------- /cob_helper_tools/cfg/HelperTools.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "cob_helper_tools" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("text_size", double_t, 0, "size parameter for marker scale", 0.5, 0.0, 10.0) 9 | 10 | exit(gen.generate(PACKAGE, "cob_helper_tools", "HelperTools")) 11 | -------------------------------------------------------------------------------- /cob_helper_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | cob_helper_tools 7 | 0.7.37 8 | Helper scripts for Care-O-bot 9 | 10 | Felix Messmer 11 | Felix Messmer 12 | 13 | Apache 2.0 14 | 15 | catkin 16 | python-setuptools 17 | python3-setuptools 18 | 19 | dynamic_reconfigure 20 | message_generation 21 | message_runtime 22 | 23 | cob_msgs 24 | cob_script_server 25 | diagnostic_msgs 26 | rospy 27 | std_srvs 28 | tf 29 | visualization_msgs 30 | 31 | 32 | -------------------------------------------------------------------------------- /cob_helper_tools/scripts/auto_init.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | from cob_auto_tools.auto_init import AutoInit 20 | 21 | 22 | if __name__ == "__main__": 23 | rospy.init_node("auto_init") 24 | rospy.loginfo("auto init running") 25 | AI = AutoInit() 26 | rospy.loginfo("auto init finished") 27 | -------------------------------------------------------------------------------- /cob_helper_tools/scripts/auto_recover.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import copy 19 | import rospy 20 | 21 | from cob_auto_tools.auto_recover import AutoRecover 22 | 23 | if __name__ == "__main__": 24 | rospy.init_node("auto_recover") 25 | AR = AutoRecover() 26 | rospy.loginfo("auto recover running") 27 | rospy.spin() 28 | -------------------------------------------------------------------------------- /cob_helper_tools/scripts/auto_tools.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from cob_auto_tools.auto_init import AutoInit 5 | from cob_auto_tools.auto_recover import AutoRecover 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node("auto_tools") 9 | rospy.loginfo("auto_tools running") 10 | 11 | if rospy.get_param('~enable_auto_init', True): 12 | AI = AutoInit() 13 | 14 | if rospy.get_param('~enable_auto_recover', True): 15 | AR = AutoRecover() 16 | 17 | rospy.spin() 18 | -------------------------------------------------------------------------------- /cob_helper_tools/scripts/fake_diagnostics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus 20 | 21 | class FakeDiagnostics(): 22 | def __init__(self, options): 23 | self._options = options 24 | self._last_publish_time = 0 25 | self._fake_diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) 26 | rospy.Timer(rospy.Duration(1.0), self.publish_stats) 27 | 28 | def publish_stats(self, event): 29 | msg = DiagnosticArray() 30 | msg.header.stamp = rospy.get_rostime() 31 | # Add all fake 32 | hostname_list = self._options.diag_hostnames.split(", ") 33 | for hostname in hostname_list: 34 | status = DiagnosticStatus() 35 | status.name = hostname 36 | status.level = DiagnosticStatus.OK 37 | status.message = "fake diagnostics" 38 | status.hardware_id = hostname 39 | msg.status.append(status) 40 | 41 | self._fake_diag_pub.publish(msg) 42 | 43 | if __name__ == '__main__': 44 | 45 | import optparse 46 | parser = optparse.OptionParser(usage="usage: fake_diagnostics.py [--diag-hostnames=hostname1, hostname2, ...]") 47 | parser.add_option("--diag-hostnames", dest="diag_hostnames", 48 | help="Fake Diagnostics") 49 | options, args = parser.parse_args(rospy.myargv()) 50 | 51 | rospy.init_node('fake_diagnostics') 52 | 53 | fake_diagnostics = FakeDiagnostics(options) 54 | 55 | rospy.spin() 56 | -------------------------------------------------------------------------------- /cob_helper_tools/scripts/fake_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | from std_srvs.srv import Trigger, TriggerResponse 20 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 21 | 22 | class FakeDriver(): 23 | 24 | def __init__(self): 25 | self.init_srv = rospy.Service('driver/init', Trigger, self.init_cb) 26 | self.recover_srv = rospy.Service('driver/recover', Trigger, self.recover_cb) 27 | self.halt_srv = rospy.Service('driver/halt', Trigger, self.halt_cb) 28 | self.shutdown_srv = rospy.Service('driver/shutdown', Trigger, self.shutdown_cb) 29 | 30 | self.initialized = False 31 | self.active = False 32 | 33 | self._fake_diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) 34 | rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) 35 | 36 | def publish_diagnostics(self, event): 37 | msg = DiagnosticArray() 38 | msg.header.stamp = rospy.get_rostime() 39 | 40 | status = DiagnosticStatus() 41 | status.name = rospy.get_name() 42 | if self.initialized: 43 | if self.active: 44 | status.level = DiagnosticStatus.OK 45 | status.message = "active" 46 | else: 47 | status.level = DiagnosticStatus.WARN 48 | status.message = "not active" 49 | else: 50 | status.level = DiagnosticStatus.WARN 51 | status.message = "not initialized" 52 | status.hardware_id = rospy.get_name() 53 | status.values.append(KeyValue(key="initialized", value=str(self.initialized))) 54 | status.values.append(KeyValue(key="active", value=str(self.active))) 55 | msg.status.append(status) 56 | 57 | self._fake_diag_pub.publish(msg) 58 | 59 | def init_cb(self, req): 60 | rospy.loginfo("init_cb") 61 | self.initialized = True 62 | self.active = True 63 | return TriggerResponse(success=True) 64 | 65 | def recover_cb(self, req): 66 | rospy.loginfo("recover_cb") 67 | self.active = True 68 | return TriggerResponse(success=True) 69 | 70 | def halt_cb(self, req): 71 | rospy.loginfo("halt_cb") 72 | self.active = False 73 | return TriggerResponse(success=True) 74 | 75 | def shutdown_cb(self, req): 76 | rospy.loginfo("shutdown_cb") 77 | self.initialized = False 78 | self.active = False 79 | return TriggerResponse(success=True) 80 | 81 | 82 | if __name__ == "__main__": 83 | rospy.init_node('fake_driver') 84 | FakeDriver() 85 | rospy.loginfo("fake_driver running") 86 | rospy.spin() 87 | 88 | -------------------------------------------------------------------------------- /cob_helper_tools/scripts/visualize_navigation_goals.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | import rospy 20 | import tf 21 | from visualization_msgs.msg import Marker 22 | from visualization_msgs.msg import MarkerArray 23 | from dynamic_reconfigure.server import Server 24 | from cob_helper_tools.cfg import HelperToolsConfig 25 | 26 | class VisualizerNavigationGoals(): 27 | def __init__(self): 28 | self.text_size = 0.5 29 | self.srv = Server(HelperToolsConfig, self.reconfigure_callback) 30 | self.pubGoals = rospy.Publisher('visualize_navigation_goals', MarkerArray, queue_size=1, latch=True) 31 | 32 | def reconfigure_callback(self, config, level): 33 | self.text_size = config.text_size 34 | return config 35 | 36 | def pubMarker(self): 37 | navigation_goals = rospy.get_param("/script_server/base", {}) 38 | 39 | markerarray = MarkerArray() 40 | i=0 41 | for name, pose in list(navigation_goals.items()): 42 | 43 | # check if pose is valid 44 | if len(pose) != 3: 45 | continue 46 | 47 | # arrow 48 | marker_arrow = Marker() 49 | marker_arrow.header.stamp = rospy.Time.now() 50 | marker_arrow.header.frame_id = "map" 51 | marker_arrow.ns = "/"+name 52 | marker_arrow.id = i 53 | marker_arrow.type = Marker.ARROW 54 | marker_arrow.action = Marker.ADD 55 | marker_arrow.scale.x = 1.0 56 | marker_arrow.scale.y = 0.1 57 | marker_arrow.scale.z = 0.1 58 | marker_arrow.color.r = 0.0 59 | marker_arrow.color.g = 0.0 60 | marker_arrow.color.b = 1.0 61 | marker_arrow.color.a = 1.0 62 | marker_arrow.pose.position.x = pose[0] 63 | marker_arrow.pose.position.y = pose[1] 64 | marker_arrow.pose.position.z = 0.2 65 | quaternion = tf.transformations.quaternion_from_euler(0, 0, pose[2]) 66 | marker_arrow.pose.orientation.x = quaternion[0] 67 | marker_arrow.pose.orientation.y = quaternion[1] 68 | marker_arrow.pose.orientation.z = quaternion[2] 69 | marker_arrow.pose.orientation.w = quaternion[3] 70 | markerarray.markers.append(marker_arrow) 71 | 72 | # text 73 | marker_text = Marker() 74 | marker_text.header.stamp = rospy.Time.now() 75 | marker_text.header.frame_id = "map" 76 | marker_text.ns = "/"+name 77 | marker_text.id = i + 1000000 78 | marker_text.type = Marker.TEXT_VIEW_FACING 79 | marker_text.action = Marker.ADD 80 | marker_text.scale.z = self.text_size 81 | marker_text.color.r = 0.0 82 | marker_text.color.g = 0.0 83 | marker_text.color.b = 1.0 84 | marker_text.color.a = 1.0 85 | marker_text.pose.position.x = pose[0] 86 | marker_text.pose.position.y = pose[1] + 0.2 87 | marker_text.pose.position.z = 0.2 88 | quaternion = tf.transformations.quaternion_from_euler(0, 0, pose[2]) 89 | marker_text.pose.orientation.x = quaternion[0] 90 | marker_text.pose.orientation.y = quaternion[1] 91 | marker_text.pose.orientation.z = quaternion[2] 92 | marker_text.pose.orientation.w = quaternion[3] 93 | marker_text.text = name 94 | markerarray.markers.append(marker_text) 95 | 96 | i = i + 1 97 | 98 | self.pubGoals.publish(markerarray) 99 | 100 | if __name__ == "__main__": 101 | rospy.init_node('navigation_goal_visualizer') 102 | p = VisualizerNavigationGoals() 103 | r = rospy.Rate(1) 104 | while not rospy.is_shutdown(): 105 | p.pubMarker() 106 | try: 107 | r.sleep() 108 | except rospy.exceptions.ROSInterruptException as e: 109 | pass 110 | -------------------------------------------------------------------------------- /cob_helper_tools/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['cob_auto_tools'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) -------------------------------------------------------------------------------- /cob_helper_tools/src/cob_auto_tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_helper_tools/src/cob_auto_tools/__init__.py -------------------------------------------------------------------------------- /cob_helper_tools/src/cob_auto_tools/auto_init.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | 20 | from cob_msgs.msg import EmergencyStopState 21 | 22 | from simple_script_server import * 23 | sss = simple_script_server() 24 | 25 | class AutoInit(): 26 | 27 | def __init__(self): 28 | self.components = rospy.get_param('~components', {}) 29 | self.max_retries = rospy.get_param('~max_retries', 50) 30 | self.retry_delay = rospy.get_param('~retry_delay', 1.0) 31 | self.em_state_ignore = rospy.get_param('~em_state_ignore', False) 32 | self.em_state = EmergencyStopState.EMSTOP 33 | rospy.Subscriber("/emergency_stop_state", EmergencyStopState, self.em_cb, queue_size=1) 34 | 35 | # wait for all components to start 36 | for component in list(self.components.keys()): 37 | rospy.loginfo("[auto_init]: Waiting for %s to start...", component) 38 | rospy.wait_for_service("/" + component + "/driver/init") 39 | 40 | # wait for emergency_stop to be released 41 | while not rospy.is_shutdown(): 42 | if not self.em_state_ignore and self.em_state == EmergencyStopState.EMSTOP: 43 | rospy.loginfo("[auto_init]: Waiting for emergency stop to be released...") 44 | try: 45 | rospy.sleep(1) 46 | except rospy.exceptions.ROSInterruptException as e: 47 | pass 48 | else: # EMFREE or EMCONFIRMED 49 | # call init for all components 50 | rospy.loginfo("[auto_init]: Initializing components") 51 | for component in list(self.components.keys()): 52 | retries = 0 53 | while not rospy.is_shutdown(): 54 | if self.max_retries > 0 and retries >= self.max_retries: 55 | rospy.logerr("[auto_init]: Could not initialize %s after %s retries", component, str(retries)) 56 | break 57 | retries += 1 58 | handle = sss.init(component) 59 | if not (handle.get_error_code() == 0): 60 | rospy.logerr("[auto_init]: Could not initialize %s. Retrying...(%s of %s, waiting for %.1f s to retry...)", component, str(retries), str(self.max_retries), self.retry_delay) 61 | rospy.sleep(self.retry_delay) 62 | else: 63 | rospy.loginfo("[auto_init]: Component %s initialized successfully", component) 64 | break 65 | break # done 66 | 67 | def em_cb(self, msg): 68 | self.em_state = msg.emergency_state 69 | -------------------------------------------------------------------------------- /cob_helper_tools/src/cob_auto_tools/auto_recover.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import copy 19 | import rospy 20 | 21 | from cob_msgs.msg import EmergencyStopState 22 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus 23 | from std_srvs.srv import Trigger, TriggerResponse 24 | 25 | from simple_script_server import * 26 | sss = simple_script_server() 27 | 28 | class AutoRecover(): 29 | 30 | def __init__(self): 31 | now = rospy.Time.now() 32 | self.em_state = EmergencyStopState.EMFREE 33 | self.recover_emergency = rospy.get_param('~recover_emergency', True) 34 | self.recover_diagnostics = rospy.get_param('~recover_diagnostics', True) 35 | self.components = rospy.get_param('~components', {}) 36 | self.components_recover_time = {} 37 | for component in list(self.components.keys()): 38 | self.components_recover_time[component] = now 39 | 40 | rospy.Service('~enable',Trigger,self.enable_cb) 41 | rospy.Service('~disable',Trigger,self.disable_cb) 42 | self.enabled = True 43 | self.subscribe() 44 | 45 | # auto recover based on emergency stop 46 | def em_cb(self, msg): 47 | if msg.emergency_state == EmergencyStopState.EMFREE and self.em_state != EmergencyStopState.EMFREE: 48 | if not self.recover_emergency: 49 | rospy.loginfo("auto_recover from emergency state is disabled") 50 | else: 51 | rospy.loginfo("auto_recover from emergency state") 52 | self.recover(list(self.components.keys())) 53 | self.em_state = copy.deepcopy(msg.emergency_state) 54 | 55 | # auto recover based on diagnostics 56 | def diagnostics_cb(self, msg): 57 | if self.recover_diagnostics: 58 | for status in msg.status: 59 | for component in list(self.components.keys()): 60 | if status.name.lower().startswith(self.components[component].lower()) and status.level > DiagnosticStatus.OK and self.em_state == EmergencyStopState.EMFREE and (rospy.Time.now() - self.components_recover_time[component] > rospy.Duration(10)): 61 | rospy.loginfo("auto_recover from diagnostic failure") 62 | self.recover([component]) 63 | else: 64 | rospy.loginfo_once("auto_recover from diagnostic failure is disabled") # pylint: disable=no-member 65 | 66 | # callback for enable service 67 | def enable_cb(self, req): 68 | if not self.enabled: 69 | self.enabled = True 70 | self.subscribe() 71 | return TriggerResponse(True, "auto recover enabled") 72 | 73 | # callback for disable service 74 | def disable_cb(self, req): 75 | if self.enabled: 76 | self.enabled = False 77 | self.unsubscribe() 78 | return TriggerResponse(True, "auto recover disabled") 79 | 80 | def subscribe(self): 81 | self.em_stop_state_subscriber = rospy.Subscriber("/emergency_stop_state", EmergencyStopState, self.em_cb, queue_size=1) 82 | self.diagnostics_subscriber = rospy.Subscriber("/diagnostics_agg", DiagnosticArray, self.diagnostics_cb, queue_size=1) 83 | 84 | def unsubscribe(self): 85 | self.em_stop_state_subscriber.unregister() 86 | self.diagnostics_subscriber.unregister() 87 | 88 | def recover(self, components): 89 | if self.enabled: 90 | for component in components: 91 | handle = sss.recover(component) 92 | if not (handle.get_error_code() == 0): 93 | rospy.logerr("[auto_recover]: Could not recover %s", component) 94 | else: 95 | rospy.loginfo("[auto_recover]: Component %s recovered successfully", component) 96 | self.components_recover_time[component] = rospy.Time.now() 97 | -------------------------------------------------------------------------------- /cob_monitoring/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_monitoring) 3 | 4 | find_package(catkin REQUIRED COMPONENTS diagnostic_updater roscpp topic_tools) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package( 9 | CATKIN_DEPENDS roscpp topic_tools 10 | ) 11 | 12 | include_directories(${catkin_INCLUDE_DIRS}) 13 | 14 | add_executable(topic_status_monitor src/topic_status_monitor.cpp) 15 | target_link_libraries(topic_status_monitor ${catkin_LIBRARIES}) 16 | 17 | ### Install ### 18 | catkin_install_python(PROGRAMS 19 | src/battery_monitor.py 20 | src/cpu_monitor.py 21 | src/ddwrt.py 22 | src/emergency_stop_monitor.py 23 | src/hd_monitor.py 24 | src/hz_monitor.py 25 | src/net_monitor.py 26 | src/ntp_monitor.py 27 | src/wifi_monitor.py 28 | src/wlan_monitor.py 29 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 30 | ) 31 | 32 | install(TARGETS topic_status_monitor 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | -------------------------------------------------------------------------------- /cob_monitoring/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | cob_monitoring 7 | 0.7.37 8 | cob_monitoring 9 | 10 | Apache 2.0 11 | 12 | http://ros.org/wiki/cob_monitoring 13 | 14 | 15 | Felix Messmer 16 | Florian Weisshardt 17 | Felix Messmer 18 | 19 | catkin 20 | python-setuptools 21 | python3-setuptools 22 | 23 | diagnostic_updater 24 | roscpp 25 | topic_tools 26 | 27 | actionlib 28 | cob_light 29 | cob_msgs 30 | cob_script_server 31 | diagnostic_msgs 32 | ifstat 33 | ipmitool 34 | ntpdate 35 | python-packaging 36 | python3-packaging 37 | python-paramiko 38 | python3-paramiko 39 | python-mechanize 40 | python3-mechanize 41 | python-psutil 42 | python3-psutil 43 | python-requests 44 | python3-requests 45 | rospy 46 | rostopic 47 | sensor_msgs 48 | std_msgs 49 | sysstat 50 | 51 | 52 | -------------------------------------------------------------------------------- /cob_monitoring/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup(packages=["netdata_interface"], package_dir={"": "src"}) 7 | 8 | setup(**d) 9 | -------------------------------------------------------------------------------- /cob_monitoring/src/ddwrt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import os, sys, string, time, getopt, re 19 | import io 20 | 21 | import mechanize 22 | import csv 23 | import gc 24 | 25 | import rospy 26 | from std_msgs.msg import Header 27 | from cob_msgs.msg import AccessPoint, Network, SiteSurvey 28 | 29 | def breakUpTrash(): 30 | for item in gc.garbage: 31 | if type(item) == dict: 32 | for s in item.keys_iter(): 33 | del item[s] 34 | del gc.garbage[:] 35 | 36 | class WifiAP: 37 | def __init__(self, hostname, username, password): 38 | self.hostname = hostname 39 | self.username = username 40 | self.password = password 41 | 42 | def newBrowser(self): 43 | # Create new browsers all the time because its data structures grow 44 | # unboundedly (texas#135) 45 | br = mechanize.Browser() 46 | #br.add_password(self.hostname, self.username, self.password) 47 | #br.set_handle_robots(None) 48 | return br 49 | 50 | def fetchSiteSurvey(self): 51 | url = "http://%s/Site_Survey.asp" % self.hostname 52 | 53 | browser = self.newBrowser() 54 | response = browser.open(url) # pylint: disable=assignment-from-none 55 | 56 | body = response.read() 57 | 58 | #make sure that we put a stamp on things 59 | header = Header() 60 | header.stamp = rospy.Time.now() 61 | networks = [] 62 | survey = SiteSurvey(header, networks) 63 | 64 | lines = body.split("\n") 65 | for i in range(len(lines)): 66 | if lines[i].startswith("var table = "): 67 | break 68 | 69 | aplines = [] 70 | for j in range(i+1, len(lines)): 71 | if lines[j].startswith(");"): break 72 | line = lines[j].strip() 73 | if not line: continue 74 | if line[0] == ",": line = line[1:] 75 | 76 | aplines.append(line) 77 | 78 | fp = io.StringIO('\n'.join(aplines)) 79 | reader = csv.reader(fp) 80 | for row in reader: 81 | essid = row[0] 82 | macattr = row[2] 83 | channel = row[3] 84 | rssi = row[4] 85 | noise = row[5] 86 | beacon = row[6] 87 | 88 | network = Network(macattr, essid, channel, rssi, noise, beacon) 89 | survey.networks.append(network) 90 | return survey 91 | 92 | def fetchBandwidthStats(self, interface): 93 | url = "http://%s/fetchif.cgi?%s" % (self.hostname, interface) 94 | 95 | browser = self.newBrowser() 96 | response = browser.open(url) # pylint: disable=assignment-from-none 97 | body = response.read() 98 | 99 | lines = body.split("\n") 100 | 101 | if len(lines) > 1: 102 | line = lines[1].strip() 103 | iparts = line.split(":", 1) 104 | parts = iparts[1].split() 105 | print(interface, parts) 106 | 107 | 108 | def fetchCurrentAP(self): 109 | url = "http://%s/Status_Wireless.live.asp" % self.hostname 110 | 111 | browser = self.newBrowser() 112 | response = browser.open(url) # pylint: disable=assignment-from-none 113 | body = response.read() 114 | 115 | line = None 116 | lines = body.split("\n") 117 | 118 | d = {} 119 | for line in lines: 120 | line = line[1:-1] 121 | line = line.replace(" ", "") 122 | parts = line.split("::", 1) 123 | if len(parts) == 2: 124 | d[parts[0]] = parts[1] 125 | 126 | essid = d.get('wl_ssid', '') 127 | wl_channel = d.get('wl_channel', '').split()[0] 128 | channel = int(wl_channel) 129 | rate = d.get('wl_rate', '') 130 | 131 | signal = None 132 | noise = None 133 | snr = None 134 | quality = None 135 | 136 | tx_power = d.get('wl_xmit', '') 137 | 138 | active_wireless = d.get('active_wireless', None) 139 | ap = None 140 | if active_wireless: 141 | active_wireless = active_wireless.replace("'", "") 142 | parts = active_wireless.split(",") 143 | macaddr = parts[0] 144 | #interface = parts[1] 145 | if len(parts) == 7: 146 | signal = int(parts[4]) 147 | noise = int(parts[5]) 148 | snr = int(parts[6]) 149 | quality = signal * 1.24 + 116 150 | else: 151 | signal = int(parts[5]) 152 | noise = int(parts[6]) 153 | snr = int(parts[7]) 154 | quality = int(parts[8])/10 155 | 156 | #self.fetchBandwidthStats(interface) 157 | 158 | #make sure that we put a stamp on things 159 | header = Header() 160 | header.stamp = rospy.Time.now() 161 | 162 | ap = AccessPoint(header=header, 163 | essid=essid, 164 | macaddr=macaddr, 165 | signal=signal, 166 | noise=noise, 167 | snr=snr, 168 | channel=channel, 169 | rate=rate, 170 | quality=quality, 171 | tx_power=tx_power) 172 | 173 | return ap 174 | 175 | 176 | def loop(): 177 | rospy.init_node("wifi_ddwrt") 178 | 179 | router_ip = rospy.get_param('~router', 'wifi-router') 180 | username = rospy.get_param('~username', 'root') 181 | password = rospy.get_param('~password', '') 182 | 183 | ap = WifiAP(router_ip, username, password) 184 | 185 | pub1 = rospy.Publisher("ddwrt/sitesurvey", SiteSurvey, queue_size=50 ) 186 | pub2 = rospy.Publisher("ddwrt/accesspoint", AccessPoint, queue_size=50 ) 187 | 188 | r = rospy.Rate(.5) 189 | lastTime = 0 190 | last_ex = '' 191 | while not rospy.is_shutdown(): 192 | breakUpTrash() # Needed because mechanize leaves data structures that the GC sees as uncollectable (texas#135) 193 | try: 194 | if time.time() - lastTime > 60: 195 | survey = ap.fetchSiteSurvey() 196 | pub1.publish(survey) 197 | lastTime = time.time() 198 | node = ap.fetchCurrentAP() 199 | if node: pub2.publish(node) 200 | last_ex = '' 201 | except Exception as e: 202 | if e != last_ex: 203 | rospy.logwarn("Caught exception %s" % e) 204 | last_ex = e 205 | r.sleep() 206 | 207 | def test(): 208 | router_ip = rospy.get_param('~router_ip', 'wifi-router') 209 | username = rospy.get_param('~username', 'root') 210 | password = rospy.get_param('~password', '') 211 | 212 | ap = WifiAP(router_ip, username, password) 213 | while 1: 214 | if 0: 215 | survey = ap.fetchSiteSurvey() 216 | print(survey) 217 | if 1: 218 | node = ap.fetchCurrentAP() 219 | print(node) 220 | 221 | def usage(progname): 222 | print(__doc__ % vars()) 223 | 224 | def main(argv, stdout, environ): 225 | progname = argv[0] 226 | optlist, _ = getopt.getopt(argv[1:], "", ["help", "test",]) 227 | 228 | testflag = 0 229 | 230 | for (field, _) in optlist: 231 | if field == "--help": 232 | usage(progname) 233 | return 234 | elif field == "--test": 235 | testflag = 1 236 | 237 | if testflag: 238 | test() 239 | return 240 | 241 | loop() 242 | 243 | if __name__ == "__main__": 244 | main(sys.argv, sys.stdout, os.environ) 245 | 246 | 247 | -------------------------------------------------------------------------------- /cob_monitoring/src/hz_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | 20 | import rospy 21 | import rostopic 22 | import copy 23 | 24 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 25 | 26 | class HzTest(): 27 | def __init__(self): 28 | rospy.init_node("hz_monitor") 29 | 30 | # get parameters 31 | try: 32 | # topic to test 33 | self.topics = rospy.get_param('~topics') 34 | # expected publishing rate 35 | self.hz = rospy.get_param('~hz') 36 | # margin of error allowed 37 | self.hzerror = rospy.get_param('~hzerror') 38 | # length of test 39 | self.window_size = float(rospy.get_param('~window_size', 100)) 40 | # name for diagnostic message 41 | self.diagnostics_name = rospy.get_param('~diagnostics_name',"") 42 | self.diagnostics_name = self.diagnostics_name.replace('/','_') 43 | self.diagnostics_period = rospy.Duration(1.0) 44 | except KeyError as e: 45 | rospy.logerr('hztest not initialized properly. Parameter [{}] not set. debug[{}] debug[{}]'.format(e, rospy.get_caller_id(), rospy.resolve_name(e.args[0]))) 46 | sys.exit(1) 47 | 48 | self.rt_HZ_store = dict() 49 | self.missing_topics = copy.deepcopy(self.topics) 50 | self.pub_diagnostics = rospy.Publisher('~diagnostics', DiagnosticArray, queue_size = 1) 51 | self.timer_diagnostics = rospy.Timer(self.diagnostics_period, self.publish_diagnostics) 52 | 53 | 54 | def run(self): 55 | # wait for first message 56 | while len(self.missing_topics) != 0 and not rospy.is_shutdown(): 57 | for topic in self.topics: 58 | msg_class, real_topic, msg_eval = rostopic.get_topic_class(topic, blocking=False) #pause hz until topic is published 59 | if real_topic: 60 | if real_topic in self.missing_topics: 61 | self.missing_topics.remove(real_topic) 62 | 63 | try: 64 | rospy.logdebug("hz monitor is waiting for type of topics {}.".format(self.missing_topics)) 65 | rospy.sleep(1.0) 66 | except rospy.exceptions.ROSInterruptException: 67 | pass 68 | 69 | # call rostopic hz 70 | self.rt_HZ_store = dict() 71 | for topic in self.topics: 72 | rt = rostopic.ROSTopicHz(self.window_size) 73 | rospy.Subscriber(topic, rospy.AnyMsg, rt.callback_hz) 74 | self.rt_HZ_store[topic] = rt 75 | rospy.loginfo("subscribed to [{}]".format(topic)) 76 | 77 | def publish_diagnostics(self, event): 78 | # set desired rates 79 | if isinstance(self.hzerror, float) or isinstance(self.hzerror, int): 80 | if self.hzerror < 0: 81 | rospy.logerr("hzerror cannot be negative") 82 | sys.exit(1) 83 | min_rate = self.hz - self.hzerror 84 | max_rate = self.hz + self.hzerror 85 | if min_rate < 0 or max_rate < 0: 86 | rospy.logerr("min_rate/max_rate cannot be negative") 87 | sys.exit(1) 88 | min_duration = 1/min_rate if min_rate > 0 else float('inf') 89 | max_duration = 1/max_rate if max_rate > 0 else float('inf') 90 | else: 91 | rospy.logerr("hzerror not float or int") 92 | sys.exit(1) 93 | 94 | # create diagnostic message 95 | array = DiagnosticArray() 96 | array.header.stamp = rospy.Time.now() 97 | hz_status = DiagnosticStatus() 98 | hz_status.name = self.diagnostics_name 99 | hz_status.level = DiagnosticStatus.OK 100 | hz_status.message = 'all publishing rates are ok' 101 | hz_status.values.append(KeyValue("topics", str(self.topics))) 102 | hz_status.values.append(KeyValue("desired_rate", str(self.hz))) 103 | hz_status.values.append(KeyValue("min_rate", str(min_rate))) 104 | hz_status.values.append(KeyValue("max_rate", str(max_rate))) 105 | hz_status.values.append(KeyValue("window_size", str(self.window_size))) 106 | rates = [] 107 | 108 | consolidated_error_messages = {} ## store and display consolidated erros messages for all the topics 109 | consolidated_error_messages.setdefault("never received message for topics", []) 110 | consolidated_error_messages.setdefault("no messages anymore for topics", []) 111 | consolidated_error_messages.setdefault("publishing rate is too low for topics", []) 112 | consolidated_error_messages.setdefault("publishing rate is too high for topics", []) 113 | 114 | if not all (k in self.rt_HZ_store for k in self.topics): 115 | hz_status.level = DiagnosticStatus.WARN 116 | hz_status.message = "could not determine type for topics {}. Probably never published on that topics.".format(self.missing_topics) 117 | array.status.append(hz_status) 118 | self.pub_diagnostics.publish(array) 119 | return 120 | 121 | # calculate actual rates 122 | for topic, rt in self.rt_HZ_store.items(): 123 | #print("rt.times {}".format(rt.times)) 124 | #print("rt.msg_tn {}".format(rt.msg_tn)) 125 | #print("rt.last_printed_tn {}".format(rt.last_printed_tn)) 126 | #print("rt.delta: {}".format(rt.msg_tn - rt.last_printed_tn)) 127 | #print("event.current_real: {}".format(event.current_real.to_sec())) 128 | #print("event.last_real: {}".format(event.last_real.to_sec())) 129 | #print("event.delta: {}".format((event.current_real - event.last_real).to_sec())) 130 | if not rt or not rt.times: 131 | hz_status.level = DiagnosticStatus.ERROR 132 | rates.append(0.0) 133 | consolidated_error_messages["never received message for topics"].append(topic) 134 | elif rt.msg_tn == rt.last_printed_tn \ 135 | and not (event.current_real.to_sec() < rt.msg_tn + min_duration): # condition to prevent WARN for topics with hz 0. else 0 144 | rt.last_printed_tn = rt.msg_tn 145 | rates.append(round(rate, 2)) 146 | if min_rate and rate < min_rate: 147 | hz_status.level = DiagnosticStatus.WARN 148 | consolidated_error_messages["publishing rate is too low for topics"].append(topic) 149 | 150 | if max_rate and rate > max_rate: 151 | hz_status.level = DiagnosticStatus.WARN 152 | consolidated_error_messages["publishing rate is too high for topics"].append(topic) 153 | 154 | if hz_status.level != DiagnosticStatus.OK: 155 | message = "" 156 | key = "never received message for topics" 157 | if len(consolidated_error_messages[key]) > 0: 158 | message += key + " " + str(consolidated_error_messages[key]) + ", " 159 | key = "no messages anymore for topics" 160 | if len(consolidated_error_messages[key]) > 0: 161 | message += key + " " + str(consolidated_error_messages[key]) + ", " 162 | key = "publishing rate is too low for topics" 163 | if len(consolidated_error_messages[key]) > 0: 164 | message += key + " " + str(consolidated_error_messages[key]) + ", " 165 | key = "publishing rate is too high for topics" 166 | if len(consolidated_error_messages[key]) > 0: 167 | message += key + " " + str(consolidated_error_messages[key]) 168 | hz_status.message = message 169 | 170 | hz_status.values.append(KeyValue("rates", str(rates))) 171 | array.status.append(hz_status) 172 | self.pub_diagnostics.publish(array) 173 | 174 | if __name__ == '__main__': 175 | hzt = HzTest() 176 | hzt.run() 177 | rospy.spin() 178 | -------------------------------------------------------------------------------- /cob_monitoring/src/netdata_interface/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/cob_monitoring/src/netdata_interface/__init__.py -------------------------------------------------------------------------------- /cob_monitoring/src/netdata_interface/netdata_interface.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import requests 3 | import numpy 4 | 5 | 6 | class NetdataInterface: 7 | def __init__(self, base_url="http://127.0.0.1:19999/api/v1"): 8 | """Create a new NetData interface. 9 | 10 | :param base_url: Base URL of the NetData interface 11 | """ 12 | self._base_url = base_url 13 | self._failed_counter_dict = {} 14 | 15 | def _request_data(self, url): 16 | res = requests.get(url) 17 | 18 | if res.status_code != 200: 19 | msg = "NetData request not successful (url='{}', status_code={})".format( 20 | url, res.status_code 21 | ) 22 | raise requests.ConnectionError(msg) 23 | 24 | return res.json() 25 | 26 | def _reset_failed_counter(self, chart, label): 27 | """Reset the failed counter for given label in chart. 28 | 29 | :param chart: Chart 30 | :param label: Label 31 | """ 32 | if chart not in self._failed_counter_dict: 33 | self._failed_counter_dict[chart] = {} 34 | 35 | self._failed_counter_dict[chart][label] = 0 36 | 37 | def _increase_failed_counter(self, chart, label): 38 | """Increase the failed counter for given label in chart. 39 | 40 | Throws an exception if failed counter is greater or equal 5. 41 | 42 | :param chart: Chart 43 | :param label: Label 44 | """ 45 | if chart not in self._failed_counter_dict: 46 | self._failed_counter_dict[chart] = {} 47 | 48 | if label not in self._failed_counter_dict[chart]: 49 | self._failed_counter_dict[chart][label] = 1 50 | else: 51 | self._failed_counter_dict[chart][label] += 1 52 | 53 | if self._failed_counter_dict[chart][label] >= 5: 54 | raise Exception( 55 | "Data from NetData was malformed {} times (chart='{}', label='{}')".format( 56 | self._failed_counter_dict[chart][label], chart, label 57 | ) 58 | ) 59 | 60 | def query_netdata_info(self): 61 | """Get NetData information.""" 62 | url = "{}/info".format(self._base_url) 63 | res = self._request_data(url) 64 | 65 | return res 66 | 67 | def query_netdata(self, chart, after): 68 | """Get data from NetData chart after a certain time. 69 | 70 | :param chart: Chart identifier 71 | :param after: Timedelta in seconds 72 | """ 73 | url = "{}/data?chart={}&format=json&after=-{}".format( 74 | self._base_url, chart, after 75 | ) 76 | res = self._request_data(url) 77 | 78 | sdata = list(zip(*res["data"])) 79 | data = dict() 80 | 81 | for idx, label in enumerate(res["labels"]): 82 | np_array = numpy.array(sdata[idx]) 83 | if np_array.dtype == object or (np_array == None).any(): 84 | msg = "Data from NetData malformed: {}".format(label) 85 | rospy.logwarn(msg) 86 | rospy.logwarn( 87 | "... malformed data for Label <{}>: {}".format(label, np_array) 88 | ) 89 | self._increase_failed_counter(chart, label) 90 | return None, msg 91 | data[label] = np_array 92 | self._reset_failed_counter(chart, label) 93 | 94 | return data, None 95 | -------------------------------------------------------------------------------- /cob_monitoring/src/ntp_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | 19 | import sys 20 | import traceback 21 | import socket 22 | from subprocess import Popen, PIPE 23 | import time 24 | import re 25 | 26 | import rospy 27 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 28 | 29 | class NtpMonitor(): 30 | def __init__(self, argv=sys.argv): 31 | rospy.init_node("ntp_monitor") 32 | self.parse_args(argv) 33 | 34 | stat = DiagnosticStatus() 35 | stat.level = DiagnosticStatus.WARN 36 | stat.name = '%s NTP Offset' % self.diag_hostname 37 | stat.message = 'No Data' 38 | stat.hardware_id = self.diag_hostname 39 | stat.values = [] 40 | self.msg = DiagnosticArray() 41 | self.msg.header.stamp = rospy.get_rostime() 42 | self.msg.status = [stat] 43 | 44 | self.update_diagnostics() 45 | 46 | self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1) 47 | self.diag_timer = rospy.Timer(rospy.Duration(1.0), self.publish_diagnostics) 48 | self.monitor_timer = rospy.Timer(rospy.Duration(60.0), self.update_diagnostics) 49 | 50 | def update_diagnostics(self, event=None): 51 | stat = DiagnosticStatus() 52 | stat.level = DiagnosticStatus.WARN 53 | stat.name = '%s NTP Offset' % self.diag_hostname 54 | stat.message = 'No Data' 55 | stat.hardware_id = self.diag_hostname 56 | stat.values = [] 57 | 58 | try: 59 | for st,host,off in [(stat, self.ntp_server, self.offset)]: 60 | p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) 61 | retcode = p.wait() 62 | stdout, stderr = p.communicate() 63 | try: 64 | stdout = stdout.decode() #python3 65 | stderr = stderr.decode() #python3 66 | except (UnicodeDecodeError, AttributeError): 67 | pass 68 | 69 | if retcode != 0: 70 | st.level = DiagnosticStatus.ERROR 71 | st.message = 'ntpdate Error' 72 | st.values = [ KeyValue(key = 'ntpdate Error', value = stderr), 73 | KeyValue(key = 'Output', value = stdout) ] 74 | continue 75 | 76 | measured_offset = float(re.search("offset (.*),", stdout).group(1))*1000000 77 | 78 | st.level = DiagnosticStatus.OK 79 | st.message = "OK" 80 | st.values = [ KeyValue("NTP Server" , self.ntp_server), 81 | KeyValue("Offset (us)", str(measured_offset)), 82 | KeyValue("Offset tolerance (us)", str(off)), 83 | KeyValue("Offset tolerance (us) for Error", str(self.error_offset)) ] 84 | 85 | if (abs(measured_offset) > off): 86 | st.level = DiagnosticStatus.WARN 87 | st.message = "NTP Offset Too High" 88 | if (abs(measured_offset) > self.error_offset): 89 | st.level = DiagnosticStatus.ERROR 90 | st.message = "NTP Offset Too High" 91 | 92 | except Exception as e: 93 | stat.level = DiagnosticStatus.ERROR 94 | stat.message = 'ntpdate Exception' 95 | stat.values = [ KeyValue(key = 'Exception', value = str(e)), KeyValue(key = 'Traceback', value = str(traceback.format_exc())) ] 96 | 97 | self.msg = DiagnosticArray() 98 | self.msg.header.stamp = rospy.get_rostime() 99 | self.msg.status = [stat] 100 | 101 | def publish_diagnostics(self, event): 102 | self.diag_pub.publish(self.msg) 103 | 104 | def parse_args(self, argv=sys.argv): 105 | import optparse 106 | parser = optparse.OptionParser(usage="usage: ntp_monitor ntp-hostname []") 107 | parser.add_option("--offset", dest="offset", 108 | action="store", default=500, 109 | help="Offset from NTP host", metavar="OFFSET") 110 | parser.add_option("--error-offset", dest="error_offset", 111 | action="store", default=5000000, 112 | help="Offset from NTP host. Above this is error", metavar="OFFSET") 113 | parser.add_option("--diag-hostname", dest="diag_hostname", 114 | help="Computer name in diagnostics output (ex: 'c1')", 115 | metavar="DIAG_HOSTNAME", 116 | action="store", default=None) 117 | options, args = parser.parse_args(rospy.myargv()) 118 | 119 | if (len(args) != 2): 120 | parser.error("Invalid arguments. Must have HOSTNAME [args]. %s" % args) 121 | print('Invalid arguments.') 122 | sys.exit(0) 123 | 124 | try: 125 | offset = int(options.offset) 126 | error_offset = int(options.error_offset) 127 | except: 128 | parser.error("Offsets must be numbers") 129 | print('Offsets must be numbers') 130 | sys.exit(0) 131 | 132 | self.ntp_server = args[1] 133 | self.diag_hostname = options.diag_hostname 134 | hostname = socket.gethostname() 135 | if self.diag_hostname is None: 136 | self.diag_hostname = hostname 137 | 138 | self.offset = rospy.get_param('~offset', offset) 139 | self.error_offset = rospy.get_param('~error_offset', error_offset) 140 | 141 | 142 | if __name__ == "__main__": 143 | ntp = NtpMonitor(argv=sys.argv) 144 | rospy.spin() 145 | -------------------------------------------------------------------------------- /cob_monitoring/src/topic_status_monitor.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 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | class GenericTopicDiagnostic 28 | { 29 | public: 30 | GenericTopicDiagnostic(std::string& topic_name, diagnostic_updater::Updater& diagnostic_updater) 31 | { 32 | ros::NodeHandle nh; 33 | double hz, hzerror; 34 | int window_size; 35 | ros::param::get("~hz", hz); 36 | ros::param::get("~hzerror", hzerror); 37 | ros::param::param("~window_size", window_size, std::ceil(hz)); 38 | 39 | min_freq_ = hz-hzerror; 40 | max_freq_ = hz+hzerror; 41 | 42 | diagnostic_updater::FrequencyStatusParam freq_param(&min_freq_, &max_freq_, 0.0, window_size); //min_freq, max_freq, tolerance (default: 0.1), window_size (default: 5) 43 | diagnostic_updater::TimeStampStatusParam stamp_param(-1, 1); //min_acceptable (default: -1), max_acceptable (default: 5) 44 | 45 | topic_diagnostic_task_.reset(new diagnostic_updater::TopicDiagnostic(topic_name, diagnostic_updater, freq_param, stamp_param)); 46 | 47 | generic_sub_ = nh.subscribe(topic_name, 1, &GenericTopicDiagnostic::topicCallback, this); 48 | } 49 | 50 | void topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg) 51 | { 52 | topic_diagnostic_task_->tick(ros::Time::now()); 53 | } 54 | 55 | double min_freq_, max_freq_; 56 | ros::Subscriber generic_sub_; 57 | boost::shared_ptr< diagnostic_updater::TopicDiagnostic > topic_diagnostic_task_; 58 | }; 59 | 60 | 61 | class TopicStatusMonitor 62 | { 63 | public: 64 | TopicStatusMonitor() 65 | { 66 | ros::NodeHandle nh; 67 | ros::param::get("~topics", topics_); 68 | ros::param::get("~diagnostics_name", hardware_id_); 69 | diagnostic_updater_.setHardwareID(hardware_id_); 70 | 71 | for(size_t i=0; i topics_; 92 | diagnostic_updater::Updater diagnostic_updater_; 93 | 94 | std::vector< GenericTopicDiagnostic* > tasks_; 95 | ros::Timer diagnostic_timer_; 96 | }; 97 | 98 | 99 | 100 | int main(int argc, char **argv) 101 | { 102 | ros::init(argc, argv, "topic_status_monitor"); 103 | TopicStatusMonitor tsm; 104 | 105 | ros::spin(); 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /cob_monitoring/src/wifi_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | 19 | 20 | import threading 21 | import sys 22 | 23 | import rospy 24 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 25 | from cob_msgs.msg import AccessPoint 26 | 27 | DIAG_NAME = 'Wifi Status (ddwrt)' 28 | WARN_TIME = 30 29 | ERROR_TIME = 60 30 | 31 | 32 | def wifi_to_diag(msg): 33 | stat = DiagnosticStatus() 34 | 35 | stat.name = DIAG_NAME 36 | stat.level = DiagnosticStatus.OK 37 | stat.message = 'OK' 38 | 39 | stat.values.append(KeyValue(key='ESSID', value=msg.essid)) 40 | stat.values.append(KeyValue(key='Mac Address', value=msg.macaddr)) 41 | stat.values.append(KeyValue(key='Signal', value=str(msg.signal))) 42 | stat.values.append(KeyValue(key='Noise', value=str(msg.noise))) 43 | stat.values.append(KeyValue(key='Sig/Noise', value=str(msg.snr))) 44 | stat.values.append(KeyValue(key='Channel', value=str(msg.channel))) 45 | stat.values.append(KeyValue(key='Rate', value=msg.rate)) 46 | stat.values.append(KeyValue(key='TX Power', value=msg.tx_power)) 47 | stat.values.append(KeyValue(key='Quality', value=str(msg.quality))) 48 | 49 | return stat 50 | 51 | def mark_diag_stale(diag_stat = None, error = False): 52 | if not diag_stat: 53 | diag_stat = DiagnosticStatus() 54 | diag_stat.message = 'No Updates' 55 | diag_stat.name = DIAG_NAME 56 | else: 57 | diag_stat.message = 'Updates Stale' 58 | 59 | diag_stat.level = DiagnosticStatus.WARN 60 | if error: 61 | diag_stat.level = DiagnosticStatus.ERROR 62 | 63 | return diag_stat 64 | 65 | class WifiMonitor(object): 66 | def __init__(self): 67 | self._mutex = threading.Lock() 68 | 69 | self._last_msg = None 70 | self._last_update_time = None 71 | self._start_time = rospy.get_time() 72 | 73 | self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=50) 74 | 75 | self._ddwrt_sub = rospy.Subscriber('ddwrt/accesspoint', AccessPoint, self._cb) 76 | 77 | def _cb(self, msg): 78 | with self._mutex: 79 | self._last_msg = msg 80 | self._last_update_time = rospy.get_time() 81 | 82 | def publish_stats(self): 83 | with self._mutex: 84 | if self._last_msg: 85 | ddwrt_stat = wifi_to_diag(self._last_msg) 86 | 87 | update_diff = rospy.get_time() - self._last_update_time 88 | if update_diff > WARN_TIME: 89 | ddwrt_stat = mark_diag_stale(ddwrt_stat) 90 | if (rospy.get_time() - self._last_update_time) > ERROR_TIME: 91 | ddwrt_stat = mark_diag_stale(ddwrt_stat, True) 92 | 93 | ddwrt_stat.values.append(KeyValue(key='Time Since Update', value=str(update_diff))) 94 | else: 95 | error_state = (rospy.get_time() - self._start_time) > ERROR_TIME 96 | ddwrt_stat = mark_diag_stale(None, error_state) 97 | ddwrt_stat.values.append(KeyValue(key='Time Since Update', value="N/A")) 98 | 99 | msg = DiagnosticArray() 100 | msg.header.stamp = rospy.get_rostime() 101 | msg.status.append(ddwrt_stat) 102 | 103 | self._diag_pub.publish(msg) 104 | 105 | 106 | if __name__ == '__main__': 107 | try: 108 | rospy.init_node('ddwrt_diag') 109 | except rospy.exceptions.ROSInitException: 110 | print('Wifi monitor is unable to initialize node. Master may not be running.') 111 | sys.exit(2) 112 | 113 | wifi_monitor = WifiMonitor() 114 | rate = rospy.Rate(1.0) 115 | 116 | try: 117 | while not rospy.is_shutdown(): 118 | rate.sleep() 119 | wifi_monitor.publish_stats() 120 | except KeyboardInterrupt: 121 | pass 122 | except Exception as e: 123 | import traceback 124 | traceback.print_exc() 125 | 126 | sys.exit(0) 127 | 128 | -------------------------------------------------------------------------------- /cob_script_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_script_server) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs message_generation rostest trajectory_msgs) 5 | 6 | catkin_python_setup() 7 | 8 | ### Message Generation ### 9 | add_message_files( 10 | FILES 11 | ScriptState.msg 12 | ) 13 | 14 | add_service_files( 15 | FILES 16 | ComposeTrajectory.srv 17 | ) 18 | 19 | add_action_files( 20 | FILES 21 | Script.action 22 | State.action 23 | ) 24 | 25 | generate_messages( 26 | DEPENDENCIES actionlib_msgs trajectory_msgs 27 | ) 28 | 29 | 30 | catkin_package( 31 | CATKIN_DEPENDS actionlib actionlib_msgs message_runtime trajectory_msgs 32 | ) 33 | 34 | ### Install ### 35 | install(DIRECTORY launch 36 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 37 | ) 38 | 39 | install(PROGRAMS src/cob_console 40 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | catkin_install_python(PROGRAMS src/cob_console_node src/script_server.py 43 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | catkin_install_python(PROGRAMS test/test_action_interface.py test/test_move.py test/test_python_api.py test/test_say.py test/test_trigger.py 47 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /cob_script_server/action/Script.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | string function_name 3 | string component_name 4 | string parameter_name 5 | string mode 6 | string service_name 7 | float32 duration 8 | bool planning 9 | --- 10 | #result definition 11 | int32 error_code 12 | --- 13 | #feedback definition 14 | -------------------------------------------------------------------------------- /cob_script_server/action/State.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | cob_script_server/ScriptState state 3 | --- 4 | #result definition 5 | int16 return_value 6 | --- 7 | #feedback definition 8 | -------------------------------------------------------------------------------- /cob_script_server/launch/script_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /cob_script_server/launch/script_server.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /cob_script_server/msg/ScriptState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string function_name 3 | string component_name 4 | string parameter_name 5 | string full_graph_name 6 | 7 | # Possible execution states 8 | byte UNKNOWN=0 9 | byte ACTIVE=1 10 | byte SUCCEEDED=2 11 | byte FAILED=3 12 | byte PAUSED=4 13 | 14 | byte state # state of execution 15 | int16 error_code # current error_code for state 16 | -------------------------------------------------------------------------------- /cob_script_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | cob_script_server 7 | 0.7.37 8 | The cob_script_server package provides a simple interface to operate Care-O-bot. It can be used via the python API or the actionlib interface. 9 | 10 | Apache 2.0 11 | 12 | http://ros.org/wiki/cob_script_server 13 | 14 | 15 | Felix Messmer 16 | Florian Weisshardt 17 | Florian Weisshardt 18 | 19 | catkin 20 | python-setuptools 21 | python3-setuptools 22 | 23 | message_generation 24 | message_runtime 25 | 26 | actionlib 27 | actionlib_msgs 28 | rostest 29 | trajectory_msgs 30 | 31 | cob_actions 32 | cob_light 33 | cob_mimic 34 | cob_sound 35 | control_msgs 36 | geometry_msgs 37 | ipython 38 | ipython3 39 | move_base_msgs 40 | python-pygraphviz 41 | python3-pygraphviz 42 | python-six 43 | python3-six 44 | rospy 45 | std_msgs 46 | std_srvs 47 | tf 48 | urdfdom_py 49 | 50 | 51 | -------------------------------------------------------------------------------- /cob_script_server/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['simple_script_server'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /cob_script_server/src/cob_console: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | SCRIPT=$(readlink -f $0) 3 | SCRIPTPATH=$(dirname $SCRIPT) 4 | 5 | #detect ipython version 6 | if command -v ipython 1>/dev/null; then PYTHON_EXECUTABLE=ipython 7 | elif command -v ipython3 1>/dev/null; then PYTHON_EXECUTABLE=ipython3 8 | elif command -v ipython2 1>/dev/null; then PYTHON_EXECUTABLE=ipython2 9 | else PYTHON_EXECUTABLE=python 10 | fi 11 | 12 | # run cob_console 13 | $PYTHON_EXECUTABLE -i "$SCRIPTPATH/cob_console_node" 14 | -------------------------------------------------------------------------------- /cob_script_server/src/cob_console_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | from simple_script_server import simple_script_server 20 | sss = simple_script_server() 21 | 22 | if __name__ == "__main__": 23 | rospy.init_node("cob_console", anonymous=True) 24 | -------------------------------------------------------------------------------- /cob_script_server/src/script_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import inspect 19 | 20 | import rospy 21 | import actionlib 22 | 23 | from cob_script_server.msg import ScriptAction, ScriptActionResult 24 | from cob_script_server.srv import ComposeTrajectory, ComposeTrajectoryResponse 25 | from simple_script_server import * 26 | sss = simple_script_server() 27 | 28 | ## Script server class which inherits from script class. 29 | # 30 | # Implements actionlib interface for the script server. 31 | # 32 | class script_server(): 33 | ## Initializes the actionlib interface of the script server. 34 | # 35 | def __init__(self): 36 | self.ns_global_prefix = "/script_server" 37 | self.script_action_server = actionlib.SimpleActionServer(self.ns_global_prefix, ScriptAction, self.execute_cb, False) 38 | self.script_action_server.start() 39 | self.compose_trajectory_service = rospy.Service('~compose_trajectory', ComposeTrajectory, self.handle_compose_trajectory) 40 | 41 | #------------------- Service section -------------------# 42 | def handle_compose_trajectory(self, req): 43 | print("compose trajectory", req.component_name, req.parameter_name) 44 | traj_msg, error_code = sss.compose_trajectory(req.component_name, req.parameter_name) 45 | if error_code != 0: 46 | return None 47 | res = ComposeTrajectoryResponse() 48 | res.trajectory = traj_msg 49 | return res 50 | 51 | #------------------- Actionlib section -------------------# 52 | ## Executes actionlib callbacks. 53 | # 54 | # \param server_goal ScriptActionGoal 55 | # 56 | def execute_cb(self, server_goal): 57 | server_result = ScriptActionResult().result 58 | if server_goal.function_name == None or server_goal.function_name.strip() == "": 59 | rospy.logerr("function name cannot be blank") 60 | return 61 | 62 | if server_goal.function_name in dir(sss): 63 | func = getattr(sss, server_goal.function_name) 64 | argspec = inspect.getargspec(func) 65 | args = {} 66 | for arg in argspec.args: 67 | if arg in dir(server_goal): 68 | serverArg = getattr(server_goal, arg) 69 | if type(serverArg) == str: 70 | try: 71 | serverArg = eval(serverArg) 72 | except: 73 | pass 74 | args[arg] = serverArg 75 | 76 | handle01 = func(*(), **args) 77 | else: 78 | rospy.logerr("function <<%s>> not supported", server_goal.function_name) 79 | self.script_action_server.set_aborted(server_result) 80 | return 81 | 82 | if 'get_error_code' in dir(handle01): 83 | server_result.error_code = handle01.get_error_code() 84 | else: 85 | rospy.logwarn("unexpected action result type <<%s>> for function %s", type(handle01), server_goal.function_name) 86 | if server_result.error_code == 0: 87 | rospy.logdebug("action result success") 88 | self.script_action_server.set_succeeded(server_result) 89 | else: 90 | rospy.logerr("action result error, error_code: " + str(server_result.error_code)) 91 | self.script_action_server.set_aborted(server_result) 92 | 93 | ## Main routine for running the script server 94 | # 95 | if __name__ == '__main__': 96 | rospy.init_node('script_server') 97 | script_server() 98 | rospy.loginfo("script_server is running") 99 | rospy.spin() 100 | -------------------------------------------------------------------------------- /cob_script_server/src/simple_script_server/__init__.py: -------------------------------------------------------------------------------- 1 | from .simple_script_server import * 2 | -------------------------------------------------------------------------------- /cob_script_server/srv/ComposeTrajectory.srv: -------------------------------------------------------------------------------- 1 | string component_name 2 | string parameter_name 3 | --- 4 | trajectory_msgs/JointTrajectory trajectory 5 | -------------------------------------------------------------------------------- /cob_script_server/test/test_action_interface.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | import unittest 20 | 21 | import rospy 22 | import actionlib 23 | from simple_script_server import * 24 | from cob_script_server.msg import ScriptAction, ScriptGoal 25 | from std_srvs.srv import Trigger, TriggerResponse 26 | from move_base_msgs.msg import MoveBaseAction, MoveBaseResult 27 | 28 | ## This test checks the correct call to commands from the cob_script_server. This does not cover the execution of the commands (this shoud be checked in the package where the calls refer to). 29 | class TestActionInterface(unittest.TestCase): 30 | def __init__(self, *args): 31 | super(TestActionInterface, self).__init__(*args) 32 | rospy.init_node('test_action_interface') 33 | self.cb_executed = False 34 | self.cb_move_executed = False 35 | self.component_name = "arm" # testing for component arm 36 | 37 | # test trigger commands 38 | def test_init(self): 39 | goal = ScriptGoal() 40 | goal.function_name = "init" 41 | goal.component_name = "arm" 42 | self.script_action_trigger(goal) 43 | 44 | def test_stop(self): 45 | goal = ScriptGoal() 46 | goal.function_name = "stop" 47 | goal.component_name = "arm" 48 | self.script_action_trigger(goal) 49 | 50 | def test_recover(self): 51 | goal = ScriptGoal() 52 | goal.function_name = "recover" 53 | goal.component_name = "arm" 54 | self.script_action_trigger(goal) 55 | 56 | def script_action_trigger(self,goal): 57 | rospy.Service("/" + goal.component_name + "_controller/" + goal.function_name, Trigger, self.cb) 58 | self.cb_executed = False 59 | 60 | client = actionlib.SimpleActionClient('/script_server', ScriptAction) 61 | 62 | if not client.wait_for_server(rospy.Duration(10)): 63 | self.fail('Action server not ready') 64 | client.send_goal(goal) 65 | if not client.wait_for_result(rospy.Duration(10)): 66 | self.fail('Action didnt give a result before timeout') 67 | 68 | if not self.cb_executed: 69 | self.fail('Service Server not called. script server error_code: ' + str(client.get_result().error_code)) 70 | 71 | def cb(self,req): 72 | self.cb_executed = True 73 | res = TriggerResponse() 74 | res.success = True 75 | return res 76 | 77 | # test move base commands 78 | # def test_move_base(self): 79 | # goal = ScriptGoal() 80 | # goal.function_name = "move" 81 | # goal.component_name = "base" 82 | # goal.parameter_name = [0,0,0] 83 | # self.script_action_move_base(goal) 84 | 85 | # def test_move_base_omni(self): #FIXME fails because client is already in DONE state (mode="" and mode="omni" is the same) 86 | # goal = ScriptGoal() 87 | # goal.function_name = "move" 88 | # goal.component_name = "base" 89 | # goal.parameter_name = "home" 90 | # goal.mode = "omni" 91 | # self.script_action_move_base(goal) 92 | 93 | # def test_move_base_diff(self): 94 | # goal = ScriptGoal() 95 | # goal.function_name = "move" 96 | # goal.component_name = "base" 97 | # goal.parameter_name = [0,0,0] 98 | # goal.mode = "diff" 99 | # self.script_action_move_base(goal) 100 | 101 | # def test_move_base_linear(self): 102 | # goal = ScriptGoal() 103 | # goal.function_name = "move" 104 | # goal.component_name = "base" 105 | # goal.parameter_name = [0,0,0] 106 | # goal.mode = "linear" 107 | # self.script_action_move_base(goal) 108 | 109 | def script_action_move_base(self,goal): 110 | if goal.mode == None or goal.mode == "" or goal.mode == "omni": 111 | as_name = "/move_base" 112 | else: 113 | as_name = "/move_base_" + goal.mode 114 | self.as_server = actionlib.SimpleActionServer(as_name, MoveBaseAction, execute_cb=self.base_cb, auto_start=False) 115 | self.as_server.start() 116 | 117 | client = actionlib.SimpleActionClient('/script_server', ScriptAction) 118 | 119 | self.cb_move_executed = False 120 | if not client.wait_for_server(rospy.Duration(10)): 121 | self.fail('Action server not ready') 122 | client.send_goal(goal) 123 | client.wait_for_result(rospy.Duration(10)) 124 | #if not client.wait_for_result(rospy.Duration(10)): 125 | # self.fail('Action didnt give a result before timeout') 126 | 127 | #if not self.cb_executed: 128 | # self.fail('Action Server not called. script server error_code: ' + str(client.get_result().error_code)) 129 | 130 | def base_cb(self, goal): 131 | self.cb_move_executed = True 132 | result = MoveBaseResult() 133 | self.as_server.set_succeeded(result) 134 | 135 | # test move trajectory commands 136 | #TODO 137 | 138 | if __name__ == '__main__': 139 | import rostest 140 | rostest.rosrun('cob_script_server', 'action_interface', TestActionInterface) 141 | -------------------------------------------------------------------------------- /cob_script_server/test/test_move.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | import unittest 20 | 21 | import rospy 22 | import actionlib 23 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryResult 24 | from move_base_msgs.msg import MoveBaseAction, MoveBaseResult 25 | from simple_script_server import * 26 | sss = simple_script_server() 27 | 28 | ## This test checks the correct call to commands from the cob_script_server. This does not cover the execution of the commands (this shoud be checked in the package where the calls refer to). 29 | class TestMove(unittest.TestCase): 30 | def __init__(self, *args): 31 | super(TestMove, self).__init__(*args) 32 | rospy.init_node('test_move') 33 | self.cb_executed = False 34 | 35 | # test move base commands 36 | def test_move_base(self): 37 | self.move_base() 38 | 39 | def test_move_base_omni(self): 40 | self.move_base(mode="omni") 41 | 42 | def test_move_base_diff(self): 43 | self.move_base(mode="diff") 44 | 45 | def test_move_base_linear(self): 46 | self.move_base(mode="linear") 47 | 48 | def move_base(self,mode=None): 49 | if mode == None or mode == "" or mode == "omni": 50 | as_name = "/move_base" 51 | else: 52 | as_name = "/move_base_" + mode 53 | self.as_server = actionlib.SimpleActionServer(as_name, MoveBaseAction, execute_cb=self.base_cb, auto_start=False) 54 | self.as_server.start() 55 | self.cb_executed = False 56 | handle = sss.move("base",[0,0,0],mode=mode) 57 | if not self.cb_executed: 58 | self.fail('Action Server not called. script server error_code: ' + str(handle.get_error_code())) 59 | 60 | def base_cb(self, goal): 61 | self.cb_executed = True 62 | result = MoveBaseResult() 63 | self.as_server.set_succeeded(result) 64 | 65 | # test move trajectory commands 66 | def test_move_traj(self): 67 | component_name = "arm" # testing for component arm 68 | as_name = "/" + component_name + "/joint_trajectory_controller/follow_joint_trajectory" 69 | self.as_server = actionlib.SimpleActionServer(as_name, FollowJointTrajectoryAction, execute_cb=self.traj_cb, auto_start=False) 70 | self.as_server.start() 71 | self.cb_executed = False 72 | handle = sss.move(component_name,[[0,0,0,0,0,0,0]]) 73 | if not self.cb_executed: 74 | self.fail('Action Server not called. script server error_code: ' + str(handle.get_error_code())) 75 | 76 | def traj_cb(self, goal): 77 | self.cb_executed = True 78 | result = FollowJointTrajectoryResult() 79 | self.as_server.set_succeeded(result) 80 | 81 | if __name__ == '__main__': 82 | import rostest 83 | rostest.rosrun('cob_script_server', 'move', TestMove) 84 | -------------------------------------------------------------------------------- /cob_script_server/test/test_python_api.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | import unittest 20 | 21 | import rospy 22 | import rostest 23 | import actionlib 24 | from std_srvs.srv import Trigger, TriggerResponse 25 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 26 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryResult 27 | from simple_script_server import * 28 | 29 | 30 | class PythonAPITest(unittest.TestCase): 31 | def __init__(self, *args): 32 | super(PythonAPITest, self).__init__(*args) 33 | rospy.init_node('test_python_api_test') 34 | self.sss=simple_script_server() 35 | self.as_cb_executed = False 36 | self.ss_stop_cb_executed = False 37 | self.ns_global_prefix = "/script_server" 38 | 39 | def test_python_api(self): 40 | # get parameters 41 | try: 42 | # component 43 | if not rospy.has_param('~command'): 44 | self.fail('Parameter command does not exist on ROS Parameter Server') 45 | command = rospy.get_param('~command') 46 | except KeyError: 47 | self.fail('Parameters not set properly') 48 | 49 | # choose command to test 50 | if command == "move": 51 | component_name = "arm" 52 | # init action server (as) 53 | ##as_name = "/" + component_name + "_controller/joint_trajectory_action" 54 | ##self.as_server = actionlib.SimpleActionServer(as_name, JointTrajectoryAction, execute_cb=self.as_cb) 55 | as_name = "/" + component_name + "_controller/follow_joint_trajectory" 56 | self.as_server = actionlib.SimpleActionServer(as_name, FollowJointTrajectoryAction, execute_cb=self.as_cb) 57 | #execute test (as all components have the same trajectory interface, we only test for arm) 58 | self.move_test(component_name,"home") # test trajectories with a single point (home) 59 | self.move_test(component_name,"grasp-to-tray") # test trajectories with multiple points (grasp-to-tray) 60 | self.move_test(component_name,"wave") # test trajectories out of other points (wave) 61 | elif command == "stop": 62 | # prepare service server (ss) 63 | ss_name = "/arm_controller/" + command 64 | self.ss_cb_executed = False 65 | rospy.Service(ss_name, Trigger, self.ss_cb) 66 | # call sss function 67 | self.sss.stop("arm") 68 | # check result 69 | if not self.ss_cb_executed: 70 | self.fail('Service Server not called') 71 | elif command == "init": 72 | # prepare service server (ss) 73 | ss_name = "/arm_controller/" + command 74 | self.ss_cb_executed = False 75 | rospy.Service(ss_name, Trigger, self.ss_cb) 76 | # call sss function 77 | self.sss.init("arm") 78 | # check result 79 | if not self.ss_cb_executed: 80 | self.fail('Service Server not called') 81 | elif command == "recover": 82 | # prepare service server (ss) 83 | ss_name = "/arm_controller/" + command 84 | self.ss_cb_executed = False 85 | rospy.Service(ss_name, Trigger, self.ss_cb) 86 | # call sss function 87 | self.sss.recover("arm") 88 | # check result 89 | if not self.ss_cb_executed: 90 | self.fail('Service Server not called') 91 | else: 92 | self.fail("Command not known to test script") 93 | 94 | def move_test(self,component_name, parameter_name): 95 | self.as_cb_executed = False 96 | 97 | # call sss functions 98 | self.sss.move(component_name,parameter_name) 99 | 100 | # check result 101 | if not self.as_cb_executed: 102 | self.fail('No goal received at action server') 103 | 104 | # get joint_names from parameter server 105 | param_string = self.ns_global_prefix + "/" + component_name + "/joint_names" 106 | if not rospy.has_param(param_string): 107 | error_msg = "parameter " + param_string +" does not exist on ROS Parameter Server" 108 | self.fail(error_msg) 109 | joint_names = rospy.get_param(param_string) 110 | 111 | # get joint values from parameter server 112 | if type(parameter_name) is str: 113 | param_string = self.ns_global_prefix + "/" + component_name + "/" + parameter_name 114 | if not rospy.has_param(param_string): 115 | error_msg = "parameter " + param_string + " does not exist on ROS Parameter Server" 116 | self.fail(error_msg) 117 | param = rospy.get_param(param_string) 118 | else: 119 | param = parameter_name 120 | 121 | # check trajectory parameters 122 | if not type(param) is list: # check outer list 123 | error_msg = "no valid parameter for " + component_name + ": not a list, aborting..." 124 | self.fail(error_msg) 125 | 126 | traj = [] 127 | 128 | for point in param: 129 | #print point,"type1 = ", type(point) 130 | if type(point) is str: 131 | if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point): 132 | error_msg = "parameter " + self.ns_global_prefix + "/" + component_name + "/" + point + " does not exist on ROS Parameter Server, aborting..." 133 | self.fail(error_msg) 134 | point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point) 135 | point = point[0] # \todo hack because only first point is used, no support for trajectories inside trajectories 136 | #print point 137 | elif type(point) is list: 138 | rospy.logdebug("point is a list") 139 | else: 140 | error_msg = "no valid parameter for " + component_name + ": not a list of lists or strings, aborting..." 141 | self.fail(error_msg) 142 | 143 | # here: point should be list of floats/ints 144 | #print point 145 | if not len(point) == len(joint_names): # check dimension 146 | error_msg = "no valid parameter for " + component_name + ": dimension should be " + len(joint_names) + " and is " + len(point) + ", aborting..." 147 | self.fail(error_msg) 148 | 149 | for value in point: 150 | #print value,"type2 = ", type(value) 151 | if not ((type(value) is float) or (type(value) is int)): # check type 152 | #print type(value) 153 | error_msg = "no valid parameter for " + component_name + ": not a list of float or int, aborting..." 154 | self.fail(error_msg) 155 | traj.append(point) 156 | 157 | traj_msg = JointTrajectory() 158 | traj_msg.header.stamp = rospy.Time.now()+rospy.Duration(0.5) 159 | traj_msg.joint_names = joint_names 160 | point_nr = 0 161 | for point in traj: 162 | point_msg = JointTrajectoryPoint() 163 | point_msg.positions = point 164 | point_msg.time_from_start=rospy.Duration(3*point_nr) # this value is set to 3 sec per point. \todo: read from parameter 165 | traj_msg.points.append(point_msg) 166 | 167 | #print traj_msg 168 | #print self.traj 169 | 170 | # check amount of trajectory points 171 | if not (len(traj_msg.points) == len(self.traj.points)): 172 | self.fail('Not the same amount of points in trajectory') 173 | 174 | # check points 175 | #print traj_msg.points 176 | #print self.traj.points 177 | for i in range(len(traj_msg.points)): 178 | # check dimension of points 179 | #print traj_msg.points[i].positions 180 | #print self.traj.points[i].positions 181 | if not (len(traj_msg.points[i].positions) == len(self.traj.points[i].positions)): 182 | self.fail('Not the same amount of values in point') 183 | 184 | # check values of points 185 | for j in range(len(traj_msg.points[i].positions)): 186 | if not (traj_msg.points[i].positions[j] == self.traj.points[i].positions[j]): 187 | self.fail('Not the same values in point') 188 | 189 | def as_cb(self, goal): 190 | #result = JointTrajectoryResult() 191 | result = FollowJointTrajectoryResult() 192 | #self.as_server.set_preempted(result) 193 | self.as_cb_executed = True 194 | self.traj = goal.trajectory 195 | print("action server callback") 196 | self.as_server.set_succeeded(result) 197 | 198 | def ss_cb(self,req): 199 | self.ss_cb_executed = True 200 | res = TriggerResponse() 201 | res.success = True 202 | return res 203 | 204 | if __name__ == '__main__': 205 | try: 206 | rostest.run('rostest', 'test_python_apt_test', PythonAPITest, sys.argv) 207 | except KeyboardInterrupt as e: 208 | pass 209 | print("exiting") 210 | 211 | -------------------------------------------------------------------------------- /cob_script_server/test/test_say.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | import unittest 20 | 21 | import rospy 22 | import rostest 23 | 24 | import actionlib 25 | from cob_sound.msg import SayAction, SayResult 26 | from simple_script_server import * 27 | sss = simple_script_server() 28 | 29 | class SayTest(unittest.TestCase): 30 | def __init__(self, *args): 31 | super(SayTest, self).__init__(*args) 32 | rospy.init_node('test_say_test') 33 | self.cb_executed = False 34 | 35 | def test_say(self): 36 | as_name = "/sound/say" 37 | self.as_server = actionlib.SimpleActionServer(as_name, SayAction, execute_cb=self.say_cb, auto_start=False) 38 | self.as_server.start() 39 | self.cb_executed = False 40 | handle = sss.say("sound", ["hello"]) 41 | if not self.cb_executed: 42 | self.fail('Service Server not called. script server error_code: ' + str(handle.get_error_code())) 43 | 44 | def say_cb(self, goal): 45 | self.cb_executed = True 46 | result = SayResult() 47 | self.as_server.set_succeeded(result) 48 | 49 | if __name__ == '__main__': 50 | try: 51 | rostest.run('rostest', 'test_say_test', SayTest, sys.argv) 52 | except KeyboardInterrupt as e: 53 | pass 54 | print("exiting") 55 | 56 | -------------------------------------------------------------------------------- /cob_script_server/test/test_trigger.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import sys 19 | import unittest 20 | 21 | import rospy 22 | import actionlib 23 | from std_srvs.srv import Trigger, TriggerResponse 24 | from move_base_msgs.msg import MoveBaseAction 25 | from control_msgs.msg import FollowJointTrajectoryAction 26 | from simple_script_server import * 27 | sss = simple_script_server() 28 | 29 | ## This test checks the correct call to commands from the cob_script_server. This does not cover the execution of the commands (this shoud be checked in the package where the calls refer to). 30 | class TestTrigger(unittest.TestCase): 31 | def __init__(self, *args): 32 | super(TestTrigger, self).__init__(*args) 33 | rospy.init_node('test_trigger') 34 | self.cb_executed = False 35 | self.component_name = "arm" 36 | self.service_ns = "/" + self.component_name + "/driver" 37 | rospy.set_param("/script_server/"+ self.component_name + "/service_ns", self.service_ns) 38 | self.action_name = "/"+ self.component_name + "/joint_trajectory_controller/follow_joint_trajectory" 39 | rospy.set_param("/script_server/"+ self.component_name + "/action_name", self.action_name) 40 | 41 | def test_init(self): 42 | rospy.Service(self.service_ns + "/init", Trigger, self.srv_cb) 43 | self.cb_executed = False 44 | handle = sss.init(self.component_name) 45 | if not self.cb_executed: 46 | self.fail('sss.init() failed. script server error_code: ' + str(handle.get_error_code())) 47 | 48 | def test_stop(self): 49 | as_name = "/move_base" 50 | self.as_server = actionlib.SimpleActionServer(as_name, MoveBaseAction, execute_cb=self.action_cb) 51 | handle = sss.stop("base") 52 | if not handle.get_error_code() == 0: 53 | self.fail('sss.stop("base") failed. script server error_code: ' + str(handle.get_error_code())) 54 | handle = sss.stop("base", mode="omni") 55 | if not handle.get_error_code() == 0: 56 | self.fail('sss.stop("base", mode="omni") failed. script server error_code: ' + str(handle.get_error_code())) 57 | 58 | as_name = "/move_base_diff" 59 | self.as_server = actionlib.SimpleActionServer(as_name, MoveBaseAction, execute_cb=self.action_cb) 60 | handle = sss.stop("base", mode="diff") 61 | if not handle.get_error_code() == 0: 62 | self.fail('sss.stop("base", mode="diff") failed. script server error_code: ' + str(handle.get_error_code())) 63 | 64 | as_name = "/move_base_linear" 65 | self.as_server = actionlib.SimpleActionServer(as_name, MoveBaseAction, execute_cb=self.action_cb) 66 | handle = sss.stop("base", mode="linear") 67 | if not handle.get_error_code() == 0: 68 | self.fail('sss.stop("base", mode="linear") failed. script server error_code: ' + str(handle.get_error_code())) 69 | 70 | self.as_server = actionlib.SimpleActionServer(self.action_name, FollowJointTrajectoryAction, execute_cb=self.action_cb) 71 | handle = sss.stop(self.component_name) 72 | if not handle.get_error_code() == 0: 73 | self.fail('sss.stop("arm") failed. script server error_code: ' + str(handle.get_error_code())) 74 | 75 | def test_recover(self): 76 | rospy.Service(self.service_ns + "/recover", Trigger, self.srv_cb) 77 | self.cb_executed = False 78 | handle = sss.recover(self.component_name) 79 | if not self.cb_executed: 80 | self.fail('sss.recover() failed. script server error_code: ' + str(handle.get_error_code())) 81 | 82 | def test_halt(self): 83 | rospy.Service(self.service_ns + "/halt", Trigger, self.srv_cb) 84 | self.cb_executed = False 85 | handle = sss.halt(self.component_name) 86 | if not self.cb_executed: 87 | self.fail('sss.halt() failed. script server error_code: ' + str(handle.get_error_code())) 88 | 89 | def srv_cb(self,req): 90 | self.cb_executed = True 91 | res = TriggerResponse() 92 | res.success = True 93 | return res 94 | 95 | def action_cb(self, goal): 96 | while not self.as_server.is_preempt_requested(): 97 | rospy.Rate(1).sleep() 98 | self.as_server.set_preempted() 99 | 100 | if __name__ == '__main__': 101 | import rostest 102 | rostest.rosrun('cob_script_server', 'trigger', TestTrigger) 103 | -------------------------------------------------------------------------------- /cob_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_teleop) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib cob_actions cob_light cob_script_server cob_sound geometry_msgs roscpp sensor_msgs std_msgs std_srvs) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS actionlib cob_actions cob_light cob_script_server cob_sound geometry_msgs roscpp sensor_msgs std_msgs std_srvs 8 | ) 9 | 10 | ### Build ### 11 | include_directories(${catkin_INCLUDE_DIRS}) 12 | 13 | add_executable(${PROJECT_NAME}_keyboard ros/src/${PROJECT_NAME}_keyboard.cpp) 14 | add_dependencies(${PROJECT_NAME}_keyboard ${catkin_EXPORTED_TARGETS}) 15 | target_link_libraries(${PROJECT_NAME}_keyboard ${catkin_LIBRARIES}) 16 | 17 | add_executable(keyboard_publisher ros/src/keyboard_publisher.cpp) 18 | add_dependencies(keyboard_publisher ${catkin_EXPORTED_TARGETS}) 19 | target_link_libraries(keyboard_publisher ${catkin_LIBRARIES}) 20 | 21 | add_executable(${PROJECT_NAME} ros/src/${PROJECT_NAME}.cpp) 22 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 23 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 24 | 25 | ### Install ### 26 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_keyboard keyboard_publisher 27 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 28 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 30 | ) 31 | 32 | install(DIRECTORY ros/launch 33 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 34 | ) 35 | -------------------------------------------------------------------------------- /cob_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_teleop 3 | Teleop node 4 | 0.7.37 5 | 6 | Felix Messmer 7 | Florian Weisshardt, Maximilian Sieber 8 | 9 | Apache 2.0 10 | http://www.ros.org/wiki/cob_teleop 11 | 12 | catkin 13 | 14 | actionlib 15 | cob_actions 16 | cob_light 17 | cob_script_server 18 | cob_sound 19 | geometry_msgs 20 | roscpp 21 | sensor_msgs 22 | std_msgs 23 | std_srvs 24 | 25 | 26 | -------------------------------------------------------------------------------- /cob_teleop/ros/launch/teleop_keyboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /cob_teleop/ros/src/cob_teleop_keyboard.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 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #define KEYCODE_A 0x61 28 | #define KEYCODE_D 0x64 29 | #define KEYCODE_S 0x73 30 | #define KEYCODE_W 0x77 31 | #define KEYCODE_Q 0x71 32 | #define KEYCODE_E 0x65 33 | 34 | #define KEYCODE_A_CAP 0x41 35 | #define KEYCODE_D_CAP 0x44 36 | #define KEYCODE_S_CAP 0x53 37 | #define KEYCODE_W_CAP 0x57 38 | #define KEYCODE_Q_CAP 0x51 39 | #define KEYCODE_E_CAP 0x45 40 | 41 | class TeleopKeyboard 42 | { 43 | private: 44 | double walk_vel, run_vel, yaw_rate, yaw_rate_run; 45 | geometry_msgs::Twist cmd; 46 | 47 | ros::NodeHandle n_; 48 | ros::Publisher vel_pub_; 49 | 50 | public: 51 | void init() 52 | { 53 | cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; 54 | 55 | vel_pub_ = n_.advertise("cmd_vel", 1); 56 | 57 | ros::NodeHandle n_private("~"); 58 | n_private.param("walk_vel", walk_vel, 0.5); 59 | n_private.param("run_vel", run_vel, 1.0); 60 | n_private.param("yaw_rate", yaw_rate, 1.0); 61 | n_private.param("yaw_run_rate", yaw_rate_run, 1.5); 62 | 63 | } 64 | 65 | TeleopKeyboard() 66 | { 67 | walk_vel = 0; 68 | run_vel = 0; 69 | yaw_rate = 0; 70 | yaw_rate_run = 0; 71 | } 72 | ~TeleopKeyboard() { } 73 | void keyboardLoop(); 74 | 75 | }; 76 | 77 | int kfd = 0; 78 | struct termios cooked, raw; 79 | 80 | void quit(int sig) 81 | { 82 | tcsetattr(kfd, TCSANOW, &cooked); 83 | exit(0); 84 | } 85 | 86 | int main(int argc, char** argv) 87 | { 88 | ros::init(argc, argv, "teleop_keyboard"); 89 | 90 | TeleopKeyboard tpk; 91 | tpk.init(); 92 | 93 | signal(SIGINT,quit); 94 | 95 | tpk.keyboardLoop(); 96 | 97 | return(0); 98 | } 99 | 100 | void TeleopKeyboard::keyboardLoop() 101 | { 102 | char c; 103 | bool dirty=false; 104 | 105 | // get the console in raw mode 106 | tcgetattr(kfd, &cooked); 107 | memcpy(&raw, &cooked, sizeof(struct termios)); 108 | raw.c_lflag &=~ (ICANON | ECHO); 109 | // Setting a new line, then end of file 110 | raw.c_cc[VEOL] = 1; 111 | raw.c_cc[VEOF] = 2; 112 | tcsetattr(kfd, TCSANOW, &raw); 113 | 114 | puts("Reading from keyboard"); 115 | puts("---------------------------"); 116 | puts("Use 'WASD' to translate"); 117 | puts("Use 'QE' to yaw"); 118 | puts("Press 'Shift' to run"); 119 | 120 | 121 | for(;;) 122 | { 123 | // get the next event from the keyboard 124 | if(read(kfd, &c, 1) < 0) 125 | { 126 | perror("read():"); 127 | exit(-1); 128 | } 129 | 130 | cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; 131 | 132 | switch(c) 133 | { 134 | // Walking 135 | case KEYCODE_W: 136 | cmd.linear.x = walk_vel; 137 | dirty = true; 138 | break; 139 | case KEYCODE_S: 140 | cmd.linear.x = - walk_vel; 141 | dirty = true; 142 | break; 143 | case KEYCODE_A: 144 | cmd.linear.y = walk_vel; 145 | dirty = true; 146 | break; 147 | case KEYCODE_D: 148 | cmd.linear.y = - walk_vel; 149 | dirty = true; 150 | break; 151 | case KEYCODE_Q: 152 | cmd.angular.z = yaw_rate; 153 | dirty = true; 154 | break; 155 | case KEYCODE_E: 156 | cmd.angular.z = - yaw_rate; 157 | dirty = true; 158 | break; 159 | 160 | // Running 161 | case KEYCODE_W_CAP: 162 | cmd.linear.x = run_vel; 163 | dirty = true; 164 | break; 165 | case KEYCODE_S_CAP: 166 | cmd.linear.x = - run_vel; 167 | dirty = true; 168 | break; 169 | case KEYCODE_A_CAP: 170 | cmd.linear.y = run_vel; 171 | dirty = true; 172 | break; 173 | case KEYCODE_D_CAP: 174 | cmd.linear.y = - run_vel; 175 | dirty = true; 176 | break; 177 | case KEYCODE_Q_CAP: 178 | cmd.angular.z = yaw_rate_run; 179 | dirty = true; 180 | break; 181 | case KEYCODE_E_CAP: 182 | cmd.angular.z = - yaw_rate_run; 183 | dirty = true; 184 | break; 185 | } 186 | 187 | 188 | if (dirty == true) 189 | { 190 | vel_pub_.publish(cmd); 191 | } 192 | 193 | 194 | } 195 | } 196 | -------------------------------------------------------------------------------- /generic_throttle/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package generic_throttle 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.37 (2024-10-02) 6 | ------------------- 7 | 8 | 0.7.36 (2024-08-05) 9 | ------------------- 10 | 11 | 0.7.35 (2024-04-30) 12 | ------------------- 13 | * 0.6.35 14 | * update changelogs 15 | * Contributors: fmessmer 16 | 17 | 0.6.35 (2024-04-18) 18 | ------------------- 19 | 20 | 0.6.34 (2024-02-18) 21 | ------------------- 22 | 23 | 0.6.33 (2023-11-06) 24 | ------------------- 25 | 26 | 0.6.32 (2023-04-29) 27 | ------------------- 28 | 29 | 0.6.31 (2023-01-04) 30 | ------------------- 31 | 32 | 0.6.30 (2022-11-17) 33 | ------------------- 34 | 35 | 0.6.29 (2022-07-29) 36 | ------------------- 37 | 38 | 0.6.28 (2022-03-15) 39 | ------------------- 40 | 41 | 0.6.27 (2022-01-12) 42 | ------------------- 43 | 44 | 0.6.26 (2021-11-26) 45 | ------------------- 46 | 47 | 0.6.25 (2021-08-02) 48 | ------------------- 49 | 50 | 0.6.24 (2021-07-02) 51 | ------------------- 52 | 53 | 0.6.23 (2021-07-01) 54 | ------------------- 55 | 56 | 0.6.22 (2021-05-10) 57 | ------------------- 58 | 59 | 0.6.21 (2021-04-06) 60 | ------------------- 61 | 62 | 0.6.20 (2021-01-25) 63 | ------------------- 64 | * Merge pull request `#294 `_ from fmessmer/fix_python3 65 | fix python3 66 | * fix python3 67 | * Merge pull request `#291 `_ from fmessmer/fix_python3 68 | fix python3 69 | * Merge pull request `#3 `_ from LoyVanBeek/fix_python3 70 | Get first key and value compatible with both Python 2 & 3 71 | * Get first key and value compatible with both Python 2 & 3 72 | * Contributors: Felix Messmer, Loy van Beek, fmessmer 73 | 74 | 0.6.19 (2020-12-02) 75 | ------------------- 76 | 77 | 0.6.18 (2020-10-21) 78 | ------------------- 79 | 80 | 0.6.17 (2020-10-17) 81 | ------------------- 82 | * Merge pull request `#284 `_ from fmessmer/test_noetic 83 | test noetic 84 | * use setuptools instead of distutils 85 | * Bump CMake version to avoid CMP0048 warning 86 | * Contributors: Felix Messmer, fmessmer 87 | 88 | 0.6.16 (2020-03-18) 89 | ------------------- 90 | * Merge pull request `#270 `_ from LoyVanBeek/feature/python3_compatibility 91 | [ci_updates] pylint + Python3 compatibility 92 | * fix more pylint errors 93 | * fix pylint errors 94 | * Merge pull request `#271 `_ from fmessmer/ci_updates 95 | [travis] ci updates 96 | * catkin_lint fixes 97 | * Contributors: Felix Messmer, fmessmer 98 | 99 | 0.6.15 (2019-11-07) 100 | ------------------- 101 | 102 | 0.6.14 (2019-08-07) 103 | ------------------- 104 | 105 | 0.6.13 (2019-07-19) 106 | ------------------ 107 | 108 | 0.6.12 (2019-06-07) 109 | ------------------- 110 | 111 | 0.6.11 (2019-04-05) 112 | ------------------- 113 | 114 | 0.6.10 (2019-03-14) 115 | ------------------- 116 | 117 | 0.6.9 (2018-07-21) 118 | ------------------ 119 | * update maintainer 120 | * Contributors: ipa-fxm 121 | 122 | 0.6.8 (2018-07-21) 123 | ------------------ 124 | 125 | 0.6.7 (2018-01-07) 126 | ------------------ 127 | * Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev 128 | * Merge pull request `#209 `_ from ipa-fxm/generic_throttle_private_param 129 | use private parameters for generic_throttle 130 | * use private parameters for generic_throttle 131 | * Merge pull request `#197 `_ from ipa-fxm/APACHE_license 132 | use license apache 2.0 133 | * use license apache 2.0 134 | * Contributors: Benjamin Maidel, Felix Messmer, ipa-fxm, ipa-uhr-mk 135 | 136 | 0.6.6 (2017-07-17) 137 | ------------------ 138 | * Change resolution factor to be a multiplicative factor and not a division. 139 | Handle resize to factor 0 as OpenCV error. 140 | * Change misleading resolution with resolution_factor 141 | * Add rostopic exec_depend 142 | * Remove cv_bridge build dependency 143 | * Update manifest and cmake list 144 | * Update README.md to resolution throttle. 145 | * Implement resolution throttle for sensor_msgs/Image. 146 | * Add @ipa-fmw as maintainer. 147 | * Add install tags for the executable node 148 | * Move throttle_node to script folder. Fix typos in README. 149 | * Update README 150 | * Rewrite lazy and latched behavior implementation 151 | * Adapt to new parameter definition layout. Parameters are now set in the node namespace. 152 | Throttled topics have "_throttled" appended to original topic name. 153 | * Add ROS node for the throttle separated from the GenericThrottle implementation 154 | * Implement lazy behavior. Lazy and Latched behavior are disabled by default 155 | * Remove delay feature. Add latched behaviour (True/False). 156 | * Remove build dependency on rospy 157 | * Remove CMakeLists.txt not needed depedencies. 158 | * Change package to format "2" and remove not needed dependencies 159 | * Update README 160 | * Moving the python package inside src/generic_throttle. Modifying accordingly CMakeLists.txt and package.xml. 161 | * remove testing files 162 | * introduce directory layer 163 | * Contributors: MattiaRacca 164 | -------------------------------------------------------------------------------- /generic_throttle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(generic_throttle) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | catkin_install_python(PROGRAMS scripts/${PROJECT_NAME}_node.py 11 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 12 | -------------------------------------------------------------------------------- /generic_throttle/README.md: -------------------------------------------------------------------------------- 1 | # generic_throttle 2 | 3 | generic_throttle is package offering a Python implementation of a throttle for topics. 4 | 5 | The package is composed of 6 | - `GenericThrottle`: class implementing the throttle 7 | - `generic_throttle_node`: ROS node wrapping around an instance of `GenericThrottle` 8 | 9 | The parameters are set as privat parameters of the node. An example .yaml file could look like this: 10 | ``` 11 | topics: # List of topics to be throttled 12 | - /topic1: {latched: False, lazy: True, topic_rate: 1.0} 13 | - /topic2: {latched: True, lazy: False, topic_rate: 20.0} 14 | - /image_topic: {latched: False, lazy: True, topic_rate: 20.0, resolution_factor: 0.5} 15 | ``` 16 | For each topic, 3 parameters must be specified: 17 | - `latched`: if `True`, the publisher of the throttled topic acts as latched (see http://docs.ros.org/api/rospy/html/rospy.topics.Publisher-class.html) 18 | - `lazy`: if `True`, the publisher of the throttled topics will check if the number of subscribers and if there are no, it will not publish the message 19 | - `topic_rate`: desired rate (in Hz) for the throttled topic. 20 | 21 | Optional parameter: 22 | - `resolution_factor`: it is meant for topics of type sensor_msgs/Image. If available, the throttle will reduce the resolution of the image by the specified factor. For example, if `resolution_factor: 0.5`, width and height will be halved. 23 | 24 | The throttle will publish on topics with name `/topic_throttled` (it appends `_throttled` to the original name). 25 | When running an instance of `generic_throttle_node`, you must define the throttle namespace to let the node discover the required parameters. An example from .launch file: 26 | ``` 27 | 28 | ``` 29 | 30 | -------------------------------------------------------------------------------- /generic_throttle/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | generic_throttle 7 | 0.7.37 8 | This package provides a throttle for ROS topics 9 | 10 | Apache 2.0 11 | 12 | Felix Messmer 13 | Florian Weisshardt 14 | Mattia Racca 15 | Mattia Racca 16 | 17 | catkin 18 | python-setuptools 19 | python3-setuptools 20 | 21 | rospy 22 | cv_bridge 23 | rostopic 24 | 25 | -------------------------------------------------------------------------------- /generic_throttle/scripts/generic_throttle_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | from generic_throttle.generic_throttle import GenericThrottle 20 | 21 | if __name__ == "__main__": 22 | rospy.init_node('generic_throttle_node', anonymous=True) 23 | gt = GenericThrottle() 24 | rospy.spin() 25 | -------------------------------------------------------------------------------- /generic_throttle/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['generic_throttle'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /generic_throttle/src/generic_throttle/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_command_tools/1deaaf7cb10911ee06b34862157dc33b95962191/generic_throttle/src/generic_throttle/__init__.py -------------------------------------------------------------------------------- /generic_throttle/src/generic_throttle/generic_throttle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import rospy 19 | import rostopic 20 | import cv2 21 | from threading import Lock 22 | from threading import currentThread 23 | from functools import partial 24 | from cv_bridge import CvBridge, CvBridgeError 25 | 26 | 27 | class GenericThrottle: 28 | def __init__(self): 29 | self.topics = None 30 | self.bridge = CvBridge() 31 | 32 | mandatory_parameters = ['topic_rate','latched', 'lazy'] 33 | 34 | topics_param_name = '~topics' 35 | if not rospy.has_param(topics_param_name): 36 | rospy.logerr('Parameter ' + topics_param_name + ' not available') 37 | 38 | # See README for format 39 | topics_list = rospy.get_param(topics_param_name, []) # type: List[Mapping] 40 | 41 | # create dictionary out of the topic list 42 | self.topics = {next(iter(item.keys())): next(iter(item.values())) for item in topics_list} 43 | 44 | # Check if each entry of topics has the mandatory parameters 45 | mandatory_flag = all(set(mandatory_parameters) <= 46 | set(element) for key, element in 47 | self.topics.items()) 48 | 49 | if(not(mandatory_flag)): 50 | rospy.logerr('Each throttled topic needs 3 parameters ' + 51 | str(mandatory_parameters)) 52 | exit(10) 53 | 54 | # Populate the dictionary for the ros topic throttling 55 | self._populate_dictionary() 56 | 57 | def timer_callback(self, event, topic_id): 58 | # The False argument is for a non blocking call 59 | locking = self.topics[topic_id]['lock'].acquire_lock(False) 60 | 61 | if not(locking): 62 | current_t = currentThread() 63 | rospy.logdebug(str(current_t.getName()) + ': cannot lock topic ' 64 | + topic_id) 65 | return 66 | 67 | publisher = self.topics[topic_id]['publisher'] 68 | subscriber = self.topics[topic_id]['subscriber'] 69 | 70 | # Check if pub and sub already exist, if not try to create them after 71 | # checking the ros topic 72 | 73 | if None in(publisher, subscriber): 74 | topic_info = rostopic.get_topic_class(topic_id) 75 | if topic_info[0] is None: 76 | rospy.logwarn('Cannot find topic ' + topic_id) 77 | 78 | self.topics[topic_id]['lock'].release_lock() 79 | return 80 | else: 81 | # Create a (latched if needed) publisher 82 | self.topics[topic_id]['publisher'] = \ 83 | rospy.Publisher(topic_id + '_throttled', topic_info[0], 84 | queue_size = 1, 85 | latch = self.topics[topic_id]['latched']) 86 | 87 | rospy.loginfo('Created publisher for ' + topic_id) 88 | 89 | # Create subscriber 90 | subscriber_partial = partial(self.subscriber_callback, 91 | topic_id=topic_id) 92 | self.topics[topic_id]['subscriber'] = \ 93 | rospy.Subscriber(topic_id, topic_info[0], 94 | subscriber_partial) 95 | rospy.loginfo('Created subscriber for ' + topic_id) 96 | 97 | self.topics[topic_id]['lock'].release_lock() 98 | return 99 | 100 | last_message = self.topics[topic_id]['last_message'] 101 | 102 | if last_message is not None: 103 | lazy_behavior = \ 104 | self.topics[topic_id]['publisher'].get_num_connections() == 0 \ 105 | and self.topics[topic_id]['lazy'] 106 | if not lazy_behavior: 107 | # if requested and possible, apply the resolution change 108 | try: 109 | resolution = self.topics[topic_id]['resolution_factor'] 110 | except KeyError: 111 | resolution = None 112 | 113 | if resolution is not None: 114 | last_message = self._resize_image(last_message, resolution) 115 | # publish the throttled message 116 | self.topics[topic_id]['publisher'].publish(last_message) 117 | 118 | self.topics[topic_id]['last_message'] = None 119 | self.topics[topic_id]['lock'].release_lock() 120 | return 121 | 122 | self.topics[topic_id]['lock'].release_lock() 123 | return 124 | 125 | def subscriber_callback(self, data, topic_id): 126 | locking = self.topics[topic_id]['lock'].acquire_lock(False) 127 | # The False argument is for a non blocking call 128 | 129 | if not (locking): 130 | current_t = currentThread() 131 | rospy.logdebug(str(current_t.getName()) + ': cannot lock topic ' 132 | + topic_id) 133 | return 134 | 135 | try: 136 | resolution = self.topics[topic_id]['resolution_factor'] 137 | except KeyError: 138 | resolution = None 139 | 140 | if resolution is not None: 141 | if data._type != 'sensor_msgs/Image': 142 | rospy.logwarn('resolution_factor option is not available for ' + 143 | data._type + '. Topic ' + topic_id + 144 | ' will not be resolution throttled.') 145 | self.topics[topic_id]['resolution_factor'] = None 146 | 147 | 148 | self.topics[topic_id]['last_message'] = data 149 | self.topics[topic_id]['lock'].release_lock() 150 | 151 | def _populate_dictionary(self): 152 | # Topic dictionary structure 153 | # {topic_id: {topic_rate, lazy, latched, subscriber, 154 | # publisher, lock, timer, last_message} 155 | 156 | for key, element in self.topics.items(): 157 | element['lock'] = Lock() 158 | element['lock'].acquire_lock() 159 | element['publisher'] = None 160 | element['subscriber'] = None 161 | # Create Timer for each topic 162 | personal_callback = partial(self.timer_callback, topic_id=key) 163 | element['timer'] = rospy.Timer( 164 | rospy.Duration(1.0 / element['topic_rate']), personal_callback) 165 | element['last_message'] = None 166 | element['lock'].release_lock() 167 | 168 | def _shutdown(self): 169 | rospy.loginfo('Stopping ' + str(rospy.get_name())) 170 | 171 | def _resize_image(self, data, resolution): 172 | old_image = None 173 | 174 | try: 175 | old_image = self.bridge.imgmsg_to_cv2(data) 176 | except CvBridgeError as e: 177 | print(e) 178 | exit(20) 179 | 180 | try: 181 | if resolution < 1: 182 | # shrinking 183 | new_image = cv2.resize(old_image, (0, 0), fx=resolution, 184 | fy=resolution, 185 | interpolation=cv2.INTER_AREA) 186 | elif resolution > 1: 187 | # enlarging 188 | new_image = cv2.resize(old_image, (0, 0), fx=resolution, 189 | fy=resolution) 190 | else: 191 | # resolution == 1 --> Don't resize at all... 192 | new_image = old_image 193 | except cv2.error as e: 194 | print(e) 195 | exit(20) 196 | 197 | try: 198 | new_message = self.bridge.cv2_to_imgmsg(new_image, 199 | encoding=data.encoding) 200 | return new_message 201 | except CvBridgeError as e: 202 | print(e) 203 | exit(20) 204 | -------------------------------------------------------------------------------- /scenario_test_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package scenario_test_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.37 (2024-10-02) 6 | ------------------- 7 | 8 | 0.7.36 (2024-08-05) 9 | ------------------- 10 | 11 | 0.7.35 (2024-04-30) 12 | ------------------- 13 | * 0.6.35 14 | * update changelogs 15 | * Contributors: fmessmer 16 | 17 | 0.6.35 (2024-04-18) 18 | ------------------- 19 | 20 | 0.6.34 (2024-02-18) 21 | ------------------- 22 | 23 | 0.6.33 (2023-11-06) 24 | ------------------- 25 | 26 | 0.6.32 (2023-04-29) 27 | ------------------- 28 | 29 | 0.6.31 (2023-01-04) 30 | ------------------- 31 | 32 | 0.6.30 (2022-11-17) 33 | ------------------- 34 | 35 | 0.6.29 (2022-07-29) 36 | ------------------- 37 | 38 | 0.6.28 (2022-03-15) 39 | ------------------- 40 | 41 | 0.6.27 (2022-01-12) 42 | ------------------- 43 | 44 | 0.6.26 (2021-11-26) 45 | ------------------- 46 | 47 | 0.6.25 (2021-08-02) 48 | ------------------- 49 | 50 | 0.6.24 (2021-07-02) 51 | ------------------- 52 | 53 | 0.6.23 (2021-07-01) 54 | ------------------- 55 | 56 | 0.6.22 (2021-05-10) 57 | ------------------- 58 | 59 | 0.6.21 (2021-04-06) 60 | ------------------- 61 | 62 | 0.6.20 (2021-01-25) 63 | ------------------- 64 | 65 | 0.6.19 (2020-12-02) 66 | ------------------- 67 | 68 | 0.6.18 (2020-10-21) 69 | ------------------- 70 | 71 | 0.6.17 (2020-10-17) 72 | ------------------- 73 | * Merge pull request `#284 `_ from fmessmer/test_noetic 74 | test noetic 75 | * use setuptools instead of distutils 76 | * Bump CMake version to avoid CMP0048 warning 77 | * Merge pull request `#283 `_ from LoyVanBeek/feature/await_connections 78 | Allow scripts to await for ScriptableActionServers to be connected 79 | * Allow scripts to await for ScriptableActionSeervers to be connected 80 | * Contributors: Felix Messmer, Loy van Beek, fmessmer 81 | 82 | 0.6.16 (2020-03-18) 83 | ------------------- 84 | * Merge pull request `#277 `_ from LoyVanBeek/feature/document_move_base 85 | Document move base 86 | * Sort CMakelist 87 | * Document other scripts 88 | * Give 'always_succeeding_move_base' a shorter name 89 | * Allow to both abort a goal but also send a result with that abort signal 90 | * fixup! Add another example for move_base 91 | * Document example_move_base further 92 | * Add another example for move_base 93 | * make publishing transform configurable 94 | * get params from private ns 95 | * get timeout param 96 | * use node name 97 | * Catch exceptions when replying to show a better error message 98 | * Log how long a eply will wait for a goal 99 | * Set default timeout to None, to match the documentation 100 | * Put move_base a private namespace 101 | * Use the default_reply_delay in other reply-methods as well by default 102 | * Make names for reply_delay and 'think' more consistent 103 | * Make result_delay an optional parameter 104 | * Add more docs for ScriptableActionServer 105 | * Add example move base script 106 | * Merge pull request `#274 `_ from LoyVanBeek/feature/scenario_test_tools 107 | Add new package for scenario test tools 108 | * Fix formatting issue, passing too many args to str.format 109 | * Make executeables installable 110 | * Add license header to relevant files 111 | * Add new package for scenario test tools 112 | * Contributors: Felix Messmer, Loy van Beek, hyb 113 | 114 | 0.6.15 (2019-11-07) 115 | ------------------- 116 | 117 | 0.6.14 (2019-08-07) 118 | ------------------- 119 | 120 | 0.6.13 (2019-07-29) 121 | ------------------- 122 | 123 | 0.6.12 (2019-06-07) 124 | ------------------- 125 | 126 | 0.6.11 (2019-04-05) 127 | ------------------- 128 | 129 | 0.6.10 (2019-03-14) 130 | ------------------- 131 | 132 | 0.6.9 (2018-07-21 15:27) 133 | ------------------------ 134 | 135 | 0.6.8 (2018-07-21 14:47) 136 | ------------------------ 137 | 138 | 0.6.7 (2018-01-07) 139 | ------------------ 140 | 141 | 0.6.6 (2017-07-17) 142 | ------------------ 143 | 144 | 0.6.5 (2016-10-10) 145 | ------------------ 146 | 147 | 0.6.4 (2016-04-01) 148 | ------------------ 149 | 150 | 0.6.3 (2015-08-25) 151 | ------------------ 152 | 153 | 0.6.2 (2015-06-17) 154 | ------------------ 155 | 156 | 0.6.1 (2014-12-15) 157 | ------------------ 158 | 159 | 0.6.0 (2014-09-18) 160 | ------------------ 161 | 162 | 0.5.2 (2014-08-28) 163 | ------------------ 164 | 165 | 0.5.1 (2014-03-27) 166 | ------------------ 167 | -------------------------------------------------------------------------------- /scenario_test_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(scenario_test_tools) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | catkin_install_python(PROGRAMS 11 | scripts/dummy_behavior.py 12 | scripts/example_move_base.py 13 | scripts/example_test.py 14 | scripts/move_base_success.py 15 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 16 | ) 17 | -------------------------------------------------------------------------------- /scenario_test_tools/README.md: -------------------------------------------------------------------------------- 1 | # Automated testing 2 | 3 | The automated tests in `scenario_test_tools` provide a basic framework for 4 | mocked, scriptable and introspectable ROS services and action servers. 5 | These are operated from the same process so they can offer a correct, integral mock for various aspects of the robot. 6 | This allows to guide the high-level behavior into edge-cases and verify they are handled as intended. 7 | 8 | The core idea is that a tests specifies a sequence of services/actions that the behavior should call. 9 | A test can inspect the request/goal and reply with e.g. success or failure and 10 | then check if the failure is handled correctly etc. 11 | 12 | # Framework 13 | 14 | The core of the framework is the `ScriptableBase`, 15 | on which `ScriptableActionserver` and `ScriptableServiceServer` are based. 16 | 17 | `ScriptableBase` allows to 18 | - introspect past goals via `received_goals`. This is simply an list of all received goals. 19 | - This can be combined with the `remember_goals` context manager, 20 | which makes goals be forgotten before entering the context and after exiting again. 21 | Useful when you have multiple tests in sequence. 22 | - set the next result to a to-be-received goal with the `reply` method. 23 | There are several similar methods: 24 | - `reply_conditionally`: which takes a callable that returns True or False based on the goal and sends back the corresponding result 25 | - `await_goal` + `reply_directly`. `await_goal` blocks and returns a goal once it's received, so the goal can be inspected. 26 | Based on that, specify the reply with `reply_directly`. 27 | - Set default results via `default_result` without bothering with `reply*` most of the time, 28 | e.g for actions that are expected to always success on the real robot, eg. for doing text-to-speech. 29 | If you want to override temporarily, use this syntax: 30 | 31 | ```python 32 | with scriptable_thing.custom_reply(): 33 | scriptable_thing.reply(SomeActionResult('non-default')) 34 | ``` 35 | 36 | # Examples 37 | The `scripts` directory contains several scripts: 38 | - `example_test.py` and `dummy_behavior.py` work together. 39 | The dummy behavior will wait for commands to have a robot charge itself and plug itself in. 40 | The test sends these commands and checks that the behavior handles this command correctly. 41 | + Run the dummy_behavior with `rosrun scenario_test_tools dummy_behavior.py` 42 | + Then start the test with `rosrun scenario_test_tools example_test.py --start_scenario=test_all` 43 | 44 | - `move_base_success.py` uses the default_result feature to have a move_base implementation that always succeeds after a given time 45 | - `example_move_base` is a more elaborate version of the above, handling each goal a differently to show other features of the framework. 46 | First goal received succeeds, 2nd is aborted and 3rd is ignored. Further goals are not expected and not handled at all. 47 | -------------------------------------------------------------------------------- /scenario_test_tools/docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /scenario_test_tools/docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | import os 8 | import sys 9 | 10 | # -- Project information ----------------------------------------------------- 11 | 12 | project = 'scenario_test_tools' 13 | copyright = '2020, Mojin Robotics GmbH' 14 | author = 'Loy van Beek' 15 | 16 | # -- Path setup -------------------------------------------------------------- 17 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.abspath(__file__), os.pardir, os.pardir, project, 'src'))) 18 | 19 | # -- General configuration --------------------------------------------------- 20 | 21 | # Add any Sphinx extension module names here, as strings. They can be 22 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 23 | # ones. 24 | extensions = [ 25 | 'sphinx.ext.autodoc', 26 | 'sphinx.ext.coverage', 27 | 'sphinx.ext.doctest', 28 | 'sphinx.ext.napoleon', 29 | ] 30 | 31 | # Add any paths that contain templates here, relative to this directory. 32 | templates_path = ['_templates'] 33 | 34 | # List of patterns, relative to source directory, that match files and 35 | # directories to ignore when looking for source files. 36 | # This pattern also affects html_static_path and html_extra_path. 37 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 38 | 39 | 40 | # -- Options for HTML output ------------------------------------------------- 41 | 42 | # The theme to use for HTML and HTML Help pages. See the documentation for 43 | # a list of builtin themes. 44 | # 45 | html_theme = 'classic' 46 | 47 | # Add any paths that contain custom static files (such as style sheets) here, 48 | # relative to this directory. They are copied after the builtin static files, 49 | # so a file named "default.css" will overwrite the builtin "default.css". 50 | html_static_path = ['_static'] -------------------------------------------------------------------------------- /scenario_test_tools/docs/index.rst: -------------------------------------------------------------------------------- 1 | .. scenario_test_tools documentation master file, created by 2 | sphinx-quickstart on Tue Jan 14 15:17:17 2020. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to scenario_test_tools's documentation! 7 | ========================================= 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | .. toctree:: 14 | :maxdepth: 1 15 | :caption: Reference 16 | 17 | modules 18 | 19 | Indices and tables 20 | ================== 21 | 22 | * :ref:`genindex` 23 | * :ref:`modindex` 24 | * :ref:`search` 25 | -------------------------------------------------------------------------------- /scenario_test_tools/docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /scenario_test_tools/docs/modules.rst: -------------------------------------------------------------------------------- 1 | .. _modules-section-label: 2 | 3 | Module API 4 | ========== 5 | 6 | scenario_test_tools 7 | ------------- 8 | 9 | .. automodule:: scenario_test_tools 10 | :members: 11 | :special-members: 12 | :show-inheritance: 13 | 14 | scenario_test_tools.scriptable_base 15 | ----------------------------- 16 | 17 | .. automodule:: scenario_test_tools.scriptable_base 18 | :members: 19 | :show-inheritance: 20 | :synopsis: The core class that defines shared behavior 21 | 22 | scenario_test_tools.scriptable_action_server 23 | ----------------------------- 24 | 25 | .. automodule:: scenario_test_tools.scriptable_action_server 26 | :members: 27 | :show-inheritance: 28 | :synopsis: A scriptable ActionServer 29 | 30 | scenario_test_tools.scriptable_service_server 31 | ----------------------------- 32 | 33 | .. automodule:: scenario_test_tools.scriptable_service_server 34 | :members: 35 | :show-inheritance: 36 | :synopsis: A scriptable Service 37 | 38 | scenario_test_tools.move_base_emulator 39 | ----------------------------- 40 | 41 | .. automodule:: scenario_test_tools.scriptable_move_base 42 | :members: 43 | :show-inheritance: 44 | :synopsis: A scriptable MoveBase emulator 45 | 46 | scenario_test_tools.util 47 | ----------------------------- 48 | 49 | .. automodule:: scenario_test_tools.util 50 | :members: 51 | :show-inheritance: 52 | :synopsis: Various small utility functions that make life a bit easier 53 | 54 | -------------------------------------------------------------------------------- /scenario_test_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | scenario_test_tools 7 | 0.7.37 8 | The scenario_test_tools package implements helpers for scriptable scenario testing. 9 | It allows to set up a test harness for eg. a state machine or other high level behavior by 10 | providing mocked implementations for various action servers and services that work together 11 | 12 | Loy van Beek 13 | Loy van Beek 14 | 15 | Apache 2.0 16 | 17 | catkin 18 | python-setuptools 19 | python3-setuptools 20 | 21 | actionlib 22 | move_base_msgs 23 | rospy 24 | std_msgs 25 | std_srvs 26 | tf 27 | 28 | 29 | cob_sound 30 | cob_srvs 31 | control_msgs 32 | geometry_msgs 33 | 34 | -------------------------------------------------------------------------------- /scenario_test_tools/scripts/dummy_behavior.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2020 Mojin Robotics GmbH 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import random 18 | import rospy 19 | import actionlib 20 | 21 | from std_msgs.msg import String, Header 22 | from cob_sound.msg import SayAction, SayGoal 23 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 24 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal, FollowJointTrajectoryResult 25 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 26 | from cob_srvs.srv import SetString, SetStringRequest 27 | from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion 28 | 29 | charger_pose = PoseStamped(header=Header(frame_id='map'), 30 | pose=Pose(position=Point(10, 10, 0), 31 | orientation=Quaternion(1, 0, 0, 0))) 32 | 33 | charger_arm_traj = FollowJointTrajectoryGoal(trajectory=JointTrajectory(points=[JointTrajectoryPoint(positions=[0])])) 34 | 35 | if __name__ == "__main__": 36 | rospy.init_node("dummy_behavior") 37 | 38 | move_base_ac = actionlib.SimpleActionClient('move_base', MoveBaseAction) 39 | say_ac = actionlib.SimpleActionClient('say', SayAction) 40 | move_arm_ac = actionlib.SimpleActionClient('/arm/joint_trajectory_controller/follow_joint_trajectory', 41 | FollowJointTrajectoryAction) 42 | dock_srv = rospy.ServiceProxy('/dock', SetString) 43 | 44 | def command_callback(msg): 45 | if 'charge' in msg.data: 46 | rospy.loginfo("I'm told to go charge, lets go") 47 | move_base_ac.send_goal_and_wait(MoveBaseGoal(target_pose=charger_pose)) 48 | 49 | rospy.loginfo("I'm going to dock") 50 | dock_result = dock_srv(SetStringRequest('charger')) 51 | 52 | if not dock_result.success: 53 | rospy.logwarn("I borked docking, let's try again") 54 | say_ac.send_goal_and_wait(SayGoal('Docking has failed, lets try again')) 55 | 56 | dock_srv(SetStringRequest('charger')) 57 | # And now let's hope it does succeed 58 | 59 | rospy.loginfo("Let's use my arm to plug myself in") 60 | move_arm_ac.send_goal_and_wait(charger_arm_traj) 61 | arm_successful = move_arm_ac.get_result().error_code == FollowJointTrajectoryResult.SUCCESSFUL 62 | if not arm_successful: 63 | rospy.logwarn("I borked using my arm, let's try again") 64 | say_ac.send_goal_and_wait(SayGoal('My arm has failed, lets try again')) 65 | move_arm_ac.send_goal_and_wait(charger_arm_traj) 66 | # Again, lets assume it works this time 67 | 68 | rospy.loginfo("Ah, finally charging, lets that juice flow in") 69 | sentence = random.choice(["Jummy, fresh juice!", "Ah, some juice!"]) 70 | say_ac.send_goal_and_wait(SayGoal(sentence)) 71 | 72 | rospy.loginfo("Test succeeded, I'm done") 73 | 74 | command_subscriber = rospy.Subscriber('/command', String, command_callback) 75 | 76 | rospy.loginfo("I'm waiting for your command, e.g. to go charge") 77 | 78 | rospy.spin() -------------------------------------------------------------------------------- /scenario_test_tools/scripts/example_move_base.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | """ 4 | This action server node will wait for 3 MoveBaseGoals and handle each differently: 5 | - succeed the first one 6 | - abort the 2nd goal 7 | - just ignore the 3rd one 8 | """ 9 | 10 | import rospy 11 | 12 | from scenario_test_tools.scriptable_move_base import ScriptableMoveBase 13 | 14 | from move_base_msgs.msg import MoveBaseAction, MoveBaseResult 15 | 16 | 17 | if __name__ == "__main__": 18 | rospy.init_node('move_base_fake') 19 | 20 | succeeded = MoveBaseResult() 21 | 22 | # By default, wait 5 secs for the robot to arrive 23 | result_delay = rospy.get_param("~result_delay", 5) 24 | # By default, wait forever for a move_base goal 25 | timeout = rospy.get_param("~timeout", None) 26 | # By default, publish transformation /map to /base_link 27 | pub_transform = rospy.get_param("~pub_transform", True) 28 | 29 | move_base = ScriptableMoveBase(rospy.get_name(), MoveBaseAction, default_result_delay=result_delay, pub_transform=pub_transform) 30 | move_base.start() 31 | rospy.loginfo("fake move base running") 32 | 33 | 34 | try: 35 | move_base.reply(succeeded, marker='1: succeeded', timeout=timeout) 36 | 37 | # The action server will abort the goal, 38 | # to check that the client will perhaps retry the navigation or otherwise handle an abort 39 | move_base.reply(move_base.ABORT_GOAL, marker='2: Aborted', reply_delay=0, timeout=timeout) 40 | 41 | # The action server will ignore the goal and never send a result. 42 | # The client will have to decide it's taking too long and cancel 43 | move_base.reply(move_base.IGNORE_GOAL, marker='3: Ignored', reply_delay=0, timeout=timeout) 44 | except AssertionError as assertion_err: 45 | rospy.logerr(assertion_err) 46 | raise assertion_err 47 | rospy.spin() -------------------------------------------------------------------------------- /scenario_test_tools/scripts/move_base_success.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | 5 | from scenario_test_tools.scriptable_move_base import ScriptableMoveBase 6 | from move_base_msgs.msg import MoveBaseAction, MoveBaseResult 7 | 8 | if __name__ == "__main__": 9 | rospy.init_node('always_succeeding_move_base') 10 | 11 | # By default, wait 5 secs for the robot to arrive 12 | result_delay = rospy.get_param("result_delay", 5) 13 | 14 | succeeded = MoveBaseResult() 15 | move_base = ScriptableMoveBase('~move_base', 16 | MoveBaseAction, 17 | default_result_delay=result_delay, 18 | default_result=succeeded) 19 | 20 | move_base.start() 21 | rospy.loginfo("always_succeeding_move_base running") 22 | 23 | rospy.spin() -------------------------------------------------------------------------------- /scenario_test_tools/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['scenario_test_tools'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /scenario_test_tools/src/scenario_test_tools/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is the top-level namespace of the scenario_test_tools package. 3 | """ -------------------------------------------------------------------------------- /scenario_test_tools/src/scenario_test_tools/scriptable_action_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2020 Mojin Robotics GmbH 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import actionlib 18 | import rospy 19 | 20 | from scenario_test_tools.scriptable_base import ScriptableBase 21 | from scenario_test_tools.util import countdown_sleep 22 | 23 | 24 | class ScriptableActionServer(ScriptableBase): 25 | """ 26 | ScriptableActionServer allows its users to determine the ActionResult to an ActionGoal. 27 | 28 | The result-type for e.g. FooAction should be FooResult, not FooActionResult! 29 | 30 | Goals can be aborted by setting `ABORT_GOAL` as the result. 31 | 32 | Goals can be ignored by setting `IGNORE_GOAL` as the result. This makes the action client never get a result 33 | 34 | Note that Actionlib uses the term 'result' for its, well, results, whereas ROS services use 'reply'. 35 | The base class `ScriptableBase` uses the 'reply' terminology. 36 | `ScriptableActionServer` should be consistent with the action terminology, 37 | so it's constructor does some translation between terms and passes 'default_reply=default_result' 38 | to the constructor of `ScriptableBase`. 39 | """ 40 | 41 | SUCCEED_GOAL = "SUCCEED_GOAL" 42 | IGNORE_GOAL = "IGNORE_GOAL" 43 | ABORT_GOAL = "ABORT_GOAL" 44 | 45 | def __init__(self, name, action_type, goal_formatter=format, result_formatter=format, default_result=None, default_result_delay=0): 46 | """ 47 | Set up a ScriptableActionServer based on the name and the type of Action it should implement 48 | 49 | :param action_type: action type (e.g. MoveBaseAction) 50 | """ 51 | ScriptableBase.__init__(self, name, 52 | goal_formatter=goal_formatter, 53 | reply_formatter=result_formatter, 54 | default_reply=default_result, 55 | default_reply_delay=default_result_delay) 56 | 57 | self._as = actionlib.SimpleActionServer(name, action_type, auto_start=False) 58 | self._as.register_goal_callback(self._execute_cb) 59 | self._as.register_preempt_callback(self._preempt_cb) 60 | 61 | def __repr__(self): 62 | return "ScriptableActionServer('{}')".format(self._name) 63 | 64 | @property 65 | def connected(self): 66 | return self._as.action_server.goal_sub.get_num_connections() >= 1 67 | 68 | def start(self): 69 | """ 70 | Start the action server and thus listen to goals 71 | """ 72 | self._as.start() 73 | 74 | def stop(self): 75 | self._as.action_server.started = False 76 | super(ScriptableActionServer, self).stop() 77 | 78 | def _execute_cb(self): 79 | """ 80 | Called when the underlying action server receives a goal. 81 | If the default_result is None, it will wait for a custom result to be set via reply* otherwise 82 | return the default_result after the given default_result_delay 83 | 84 | In the reply-case,it then notifies self.reply* (which should be called by a test script outside this class), 85 | after which self.reply* determines the result to the goal. 86 | Then it notifies _execute_cb that the result has been determined so that _execute_cb can send it 87 | """ 88 | 89 | self._current_goal = self._as.accept_new_goal() 90 | self._received_goals += [self._current_goal] 91 | try: 92 | goal_str = self.goal_formatter(self._current_goal) 93 | except Exception as e: 94 | rospy.logerr("goal_formatter of {} raised an exception: {}".format(self._name, e)) 95 | goal_str = self._current_goal 96 | print('{}.execute: Goal: {}'.format(self._name, goal_str)) 97 | 98 | if self.default_reply is not None: 99 | countdown_sleep(self._default_reply_delay, text="{}.execute: Wait for {}s. ".format(self._name, self._default_reply_delay) + "Remaining {}s...") 100 | self._call_pre_reply_callbacks(self._current_goal, self.default_reply) 101 | self._send_result(self.default_reply) 102 | else: 103 | self._request.set() 104 | # Now, wait for action to be called, which sets the _reply event AND the _next_reply 105 | self._reply.wait() 106 | self._reply.clear() 107 | 108 | self._call_pre_reply_callbacks(self._current_goal, self._next_reply) 109 | self._send_result(self._next_reply) 110 | 111 | self._next_reply = None 112 | self._current_goal = None 113 | self._sent.set() 114 | 115 | def _send_result(self, result_tuple): 116 | """ 117 | Send the result and deal with ignored and aborted goals 118 | 119 | :param result: a Result associated with the Action-type of this Server 120 | """ 121 | 122 | if isinstance(result_tuple, tuple): 123 | # In this case, result[0] is the actual result and result[1] is an success/abort/ignore action 124 | result, action = result_tuple 125 | else: 126 | # result_tuple is not really a tuple 127 | if result_tuple == self.ABORT_GOAL: 128 | result, action = None, self.ABORT_GOAL 129 | elif result_tuple == self.IGNORE_GOAL: 130 | result, action = None, self.IGNORE_GOAL 131 | else: 132 | result, action = result_tuple, self.SUCCEED_GOAL 133 | 134 | if action == self.SUCCEED_GOAL: 135 | try: 136 | result_str = self.result_formatter(result) 137 | except Exception as e: 138 | rospy.logerr("result_formatter of {} raised an exception: {}".format(self._name, e)) 139 | result_str = self._current_goal 140 | print("{}.execute: Result: {}".format(self._name, result_str)) 141 | self._as.set_succeeded(result) 142 | elif action == self.ABORT_GOAL: 143 | print("{}.execute: Result: {}, action: {}".format(self._name, result, action)) 144 | self._as.set_aborted(result) 145 | elif action == self.IGNORE_GOAL: 146 | print("{}.execute: Result: {}, action: {}".format(self._name, result, action)) 147 | # Do not send a reply at all, to see how the client deals with it 148 | pass 149 | 150 | def _preempt_cb(self): 151 | """ 152 | Handle action preemption by the action client 153 | """ 154 | if self._as.is_active(): 155 | self.preemption = True 156 | self._as.set_preempted() 157 | -------------------------------------------------------------------------------- /scenario_test_tools/src/scenario_test_tools/scriptable_move_base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2020 Mojin Robotics GmbH 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import math 18 | 19 | import rospy 20 | import tf 21 | 22 | from scenario_test_tools.scriptable_action_server import ScriptableActionServer 23 | from scenario_test_tools.util import countdown_sleep, round_tuple 24 | 25 | 26 | def format_move_base_goal(mbg): 27 | roll, pitch, yaw = tf.transformations.euler_from_quaternion( 28 | [mbg.target_pose.pose.orientation.x, mbg.target_pose.pose.orientation.y, mbg.target_pose.pose.orientation.z, 29 | mbg.target_pose.pose.orientation.w]) 30 | return "MoveBase to: (x={x}, y={y}, yaw={yaw})".format( 31 | x=mbg.target_pose.pose.position.x, 32 | y=mbg.target_pose.pose.position.y, 33 | yaw=yaw) 34 | 35 | 36 | class ScriptableMoveBase(ScriptableActionServer): 37 | def __init__(self, name, action_type, goal_formatter=format, result_formatter=format, default_result=None, default_result_delay=0, pub_transform=True): 38 | ScriptableActionServer.__init__(self, 39 | name=name, 40 | action_type=action_type, 41 | goal_formatter=goal_formatter, 42 | result_formatter=result_formatter, 43 | default_result=default_result, 44 | default_result_delay=default_result_delay) 45 | 46 | self.br = tf.TransformBroadcaster() 47 | self.pose_bl = [0, 0, 0] 48 | if pub_transform: 49 | self._base_link_timer = rospy.Timer(rospy.Duration(0.1), self._pub_transform_bl) 50 | 51 | def stop(self): 52 | self._base_link_timer.shutdown() 53 | super(ScriptableMoveBase, self).stop() 54 | 55 | def _execute_cb(self): 56 | """ 57 | Called when the underlying action server receives a goal. 58 | If the default_result is None, it will wait for a custom result to be set via reply* otherwise 59 | return the default_result after the given default_result_delay 60 | 61 | In the reply-case,it then notifies self.reply* (which should be called by a test script outside this class), 62 | after which self.reply* determines the result to the goal. 63 | Then it notifies _execute_cb that the result has been determined so that _execute_cb can send it 64 | """ 65 | print('\n######## {}.execute ###########'.format(self._name)) 66 | 67 | self._current_goal = self._as.accept_new_goal() 68 | self._received_goals.append(self._current_goal) 69 | 70 | try: 71 | goal_str = self.goal_formatter(self._current_goal) 72 | except Exception as e: 73 | rospy.logerr("goal_formatter of {} raised an exception: {}".format(self._name, e)) 74 | goal_str = self._current_goal 75 | print('{}.execute: From {} to Goal: {}'.format(self._name, self.pose_bl, goal_str)) 76 | 77 | if self.default_reply is not None: 78 | countdown_sleep(self._default_reply_delay, text="{}.execute: Wait for {}s. ".format(self._name, self._default_reply_delay) + "Remaining {}s...") 79 | self._call_pre_reply_callbacks(self._current_goal, self.default_reply) 80 | self._send_result(self.default_reply) 81 | else: 82 | self._request.set() 83 | # Now, wait for to be called, which sets the _reply event AND the _next_reply 84 | self._reply.wait() 85 | self._reply.clear() 86 | 87 | self._call_pre_reply_callbacks(self._current_goal, self._next_reply) 88 | self._send_result(self._next_reply) 89 | 90 | self._next_reply = None 91 | self._current_goal = None 92 | self._sent.set() 93 | 94 | def _send_result(self, result): 95 | if result is not self.IGNORE_GOAL and result is not self.ABORT_GOAL: 96 | try: 97 | result_str = self.result_formatter(result) 98 | except Exception as e: 99 | result_str = result 100 | print("{}.execute: Result: {}".format(self._name, result_str)) 101 | self.pose_bl = self.goal_to_xy_theta(self.current_goal) 102 | self._as.set_succeeded(result) 103 | elif result == self.ABORT_GOAL: 104 | print("{}.execute: Result: {}".format(self._name, result)) 105 | self._as.set_aborted() 106 | elif result == self.IGNORE_GOAL: 107 | print("{}.execute: Result: {}".format(self._name, result)) 108 | # Do not send a reply at all, to see how the client deals with it 109 | pass 110 | 111 | def _pub_transform_bl(self, event): 112 | """ 113 | Publish the transform from /map to /base_link 114 | :param event: timer event 115 | """ 116 | self.br.sendTransform((self.pose_bl[0], self.pose_bl[1], 0), 117 | tf.transformations.quaternion_from_euler(0, 0, self.pose_bl[2]), 118 | rospy.Time.now(), 119 | "/base_link", 120 | "/map") 121 | 122 | def distance(self, pose1, pose2): 123 | """ 124 | Calculate the distance between two x, y, theta arrays 125 | 126 | :param pose1: Position to calculate distance from 127 | :type pose1: [x, y, theta] 128 | :param pose2: Position to calculate distance to 129 | :type pose2: [x, y, theta] 130 | :return: cartesian distance between the positions 131 | :rtype float 132 | """ 133 | if (len(pose1) == len(pose2) and len(pose1) == 3): 134 | dist = math.sqrt(pow(pose2[0] - pose1[0], 2) + pow(pose2[1] - pose1[1], 2)) 135 | else: 136 | dist = 0.0 137 | return dist 138 | 139 | @staticmethod 140 | def goal_to_xy_theta(goal): 141 | """ 142 | Convert a MoveBaseGoal to a much condensed representation: a list of [x, y, theta] 143 | 144 | :param goal: A moveBaseGoal 145 | :return: a compact representation of the goal 146 | :rtype List[float] 147 | """ 148 | roll, pitch, yaw = tf.transformations.euler_from_quaternion( 149 | [goal.target_pose.pose.orientation.x, 150 | goal.target_pose.pose.orientation.y, 151 | goal.target_pose.pose.orientation.z, 152 | goal.target_pose.pose.orientation.w]) 153 | return round_tuple((goal.target_pose.pose.position.x, 154 | goal.target_pose.pose.position.y, 155 | yaw), 1) 156 | 157 | -------------------------------------------------------------------------------- /scenario_test_tools/src/scenario_test_tools/scriptable_service_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2020 Mojin Robotics GmbH 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | 19 | from scenario_test_tools.scriptable_base import ScriptableBase 20 | from scenario_test_tools.util import countdown_sleep 21 | 22 | 23 | class ScriptableServiceServer(ScriptableBase): 24 | """ 25 | ScriptableServiceServer allows its users to determine the Response to a service Request 26 | """ 27 | def __init__(self, name, service_type, request_formatter=format, response_formatter=format, default_response=None, default_response_delay=0): 28 | """ 29 | Set up a ScriptableServiceServer based on the name and the type ofService is should implement 30 | :param service_type: Service type (e.g. std_srvs/SetBool) 31 | """ 32 | ScriptableBase.__init__(self, name, 33 | goal_formatter=request_formatter, 34 | reply_formatter=response_formatter, 35 | default_reply=default_response, 36 | default_reply_delay=default_response_delay) 37 | 38 | self._srv = rospy.Service(name, service_type, self._execute_cb) 39 | 40 | def __repr__(self): 41 | return "ScriptableServiceServer('{}')".format(self._name) 42 | 43 | def stop(self): 44 | super(ScriptableServiceServer, self).stop() 45 | 46 | def _execute_cb(self, request): 47 | """ 48 | Called when the underlying service receives a goal. 49 | If the default_result is None, it will wait for a custom result to be set via reply* otherwise 50 | return the default_result after the given default_result_delay 51 | 52 | In the reply-case,it then notifies self.reply* (which should be called by a test script outside this class), 53 | after which self.reply* determines the result to the goal. 54 | Then it notifies _execute_cb that the result has been determined so that _execute_cb can send it 55 | """ 56 | 57 | self._current_goal = request 58 | try: 59 | request_str = self.goal_formatter(self._current_goal) 60 | except Exception as e: 61 | rospy.logerr("request_formatter of {} raised an exception: {}".format(self._name, e)) 62 | request_str = self._current_goal 63 | print('{}.execute: Request: {}'.format(self._name, request_str)) 64 | 65 | if self.default_reply is not None: 66 | result = self.default_reply 67 | countdown_sleep(self._default_reply_delay, text="{}.execute: Wait for {}s. ".format(self._name, self._default_reply_delay) + "Remaining {}s...") 68 | else: 69 | self._request.set() 70 | # Now, wait for to be called, which sets the _reply event AND the _next_reply 71 | self._reply.wait() 72 | self._reply.clear() 73 | 74 | result = self._next_reply 75 | 76 | self._next_reply = None 77 | self._current_goal = None 78 | self._sent.set() 79 | 80 | self._call_pre_reply_callbacks(request, result) 81 | return result 82 | -------------------------------------------------------------------------------- /scenario_test_tools/src/scenario_test_tools/util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright 2020 Mojin Robotics GmbH 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import sys 18 | 19 | import rospy 20 | 21 | 22 | def countdown_sleep(duration, stepsize=1, text="{}"): 23 | """ 24 | Print the numbers of a countdown on the terminal in-place, without using a new line for each number 25 | 26 | :param duration: How long to count down 27 | :param stepsize: Time between steps 28 | :param text: What text to display each tick. Must include a format string, eg. 'launch in {}...' 29 | """ 30 | step, substep = divmod(duration, stepsize) 31 | for i in range(int(step), 0, -stepsize): 32 | if not rospy.is_shutdown(): 33 | sys.stdout.write("\r" + text.format(i)) 34 | sys.stdout.flush() 35 | rospy.sleep(stepsize) 36 | rospy.sleep(substep) 37 | 38 | if step > 0: sys.stdout.write('\n') 39 | sys.stdout.flush() 40 | 41 | def round_tuple(tup, decimals): 42 | """ 43 | Rounding operation broadcast over a tuple 44 | 45 | :param tup: tuple to be rounded 46 | :param decimals: how many decimals to round to 47 | :return: tuple of same dimension but with lower precision 48 | """ 49 | return tuple([round(elem, decimals) for elem in tup]) --------------------------------------------------------------------------------