├── .github ├── PULL_REQUEST_TEMPLATE.md ├── dependabot.yml └── workflows │ └── main.yml ├── .gitignore ├── LICENSE ├── README.md ├── cob_driver ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── cob_light ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ └── SetLightMode.action ├── common │ ├── include │ │ ├── breathColorMode.h │ │ ├── breathMode.h │ │ ├── circleColorMode.h │ │ ├── colorUtils.h │ │ ├── concurrentQueue.h │ │ ├── distApproxMode.h │ │ ├── fadeColorMode.h │ │ ├── flashMode.h │ │ ├── glowColorMode.h │ │ ├── kitMode.h │ │ ├── mode.h │ │ ├── modeExecutor.h │ │ ├── modeFactory.h │ │ ├── modeRunner.h │ │ ├── sequenceMode.h │ │ ├── serialIO.h │ │ ├── staticMode.h │ │ ├── sweepColorMode.h │ │ ├── turnIndicatorMode.h │ │ └── xmasMode.h │ └── src │ │ ├── modeExecutor.cpp │ │ ├── modeFactory.cpp │ │ └── serialIO.cpp ├── msg │ ├── ColorRGBAArray.msg │ ├── LightMode.msg │ ├── LightModes.msg │ └── Sequence.msg ├── package.xml ├── ros │ ├── include │ │ ├── colorO.h │ │ ├── colorOMarker.h │ │ ├── colorOSim.h │ │ ├── iColorO.h │ │ ├── ms35.h │ │ └── stageprofi.h │ ├── scripts │ │ ├── light_action_test.py │ │ ├── light_service_test.py │ │ ├── test.py │ │ ├── test_sprofi_circle.py │ │ ├── test_sprofi_modes.py │ │ └── test_sprofi_part.py │ └── src │ │ ├── cob_light.cpp │ │ ├── colorO.cpp │ │ ├── colorOMarker.cpp │ │ ├── colorOSim.cpp │ │ ├── ms35.cpp │ │ └── stageprofi.cpp └── srv │ ├── SetLightMode.srv │ └── StopLightMode.srv ├── cob_mimic ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ └── SetMimic.action ├── common │ ├── afraid.mp4 │ ├── angry.mp4 │ ├── asking.mp4 │ ├── blinking.mp4 │ ├── blinking_left.mp4 │ ├── blinking_right.mp4 │ ├── bored.mp4 │ ├── busy.mp4 │ ├── confused.mp4 │ ├── default.jpg │ ├── default.mp4 │ ├── falling_asleep.mp4 │ ├── happy.mp4 │ ├── laughing.mp4 │ ├── no.mp4 │ ├── original │ │ ├── afraid.mp4 │ │ ├── angry.mp4 │ │ ├── asking.mp4 │ │ ├── blinking.mp4 │ │ ├── blinking_left.mp4 │ │ ├── blinking_right.mp4 │ │ ├── bored.mp4 │ │ ├── busy.mp4 │ │ ├── confused.mp4 │ │ ├── default.mp4 │ │ ├── falling_asleep.mp4 │ │ ├── happy.mp4 │ │ ├── laughing.mp4 │ │ ├── no.mp4 │ │ ├── sad.mp4 │ │ ├── searching.mp4 │ │ ├── sleeping.mp4 │ │ ├── surprised.mp4 │ │ ├── tired.mp4 │ │ ├── waking_up.mp4 │ │ └── yes.mp4 │ ├── sad.mp4 │ ├── searching.mp4 │ ├── sleeping.jpg │ ├── sleeping.mp4 │ ├── surprised.mp4 │ ├── tired.mp4 │ ├── waking_up.mp4 │ └── yes.mp4 ├── package.xml ├── src │ ├── mimic_node.cpp │ └── test_mimic.py └── srv │ └── SetMimic.srv ├── cob_phidgets ├── CHANGELOG.rst ├── CMakeLists.txt ├── common │ ├── include │ │ └── cob_phidgets │ │ │ ├── phidget.h │ │ │ └── phidgetik.h │ └── src │ │ ├── phidget.cpp │ │ └── phidgetik.cpp ├── msg │ ├── AnalogSensor.msg │ └── DigitalSensor.msg ├── package.xml ├── ros │ ├── include │ │ └── cob_phidgets │ │ │ ├── phidget_manager.h │ │ │ └── phidgetik_ros.h │ └── src │ │ ├── cob_phidgets_sim_node.cpp │ │ ├── ifkit.c │ │ ├── phidget_manager.cpp │ │ ├── phidget_sensors_node.cpp │ │ ├── phidgetik_ros.cpp │ │ ├── phidgets_range_sensors.cpp │ │ ├── set_device_name.cpp │ │ ├── simulated_range_sensors.py │ │ └── tablet_leer.c └── srv │ ├── SetDataRate.srv │ ├── SetDigitalSensor.srv │ └── SetTriggerValue.srv ├── cob_scan_unifier ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── cob_scan_unifier │ │ └── scan_unifier_node.h ├── package.xml └── src │ └── scan_unifier_node.cpp ├── cob_sick_s300 ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── common │ ├── include │ │ └── cob_sick_s300 │ │ │ ├── ScannerSickS300.h │ │ │ ├── SerialIO.h │ │ │ └── TelegramS300.h │ └── src │ │ ├── ScannerSickS300.cpp │ │ └── SerialIO.cpp ├── package.xml └── ros │ ├── src │ ├── cob_scan_filter.cpp │ └── cob_sick_s300.cpp │ └── test │ ├── dummy_config.yaml │ └── dummy_launch.xml ├── cob_sound ├── CHANGELOG.rst ├── CMakeLists.txt ├── action │ ├── Play.action │ └── Say.action ├── fix_swift_for_precise.sh ├── package.xml └── ros │ └── src │ ├── sound.cpp │ └── test_client.py └── laser_scan_densifier ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── laser_scan_densifier │ ├── laser_scan_densifier.h │ └── laser_scan_densifier_nodelet.h ├── laser_scan_densifier_nodelet.xml ├── package.xml └── src ├── laser_scan_densifier.cpp ├── laser_scan_densifier_node.cpp └── laser_scan_densifier_nodelet.cpp /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | - fixes # 2 | - new source code files added (proper [APACHE license header](https://github.com/ipa320/setup/tree/master/templates) **must** be used) 3 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | --- 2 | version: 2 3 | updates: 4 | - package-ecosystem: "github-actions" 5 | directory: "/" 6 | schedule: 7 | interval: "weekly" 8 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: GHA CI 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: 7 | - cron: "0 0 * * 0" # every Sunday at midnight 8 | workflow_dispatch: 9 | 10 | jobs: 11 | industrial_ci: 12 | name: GHA CI 13 | runs-on: ubuntu-latest 14 | timeout-minutes: 60 15 | 16 | env: 17 | ADDITIONAL_DEBS: 'apt-utils dialog git' 18 | CATKIN_LINT: pedantic 19 | CATKIN_LINT_ARGS: '--ignore description_boilerplate' 20 | CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release 21 | PYLINT_ARGS: '--output-format=parseable --errors-only' 22 | PYLINT_CHECK: true 23 | ROS_REPO: main 24 | 25 | strategy: 26 | matrix: 27 | include: 28 | - { ROS_DISTRO: noetic } 29 | 30 | steps: 31 | - uses: actions/checkout@v5 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_driver 2 | =========== 3 | 4 | ## GitHub Actions - Continuous Integration 5 | 6 | CI-Status ```noetic-devel```: [![GHA CI](https://github.com/4am-robotics/cob_driver/actions/workflows/main.yml/badge.svg?branch=noetic-devel)](https://github.com/4am-robotics/cob_driver/actions/workflows/main.yml?query=branch%3Anoetic-devel) 7 | -------------------------------------------------------------------------------- /cob_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_driver) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /cob_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_driver 3 | 0.8.18 4 | The cob_driver stack includes packages that provide access to the Care-O-bot hardware through ROS messages, services and actions. E.g. for mobile base, arm, camera sensors, laser scanners, etc... 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_driver 9 | 10 | 11 | Felix Messmer 12 | Florian Weisshardt 13 | 14 | catkin 15 | 16 | cob_light 17 | cob_mimic 18 | cob_phidgets 19 | cob_scan_unifier 20 | cob_sick_s300 21 | cob_sound 22 | laser_scan_densifier 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /cob_light/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_light) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | actionlib 8 | actionlib_msgs 9 | diagnostic_msgs 10 | message_generation 11 | roscpp 12 | sensor_msgs 13 | std_msgs 14 | visualization_msgs) 15 | 16 | find_package(Boost REQUIRED COMPONENTS thread) 17 | 18 | ### Message Generation ### 19 | add_message_files( 20 | FILES 21 | ColorRGBAArray.msg 22 | LightMode.msg 23 | LightModes.msg 24 | Sequence.msg 25 | ) 26 | 27 | add_service_files( 28 | FILES 29 | SetLightMode.srv 30 | StopLightMode.srv 31 | ) 32 | 33 | add_action_files( 34 | FILES 35 | SetLightMode.action 36 | ) 37 | 38 | generate_messages( 39 | DEPENDENCIES actionlib_msgs sensor_msgs std_msgs visualization_msgs 40 | ) 41 | 42 | catkin_package( 43 | CATKIN_DEPENDS actionlib actionlib_msgs diagnostic_msgs message_runtime roscpp sensor_msgs std_msgs visualization_msgs 44 | ) 45 | 46 | ### BUILD ### 47 | #pkg_check_modules(GTKMM REQUIRED gtkmm-3.0) 48 | #pkg_check_modules(GTHREAD REQUIRED gthread-2.0) 49 | 50 | add_compile_options(-Wno-narrowing) 51 | 52 | include_directories(${PROJECT_SOURCE_DIR}/ros/include ${PROJECT_SOURCE_DIR}/common/include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 53 | set(HEADERS 54 | ${PROJECT_SOURCE_DIR}/common/include/breathColorMode.h 55 | ${PROJECT_SOURCE_DIR}/common/include/breathMode.h 56 | ${PROJECT_SOURCE_DIR}/common/include/circleColorMode.h 57 | ${PROJECT_SOURCE_DIR}/common/include/colorUtils.h 58 | ${PROJECT_SOURCE_DIR}/common/include/concurrentQueue.h 59 | ${PROJECT_SOURCE_DIR}/common/include/fadeColorMode.h 60 | ${PROJECT_SOURCE_DIR}/common/include/flashMode.h 61 | ${PROJECT_SOURCE_DIR}/common/include/mode.h 62 | ${PROJECT_SOURCE_DIR}/common/include/modeExecutor.h 63 | ${PROJECT_SOURCE_DIR}/common/include/modeFactory.h 64 | ${PROJECT_SOURCE_DIR}/common/include/modeRunner.h 65 | ${PROJECT_SOURCE_DIR}/common/include/sequenceMode.h 66 | ${PROJECT_SOURCE_DIR}/common/include/serialIO.h 67 | ${PROJECT_SOURCE_DIR}/common/include/staticMode.h 68 | ${PROJECT_SOURCE_DIR}/common/include/sweepColorMode.h 69 | ${PROJECT_SOURCE_DIR}/common/include/distApproxMode.h 70 | ${PROJECT_SOURCE_DIR}/common/include/xmasMode.h 71 | ${PROJECT_SOURCE_DIR}/common/include/kitMode.h 72 | ${PROJECT_SOURCE_DIR}/common/include/turnIndicatorMode.h 73 | ${PROJECT_SOURCE_DIR}/ros/include/colorO.h 74 | ${PROJECT_SOURCE_DIR}/ros/include/colorOMarker.h 75 | ${PROJECT_SOURCE_DIR}/ros/include/colorOSim.h 76 | ${PROJECT_SOURCE_DIR}/ros/include/iColorO.h 77 | ${PROJECT_SOURCE_DIR}/ros/include/ms35.h 78 | ${PROJECT_SOURCE_DIR}/ros/include/stageprofi.h 79 | ) 80 | 81 | 82 | add_executable(${PROJECT_NAME} 83 | common/src/modeExecutor.cpp 84 | common/src/modeFactory.cpp 85 | common/src/serialIO.cpp 86 | ros/src/${PROJECT_NAME}.cpp 87 | ros/src/colorO.cpp 88 | ros/src/colorOMarker.cpp 89 | ros/src/colorOSim.cpp 90 | ros/src/ms35.cpp 91 | ros/src/stageprofi.cpp) 92 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 93 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 94 | 95 | ### INSTALL ### 96 | install(TARGETS ${PROJECT_NAME} 97 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 98 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 99 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 100 | ) 101 | 102 | catkin_install_python(PROGRAMS 103 | ros/scripts/test.py 104 | ros/scripts/test_sprofi_circle.py 105 | ros/scripts/test_sprofi_modes.py 106 | ros/scripts/test_sprofi_part.py 107 | ros/scripts/light_action_test.py 108 | ros/scripts/light_service_test.py 109 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 110 | ) 111 | -------------------------------------------------------------------------------- /cob_light/action/SetLightMode.action: -------------------------------------------------------------------------------- 1 | # The goal 2 | cob_light/LightMode mode 3 | --- 4 | # The results 5 | uint8 active_mode 6 | uint8 active_priority 7 | uint64 track_id 8 | --- 9 | # The feedback 10 | uint8 active_mode 11 | uint8 active_priority 12 | -------------------------------------------------------------------------------- /cob_light/common/include/breathColorMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef BREATHCOLORMODE_H 19 | #define BREATHCOLORMODE_H 20 | 21 | #include 22 | 23 | class BreathColorMode : public Mode 24 | { 25 | public: 26 | BreathColorMode(color::rgba color, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout), _timer_inc(0.0) 28 | { 29 | _color = color; 30 | h = 0.0; 31 | _inc = ((M_PI*2) / UPDATE_RATE_HZ) * _freq; 32 | } 33 | 34 | void execute() 35 | { 36 | float r = 0; 37 | float g = 0; 38 | float b = 0; 39 | 40 | color::Color::hsv2rgb(h, 1.0, 1.0, r, g, b); 41 | 42 | h += 0.001; 43 | if(h > 1.0) h = 0.0; 44 | 45 | //double fV = (exp(sin(_timer_inc))-1.0/M_E)*(1.000/(M_E-1.0/M_E)); 46 | double fV = (exp(sin(_timer_inc))-0.36787944)*0.42545906411; 47 | 48 | _timer_inc += _inc; 49 | if(_timer_inc >= M_PI*2) 50 | { 51 | _timer_inc = 0.0; 52 | _pulsed++; 53 | } 54 | 55 | color::rgba col; 56 | col.r = r; 57 | col.g = g; 58 | col.b = b; 59 | col.a = fV; 60 | 61 | m_sigColorReady(col); 62 | } 63 | 64 | std::string getName(){ return std::string("BreathColorMode"); } 65 | 66 | private: 67 | double _timer_inc; 68 | double _inc; 69 | float h; 70 | }; 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /cob_light/common/include/breathMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef BREATHMODE_H 19 | #define BREATHMODE_H 20 | 21 | #include 22 | 23 | class BreathMode : public Mode 24 | { 25 | public: 26 | BreathMode(color::rgba color, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout), _timer_inc(0.0) 28 | { 29 | _color = color; 30 | _init_color = color; 31 | _inc = ((M_PI*2) / UPDATE_RATE_HZ) * _freq; 32 | } 33 | 34 | void execute() 35 | { 36 | //double fV = (exp(sin(_timer_inc))-1.0/M_E)*(1.000/(M_E-1.0/M_E)); 37 | double fV = (exp(-cos(_timer_inc))-0.36787944)*0.42545906411; 38 | 39 | _timer_inc += _inc; 40 | if(_timer_inc >= M_PI*2) 41 | { 42 | _timer_inc = 0.0; 43 | _pulsed++; 44 | } 45 | 46 | _color.a = fV * _init_color.a; 47 | _color.a = _color.a > 1 ? 1 : _color.a < 0 ? 0 : _color.a; 48 | 49 | m_sigColorReady(_color); 50 | } 51 | 52 | std::string getName(){ return std::string("BreathMode"); } 53 | 54 | private: 55 | double _timer_inc; 56 | double _inc; 57 | }; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /cob_light/common/include/circleColorMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef CIRCLECOLORMODE_H 19 | #define CIRCLECOLORMODE_H 20 | 21 | #include 22 | #include 23 | 24 | class CircleColorMode : public Mode 25 | { 26 | public: 27 | CircleColorMode(std::vector colors, size_t num_leds, int priority = 0, double freq = 5, int pulses = 0, double timeout = 0) 28 | :Mode(priority, freq, pulses, timeout), _toggle(false), _timer_inc(0), _num_leds(num_leds) 29 | { 30 | _colors = colors; 31 | _colors.resize(num_leds); 32 | if(_pulses != 0) 33 | { 34 | _pulses *=2; 35 | _pulses +=1; 36 | } 37 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 38 | } 39 | 40 | void execute() 41 | { 42 | if(_timer_inc >= 1.0) 43 | { 44 | std::rotate(_colors.begin(), _colors.begin()+1, _colors.end()); 45 | _pulsed++; 46 | m_sigColorsReady(_colors); 47 | _timer_inc = 0.0; 48 | } 49 | else 50 | _timer_inc += _inc; 51 | } 52 | 53 | std::string getName(){ return std::string("CircleColorMode"); } 54 | 55 | private: 56 | bool _toggle; 57 | double _timer_inc; 58 | double _inc; 59 | size_t _num_leds; 60 | }; 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /cob_light/common/include/colorUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COLOR_UTILS_H 19 | #define COLOR_UTILS_H 20 | 21 | #include 22 | #include 23 | 24 | namespace color 25 | { 26 | struct rgba 27 | { 28 | rgba(): r(0.0), g(0.0), b(0.0), a(0.0) {} 29 | float r; 30 | float g; 31 | float b; 32 | float a; 33 | }; 34 | 35 | struct rgb 36 | { 37 | rgb(): r(0.0), g(0.0), b(0.0){} 38 | float r; 39 | float g; 40 | float b; 41 | }; 42 | 43 | struct hsv 44 | { 45 | hsv(): h(0.0), s(0.0), v(0.0) {} 46 | float h; 47 | float s; 48 | float v; 49 | }; 50 | 51 | class Color 52 | { 53 | public: 54 | static void rgb2hsv (float r, float g, float b, float &h, float &s, float &v) 55 | { 56 | double var_R = r; 57 | double var_G = g; 58 | double var_B = b; 59 | 60 | double var_Min = std::min(std::min(var_R,var_G),var_B); 61 | double var_Max = std::max(std::max(var_R,var_G),var_B); 62 | double del_Max = var_Max - var_Min; 63 | v = var_Max; 64 | if (fabs(del_Max)<0.00001) { 65 | h = 0; 66 | s = 0; 67 | } 68 | else { 69 | s = del_Max/var_Max; 70 | 71 | if ( var_R == var_Max ) h = (var_G - var_B)/del_Max; 72 | else if ( var_G == var_Max ) h = 2.0 + (var_B - var_R)/del_Max; 73 | else if ( var_B == var_Max ) h = 4.0 + (var_R - var_G)/del_Max; 74 | h /= 6.0; 75 | 76 | if ( h < 0 ) h += 1; 77 | if ( h > 1 ) h -= 1; 78 | } 79 | } 80 | 81 | static void hsv2rgb (float h, float s, float v, float &r, float &g, float &b) 82 | { 83 | float h1 = h*6; // sector 0 to 5 84 | int i = floor( h1 ); 85 | float f = h1 - i; // fractional part of h 86 | 87 | float p = v * ( 1 - s ); 88 | float q = v * ( 1 - s * f ); 89 | float t = v * ( 1 - s * ( 1 - f ) ); 90 | 91 | if (i==0) {r = v; g = t; b = p;} 92 | else if (i==1) {r = q; g = v; b = p;} 93 | else if (i==2) {r = p; g = v; b = t;} 94 | else if (i==3) {r = p; g = q; b = v;} 95 | else if (i==4) {r = t; g = p; b = v;} 96 | else if (i==5) {r = v; g = p; b = q;} 97 | } 98 | 99 | static float linearInterpolate(float a, float b, float t) 100 | { 101 | return a * (1 - t) + b * t; 102 | } 103 | 104 | static color::rgba interpolateColor(color::rgba start, color::rgba goal, float t) 105 | { 106 | color::hsv ca; 107 | color::hsv cb; 108 | color::hsv cr; 109 | color::rgba a, b; 110 | a = start; 111 | b = goal; 112 | 113 | a.r *= a.a; 114 | a.g *= a.a; 115 | a.b *= a.a; 116 | b.r *= b.a; 117 | b.g *= b.a; 118 | b.b *= b.a; 119 | color::Color::rgb2hsv(a.r, a.g, a.b, ca.h, ca.s, ca.v); 120 | color::Color::rgb2hsv(b.r, b.g, b.b, cb.h, cb.s, cb.v); 121 | 122 | cr.h = linearInterpolate(ca.h, cb.h, t); 123 | cr.s = linearInterpolate(ca.s, cb.s, t); 124 | cr.v = linearInterpolate(ca.v, cb.v, t); 125 | 126 | color::rgba result; 127 | color::Color::hsv2rgb(cr.h, cr.s, cr.v, result.r, result.g, result.b); 128 | result.a = 1.0; 129 | 130 | return result; 131 | } 132 | }; 133 | } 134 | #endif 135 | -------------------------------------------------------------------------------- /cob_light/common/include/concurrentQueue.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | template 23 | class ConcurrentQueue 24 | { 25 | private: 26 | boost::circular_buffer _queue; 27 | mutable boost::mutex _mutex; 28 | boost::condition_variable _condition; 29 | public: 30 | ConcurrentQueue() 31 | : _queue(2) 32 | { 33 | } 34 | 35 | bool empty() const 36 | { 37 | boost::mutex::scoped_lock lock(_mutex); 38 | return _queue.empty(); 39 | } 40 | 41 | void push(T const& data) 42 | { 43 | boost::mutex::scoped_lock lock(_mutex); 44 | _queue.push_back(data); 45 | lock.unlock(); 46 | _condition.notify_one(); 47 | } 48 | 49 | bool pop(T& data) 50 | { 51 | boost::mutex::scoped_lock lock(_mutex); 52 | if(_queue.empty()) 53 | return false; 54 | 55 | data = _queue.front(); 56 | _queue.pop_front(); 57 | 58 | return true; 59 | } 60 | 61 | void wait_pop(T& data) 62 | { 63 | boost::mutex::scoped_lock lock(_mutex); 64 | while(_queue.empty()) 65 | _condition.wait(lock); 66 | 67 | data=_queue.front(); 68 | _queue.pop_front(); 69 | } 70 | }; 71 | -------------------------------------------------------------------------------- /cob_light/common/include/distApproxMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef DISTAPPROXMODE_H 19 | #define DISTAPPROXMODE_H 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | class DistApproxMode : public Mode 33 | { 34 | public: 35 | DistApproxMode(size_t num_leds, int priority = 0, double freq = 5, int pulses = 0, double timeout = 0) 36 | :Mode(priority, freq, pulses, timeout), _timer_inc(0), _num_leds(num_leds) 37 | { 38 | ros::NodeHandle nh; 39 | sub_scan = nh.subscribe("/scan_unified", 1, &DistApproxMode::scan_callback, this); 40 | //use static freq for this mode 41 | _inc = (1. / UPDATE_RATE_HZ) * UPDATE_FREQ; 42 | 43 | _colors.resize(_num_leds); 44 | 45 | c_red.a = 1; c_red.r = 1; c_red.g = 0; c_red.b = 0; 46 | c_green.a = 1; c_green.r = 0; c_green.g = 1; c_green.b = 0; 47 | c_off.a = 0; c_off.r = 0; c_off.g = 0; c_off.b = 0; 48 | c_default.a = 0.1; c_default.r = 0; c_default.g = 1.0; c_default.b = 0; 49 | } 50 | 51 | void scan_callback(const sensor_msgs::LaserScanConstPtr& msg) 52 | { 53 | boost::mutex::scoped_lock lock(mutex); 54 | scan = *msg; 55 | } 56 | 57 | void execute() 58 | { 59 | if(_timer_inc >= 1.0) 60 | { 61 | //rotate scan array 62 | mutex.lock(); 63 | std::vector ranges = scan.ranges; 64 | mutex.unlock(); 65 | float sector_size = (float)ranges.size() / (float)_num_leds; 66 | std::vector sectors; 67 | sectors.assign(_num_leds, 0.0); 68 | 69 | //calculate sector values 70 | for(int i = 0; i < _num_leds; i++) 71 | { 72 | float sum = std::numeric_limits::max(); 73 | //float sum = 0; 74 | for(int j = i*(int)sector_size; j < (i+1)*(int)sector_size; j++) 75 | { 76 | if(ranges.at(j) != 0){ 77 | sum = std::min(ranges.at(j), sum); 78 | } 79 | } 80 | sectors.at(i) = sum; 81 | } 82 | 83 | for(int i = 0; i < _num_leds; i++) 84 | { 85 | color::rgba col; 86 | 87 | float mean = 0; 88 | if(i == 0) 89 | { 90 | mean = sectors.back()+sectors.at(i)/2.0f; 91 | } 92 | else 93 | { 94 | mean = (sectors.at(i)+sectors.at(i-1))/2.0f; 95 | } 96 | if(mean > DIST_MAX) 97 | { 98 | col = c_default; 99 | } 100 | else 101 | { 102 | float t = (boost::algorithm::clamp(mean, DIST_MIN, DIST_MAX) - DIST_MIN)/(DIST_MAX - DIST_MIN); 103 | col = color::Color::interpolateColor(c_red, c_green, t); 104 | } 105 | 106 | _colors.at(i) = col; 107 | } 108 | _pulsed++; 109 | m_sigColorsReady(_colors); 110 | _timer_inc = 0.0; 111 | } 112 | else 113 | _timer_inc += _inc; 114 | } 115 | 116 | std::string getName(){ return std::string("DistApproxMode"); } 117 | 118 | static constexpr float DIST_MIN = 0.3f; 119 | static constexpr float DIST_MAX = 2.0f; 120 | static constexpr double UPDATE_FREQ = 50.0; 121 | 122 | private: 123 | double _timer_inc; 124 | double _inc; 125 | size_t _num_leds; 126 | 127 | sensor_msgs::LaserScan scan; 128 | 129 | ros::Subscriber sub_scan; 130 | boost::mutex mutex; 131 | color::rgba c_red; 132 | color::rgba c_green; 133 | color::rgba c_off; 134 | color::rgba c_default; 135 | }; 136 | 137 | constexpr float DistApproxMode::DIST_MIN; 138 | constexpr float DistApproxMode::DIST_MAX; 139 | constexpr double DistApproxMode::UPDATE_FREQ; 140 | 141 | #endif 142 | -------------------------------------------------------------------------------- /cob_light/common/include/fadeColorMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef FADECOLORMODE_H 19 | #define FADECOLORMODE_H 20 | 21 | #include 22 | 23 | class FadeColorMode : public Mode 24 | { 25 | public: 26 | FadeColorMode(color::rgba color, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout) 28 | { 29 | _color = color; 30 | 31 | doOnce = true; 32 | h = 0.0; 33 | h_s = 0.0; 34 | h_t = 0.0; 35 | 36 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 37 | } 38 | 39 | void execute() 40 | { 41 | float r = 0; 42 | float g = 0; 43 | float b = 0; 44 | float s, v; 45 | 46 | if(doOnce == true) 47 | { 48 | color::Color::rgb2hsv(_color.r, _color.g, _color.b, h, s, v); 49 | h_s = h; 50 | h_t = h; 51 | h_s += 1; 52 | doOnce = false; 53 | } 54 | 55 | color::Color::hsv2rgb(h, 1.0, 1.0, r, g, b); 56 | 57 | h += _inc; 58 | h_t += _inc; 59 | if(h > 1.0) 60 | h = 0.0; 61 | 62 | if(h_t >= h_s) 63 | { 64 | _pulsed++; h_s+=1; 65 | } 66 | 67 | color::rgba col; 68 | col.r = r; 69 | col.g = g; 70 | col.b = b; 71 | col.a = _color.a; 72 | 73 | m_sigColorReady(col); 74 | } 75 | 76 | std::string getName(){ return std::string("FadeColorMode"); } 77 | 78 | private: 79 | bool doOnce; 80 | float h; 81 | float h_s; 82 | float h_t; 83 | double _inc; 84 | }; 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /cob_light/common/include/flashMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef FLASHMODE_H 19 | #define FLASHMODE_H 20 | 21 | #include 22 | 23 | class FlashMode : public Mode 24 | { 25 | public: 26 | FlashMode(color::rgba color, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout), _toggle(false), _timer_inc(0) 28 | { 29 | _color = color; 30 | if(_pulses != 0) 31 | { 32 | _pulses *=2; 33 | _pulses +=1; 34 | } 35 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 36 | } 37 | 38 | void execute() 39 | { 40 | color::rgba col; 41 | col.r = _color.r; 42 | col.g = _color.g; 43 | col.b = _color.b; 44 | if(_timer_inc >= 1.0) 45 | { 46 | col.a = _color.a * (int)_toggle; 47 | _pulsed++; 48 | _toggle = !_toggle; 49 | m_sigColorReady(col); 50 | _timer_inc = 0.0; 51 | } 52 | else 53 | _timer_inc += _inc; 54 | } 55 | 56 | std::string getName(){ return std::string("FlashMode"); } 57 | 58 | private: 59 | bool _toggle; 60 | double _timer_inc; 61 | double _inc; 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /cob_light/common/include/glowColorMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef GLOWCOLORMODE_H 19 | #define GLOWCOLORMODE_H 20 | 21 | #include 22 | 23 | class GlowColorMode : public Mode 24 | { 25 | public: 26 | GlowColorMode(color::rgba color, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout), _timer_inc(0.0) 28 | { 29 | _color = color; 30 | h = 0.0; 31 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 32 | h_inc=0; 33 | color::Color::rgb2hsv(color.r, color.g, color.b, h, s, v); 34 | } 35 | 36 | void execute() 37 | { 38 | static int sign = 1; 39 | float r = 0; 40 | float g = 0; 41 | float b = 0; 42 | 43 | if(_timer_inc >= 1.0) 44 | { 45 | double tmp = h; 46 | 47 | h_inc += 0.001 * sign; 48 | if(h_inc >= 0.01 || h_inc <= -0.01) 49 | { 50 | sign *= -1; 51 | _pulsed++; 52 | } 53 | h += h_inc; 54 | if( h < 0) 55 | h = 1 + h; 56 | 57 | //double fV = (exp(sin(_timer_inc))-1.0/M_E)*(1.000/(M_E-1.0/M_E)); 58 | double fV = (exp(sin( (M_PI/2)+h_inc*60 ))-0.36787944)*0.42545906411; 59 | 60 | color::Color::hsv2rgb(h, s, v, r, g, b); 61 | 62 | color::rgba col; 63 | col.r = r; 64 | col.g = g; 65 | col.b = b; 66 | col.a = fV; 67 | 68 | _timer_inc = 0.0; 69 | m_sigColorReady(col); 70 | h = tmp; 71 | } 72 | else 73 | _timer_inc += _inc; 74 | } 75 | 76 | std::string getName(){ return std::string("GlowColorMode"); } 77 | 78 | private: 79 | double _timer_inc; 80 | double _inc; 81 | float h, s, v; 82 | double h_inc; 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /cob_light/common/include/kitMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Mojin Robotics GmbH https://www.mojin-robotics.de 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef KITMODE_H 19 | #define KITMODE_H 20 | 21 | #include 22 | 23 | class KitMode : public Mode 24 | { 25 | public: 26 | KitMode(color::rgba color, size_t num_leds, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout), _num_leds(num_leds) 28 | { 29 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 30 | 31 | _color = color; 32 | _colors.assign(num_leds, color::rgba()); 33 | _colors[0] = color; 34 | _pos = 0; 35 | _dir = 1; 36 | _fade_fac = 0.8; 37 | } 38 | 39 | void execute() 40 | { 41 | if(_timer_inc >= 1.0) 42 | { 43 | for(int i = 0; i < _num_leds; i++) 44 | { 45 | 46 | _colors[i].a = _colors[i].a * _fade_fac; 47 | } 48 | for(int j = 0; j < 5; j++) 49 | { 50 | _pos += _dir; 51 | if(_pos == -1) 52 | { 53 | _pos = 0; 54 | _dir = 1; 55 | } 56 | else if(_pos == _num_leds) 57 | { 58 | _pos = _num_leds-1; 59 | _dir = -1; 60 | } 61 | 62 | _colors[_pos] = _color; 63 | 64 | } 65 | m_sigColorsReady(_colors); 66 | 67 | _timer_inc = 0.0; 68 | } 69 | else 70 | _timer_inc += _inc; 71 | 72 | } 73 | 74 | std::string getName(){ return std::string("KitMode"); } 75 | 76 | private: 77 | double _timer_inc; 78 | double _inc; 79 | double _pos; 80 | double _dir; 81 | int _num_leds; 82 | double _fade_fac; 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /cob_light/common/include/mode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef MODE_H 19 | #define MODE_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | class Mode 27 | { 28 | public: 29 | Mode(int priority = 0, double freq = 0, int pulses = 0, double timeout = 0) 30 | : _priority(priority), _freq(freq), _pulses(pulses), _timeout(timeout), 31 | _finished(false), _pulsed(0), _isStopRequested(false), _isPauseRequested(false), 32 | _isRunning(false) 33 | { 34 | if(this->getFrequency() == 0.0) 35 | this->setFrequency(1.0); 36 | } 37 | virtual ~Mode(){} 38 | 39 | void start() 40 | { 41 | if(_thread == NULL) 42 | _thread.reset(new boost::thread(&Mode::run, this)); 43 | if(isPauseRequested()) 44 | { 45 | boost::mutex::scoped_lock lock(_mutex_pause); 46 | _isPauseRequested = false; 47 | _cond_pause.notify_one(); 48 | } 49 | _isRunning = true; 50 | } 51 | 52 | void stop() 53 | { 54 | _mutex.lock(); 55 | _isStopRequested = true; 56 | _mutex.unlock(); 57 | if(isPauseRequested()) 58 | { 59 | _isPauseRequested = false; 60 | boost::mutex::scoped_lock lock(_mutex_pause); 61 | _cond_pause.notify_one(); 62 | } 63 | if(_thread != NULL) 64 | { 65 | _thread->join(); 66 | _thread.reset(); 67 | } 68 | _isStopRequested = false; 69 | _isRunning = false; 70 | } 71 | 72 | void pause() 73 | { 74 | _mutex.lock(); 75 | _isPauseRequested = true; 76 | _mutex.unlock(); 77 | _isRunning = false; 78 | } 79 | 80 | virtual void execute() = 0; 81 | 82 | virtual std::string getName() = 0; 83 | 84 | bool finished(){ return _finished; } 85 | 86 | void setPriority(int priority){ _priority = priority; } 87 | int getPriority(){ return _priority; } 88 | 89 | void setTimeout(double timeout){ _timeout = timeout; } 90 | double getTimeout(){ return _timeout; } 91 | 92 | void setFrequency(double freq){ _freq = freq; } 93 | double getFrequency(){ return _freq; } 94 | 95 | void setPulses(int pulses){ _pulses = pulses; } 96 | int getPulses(){ return _pulses; } 97 | 98 | int pulsed(){ return _pulsed; } 99 | 100 | void setColor(color::rgba color){ _color = color; } 101 | color::rgba getColor(){ return _color; } 102 | 103 | void setActualColor(color::rgba color){ _actualColor = color; } 104 | color::rgba getActualColor(){ return _color; } 105 | 106 | bool isRunning(){ return _isRunning; } 107 | 108 | boost::signals2::signal* signalColorReady(){ return &m_sigColorReady; } 109 | boost::signals2::signal &colors)>* signalColorsReady(){ return &m_sigColorsReady; } 110 | boost::signals2::signal* signalModeFinished(){ return &m_sigFinished; } 111 | 112 | protected: 113 | int _priority; 114 | double _freq; 115 | int _pulses; 116 | double _timeout; 117 | 118 | bool _finished; 119 | int _pulsed; 120 | 121 | color::rgba _color; 122 | std::vector _colors; 123 | color::rgba _actualColor; 124 | color::rgba _init_color; 125 | 126 | static const unsigned int UPDATE_RATE_HZ = 100; 127 | 128 | boost::signals2::signal m_sigColorReady; 129 | boost::signals2::signal &colors)> m_sigColorsReady; 130 | boost::signals2::signal m_sigFinished; 131 | 132 | private: 133 | boost::shared_ptr _thread; 134 | boost::mutex _mutex; 135 | boost::mutex _mutex_pause; 136 | bool _isStopRequested; 137 | bool _isPauseRequested; 138 | bool _isRunning; 139 | 140 | boost::condition_variable _cond_pause; 141 | 142 | bool isStopRequested() 143 | { 144 | bool ret; 145 | _mutex.lock(); 146 | ret = _isStopRequested; 147 | _mutex.unlock(); 148 | return ret; 149 | } 150 | 151 | bool isPauseRequested() 152 | { 153 | bool ret; 154 | _mutex.lock(); 155 | ret = _isPauseRequested; 156 | _mutex.unlock(); 157 | return ret; 158 | } 159 | 160 | protected: 161 | virtual void run() 162 | { 163 | ros::Rate r(UPDATE_RATE_HZ); 164 | 165 | ros::Time timeStart = ros::Time::now(); 166 | 167 | while(!isStopRequested() && !ros::isShuttingDown()) 168 | { 169 | while(isPauseRequested()) 170 | { 171 | boost::mutex::scoped_lock lock(_mutex_pause); 172 | _cond_pause.wait(lock); 173 | } 174 | this->execute(); 175 | 176 | if((this->getPulses() != 0) && 177 | (this->getPulses() <= this->pulsed())) 178 | break; 179 | 180 | if(this->getTimeout() != 0) 181 | { 182 | ros::Duration timePassed = ros::Time::now() - timeStart; 183 | if(timePassed.toSec() >= this->getTimeout()) 184 | break; 185 | } 186 | r.sleep(); 187 | } 188 | ROS_DEBUG("Mode %s finished",this->getName().c_str()); 189 | if(!isStopRequested()) 190 | m_sigFinished(this->getPriority()); 191 | } 192 | }; 193 | 194 | #endif 195 | -------------------------------------------------------------------------------- /cob_light/common/include/modeExecutor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef MODEEXECUTOR_H 19 | #define MODEEXECUTOR_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | class ModeExecutor 29 | { 30 | public: 31 | ModeExecutor(IColorO* colorO); 32 | ~ModeExecutor(); 33 | 34 | uint64_t execute(boost::shared_ptr mode); 35 | uint64_t execute(cob_light::LightMode requestMode); 36 | 37 | int getExecutingPriority(); 38 | int getExecutingMode(); 39 | uint64_t getExecutingUId(); 40 | 41 | void pause(); 42 | void resume(); 43 | void stop(); 44 | bool stop(uint64_t uId); 45 | 46 | void setDefaultPriority(int priority); 47 | 48 | private: 49 | IColorO* _colorO; 50 | 51 | boost::shared_ptr _activeMode; 52 | std::map, std::greater > _mapActiveModes; 53 | color::rgba _activeColor; 54 | 55 | bool _stopRequested; 56 | int default_priority; 57 | 58 | void onModeFinishedReceived(int prio); 59 | void onColorSetReceived(color::rgba color); 60 | }; 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /cob_light/common/include/modeFactory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef MODEFACTORY_H 19 | #define MODEFACTORY_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | class ModeFactory 28 | { 29 | public: 30 | ModeFactory(); 31 | ~ModeFactory(); 32 | 33 | static boost::shared_ptr create(cob_light::LightMode requestMode, IColorO* colorO); 34 | static boost::shared_ptr create(std::string mode, color::rgba color); 35 | 36 | static int type(Mode *mode); 37 | 38 | private: 39 | IColorO* _colorO; 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /cob_light/common/include/modeRunner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef MODERUNNER_H 19 | #define MODERUNNER_H 20 | 21 | class ModeRunner 22 | { 23 | public: 24 | ModeRunner(); 25 | virtual ~ModeRunner(); 26 | 27 | void start(Mode* mode); 28 | 29 | void stop(); 30 | 31 | void join(); 32 | 33 | private: 34 | boost::shared_ptr m_thread_ptr; 35 | 36 | boost::signals2::signal m_sigFinished; 37 | 38 | void run(); 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /cob_light/common/include/sequenceMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef SEQUENCEMODE_H 19 | #define SEQUENCEMODE_H 20 | 21 | #include 22 | 23 | typedef struct sequence 24 | { 25 | color::rgba color; 26 | double holdtime; 27 | double crosstime; 28 | }seq_t; 29 | 30 | enum State{INIT_SEQ,CROSSFADE,HOLD,NEXT,BEGIN}; 31 | 32 | class SequenceMode : public Mode 33 | { 34 | public: 35 | SequenceMode(std::vector sequences, int priority = 0, double freq = 0.25, int pulses = 0, double timeout = 0) 36 | :Mode(priority, freq, pulses, timeout), _init(true), _int_inc(0.0), _int_count(0.0) 37 | { 38 | _seqences = sequences; 39 | _state = INIT_SEQ; 40 | } 41 | 42 | void execute() 43 | { 44 | switch (_state) 45 | { 46 | case INIT_SEQ: 47 | //std::cout<<"INIT_SEQ"< _seqences; 103 | int _seqidx; 104 | bool _init; 105 | float _int_inc; 106 | float _int_count; 107 | int _state; 108 | 109 | color::rgba _color; 110 | 111 | color::rgba interpolateColor(color::rgba start, color::rgba goal, float t) 112 | { 113 | color::hsv ca; 114 | color::hsv cb; 115 | color::hsv cr; 116 | color::rgba a, b; 117 | a = start; 118 | b = goal; 119 | 120 | a.r *= a.a; 121 | a.g *= a.a; 122 | a.b *= a.a; 123 | b.r *= b.a; 124 | b.g *= b.a; 125 | b.b *= b.a; 126 | color::Color::rgb2hsv(a.r, a.g, a.b, ca.h, ca.s, ca.v); 127 | color::Color::rgb2hsv(b.r, b.g, b.b, cb.h, cb.s, cb.v); 128 | 129 | cr.h = linearInterpolate(ca.h, cb.h, t); 130 | cr.s = linearInterpolate(ca.s, cb.s, t); 131 | cr.v = linearInterpolate(ca.v, cb.v, t); 132 | 133 | color::rgba result; 134 | color::Color::hsv2rgb(cr.h, cr.s, cr.v, result.r, result.g, result.b); 135 | result.a = 1.0; 136 | 137 | // std::cout<<"Original h:"< 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | typedef struct ioData{ 35 | const char* buf; 36 | size_t len; 37 | } ioData_t; 38 | 39 | class SerialIO 40 | { 41 | public: 42 | // Constructor 43 | SerialIO(); 44 | // Destructor 45 | ~SerialIO(); 46 | 47 | // Open Serial Port 48 | int openPort(std::string devicestring, int baudrate); 49 | 50 | // Send Data to Serial Port 51 | int sendData(std::string value); 52 | 53 | // Send Data to Serial Port 54 | int sendData(const char* data, size_t len); 55 | 56 | // Read Data from Serial Port 57 | int readData(std::string &value, size_t nBytes); 58 | 59 | void enqueueData(std::vector data); 60 | 61 | void enqueueData(const char* data, size_t len); 62 | 63 | // Check if Serial Port is opened 64 | bool isOpen(); 65 | 66 | // Close Serial Port 67 | void closePort(); 68 | 69 | bool recover(); 70 | 71 | void start(); 72 | void stop(); 73 | 74 | private: 75 | //ioQueue 76 | ConcurrentQueue > _oQueue; 77 | 78 | boost::shared_ptr _thread; 79 | boost::mutex _mutex; 80 | boost::condition_variable _condition; 81 | 82 | // filedescriptor 83 | int _fd; 84 | // serial port settings 85 | struct termios port_settings; 86 | // device string 87 | std::string _device_string; 88 | // baudrate 89 | int _baudrate; 90 | 91 | // resolve int to baudrate 92 | speed_t getBaudFromInt(int baud); 93 | 94 | static const int maxUpdateRate = 50; 95 | 96 | void run(); 97 | }; 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /cob_light/common/include/staticMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef STATICMODE_H 19 | #define STATICMODE_H 20 | 21 | #include 22 | 23 | class StaticMode : public Mode 24 | { 25 | public: 26 | StaticMode(color::rgba color, int priority = 0, double freq = 0, int pulses = 0, double timeout = 0) 27 | :Mode(priority, freq, pulses, timeout), _timer_inc(0) 28 | { 29 | _color = color; 30 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 31 | } 32 | 33 | void execute() 34 | { 35 | if(_timer_inc == 0.0) 36 | m_sigColorReady(_color); 37 | if(_timer_inc >= 1.0) 38 | _timer_inc = 0.0; 39 | else 40 | _timer_inc += _inc; 41 | } 42 | 43 | std::string getName(){ return std::string("StaticMode"); } 44 | 45 | private: 46 | double _timer_inc; 47 | double _inc; 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /cob_light/common/include/sweepColorMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef SWEEPCOLORMODE_H 19 | #define SWEEPCOLORMODE_H 20 | 21 | #include 22 | #include 23 | 24 | class SweepColorMode : public Mode 25 | { 26 | public: 27 | SweepColorMode(std::vector colors, size_t num_leds, int priority = 0, double freq = 5, int pulses = 0, double timeout = 0) 28 | :Mode(priority, freq, pulses, timeout), _toggle(false), _timer_inc(0), _num_leds(num_leds) 29 | { 30 | _colors = colors; 31 | 32 | _startcolor.r = 0.0; 33 | _startcolor.g = 0.4; 34 | _startcolor.b = 0.28; 35 | _startcolor.a = 1.0; 36 | if(_colors.size() == 1) 37 | _startcolor = _colors[0]; 38 | _colors.resize(_num_leds); 39 | if(_num_leds%2 == 0) 40 | { 41 | _colors[_num_leds/2] = _startcolor; 42 | _colors[_num_leds/2-1] = _startcolor; 43 | } 44 | else 45 | _colors[_num_leds/2] = _startcolor; 46 | _pos = 0; 47 | 48 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 49 | } 50 | 51 | void execute() 52 | { 53 | if(_timer_inc >= 1.0) 54 | { 55 | for(int i = _num_leds/2; i < _num_leds; i++) 56 | { 57 | _colors[i].a *= 0.7; 58 | } 59 | for(int i = _num_leds/2-1; i >= 0; i--) 60 | { 61 | _colors[i].a *= 0.7; 62 | } 63 | _colors[(_num_leds/2)+_pos] = _startcolor; 64 | _colors[(_num_leds/2)-1-_pos] = _startcolor; 65 | 66 | _pos++; 67 | if(_pos >= (_num_leds/2)) 68 | _pos = 0; 69 | 70 | _pulsed++; 71 | m_sigColorsReady(_colors); 72 | _timer_inc = 0.0; 73 | } 74 | else 75 | _timer_inc += _inc; 76 | } 77 | 78 | std::string getName(){ return std::string("SweepColorMode"); } 79 | 80 | private: 81 | bool _toggle; 82 | double _timer_inc; 83 | double _inc; 84 | size_t _num_leds; 85 | int _pos; 86 | color::rgba _startcolor; 87 | }; 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /cob_light/common/include/turnIndicatorMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Mojin Robotics GmbH https://www.mojin-robotics.de 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 | #ifndef TURNINDICATORMODE_H 18 | #define TURNINDICATORMODE_H 19 | 20 | #include 21 | 22 | class TurnIndicatorMode : public Mode { 23 | public: 24 | TurnIndicatorMode(color::rgba color, size_t num_leds, int dir, 25 | int priority = 0, double freq = 0.25, int pulses = 0, 26 | double timeout = 0) 27 | : Mode(priority, freq, pulses, timeout), _num_leds(num_leds), 28 | _timer_inc(0), _toggle(false) { 29 | _color = color; 30 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 31 | 32 | _colors.assign(_num_leds, color::rgba()); 33 | _dir = dir; 34 | // If the direction is -1 (left), then the LED indices are set to to the 35 | // first and last quarters of the LEDs. 36 | if (dir == -1) { 37 | size_t from = 0; 38 | size_t until = _num_leds / 4; 39 | for (size_t i = from; i < until; ++i) { 40 | _led_indices.push_back(i); 41 | } 42 | from = 3 * _num_leds / 4; 43 | until = _num_leds; 44 | for (size_t i = from; i < until; ++i) { 45 | _led_indices.push_back(i); 46 | } 47 | } else { 48 | // If the direction is not -1 (right), then the LED indices are set to the 49 | // second and third quarters of the LEDs. 50 | size_t from = _num_leds / 4; 51 | size_t until = 3 * _num_leds / 4; 52 | for (size_t i = from; i < until; ++i) { 53 | _led_indices.push_back(i); 54 | } 55 | } 56 | } 57 | 58 | void execute() { 59 | if (_timer_inc >= 1.0) { 60 | color::rgba col; 61 | col.r = _color.r; 62 | col.g = _color.g; 63 | col.b = _color.b; 64 | col.a = _color.a * (int)_toggle; 65 | _toggle = !_toggle; 66 | for (size_t i = 0; i < _led_indices.size(); ++i) { 67 | _colors[_led_indices[i]] = col; 68 | } 69 | m_sigColorsReady(_colors); 70 | _timer_inc = 0.0; 71 | } else { 72 | _timer_inc += _inc; 73 | } 74 | } 75 | 76 | std::string getName() { return std::string("TurnIndicatorMode"); } 77 | 78 | private: 79 | double _timer_inc; 80 | double _inc; 81 | double _dir; 82 | int _num_leds; 83 | bool _toggle; 84 | std::vector _led_indices; 85 | }; 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /cob_light/common/include/xmasMode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef XMASMODE_H 19 | #define XMASMODE_H 20 | 21 | #include 22 | #include 23 | 24 | class XMasMode : public Mode 25 | { 26 | public: 27 | XMasMode(size_t num_leds, int priority = 0, double freq = 5, int pulses = 0, double timeout = 0) 28 | :Mode(priority, freq, pulses, timeout), _toggle(false), _timer_inc(0), _num_leds(num_leds), _chucksize(5) 29 | { 30 | _colors.resize(num_leds); 31 | if(_num_leds >= _chucksize*2) 32 | { 33 | int n_chunks = _num_leds / _chucksize + (_num_leds%_chucksize ? 1 : 0); 34 | int k = 0; 35 | for(int i = 0; i < n_chunks; i++) 36 | { 37 | for(int j = 0; j < _chucksize; j++) 38 | { 39 | if(k >= _num_leds) 40 | break; 41 | if(i%2 == 0) 42 | { 43 | _colors.at(k).r = 1.0; _colors.at(k).g = 0.0; _colors.at(k).b = 0.0; _colors.at(k).a = 1.0; 44 | } 45 | else 46 | { 47 | _colors.at(k).r = 1.0; _colors.at(k).g = 1.0; _colors.at(k).b = 1.0; _colors.at(k).a = 1.0; 48 | } 49 | k++; 50 | } 51 | } 52 | } 53 | else 54 | { 55 | for(int i = 0; i < _num_leds; i++) 56 | { 57 | if(i%2==0) 58 | { 59 | _colors.at(i).r = 1.0; _colors.at(i).g = 0.0; _colors.at(i).b = 0.0; _colors.at(i).a = 1.0; 60 | } 61 | else 62 | { 63 | _colors.at(i).r = 1.0; _colors.at(i).g = 1.0; _colors.at(i).b = 1.0; _colors.at(i).a = 1.0; 64 | } 65 | } 66 | } 67 | if(_pulses != 0) 68 | { 69 | _pulses *=2; 70 | _pulses +=1; 71 | } 72 | _inc = (1. / UPDATE_RATE_HZ) * _freq; 73 | } 74 | 75 | void execute() 76 | { 77 | if(_timer_inc >= 1.0) 78 | { 79 | if(_num_leds >= _chucksize*2) 80 | std::rotate(_colors.begin(), _colors.begin()+_chucksize, _colors.end()); 81 | else 82 | std::rotate(_colors.begin(), _colors.begin()+1, _colors.end()); 83 | _pulsed++; 84 | m_sigColorsReady(_colors); 85 | _timer_inc = 0.0; 86 | } 87 | else 88 | _timer_inc += _inc; 89 | } 90 | 91 | std::string getName(){ return std::string("XMasMode"); } 92 | 93 | private: 94 | bool _toggle; 95 | double _timer_inc; 96 | double _inc; 97 | size_t _num_leds; 98 | size_t _chucksize; 99 | }; 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /cob_light/common/src/serialIO.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 "serialIO.h" 19 | #include "sys/select.h" 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | SerialIO::SerialIO() : 26 | _fd(-1), _device_string(""), _baudrate(9600) 27 | { 28 | } 29 | 30 | SerialIO::~SerialIO() 31 | { 32 | stop(); 33 | closePort(); 34 | } 35 | 36 | // Open Serial Port 37 | int SerialIO::openPort(std::string devicestring, int baudrate) 38 | { 39 | if(_fd != -1) return _fd; 40 | 41 | _device_string = devicestring; 42 | _baudrate = baudrate; 43 | 44 | speed_t baud = getBaudFromInt(baudrate); 45 | std::memset(&port_settings,0,sizeof(port_settings)); 46 | port_settings.c_iflag = 0; 47 | port_settings.c_oflag = 0; 48 | port_settings.c_cflag = CS8|CREAD|CLOCAL; 49 | port_settings.c_lflag = 0; 50 | port_settings.c_cc[VMIN]=1; 51 | port_settings.c_cc[VTIME]=5; 52 | 53 | _fd=open(devicestring.c_str(), O_RDWR | O_NONBLOCK); 54 | cfsetospeed(&port_settings, baud); 55 | cfsetispeed(&port_settings, baud); 56 | 57 | tcsetattr(_fd, TCSANOW, &port_settings); 58 | 59 | return _fd; 60 | } 61 | 62 | // Send Data to Serial Port 63 | int SerialIO::sendData(std::string value) 64 | { 65 | boost::mutex::scoped_lock lock(_mutex); 66 | int wrote = -1; 67 | if(_fd != -1) 68 | wrote = write(_fd, value.c_str(), value.length()); 69 | return wrote; 70 | } 71 | 72 | // Send Data to Serial Port 73 | int SerialIO::sendData(const char* data, size_t len) 74 | { 75 | boost::mutex::scoped_lock lock(_mutex); 76 | int wrote = -1; 77 | if(_fd != -1) 78 | wrote = write(_fd, data, len); 79 | return wrote; 80 | } 81 | 82 | // Read Data from Serial Port 83 | int SerialIO::readData(std::string &value, size_t nBytes) 84 | { 85 | boost::mutex::scoped_lock lock(_mutex); 86 | fd_set fds; 87 | FD_ZERO(&fds); 88 | FD_SET(_fd, &fds); 89 | struct timeval timeout = {0, 100000}; 90 | char buffer[32]; 91 | size_t rec = -1; 92 | if(select(_fd+1, &fds, NULL, NULL, &timeout)) 93 | { 94 | rec = read(_fd, buffer, nBytes); 95 | value = std::string(buffer, rec); 96 | } 97 | 98 | return rec; 99 | } 100 | 101 | void SerialIO::start() 102 | { 103 | if(_thread == NULL) 104 | _thread.reset(new boost::thread(&SerialIO::run, this)); 105 | } 106 | 107 | void SerialIO::stop() 108 | { 109 | if(_thread != NULL) 110 | { 111 | _thread->interrupt(); 112 | _thread->join(); 113 | _thread.reset(); 114 | } 115 | } 116 | 117 | void SerialIO::run() 118 | { 119 | ros::Rate r(maxUpdateRate); 120 | std::vector data; 121 | while(true) 122 | { 123 | _oQueue.wait_pop(data); 124 | for(size_t i = 0; i < data.size(); i++) 125 | this->sendData(data[i].buf, data[i].len); 126 | r.sleep(); 127 | } 128 | } 129 | 130 | void SerialIO::enqueueData(std::vector data) 131 | { 132 | _oQueue.push(data); 133 | } 134 | 135 | void SerialIO::enqueueData(const char* buf, size_t len) 136 | { 137 | struct ioData data; 138 | data.buf=buf; 139 | data.len=len; 140 | std::vector vec; 141 | vec.push_back(data); 142 | _oQueue.push(vec); 143 | } 144 | 145 | // Check if Serial Port is opened 146 | bool SerialIO::isOpen() 147 | { 148 | return (_fd != -1); 149 | } 150 | 151 | // Close Serial Port 152 | void SerialIO::closePort() 153 | { 154 | if(_fd != -1) 155 | { 156 | close(_fd); 157 | _fd = -1; 158 | } 159 | } 160 | 161 | bool SerialIO::recover() 162 | { 163 | closePort(); 164 | if(openPort(_device_string, _baudrate)) 165 | { 166 | usleep(50000); 167 | tcflush(_fd, TCIOFLUSH); 168 | return true; 169 | } 170 | else 171 | return false; 172 | } 173 | 174 | 175 | speed_t SerialIO::getBaudFromInt(int baud) 176 | { 177 | speed_t ret; 178 | switch(baud) 179 | { 180 | case 0: ret=B0; break; 181 | case 50: ret=B50; break; 182 | case 75: ret=B75; break; 183 | case 110: ret=B110; break; 184 | case 134: ret=B134; break; 185 | case 150: ret=B150; break; 186 | case 200: ret=B200; break; 187 | case 300: ret=B300; break; 188 | case 1200: ret=B1200; break; 189 | case 1800: ret=B1800; break; 190 | case 2400: ret=B2400; break; 191 | case 4800: ret=B4800; break; 192 | case 9600: ret=B9600; break; 193 | case 19200: ret=B19200; break; 194 | case 38400: ret=B38400; break; 195 | case 57600: ret=B57600; break; 196 | case 115200: ret=B115200; break; 197 | case 230400: ret=B230400; break; 198 | default: 199 | ret=B230400; 200 | break; 201 | } 202 | return ret; 203 | } 204 | -------------------------------------------------------------------------------- /cob_light/msg/ColorRGBAArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/ColorRGBA[] colors 2 | -------------------------------------------------------------------------------- /cob_light/msg/LightMode.msg: -------------------------------------------------------------------------------- 1 | uint8 mode # mode to switch on 2 | 3 | float32 frequency # in Hz 4 | float32 timeout # in s, requested mode will be executed for max timeout s. 5 | # default is 0 and means no timeout. 6 | int32 pulses # specifies the amount of pulses which will be executed. 7 | # eg: mode = flash, pulses = 2. Means the light will flash two times 8 | int8 priority # priority [-20,20] default = 0. Modes with same or higher priorities will 9 | # be executed. 10 | std_msgs/ColorRGBA[] colors # array of colors. 11 | # size > 1: each color represent one led (size should match the amount of leds) 12 | # size = 1: one color for all leds or color for specific mode 13 | # size = 0: undefined 14 | cob_light/Sequence[] sequences # an array of sequence definitions, used only if mode is set to SEQ 15 | -------------------------------------------------------------------------------- /cob_light/msg/LightModes.msg: -------------------------------------------------------------------------------- 1 | uint8 NONE = 0 # will turn everything off 2 | uint8 STATIC = 1 # will change the LEDs to "color" 3 | uint8 FLASH = 2 # will change the LEDs frequently with "frequency" from "color" to black 4 | uint8 BREATH = 3 # will change the LEDs smoothly with "frequency" from "color" to black 5 | uint8 BREATH_COLOR = 4 # will change the LEDs smoothly with "frequency" from "color" to black 6 | # and flips color in time 7 | uint8 FADE_COLOR = 5 # will fade the colors in rainbow 8 | uint8 SEQ = 6 # executes one sequence after another as defined in sequences array 9 | uint8 CIRCLE_COLORS = 7 # circle through specific colors, if one color is set only one color will circle 10 | # if more than one color is set, that this colors will circle through 11 | uint8 SWEEP = 8 # circle color from front to back on both sides 12 | uint8 DIST_APPROX = 9 # color led circle from green to red depended on the measured laserscan dists 13 | uint8 GLOW = 10 # glow the color 14 | uint8 XMAS = 11 # red white christmas mode 15 | uint8 KIT = 12 # Knight Rider mode 16 | uint8 TURN_LEFT = 13 # Turn Indicator left 17 | uint8 TURN_RIGHT = 14 # Turn Indicator right 18 | -------------------------------------------------------------------------------- /cob_light/msg/Sequence.msg: -------------------------------------------------------------------------------- 1 | std_msgs/ColorRGBA color #sequence color 2 | float32 hold_time #time how long the color should be hold [s] 3 | float32 cross_time #time how long it take to fade to this color [s] -------------------------------------------------------------------------------- /cob_light/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_light 3 | 0.8.18 4 | This package contains scripts to operate the LED lights on Care-O-bot. 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_light 9 | 10 | 11 | Felix Messmer 12 | Benjamin Maidel 13 | Benjamin Maidel 14 | 15 | catkin 16 | 17 | message_generation 18 | message_runtime 19 | 20 | actionlib 21 | actionlib_msgs 22 | boost 23 | diagnostic_msgs 24 | roscpp 25 | sensor_msgs 26 | std_msgs 27 | visualization_msgs 28 | 29 | rospy 30 | 31 | 32 | -------------------------------------------------------------------------------- /cob_light/ros/include/colorO.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COLORO_H 19 | #define COLORO_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | class ColorO : public IColorO 28 | { 29 | public: 30 | ColorO(SerialIO* serialIO); 31 | virtual ~ColorO(); 32 | 33 | bool init(); 34 | void setColor(color::rgba color); 35 | void setColorMulti(std::vector &colors); 36 | 37 | private: 38 | SerialIO* _serialIO; 39 | std::stringstream _ssOut; 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /cob_light/ros/include/colorOMarker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Mojin Robotics GmbH https://www.mojin-robotics.de 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COLOROMARKER_H 19 | #define COLOROMARKER_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | class ColorOMarker : public IColorO 29 | { 30 | public: 31 | ColorOMarker(ros::NodeHandle* nh); 32 | virtual ~ColorOMarker(); 33 | 34 | bool init(); 35 | void setColor(color::rgba color); 36 | void setColorMulti(std::vector &colors); 37 | 38 | private: 39 | ros::NodeHandle* p_nh; 40 | ros::Publisher _pubMarkerArray; 41 | }; 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /cob_light/ros/include/colorOSim.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COLOROSIM_H 19 | #define COLOROSIM_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | class ColorOSim : public IColorO 28 | { 29 | public: 30 | ColorOSim(ros::NodeHandle* nh); 31 | virtual ~ColorOSim(); 32 | 33 | bool init(); 34 | void setColor(color::rgba color); 35 | void setColorMulti(std::vector &colors); 36 | 37 | private: 38 | ros::NodeHandle* p_nh; 39 | ros::Publisher _pubSimulation; 40 | ros::Publisher _pubSimulationMulti; 41 | }; 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /cob_light/ros/include/iColorO.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef ICOLORO_H 19 | #define ICOLORO_H 20 | 21 | #include 22 | #include 23 | 24 | class IColorO 25 | { 26 | public: 27 | IColorO() : _initialized(false), _invertMask(0), _num_leds(1){;} 28 | virtual ~IColorO(){;} 29 | 30 | virtual bool init() = 0; 31 | virtual void setColor(color::rgba color) = 0; 32 | virtual void setColorMulti(std::vector &colors) = 0; 33 | 34 | void setMask(int mask){ _invertMask = mask; } 35 | void setNumLeds(size_t num_leds){ _num_leds = num_leds; } 36 | int getNumLeds(){ return _num_leds; } 37 | 38 | boost::signals2::signal* signalColorSet(){ return &m_sigColorSet; } 39 | boost::signals2::signal colors) >* signalColorsSet(){ return &m_sigColorsSet; } 40 | 41 | protected: 42 | bool _initialized; 43 | int _invertMask; 44 | int _num_leds; 45 | boost::signals2::signal m_sigColorSet; 46 | boost::signals2::signal colors) > m_sigColorsSet; 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /cob_light/ros/include/ms35.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef ES35_H 19 | #define ES35_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | class MS35 : public IColorO 28 | { 29 | public: 30 | MS35(SerialIO* serialIO); 31 | virtual ~MS35(); 32 | 33 | bool init(); 34 | void setColor(color::rgba color); 35 | void setColorMulti(std::vector &colors); 36 | 37 | private: 38 | SerialIO* _serialIO; 39 | std::stringstream _ssOut; 40 | static const int PACKAGE_SIZE = 9; 41 | char buffer[PACKAGE_SIZE]; 42 | 43 | int sendData(const char* data, size_t len); 44 | unsigned short int getChecksum(const char* data, size_t len); 45 | }; 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /cob_light/ros/include/stageprofi.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef COB_DRIVER_STAGE_PROFI_H 19 | #define COB_DRIVER_STAGE_PROFI_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | class StageProfi : public IColorO 28 | { 29 | public: 30 | StageProfi(SerialIO* serialIO, unsigned int leds, int led_offset); 31 | virtual ~StageProfi(); 32 | 33 | bool init(); 34 | 35 | void setColor(color::rgba color); 36 | void setColorMulti(std::vector &colors); 37 | 38 | private: 39 | SerialIO* _serialIO; 40 | std::stringstream _ssOut; 41 | int _led_offset; 42 | static const unsigned int HEADER_SIZE = 4; 43 | static const unsigned int MAX_CHANNELS = 255; 44 | 45 | bool recover(); 46 | bool sendDMX(uint16_t start, const char* buf, unsigned int length); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /cob_light/ros/scripts/test.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 | 20 | import rospy 21 | from std_msgs.msg import ColorRGBA 22 | 23 | def changeColor(): 24 | pub = rospy.Publisher('light_controller/command', ColorRGBA, queue_size=1) 25 | rospy.init_node('light_test') 26 | #color in rgb color space ranging from 0 to 1 27 | red = ColorRGBA() 28 | red.r = 1 29 | red.g = 0 30 | red.b = 0 31 | red.a = 1 32 | 33 | yellow = ColorRGBA() 34 | yellow.r = 0.4 35 | yellow.g = 1 36 | yellow.b = 0 37 | yellow.a = 1 38 | 39 | green = ColorRGBA() 40 | green.r = 0 41 | green.g = 1 42 | green.b = 0 43 | green.a = 1 44 | 45 | blue = ColorRGBA() 46 | blue.r = 0 47 | blue.g = 0 48 | blue.b = 1 49 | blue.a = 1 50 | 51 | white = ColorRGBA() 52 | white.r = 0.3 53 | white.g = 1 54 | white.b = 0.3 55 | white.a = 1 56 | 57 | for color in [red,yellow,green,white,blue,green]: 58 | rospy.loginfo("Setting rgb to %s [%d, %d, %d]",color.r,color.g,color.b,color.a) 59 | pub.publish(color) 60 | time.sleep(3) 61 | 62 | if __name__ == '__main__': 63 | try: 64 | changeColor() 65 | except rospy.ROSInterruptException: pass 66 | 67 | -------------------------------------------------------------------------------- /cob_light/ros/scripts/test_sprofi_circle.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 | 20 | import rospy 21 | from std_msgs.msg import ColorRGBA 22 | from cob_light.srv import * 23 | from cob_light.msg import * 24 | 25 | def changeColor(): 26 | rospy.wait_for_service('/light_controller/mode') 27 | control_lights = rospy.ServiceProxy('/light_controller/mode', SetLightMode) 28 | 29 | light_mode = LightMode() 30 | #color in rgb color space ranging from 0 to 1 31 | red = ColorRGBA() 32 | red.r = 1 33 | red.g = 0 34 | red.b = 0 35 | red.a = 0.7 36 | 37 | light_red = ColorRGBA() 38 | light_red.r = 1 39 | light_red.g = 0 40 | light_red.b = 0 41 | light_red.a = 0.3 42 | 43 | off = ColorRGBA() 44 | off.r = 1 45 | off.g = 0 46 | off.b = 0 47 | off.a = 0.01 48 | 49 | dimm = ColorRGBA() 50 | dimm.r = 1 51 | dimm.g = 0 52 | dimm.b = 0 53 | dimm.a = 0.04 54 | 55 | yellow = ColorRGBA() 56 | yellow.r = 1 57 | yellow.g = 1 58 | yellow.b = 0 59 | yellow.a = 1 60 | 61 | green = ColorRGBA() 62 | green.r = 0 63 | green.g = 1.0 64 | green.b = 0 65 | green.a = 1.0 66 | 67 | blue = ColorRGBA() 68 | blue.r = 0 69 | blue.g = 1 70 | blue.b = 0.7 71 | blue.a = 0.4 72 | 73 | white = ColorRGBA() 74 | white.r = 0.3 75 | white.g = 1 76 | white.b = 0.3 77 | white.a = 1 78 | 79 | light_mode.mode = 7 80 | light_mode.frequency=40 81 | light_mode.colors.append(green); 82 | light_mode.colors.append(green); 83 | light_mode.colors.append(green); 84 | light_mode.colors.append(green); 85 | light_mode.colors.append(green); 86 | light_mode.colors.append(green); 87 | light_mode.colors.append(green); 88 | light_mode.colors.append(green); 89 | light_mode.colors.append(green); 90 | light_mode.colors.append(green); 91 | light_mode.colors.append(green); 92 | light_mode.colors.append(green); 93 | light_mode.colors.append(green); 94 | light_mode.colors.append(green); 95 | light_mode.colors.append(green); 96 | light_mode.colors.append(blue); 97 | light_mode.colors.append(blue); 98 | light_mode.colors.append(blue); 99 | light_mode.colors.append(blue); 100 | light_mode.colors.append(blue); 101 | light_mode.colors.append(blue); 102 | light_mode.colors.append(blue); 103 | light_mode.colors.append(blue); 104 | light_mode.colors.append(blue); 105 | light_mode.colors.append(blue); 106 | light_mode.colors.append(blue); 107 | light_mode.colors.append(blue); 108 | light_mode.colors.append(blue); 109 | light_mode.colors.append(blue); 110 | light_mode.colors.append(blue); 111 | 112 | light_mode.colors.append(green); 113 | light_mode.colors.append(green); 114 | light_mode.colors.append(green); 115 | light_mode.colors.append(green); 116 | light_mode.colors.append(green); 117 | light_mode.colors.append(green); 118 | light_mode.colors.append(green); 119 | light_mode.colors.append(green); 120 | light_mode.colors.append(green); 121 | light_mode.colors.append(green); 122 | light_mode.colors.append(green); 123 | light_mode.colors.append(green); 124 | light_mode.colors.append(green); 125 | light_mode.colors.append(green); 126 | light_mode.colors.append(green); 127 | light_mode.colors.append(blue); 128 | light_mode.colors.append(blue); 129 | light_mode.colors.append(blue); 130 | light_mode.colors.append(blue); 131 | light_mode.colors.append(blue); 132 | light_mode.colors.append(blue); 133 | light_mode.colors.append(blue); 134 | light_mode.colors.append(blue); 135 | light_mode.colors.append(blue); 136 | light_mode.colors.append(blue); 137 | light_mode.colors.append(blue); 138 | light_mode.colors.append(blue); 139 | light_mode.colors.append(blue); 140 | light_mode.colors.append(blue); 141 | light_mode.colors.append(blue); 142 | try: 143 | resp1 = control_lights(light_mode) 144 | print(resp1) 145 | except rospy.ServiceException as e: 146 | print("Service call failed: %s"%e) 147 | 148 | time.sleep(6) 149 | 150 | if __name__ == '__main__': 151 | try: 152 | rospy.init_node('light_test') 153 | changeColor() 154 | except rospy.ROSInterruptException: 155 | pass 156 | 157 | -------------------------------------------------------------------------------- /cob_light/ros/scripts/test_sprofi_modes.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 | 20 | import rospy 21 | from std_msgs.msg import ColorRGBA 22 | from cob_light.srv import * 23 | from cob_light.msg import * 24 | 25 | def changeColor(): 26 | rospy.wait_for_service('/light_controller/mode') 27 | control_lights = rospy.ServiceProxy('/light_controller/mode', SetLightMode) 28 | rospy.init_node('light_test') 29 | light_mode = LightMode() 30 | #color in rgb color space ranging from 0 to 1 31 | red = ColorRGBA() 32 | red.r = 1 33 | red.g = 0 34 | red.b = 0 35 | red.a = 0.7 36 | 37 | light_red = ColorRGBA() 38 | light_red.r = 1 39 | light_red.g = 0 40 | light_red.b = 0 41 | light_red.a = 0.3 42 | 43 | off = ColorRGBA() 44 | off.r = 1 45 | off.g = 0 46 | off.b = 0 47 | off.a = 0.01 48 | 49 | dimm = ColorRGBA() 50 | dimm.r = 1 51 | dimm.g = 0 52 | dimm.b = 0 53 | dimm.a = 0.04 54 | 55 | yellow = ColorRGBA() 56 | yellow.r = 1 57 | yellow.g = 1 58 | yellow.b = 0 59 | yellow.a = 1 60 | 61 | green = ColorRGBA() 62 | green.r = 0 63 | green.g = 1 64 | green.b = 0 65 | green.a = 1 66 | 67 | blue = ColorRGBA() 68 | blue.r = 0 69 | blue.g = 1 70 | blue.b = 0.7 71 | blue.a = 0.4 72 | 73 | white = ColorRGBA() 74 | white.r = 0.3 75 | white.g = 1 76 | white.b = 0.3 77 | white.a = 1 78 | 79 | light_mode.mode = 6 80 | light_mode.frequency=100 81 | seq1 = Sequence() 82 | seq1.color = blue 83 | seq1.hold_time = 4; 84 | seq1.cross_time = 1; 85 | light_mode.sequences.append(seq1); 86 | 87 | seq2 = Sequence() 88 | seq2.color = red; 89 | seq2.hold_time = 4; 90 | seq2.cross_time = 1; 91 | light_mode.sequences.append(seq2); 92 | 93 | # seq3 = Sequence() 94 | # seq3.color = off; 95 | # seq3.hold_time = 0.06; 96 | # seq3.cross_time = 0.05; 97 | # light_mode.sequences.append(seq3); 98 | 99 | # seq4 = Sequence() 100 | # seq4.color = light_red; 101 | # seq4.hold_time = 0.08; 102 | # seq4.cross_time = 0.05; 103 | # light_mode.sequences.append(seq4); 104 | # 105 | try: 106 | resp1 = control_lights(light_mode) 107 | print(resp1) 108 | except rospy.ServiceException as e: 109 | print("Service call failed: %s"%e) 110 | 111 | time.sleep(6) 112 | 113 | if __name__ == '__main__': 114 | try: 115 | rospy.init_node('light_test') 116 | changeColor() 117 | except rospy.ROSInterruptException: 118 | pass 119 | 120 | -------------------------------------------------------------------------------- /cob_light/ros/scripts/test_sprofi_part.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 | 20 | import rospy 21 | from std_msgs.msg import ColorRGBA 22 | from cob_light.msg import LightMode 23 | 24 | def changeColor(): 25 | pub = rospy.Publisher('light_controller/command_mode', LightMode, queue_size=1) 26 | rospy.init_node('light_test') 27 | light_mode = LightMode() 28 | #color in rgb color space ranging from 0 to 1 29 | red = ColorRGBA() 30 | red.r = 1 31 | red.g = 0 32 | red.b = 0 33 | red.a = 1 34 | 35 | yellow = ColorRGBA() 36 | yellow.r = 0.4 37 | yellow.g = 1 38 | yellow.b = 0 39 | yellow.a = 1 40 | 41 | green = ColorRGBA() 42 | green.r = 0 43 | green.g = 1 44 | green.b = 0 45 | green.a = 1 46 | 47 | blue = ColorRGBA() 48 | blue.r = 0 49 | blue.g = 0 50 | blue.b = 1 51 | blue.a = 1 52 | 53 | white = ColorRGBA() 54 | white.r = 0.3 55 | white.g = 1 56 | white.b = 0.3 57 | white.a = 1 58 | 59 | for color in [red,yellow,green,white,blue,green]: 60 | rospy.loginfo("Setting rgb to %s [%d, %d, %d]",color.r,color.g,color.b,color.a) 61 | light_mode.colors= 27*[color] 62 | pub.publish(light_mode) 63 | time.sleep(3) 64 | 65 | if __name__ == '__main__': 66 | try: 67 | changeColor() 68 | except rospy.ROSInterruptException: pass 69 | 70 | -------------------------------------------------------------------------------- /cob_light/ros/src/colorO.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | #include 20 | 21 | ColorO::ColorO(SerialIO* serialIO) 22 | { 23 | _serialIO = serialIO; 24 | } 25 | 26 | ColorO::~ColorO() 27 | { 28 | } 29 | 30 | bool ColorO::init() 31 | { 32 | return true; 33 | } 34 | 35 | void ColorO::setColorMulti(std::vector &colors) 36 | { 37 | 38 | } 39 | 40 | void ColorO::setColor(color::rgba color) 41 | { 42 | int bytes_wrote = 0; 43 | 44 | color::rgba color_tmp = color; 45 | 46 | //calculate rgb spektrum for spezific alpha value, because 47 | //led board is not supporting alpha values 48 | color.r *= color.a; 49 | color.g *= color.a; 50 | color.b *= color.a; 51 | //led board value spektrum is from 0 - 999. 52 | //at r@w's led strip, 0 means fully lighted and 999 light off(_invertMask) 53 | color.r = (fabs(_invertMask-color.r) * 999.0); 54 | color.g = (fabs(_invertMask-color.g) * 999.0); 55 | color.b = (fabs(_invertMask-color.b) * 999.0); 56 | 57 | _ssOut.clear(); 58 | _ssOut.str(""); 59 | _ssOut << (int)color.r << " " << (int)color.g << " " << (int)color.b << "\n\r"; 60 | 61 | // send data over serial port 62 | bytes_wrote = _serialIO->sendData(_ssOut.str()); 63 | if(bytes_wrote == -1) 64 | { 65 | ROS_WARN("Can not write to serial port. Port closed!"); 66 | } 67 | else 68 | { 69 | ROS_DEBUG("Wrote [%s] with %i bytes from %i bytes", \ 70 | _ssOut.str().c_str(), bytes_wrote, (int)_ssOut.str().length()); 71 | m_sigColorSet(color_tmp); 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /cob_light/ros/src/colorOMarker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Mojin Robotics GmbH https://www.mojin-robotics.de 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 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | ColorOMarker::ColorOMarker(ros::NodeHandle* nh) 23 | { 24 | p_nh = nh; 25 | _pubMarkerArray = p_nh->advertise("markers", 1); 26 | } 27 | 28 | ColorOMarker::~ColorOMarker() 29 | { 30 | } 31 | 32 | bool ColorOMarker::init() 33 | { 34 | return true; 35 | } 36 | 37 | void ColorOMarker::setColor(color::rgba color) 38 | { 39 | visualization_msgs::MarkerArray marker_array; 40 | 41 | for (auto i = 0; i < this->getNumLeds(); ++i) 42 | { 43 | visualization_msgs::Marker marker; 44 | marker.header.frame_id = "light"; 45 | marker.header.stamp = ros::Time::now(); 46 | marker.ns = std::string("led"); 47 | marker.id = i; 48 | marker.type = visualization_msgs::Marker::SPHERE; 49 | marker.action = visualization_msgs::Marker::ADD; 50 | marker.pose = geometry_msgs::Pose(); 51 | marker.pose.position.x = 0.01*i; 52 | marker.pose.orientation.w = 1; 53 | marker.scale.x = 0.01; 54 | marker.scale.y = 0.01; 55 | marker.scale.z = 0.01; 56 | marker.color.r = color.r; 57 | marker.color.g = color.g; 58 | marker.color.b = color.b; 59 | marker.color.a = color.a; 60 | marker_array.markers.push_back(marker); 61 | } 62 | _pubMarkerArray.publish(marker_array); 63 | } 64 | 65 | void ColorOMarker::setColorMulti(std::vector &colors) 66 | { 67 | visualization_msgs::MarkerArray marker_array; 68 | 69 | for (auto i = 0; i < colors.size(); ++i) 70 | { 71 | visualization_msgs::Marker marker; 72 | marker.header.frame_id = "light"; 73 | marker.header.stamp = ros::Time::now(); 74 | marker.ns = std::string("led"); 75 | marker.id = i; 76 | marker.type = visualization_msgs::Marker::SPHERE; 77 | marker.action = visualization_msgs::Marker::ADD; 78 | marker.pose = geometry_msgs::Pose(); 79 | marker.pose.position.x = 0.01*i; 80 | marker.pose.orientation.w = 1; 81 | marker.scale.x = 0.01; 82 | marker.scale.y = 0.01; 83 | marker.scale.z = 0.01; 84 | marker.color.r = colors[i].r; 85 | marker.color.g = colors[i].g; 86 | marker.color.b = colors[i].b; 87 | marker.color.a = colors[i].a; 88 | marker_array.markers.push_back(marker); 89 | } 90 | _pubMarkerArray.publish(marker_array); 91 | 92 | //_pubSimulationMulti.publish(colorMsg); 93 | m_sigColorSet(colors[0]); 94 | } 95 | -------------------------------------------------------------------------------- /cob_light/ros/src/colorOSim.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 | #include 18 | #include 19 | #include 20 | 21 | ColorOSim::ColorOSim(ros::NodeHandle* nh) 22 | { 23 | p_nh = nh; 24 | _pubSimulation = p_nh->advertise("debug",2); 25 | _pubSimulationMulti = p_nh->advertise("debugMulti", 2); 26 | } 27 | 28 | ColorOSim::~ColorOSim() 29 | { 30 | } 31 | 32 | bool ColorOSim::init() 33 | { 34 | return true; 35 | } 36 | 37 | void ColorOSim::setColor(color::rgba color) 38 | { 39 | std_msgs::ColorRGBA _color; 40 | _color.r = color.r; 41 | _color.g = color.g; 42 | _color.b = color.b; 43 | _color.a = color.a; 44 | 45 | _pubSimulation.publish(_color); 46 | m_sigColorSet(color); 47 | } 48 | 49 | void ColorOSim::setColorMulti(std::vector &colors) 50 | { 51 | std_msgs::ColorRGBA color; 52 | cob_light::ColorRGBAArray colorMsg; 53 | for(size_t i = 0; i < colors.size(); ++i) 54 | { 55 | color.r = colors[i].r; 56 | color.g = colors[i].g; 57 | color.b = colors[i].b; 58 | color.a = colors[i].a; 59 | colorMsg.colors.push_back(color); 60 | } 61 | 62 | _pubSimulationMulti.publish(colorMsg); 63 | m_sigColorSet(colors[0]); 64 | } 65 | -------------------------------------------------------------------------------- /cob_light/ros/src/ms35.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 | MS35::MS35(SerialIO* serialIO) 25 | { 26 | _serialIO = serialIO; 27 | } 28 | 29 | MS35::~MS35() 30 | { 31 | } 32 | 33 | bool MS35::init() 34 | { 35 | bool ret = false; 36 | const char init_data[] = { 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd }; 37 | int init_len = sizeof(init_data) / sizeof(init_data[0]); 38 | const char startup_data[] = { 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 39 | int startup_len = sizeof(startup_data) / sizeof(startup_data[0]); 40 | char init_buf[18]; 41 | 42 | //write data until controller is ready to receive valid package 43 | char tmp = 0xfd; 44 | std::string recv; 45 | int bytes_recv; 46 | do{ 47 | _serialIO->sendData(&tmp, 1); 48 | bytes_recv = _serialIO->readData(recv, 1); 49 | 50 | }while(bytes_recv <= 0); 51 | 52 | unsigned short int crc = getChecksum(startup_data, startup_len); 53 | 54 | memcpy(&init_buf, init_data, init_len); 55 | memcpy(&init_buf[init_len], startup_data, startup_len); 56 | init_buf[init_len+startup_len] = ((char*)&crc)[1]; 57 | init_buf[init_len+startup_len+1] = ((char*)&crc)[0]; 58 | if(sendData(init_buf, 18) == 18) 59 | ret = true; 60 | return ret; 61 | } 62 | 63 | unsigned short int MS35::getChecksum(const char* data, size_t len) 64 | { 65 | unsigned int ret; 66 | boost::crc_16_type checksum_agent; 67 | checksum_agent.process_bytes(data, len); 68 | return checksum_agent.checksum(); 69 | } 70 | 71 | int MS35::sendData(const char* data, size_t len) 72 | { 73 | int bytes_wrote = 0; 74 | 75 | bytes_wrote = _serialIO->sendData(data, len); 76 | 77 | if(bytes_wrote == -1) 78 | ROS_WARN("Can not write to serial port. Port closed!"); 79 | else 80 | ROS_DEBUG("Wrote [%s] with %i bytes from %lu bytes", data, bytes_wrote, len); 81 | 82 | return bytes_wrote; 83 | } 84 | 85 | void MS35::setColorMulti(std::vector &colors) 86 | { 87 | 88 | } 89 | 90 | void MS35::setColor(color::rgba color) 91 | { 92 | color::rgba color_tmp = color; 93 | 94 | //calculate rgb spektrum for spezific alpha value, because 95 | //led board is not supporting alpha values 96 | color.r *= color.a; 97 | color.g *= color.a; 98 | color.b *= color.a; 99 | 100 | //shift values into 8 bit rgb space 101 | color.r = fabs(color.r * 255); 102 | color.g = fabs(color.g * 255); 103 | color.b = fabs(color.b * 255); 104 | 105 | buffer[0] = 0x01; buffer[1] = 0x00; 106 | buffer[2] = (int)color.r; buffer[3]=(int)color.g; buffer[4]=(int)color.b; 107 | buffer[5] = 0x00; buffer[6]=0x00; 108 | 109 | unsigned short int crc = getChecksum(buffer, 7); 110 | buffer[7] = ((char*)&crc)[1]; 111 | buffer[8] = ((char*)&crc)[0]; 112 | 113 | if(sendData(buffer, PACKAGE_SIZE) == PACKAGE_SIZE) 114 | m_sigColorSet(color_tmp); 115 | else 116 | ROS_ERROR("Could not write to serial port"); 117 | } 118 | -------------------------------------------------------------------------------- /cob_light/ros/src/stageprofi.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 | StageProfi::StageProfi(SerialIO* serialIO, unsigned int leds, int led_offset) 25 | { 26 | _serialIO = serialIO; 27 | _num_leds = leds; 28 | _led_offset = led_offset; 29 | } 30 | 31 | StageProfi::~StageProfi() 32 | { 33 | } 34 | 35 | bool StageProfi::init() 36 | { 37 | bool ret = false; 38 | const char init_data[] = { 'C', '?' }; 39 | int init_len = sizeof(init_data) / sizeof(init_data[0]); 40 | 41 | char init_buf[2]; 42 | 43 | memcpy(&init_buf, init_data, init_len); 44 | if (_serialIO->sendData(init_buf, 2) == 2) 45 | { 46 | std::string rec; 47 | if(_serialIO->readData(rec, 1) == 1) 48 | ret = true; 49 | } 50 | else 51 | ROS_ERROR("Sending init command to stageprofi failed"); 52 | return ret; 53 | } 54 | 55 | void StageProfi::setColor(color::rgba color) 56 | { 57 | color::rgba color_tmp = color; 58 | 59 | color_tmp.r *= color.a; 60 | color_tmp.g *= color.a; 61 | color_tmp.b *= color.a; 62 | 63 | color_tmp.r = fabs(color_tmp.r * 255); 64 | color_tmp.g = fabs(color_tmp.g * 255); 65 | color_tmp.b = fabs(color_tmp.b * 255); 66 | 67 | unsigned int num_channels = _num_leds * 3; 68 | char channelbuffer[num_channels]; 69 | 70 | for (int i = 0; i < _num_leds; i++) 71 | { 72 | channelbuffer[i * 3] = (int) color_tmp.r; 73 | channelbuffer[i * 3 + 1] = (int) color_tmp.g; 74 | channelbuffer[i * 3 + 2] = (int) color_tmp.b; 75 | } 76 | 77 | uint16_t index = 0; 78 | while (index < num_channels) 79 | { 80 | unsigned int size = std::min((unsigned int) MAX_CHANNELS, 81 | num_channels - index); 82 | if (sendDMX(index, channelbuffer + index, size)) 83 | index += size; 84 | else 85 | { 86 | ROS_ERROR("Sending color to stageprofi failed"); 87 | this->recover(); 88 | break; 89 | } 90 | } 91 | m_sigColorSet(color); 92 | } 93 | 94 | void StageProfi::setColorMulti(std::vector &colors) 95 | { 96 | color::rgba color_tmp; 97 | unsigned int num_channels = _num_leds * 3; 98 | char channelbuffer[num_channels]; 99 | 100 | std::vector color_out = colors; 101 | //shift lex index by offset 102 | if(_led_offset != 0) 103 | { 104 | if(_led_offset > 0) 105 | std::rotate(color_out.begin(), color_out.begin()+_led_offset, color_out.end()); 106 | else 107 | std::rotate(color_out.begin(), color_out.begin()+color_out.size()+_led_offset, color_out.end()); 108 | } 109 | 110 | for (int i = 0; i < _num_leds || i < colors.size(); i++) 111 | { 112 | color_tmp.r = color_out[i].r * color_out[i].a; 113 | color_tmp.g = color_out[i].g * color_out[i].a; 114 | color_tmp.b = color_out[i].b * color_out[i].a; 115 | 116 | color_tmp.r = fabs(color_tmp.r * 255); 117 | color_tmp.g = fabs(color_tmp.g * 255); 118 | color_tmp.b = fabs(color_tmp.b * 255); 119 | 120 | channelbuffer[i * 3] = (int) color_tmp.r; 121 | channelbuffer[i * 3 + 1] = (int) color_tmp.g; 122 | channelbuffer[i * 3 + 2] = (int) color_tmp.b; 123 | } 124 | 125 | uint16_t index = 0; 126 | while (index < num_channels) 127 | { 128 | unsigned int size = std::min((unsigned int) MAX_CHANNELS, 129 | num_channels - index); 130 | if (sendDMX(index, channelbuffer + index, size)) 131 | index += size; 132 | else 133 | { 134 | ROS_ERROR("Sending color to stageprofi failed"); 135 | this->recover(); 136 | break; 137 | } 138 | } 139 | m_sigColorSet(colors[0]); 140 | } 141 | 142 | bool StageProfi::sendDMX(uint16_t start, const char* buf, unsigned int length) 143 | { 144 | bool ret = false; 145 | std::string recv; 146 | char msg[MAX_CHANNELS + HEADER_SIZE]; 147 | 148 | unsigned int len = std::min((unsigned int) MAX_CHANNELS, length); 149 | 150 | msg[0] = 0xFF; 151 | msg[1] = start & 0xFF; 152 | msg[2] = (start >> 8) & 0xFF; 153 | msg[3] = len; 154 | 155 | memcpy(msg + HEADER_SIZE, buf, len); 156 | 157 | const int bytes_to_send = len + HEADER_SIZE; 158 | 159 | //send color command to controller 160 | ret = _serialIO->sendData(msg, bytes_to_send) == bytes_to_send; 161 | //receive ack 162 | ret &= _serialIO->readData(recv, 1) == 1; 163 | return ret; 164 | } 165 | 166 | bool StageProfi::recover() 167 | { 168 | ROS_WARN("Trying to recover stagedriver"); 169 | if(_serialIO->recover() && this->init()) 170 | return true; 171 | else 172 | return false; 173 | } 174 | -------------------------------------------------------------------------------- /cob_light/srv/SetLightMode.srv: -------------------------------------------------------------------------------- 1 | cob_light/LightMode mode 2 | --- 3 | uint8 active_mode 4 | uint8 active_priority 5 | uint64 track_id 6 | -------------------------------------------------------------------------------- /cob_light/srv/StopLightMode.srv: -------------------------------------------------------------------------------- 1 | uint64 track_id 2 | --- 3 | uint8 active_mode 4 | uint8 active_priority 5 | uint64 track_id 6 | -------------------------------------------------------------------------------- /cob_mimic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_mimic) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs diagnostic_msgs diagnostic_updater message_generation roscpp roslib) 5 | find_package(PkgConfig REQUIRED) 6 | find_package(Boost REQUIRED COMPONENTS filesystem thread) 7 | pkg_check_modules(libvlc REQUIRED libvlc) 8 | 9 | ### Message Generation ### 10 | add_service_files(DIRECTORY srv FILES 11 | SetMimic.srv 12 | ) 13 | 14 | add_action_files(DIRECTORY action FILES 15 | SetMimic.action 16 | ) 17 | 18 | generate_messages(DEPENDENCIES actionlib_msgs) 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS actionlib actionlib_msgs diagnostic_msgs message_runtime roscpp 22 | ) 23 | 24 | ### BUILD ### 25 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 26 | 27 | add_executable(mimic src/mimic_node.cpp) 28 | add_dependencies(mimic ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 29 | target_link_libraries(mimic ${catkin_LIBRARIES} ${libvlc_LIBRARIES} ${Boost_LIBRARIES}) 30 | 31 | ### INSTALL ### 32 | install(TARGETS mimic 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | catkin_install_python(PROGRAMS src/test_mimic.py 39 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | 42 | install(DIRECTORY common 43 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 44 | ) 45 | -------------------------------------------------------------------------------- /cob_mimic/action/SetMimic.action: -------------------------------------------------------------------------------- 1 | string mimic 2 | float32 speed 3 | int64 repeat 4 | --- 5 | --- 6 | -------------------------------------------------------------------------------- /cob_mimic/common/afraid.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/afraid.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/angry.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/angry.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/asking.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/asking.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/blinking.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/blinking.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/blinking_left.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/blinking_left.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/blinking_right.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/blinking_right.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/bored.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/bored.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/busy.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/busy.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/confused.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/confused.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/default.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/default.jpg -------------------------------------------------------------------------------- /cob_mimic/common/default.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/default.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/falling_asleep.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/falling_asleep.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/happy.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/happy.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/laughing.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/laughing.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/no.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/no.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/afraid.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/afraid.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/angry.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/angry.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/asking.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/asking.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/blinking.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/blinking.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/blinking_left.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/blinking_left.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/blinking_right.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/blinking_right.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/bored.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/bored.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/busy.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/busy.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/confused.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/confused.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/default.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/default.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/falling_asleep.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/falling_asleep.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/happy.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/happy.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/laughing.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/laughing.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/no.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/no.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/sad.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/sad.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/searching.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/searching.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/sleeping.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/sleeping.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/surprised.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/surprised.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/tired.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/tired.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/waking_up.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/waking_up.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/original/yes.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/original/yes.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/sad.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/sad.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/searching.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/searching.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/sleeping.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/sleeping.jpg -------------------------------------------------------------------------------- /cob_mimic/common/sleeping.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/sleeping.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/surprised.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/surprised.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/tired.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/tired.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/waking_up.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/waking_up.mp4 -------------------------------------------------------------------------------- /cob_mimic/common/yes.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4am-robotics/cob_driver/b6b3fc11eab53f01e32e5bdf2452b182fd3e9bd1/cob_mimic/common/yes.mp4 -------------------------------------------------------------------------------- /cob_mimic/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_mimic 3 | 0.8.18 4 | This package implements the Care-O-bot mimic 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_mimic 9 | 10 | Felix Messmer 11 | Benjamin Maidel 12 | Nadia Hammoudeh Garcia 13 | Benjamin Maidel 14 | 15 | catkin 16 | 17 | message_generation 18 | message_runtime 19 | 20 | actionlib_msgs 21 | actionlib 22 | boost 23 | diagnostic_updater 24 | diagnostic_msgs 25 | libvlc-dev 26 | roscpp 27 | roslib 28 | vlc 29 | 30 | rospy 31 | 32 | 33 | -------------------------------------------------------------------------------- /cob_mimic/src/test_mimic.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 | import sys 21 | 22 | # Brings in the SimpleActionClient 23 | import actionlib 24 | 25 | # Brings in the messages used by the set_mimic action, including the 26 | # goal message and the result message. 27 | import cob_mimic.msg 28 | 29 | def mimic_client(): 30 | # Creates the SimpleActionClient, passing the type of the action 31 | # to the constructor. 32 | client = actionlib.SimpleActionClient('/mimic/set_mimic', cob_mimic.msg.SetMimicAction) 33 | 34 | # Waits until the action server has started up and started 35 | # listening for goals. 36 | client.wait_for_server() 37 | 38 | # Creates a goal to send to the action server. 39 | goal = cob_mimic.msg.SetMimicGoal() 40 | goal.mimic = sys.argv[1] 41 | 42 | # Sends the goal to the action server. 43 | client.send_goal(goal) 44 | 45 | # Waits for the server to finish performing the action. 46 | client.wait_for_result() 47 | 48 | # Prints out the result of executing the action 49 | return client.get_result() 50 | 51 | if __name__ == '__main__': 52 | try: 53 | # Initializes a rospy node so that the SimpleActionClient can 54 | # publish and subscribe over ROS. 55 | if len(sys.argv) != 2: 56 | print("Error: please specify mimic string as input") 57 | sys.exit(1) 58 | 59 | rospy.init_node('mimic_test_node') 60 | result = mimic_client() 61 | except rospy.ROSInterruptException: 62 | print("program interrupted before completion") 63 | -------------------------------------------------------------------------------- /cob_mimic/srv/SetMimic.srv: -------------------------------------------------------------------------------- 1 | string mimic 2 | float32 speed 3 | int64 repeat 4 | --- 5 | bool success 6 | string message 7 | -------------------------------------------------------------------------------- /cob_phidgets/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_phidgets) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS libphidgets message_generation roscpp sensor_msgs std_msgs) 7 | 8 | ### Message Generation ### 9 | add_message_files( 10 | FILES 11 | AnalogSensor.msg 12 | DigitalSensor.msg 13 | ) 14 | 15 | add_service_files( 16 | FILES 17 | SetDataRate.srv 18 | SetDigitalSensor.srv 19 | SetTriggerValue.srv 20 | ) 21 | 22 | generate_messages(DEPENDENCIES std_msgs) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS message_runtime roscpp sensor_msgs std_msgs 26 | ) 27 | 28 | ### BUILD ### 29 | include_directories(${PROJECT_SOURCE_DIR}/ros/include ${PROJECT_SOURCE_DIR}/common/include ${catkin_INCLUDE_DIRS}) 30 | 31 | add_executable(range_sensors ros/src/phidgets_range_sensors.cpp) 32 | add_dependencies(range_sensors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 33 | target_link_libraries(range_sensors ${catkin_LIBRARIES}) 34 | 35 | add_executable(device_name ros/src/set_device_name.cpp) 36 | add_dependencies(device_name ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 37 | target_link_libraries(device_name ${catkin_LIBRARIES}) 38 | 39 | add_executable(phidget_sensors 40 | common/src/phidget.cpp 41 | common/src/phidgetik.cpp 42 | ros/src/phidget_manager.cpp 43 | ros/src/phidget_sensors_node.cpp 44 | ros/src/phidgetik_ros.cpp) 45 | add_dependencies(phidget_sensors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 46 | target_link_libraries(phidget_sensors ${catkin_LIBRARIES}) 47 | 48 | add_executable(phidget_sensors_sim ros/src/cob_phidgets_sim_node.cpp) 49 | add_dependencies(phidget_sensors_sim ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 50 | target_link_libraries(phidget_sensors_sim ${catkin_LIBRARIES}) 51 | 52 | ### INSTALL ### 53 | install(TARGETS device_name phidget_sensors phidget_sensors_sim range_sensors 54 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 57 | ) 58 | 59 | catkin_install_python(PROGRAMS ros/src/simulated_range_sensors.py 60 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/ros/src 61 | ) 62 | -------------------------------------------------------------------------------- /cob_phidgets/common/include/cob_phidgets/phidget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef _PHIDGET_H_ 19 | #define _PHIDGET_H_ 20 | 21 | #include 22 | #include 23 | 24 | class Phidget 25 | { 26 | public: 27 | enum class SensingMode{EVENT=0, POLLING=1}; 28 | 29 | ~Phidget(); 30 | 31 | auto open(int serial_number) -> int; 32 | auto close(int serial_number) -> int; 33 | auto waitForAttachment(int timeout) -> int; 34 | auto getDeviceType() -> std::string; 35 | auto getDeviceName() -> std::string; 36 | auto getDeviceLabel() -> std::string; 37 | auto getLibraryVersion() -> std::string; 38 | auto getDeviceSerialNumber() -> int; 39 | auto getDeviceVersion() -> int; 40 | 41 | virtual auto update() -> void; 42 | 43 | static auto getErrorDescription(int errorCode) -> std::string; 44 | 45 | protected: 46 | CPhidgetHandle* _phiHandle; 47 | int _serialNumber; 48 | int _last_error; 49 | SensingMode _sensMode; 50 | 51 | Phidget(CPhidgetHandle * handle, SensingMode mode); 52 | 53 | virtual auto attachHandler() -> int; 54 | virtual auto detachHandler() -> int; 55 | }; 56 | #endif //_PHIDGET_H_ 57 | -------------------------------------------------------------------------------- /cob_phidgets/common/include/cob_phidgets/phidgetik.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef _PHIDGETIK_H_ 19 | #define _PHIDGETIK_H_ 20 | 21 | #include 22 | 23 | class PhidgetIK: public Phidget 24 | { 25 | public: 26 | PhidgetIK(SensingMode mode); 27 | ~PhidgetIK(); 28 | 29 | auto init(int serial_number) -> int; 30 | 31 | auto getInputCount() -> int; 32 | auto getOutputCount() -> int; 33 | auto getSensorCount() -> int; 34 | 35 | auto getInputState(int index) -> int; 36 | 37 | auto getOutputState(int index) -> int; 38 | auto setOutputState(int index, int state) -> int; 39 | 40 | auto getSensorValue(int index) -> int; 41 | auto getSensorRawValue(int index) -> int; 42 | 43 | auto getSensorChangeTrigger(int index) -> int; 44 | auto setSensorChangeTrigger(int index, int trigger) -> int; 45 | 46 | auto getRatiometric() -> int; 47 | auto setRatiometric(int ratiometric) -> int; 48 | 49 | auto getDataRate(int index) -> int; 50 | auto setDataRate(int index, int datarate) -> int; 51 | 52 | auto getDataRateMax(int index) -> int; 53 | auto getDataRateMin(int index) -> int; 54 | 55 | auto getError() -> int; 56 | 57 | virtual auto update() -> void; 58 | 59 | protected: 60 | CPhidgetInterfaceKitHandle _iKitHandle; 61 | 62 | virtual int attachHandler(); 63 | virtual int detachHandler(); 64 | 65 | virtual int inputChangeHandler(int index, int inputState); 66 | virtual int outputChangeHandler(int index, int outputState); 67 | virtual int sensorChangeHandler(int index, int sensorValue); 68 | 69 | private: 70 | static auto attachDelegate(CPhidgetHandle phid, void *userptr) -> int; 71 | 72 | static auto inputChangeDelegate(CPhidgetInterfaceKitHandle phid, 73 | void *userPtr, int index, int inputState) -> int; 74 | static auto outputChangeDelegate(CPhidgetInterfaceKitHandle phid, 75 | void *userPtr, int index, int outputState) -> int; 76 | static auto sensorChangeDelegate(CPhidgetInterfaceKitHandle phid, 77 | void *userPtr, int index, int sensorValue) -> int; 78 | }; 79 | #endif //_PHIDGETIK_H_ 80 | -------------------------------------------------------------------------------- /cob_phidgets/common/src/phidget.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 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | Phidget::Phidget(CPhidgetHandle* handle, SensingMode mode) 24 | : _phiHandle(handle), _serialNumber(-1), _last_error(-1), _sensMode(mode) 25 | { 26 | } 27 | 28 | Phidget::~Phidget() 29 | { 30 | CPhidget_close(*_phiHandle); 31 | CPhidget_delete(*_phiHandle); 32 | } 33 | 34 | auto Phidget::open(int serial_number) -> int 35 | { 36 | return CPhidget_open(*_phiHandle, serial_number); 37 | } 38 | 39 | auto Phidget::close(int serial_number) -> int 40 | { 41 | return CPhidget_close(*_phiHandle); 42 | } 43 | 44 | auto Phidget::waitForAttachment(int timeout) -> int 45 | { 46 | return CPhidget_waitForAttachment(*_phiHandle, timeout); 47 | } 48 | 49 | auto Phidget::getDeviceType() -> std::string 50 | { 51 | char a[256]; 52 | const char * deviceptr = a; 53 | CPhidget_getDeviceType(*_phiHandle, &deviceptr); 54 | return std::string(deviceptr); 55 | } 56 | 57 | auto Phidget::getDeviceName() -> std::string 58 | { 59 | char a[256]; 60 | const char * deviceptr = a; 61 | CPhidget_getDeviceName(*_phiHandle, &deviceptr); 62 | return std::string(deviceptr); 63 | } 64 | 65 | auto Phidget::getDeviceLabel() -> std::string 66 | { 67 | char a[256]; 68 | const char * deviceptr = a; 69 | CPhidget_getDeviceType(*_phiHandle, &deviceptr); 70 | return std::string(deviceptr); 71 | } 72 | 73 | auto Phidget::getLibraryVersion() -> std::string 74 | { 75 | char a[256]; 76 | const char * deviceptr = a; 77 | CPhidget_getLibraryVersion(&deviceptr); 78 | return std::string(deviceptr); 79 | } 80 | 81 | auto Phidget::getDeviceSerialNumber() -> int 82 | { 83 | int sernum; 84 | CPhidget_getSerialNumber(*_phiHandle, &sernum); 85 | _serialNumber = sernum; 86 | return sernum; 87 | } 88 | 89 | auto Phidget::getDeviceVersion() -> int 90 | { 91 | int version; 92 | CPhidget_getDeviceVersion(*_phiHandle, &version); 93 | return version; 94 | } 95 | 96 | auto Phidget::getErrorDescription(int errorCode) -> std::string 97 | { 98 | char a[256]; 99 | const char * errorPtr = a; 100 | CPhidget_getErrorDescription(errorCode, &errorPtr); 101 | return std::string(errorPtr); 102 | } 103 | 104 | auto Phidget::attachHandler() -> int 105 | { 106 | printf("attachHandler()"); 107 | return 0; 108 | } 109 | 110 | auto Phidget::detachHandler() -> int 111 | { 112 | printf("detachHandler()"); 113 | return 0; 114 | } 115 | 116 | auto Phidget::update() -> void 117 | { 118 | printf("Phidget::update()"); 119 | } 120 | -------------------------------------------------------------------------------- /cob_phidgets/msg/AnalogSensor.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string[] uri 3 | int16[] value 4 | -------------------------------------------------------------------------------- /cob_phidgets/msg/DigitalSensor.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string[] uri 3 | int8[] state -------------------------------------------------------------------------------- /cob_phidgets/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_phidgets 3 | 0.8.18 4 | cob_phidgets 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_phidgets 9 | 10 | 11 | Benjamin Maidel 12 | Florian Weisshardt 13 | 14 | catkin 15 | 16 | message_generation 17 | message_runtime 18 | 19 | libphidgets 20 | roscpp 21 | sensor_msgs 22 | std_msgs 23 | 24 | rospy 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /cob_phidgets/ros/include/cob_phidgets/phidget_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef _PHIDGETMANAGER_H_ 19 | #define _PHIDGETMANAGER_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | struct AttachedDevice 26 | { 27 | int serial_num; 28 | std::string name; 29 | }; 30 | 31 | class PhidgetManager 32 | { 33 | public: 34 | PhidgetManager(); 35 | ~PhidgetManager(); 36 | 37 | auto getAttachedDevices()-> std::vector; 38 | private: 39 | CPhidgetManagerHandle _manHandle; 40 | std::vector _attachedDevices; 41 | }; 42 | #endif //_PHIDGETMANAGER_H_ 43 | -------------------------------------------------------------------------------- /cob_phidgets/ros/include/cob_phidgets/phidgetik_ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef _PHIDGETIKROS_H_ 19 | #define _PHIDGETIKROS_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | class PhidgetIKROS: public PhidgetIK 34 | { 35 | public: 36 | PhidgetIKROS(ros::NodeHandle nh, int serial_num, std::string board_name, XmlRpc::XmlRpcValue* sensor_params, SensingMode mode); 37 | ~PhidgetIKROS(); 38 | 39 | private: 40 | ros::NodeHandle _nh; 41 | ros::Publisher _pubAnalog; 42 | ros::Publisher _pubDigital; 43 | ros::Subscriber _subDigital; 44 | ros::ServiceServer _srvDigitalOut; 45 | ros::ServiceServer _srvDataRate; 46 | ros::ServiceServer _srvTriggerValue; 47 | 48 | int _serial_num; 49 | std::string _board_name; 50 | 51 | struct OutputCompare 52 | { 53 | bool updated; 54 | int index; 55 | int state; 56 | }; 57 | 58 | OutputCompare _outputChanged; 59 | std::mutex _mutex; 60 | 61 | std::map _indexNameMapAnalog; 62 | std::map _indexNameMapAnalogRev; 63 | std::map _indexNameMapDigitalIn; 64 | std::map _indexNameMapDigitalInRev; 65 | std::map _indexNameMapDigitalOut; 66 | std::map _indexNameMapDigitalOutRev; 67 | std::map::iterator _indexNameMapItr; 68 | std::map::iterator _indexNameMapRevItr; 69 | 70 | auto readParams(XmlRpc::XmlRpcValue* sensor_params) -> void; 71 | 72 | auto update() -> void; 73 | 74 | auto attachHandler() -> int; 75 | auto detachHandler() -> int; 76 | 77 | auto inputChangeHandler(int index, int inputState) -> int; 78 | auto outputChangeHandler(int index, int outputState) -> int; 79 | auto sensorChangeHandler(int index, int sensorValue) -> int; 80 | 81 | auto onDigitalOutCallback(const cob_phidgets::DigitalSensorConstPtr& msg) -> void; 82 | auto setDigitalOutCallback(cob_phidgets::SetDigitalSensor::Request &req, 83 | cob_phidgets::SetDigitalSensor::Response &res) -> bool; 84 | auto setDataRateCallback(cob_phidgets::SetDataRate::Request &req, 85 | cob_phidgets::SetDataRate::Response &res) -> bool; 86 | auto setTriggerValueCallback(cob_phidgets::SetTriggerValue::Request &req, 87 | cob_phidgets::SetTriggerValue::Response &res) -> bool; 88 | }; 89 | #endif //_PHIDGETIK_H_ 90 | -------------------------------------------------------------------------------- /cob_phidgets/ros/src/cob_phidgets_sim_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | class PhidgetIKROSSim 27 | { 28 | public: 29 | PhidgetIKROSSim() 30 | { 31 | _pubAnalog = _nh.advertise("analog_sensors", 1); 32 | _pubDigital = _nh.advertise("digital_sensors", 1); 33 | _subDigital = _nh.subscribe("set_digital_sensor", 1, &PhidgetIKROSSim::onDigitalOutCallback, this); 34 | 35 | _srvDigitalOut = _nh.advertiseService("set_digital", &PhidgetIKROSSim::setDigitalOutCallback, this); 36 | _srvDataRate = _nh.advertiseService("set_data_rate", &PhidgetIKROSSim::setDataRateCallback, this); 37 | _srvTriggerValue = _nh.advertiseService("set_trigger_value", &PhidgetIKROSSim::setTriggerValueCallback, this); 38 | }; 39 | ~PhidgetIKROSSim(){}; 40 | private: 41 | ros::NodeHandle _nh; 42 | ros::Publisher _pubAnalog; 43 | ros::Publisher _pubDigital; 44 | ros::Subscriber _subDigital; 45 | ros::ServiceServer _srvDigitalOut; 46 | ros::ServiceServer _srvDataRate; 47 | ros::ServiceServer _srvTriggerValue; 48 | 49 | auto onDigitalOutCallback(const cob_phidgets::DigitalSensorConstPtr& msg) -> void 50 | { 51 | 52 | } 53 | auto setDigitalOutCallback(cob_phidgets::SetDigitalSensor::Request &req, 54 | cob_phidgets::SetDigitalSensor::Response &res) -> bool 55 | { 56 | res.state = req.state; 57 | res.uri = req.uri; 58 | return true; 59 | } 60 | auto setDataRateCallback(cob_phidgets::SetDataRate::Request &req, 61 | cob_phidgets::SetDataRate::Response &res) -> bool 62 | { 63 | return true; 64 | } 65 | auto setTriggerValueCallback(cob_phidgets::SetTriggerValue::Request &req, 66 | cob_phidgets::SetTriggerValue::Response &res) -> bool 67 | { 68 | return true; 69 | } 70 | }; 71 | 72 | 73 | int main(int argc, char **argv) 74 | { 75 | ros::init(argc, argv, "cob_phidgets_sim"); 76 | PhidgetIKROSSim cob_phidgets_sim; 77 | ros::spin(); 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /cob_phidgets/ros/src/ifkit.c: -------------------------------------------------------------------------------- 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 | // InterfacekitTest.cpp : Defines the entry point for the console application. 19 | // 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | void display_generic_properties(CPhidgetHandle phid) 28 | { 29 | int sernum, version; 30 | const char *deviceptr; 31 | CPhidget_getDeviceType(phid, &deviceptr); 32 | CPhidget_getSerialNumber(phid, &sernum); 33 | CPhidget_getDeviceVersion(phid, &version); 34 | 35 | printf("%s\n", deviceptr); 36 | printf("Version: %8d SerialNumber: %10d\n", version, sernum); 37 | return; 38 | } 39 | 40 | 41 | int IFK_DetachHandler(CPhidgetHandle IFK, void *userptr) 42 | { 43 | printf("Detach handler ran!\n"); 44 | return 0; 45 | } 46 | 47 | int IFK_ErrorHandler(CPhidgetHandle IFK, void *userptr, int ErrorCode, const char *unknown) 48 | { 49 | printf("Error handler ran!\n"); 50 | return 0; 51 | } 52 | 53 | int IFK_OutputChangeHandler(CPhidgetInterfaceKitHandle IFK, void *userptr, int Index, int Value) 54 | { 55 | printf("Output %d is %d\n", Index, Value); 56 | return 0; 57 | } 58 | 59 | int IFK_InputChangeHandler(CPhidgetInterfaceKitHandle IFK, void *userptr, int Index, int Value) 60 | { 61 | printf("Input %d is %d\n", Index, Value); 62 | return 0; 63 | } 64 | 65 | int IFK_SensorChangeHandler(CPhidgetInterfaceKitHandle IFK, void *userptr, int Index, int Value) 66 | { 67 | printf("Sensor %d is %d\n", Index, Value); 68 | return 0; 69 | } 70 | 71 | 72 | int IFK_AttachHandler(CPhidgetHandle IFK, void *userptr) 73 | { 74 | CPhidgetInterfaceKit_setSensorChangeTrigger((CPhidgetInterfaceKitHandle)IFK, 0, 0); 75 | printf("Attach handler ran!\n"); 76 | return 0; 77 | } 78 | 79 | int test_interfacekit() 80 | { 81 | int numInputs, numOutputs, numSensors; 82 | int err; 83 | CPhidgetInterfaceKitHandle IFK = 0; 84 | 85 | //CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL); 86 | 87 | CPhidgetInterfaceKit_create(&IFK); 88 | 89 | CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler, NULL); 90 | CPhidgetInterfaceKit_set_OnOutputChange_Handler(IFK, IFK_OutputChangeHandler, NULL); 91 | CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL); 92 | CPhidget_set_OnAttach_Handler((CPhidgetHandle)IFK, IFK_AttachHandler, NULL); 93 | CPhidget_set_OnDetach_Handler((CPhidgetHandle)IFK, IFK_DetachHandler, NULL); 94 | CPhidget_set_OnError_Handler((CPhidgetHandle)IFK, IFK_ErrorHandler, NULL); 95 | 96 | CPhidget_open((CPhidgetHandle)IFK, -1); 97 | 98 | //wait 5 seconds for attachment 99 | if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 5000)) != EPHIDGET_OK ) 100 | { 101 | const char *errStr; 102 | CPhidget_getErrorDescription(err, &errStr); 103 | printf("Error waiting for attachment: (%d): %s\n",err,errStr); 104 | goto exit; 105 | } 106 | 107 | display_generic_properties((CPhidgetHandle)IFK); 108 | CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle)IFK, &numOutputs); 109 | CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle)IFK, &numInputs); 110 | CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle)IFK, &numSensors); 111 | 112 | CPhidgetInterfaceKit_setOutputState((CPhidgetInterfaceKitHandle)IFK, 0, 1); 113 | 114 | printf("Sensors:%d Inputs:%d Outputs:%d\n", numSensors, numInputs, numOutputs); 115 | 116 | while(1) 117 | { 118 | CPhidgetInterfaceKit_setOutputState(IFK, 7, 1); 119 | CPhidgetInterfaceKit_setOutputState(IFK, 7, 0); 120 | } 121 | 122 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 1); 123 | sleep(1); 124 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 0); 125 | sleep(1); 126 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 1); 127 | sleep(1); 128 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 0); 129 | 130 | sleep(5); 131 | 132 | exit: 133 | CPhidget_close((CPhidgetHandle)IFK); 134 | CPhidget_delete((CPhidgetHandle)IFK); 135 | 136 | return 0; 137 | } 138 | 139 | int main(int argc, char* argv[]) 140 | { 141 | test_interfacekit(); 142 | return 0; 143 | } 144 | 145 | -------------------------------------------------------------------------------- /cob_phidgets/ros/src/phidget_manager.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 | 23 | PhidgetManager::PhidgetManager() 24 | : _manHandle(0) 25 | { 26 | CPhidgetManager_create(&_manHandle); 27 | CPhidgetManager_open((CPhidgetManagerHandle) _manHandle); 28 | 29 | sleep(2); 30 | } 31 | 32 | PhidgetManager::~PhidgetManager() 33 | { 34 | // Close the manager 35 | CPhidgetManager_close((CPhidgetManagerHandle) _manHandle); 36 | CPhidgetManager_delete((CPhidgetManagerHandle) _manHandle); 37 | 38 | usleep(500000); //0.5s 39 | } 40 | 41 | auto PhidgetManager::getAttachedDevices()-> std::vector 42 | { 43 | CPhidgetHandle* phidgetList; 44 | int count; 45 | ROS_INFO("getting attached Devices"); 46 | CPhidgetManager_getAttachedDevices((CPhidgetManagerHandle) _manHandle, &phidgetList, &count); 47 | 48 | std::vector attachedDevices; 49 | int serialNumber; 50 | const char *name; 51 | 52 | // Iterate over the returned Phidget data 53 | for (int i = 0; i < count; i++) { 54 | CPhidget_getDeviceName(phidgetList[i], &name); 55 | CPhidget_getSerialNumber(phidgetList[i], &serialNumber); 56 | ROS_INFO("Found %s, with serial: %d", name, serialNumber); 57 | // Store name and serial number into a persistent variable 58 | AttachedDevice device{serialNumber, name}; 59 | attachedDevices.push_back(device); 60 | } 61 | 62 | // Use the Phidget API to free the memory in the phidgetList Array 63 | CPhidgetManager_freeAttachedDevicesArray(phidgetList); 64 | 65 | return attachedDevices; 66 | } 67 | -------------------------------------------------------------------------------- /cob_phidgets/ros/src/phidget_sensors_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | int main(int argc, char **argv) 23 | { 24 | //init ros 25 | ros::init(argc, argv, "cob_phidgets"); 26 | 27 | int freq; 28 | std::string update_mode; 29 | std::vector> phidgets; 30 | ros::NodeHandle nodeHandle; 31 | ros::NodeHandle nh("~"); 32 | std::map > phidget_params_map; 33 | std::map >::iterator phidget_params_map_itr; 34 | 35 | //get default params from server 36 | nh.param("frequency",freq, 30); 37 | nh.param("update_mode", update_mode, "polling"); 38 | 39 | //get board params from server 40 | XmlRpc::XmlRpcValue phidget_params; 41 | if(nh.getParam("boards",phidget_params)) 42 | { 43 | for(auto& board : phidget_params) 44 | { 45 | std::string board_name = board.first; 46 | ROS_DEBUG("Found Board with name '%s' in params", board_name.c_str()); 47 | if(!board.second.hasMember("serial_num")) 48 | { 49 | ROS_ERROR("Board '%s' has no serial_num param in phidget yaml config",board_name.c_str()); 50 | continue; 51 | } 52 | XmlRpc::XmlRpcValue board_desc = board.second; 53 | ros::NodeHandle boards_nh(nh, "boards"); 54 | ros::NodeHandle sensors_nh(boards_nh, board_name); 55 | XmlRpc::XmlRpcValue board_param; 56 | if(!sensors_nh.getParam("sensors", board_param)) 57 | { 58 | ROS_ERROR("Board '%s' has no sensors param in phidget yaml config",board_name.c_str()); 59 | continue; 60 | } 61 | phidget_params_map.insert(std::make_pair(board_desc["serial_num"], 62 | std::make_pair(board_name, board_param))); 63 | } 64 | } 65 | else 66 | { 67 | ROS_WARN("No params for the phidget boards on the Paramserver using default name/port values"); 68 | } 69 | 70 | //look for attached devices 71 | PhidgetManager* manager = new PhidgetManager(); 72 | auto devices = manager->getAttachedDevices(); 73 | delete manager; 74 | 75 | //set the update method 76 | PhidgetIK::SensingMode sensMode; 77 | if(update_mode == "event") 78 | sensMode = PhidgetIK::SensingMode::EVENT; 79 | else if(update_mode == "polling") 80 | sensMode = PhidgetIK::SensingMode::POLLING; 81 | else 82 | { 83 | ROS_WARN("Unknown update mode '%s' use polling instead", update_mode.c_str()); 84 | sensMode = PhidgetIK::SensingMode::POLLING; 85 | } 86 | 87 | //if no devices attached exit 88 | if(devices.size() > 0) 89 | { 90 | //loop over all found devices and init them 91 | for(auto& device : devices) 92 | { 93 | phidget_params_map_itr = phidget_params_map.find(device.serial_num); 94 | XmlRpc::XmlRpcValue *sensors_param = (phidget_params_map_itr != phidget_params_map.end()) ? &((*phidget_params_map_itr).second.second) : nullptr; 95 | std::string name; 96 | if(sensors_param != nullptr) 97 | name = (*phidget_params_map_itr).second.first; 98 | else 99 | { 100 | ROS_WARN("Could not find parameters for Board with serial: %d. Using default params!", device.serial_num); 101 | std::stringstream ss; ss << device.serial_num; 102 | name = ss.str(); 103 | } 104 | phidgets.push_back( 105 | std::make_shared(nodeHandle, device.serial_num, name, sensors_param, sensMode)); 106 | } 107 | 108 | ros::Rate loop_rate(freq); 109 | while (ros::ok()) 110 | { 111 | //update devices if update method is polling 112 | if(sensMode == PhidgetIK::SensingMode::POLLING) 113 | { 114 | for(auto& phidget : phidgets) 115 | { 116 | phidget->update(); 117 | } 118 | } 119 | ros::spinOnce(); 120 | loop_rate.sleep(); 121 | } 122 | } 123 | else 124 | { 125 | ROS_ERROR("Phidget Manager could not find any attached devices"); 126 | } 127 | 128 | return 0; 129 | } 130 | -------------------------------------------------------------------------------- /cob_phidgets/ros/src/simulated_range_sensors.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 sensor_msgs.msg import * 20 | 21 | class GazeboVirtualRangeSensor(): 22 | 23 | def __init__(self, laser_topic, range_topic): 24 | rospy.Subscriber(laser_topic, LaserScan, self.laser_callback) 25 | # rospy.logdebug("subscribed to laser topic: %s", laser_topic) 26 | self.pub = rospy.Publisher(range_topic, Range, queue_size=1) 27 | # rospy.loginfo("topic '" + range_topic + "' advertised") 28 | rospy.loginfo("adding sensor converting from topic '" + laser_topic + "' to topic '" + range_topic + "'") 29 | 30 | def laser_callback(self, msg): 31 | sensor_range = Range() 32 | 33 | # set header 34 | sensor_range.header = msg.header 35 | 36 | # set range from average out of laser message 37 | if len(msg.ranges) > 0: 38 | sensor_range.range = sum(msg.ranges)/len(msg.ranges) 39 | else: 40 | sensor_range.range = 0 41 | 42 | # set min and max range 43 | sensor_range.min_range = msg.range_min 44 | sensor_range.max_range = msg.range_max 45 | 46 | # set field of view 47 | sensor_range.field_of_view = msg.angle_max - msg.angle_min 48 | 49 | # set radiation type to IR (=1) 50 | sensor_range.radiation_type = 1 51 | 52 | # publish range 53 | self.pub.publish(sensor_range) 54 | 55 | if __name__ == "__main__": 56 | rospy.init_node('tactile_sensors') 57 | rospy.sleep(0.5) 58 | 59 | if not rospy.has_param('~sensors'): 60 | rospy.logerr("no sensors specified") 61 | exit(1) 62 | 63 | sensors = rospy.get_param('~sensors') 64 | 65 | if type(sensors) != list: 66 | rospy.logerr("sensors parameter not a list") 67 | exit(1) 68 | 69 | for sensor in sensors: 70 | GazeboVirtualRangeSensor(sensor['laser_topic'], sensor['range_topic']) 71 | 72 | rospy.spin() 73 | -------------------------------------------------------------------------------- /cob_phidgets/ros/src/tablet_leer.c: -------------------------------------------------------------------------------- 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 | // InterfacekitTest.cpp : Defines the entry point for the console application. 19 | // 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | int wert[100]; 27 | int i; 28 | 29 | void display_generic_properties(CPhidgetHandle phid) 30 | { 31 | int sernum, version; 32 | const char *deviceptr, *label; 33 | CPhidget_getDeviceType(phid, &deviceptr); 34 | CPhidget_getSerialNumber(phid, &sernum); 35 | CPhidget_getDeviceVersion(phid, &version); 36 | CPhidget_getDeviceLabel(phid, &label); 37 | 38 | printf("%s\n", deviceptr); 39 | printf("Version: %8d SerialNumber: %10d\n", version, sernum); 40 | printf("Label: %s\n", label); 41 | return; 42 | } 43 | 44 | 45 | int IFK_SensorChangeHandler(CPhidgetInterfaceKitHandle IFK, void *userptr, int Index, int Value) 46 | { 47 | 48 | //printf("Sensor %d is %d\n", Index, Value); 49 | if (i <10) 50 | { 51 | CPhidgetInterfaceKit_getSensorValue(IFK,0, &wert[0+(10*i)]); 52 | //printf("Sensor0 manually is %d\n", wert[0]); 53 | CPhidgetInterfaceKit_getSensorValue(IFK,1, &wert[1+(10*i)]); 54 | //printf("Sensor0 manually is %d\n", wert[0]); 55 | CPhidgetInterfaceKit_getSensorValue(IFK,2, &wert[2+(10*i)]); 56 | //printf("Sensor0 manually is %d\n", wert[0]); 57 | CPhidgetInterfaceKit_getSensorValue(IFK,3, &wert[3+(10*i)]); 58 | //printf("Sensor0 manually is %d\n", wert[0]); 59 | } 60 | 61 | else 62 | { 63 | 64 | //CPhidgetInterfaceKit_getSensorValue(IFK,1, &wert[1]); 65 | //printf("Sensor1 manually is %d\n", wert[1]); 66 | //CPhidgetInterfaceKit_getSensorValue(IFK,2, &wert[2]); 67 | //printf("Sensor2 manually is %d\n", wert[2]); 68 | //CPhidgetInterfaceKit_getSensorValue(IFK,3, &wert[3]); 69 | //printf("Sensor0123 manually are %d %d %d %d ; \n", wert[0],wert[1],wert[2],wert[3]); 70 | wert[94] = ((wert[0] + wert[10]+wert[20]+wert[30]+wert[40]+wert[50]+wert[60]+wert[70]+wert[80]+wert[90]) / 10); 71 | wert[95] = ((wert[1] + wert[11]+wert[21]+wert[31]+wert[41]+wert[51]+wert[61]+wert[71]+wert[81]+wert[91]) / 10); 72 | wert[96] = ((wert[2] + wert[12]+wert[22]+wert[32]+wert[42]+wert[52]+wert[62]+wert[72]+wert[82]+wert[92]) / 10); 73 | wert[97] = ((wert[3] + wert[13]+wert[23]+wert[33]+wert[43]+wert[53]+wert[63]+wert[73]+wert[83]+wert[93]) / 10); 74 | 75 | if ((wert[94] < 120) && (wert[95] <300) && (wert[96] < 110) && (wert[97] < 120)) 76 | { 77 | printf("Sensor0123 manually are %d %d %d %d ; \n", wert[94],wert[95],wert[96],wert[97]); 78 | printf("tablet empty\n"); 79 | } 80 | else 81 | { 82 | printf("Sensor0123 manually are %d %d %d %d ; \n", wert[94],wert[95],wert[96],wert[97]); 83 | printf("tablet NOT empty!\n"); 84 | } 85 | 86 | for( i = 0; i < 100; i++) 87 | wert[i]=0; 88 | i =0; 89 | } 90 | i++; 91 | printf("i is %d \n",i); 92 | return 0; 93 | } 94 | 95 | 96 | int IFK_AttachHandler(CPhidgetHandle IFK, void *userptr) 97 | { 98 | //CPhidgetInterfaceKit_setSensorChangeTrigger((CPhidgetInterfaceKitHandle)IFK, 0, 0); 99 | printf("Attach handler ran!\n"); 100 | return 0; 101 | } 102 | 103 | int test_interfacekit() 104 | { 105 | int numInputs, numOutputs, numSensors; 106 | int err; 107 | 108 | CPhidgetInterfaceKitHandle IFK = 0; 109 | CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL); 110 | CPhidgetInterfaceKit_create(&IFK); 111 | CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL); 112 | CPhidget_set_OnAttach_Handler((CPhidgetHandle)IFK, IFK_AttachHandler, NULL); 113 | //opening phidget 114 | CPhidget_open((CPhidgetHandle)IFK, -1); 115 | 116 | //wait 5 seconds for attachment 117 | if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 0)) != EPHIDGET_OK ) 118 | { 119 | const char *errStr; 120 | CPhidget_getErrorDescription(err, &errStr); 121 | printf("Error waiting for attachment: (%d): %s\n",err,errStr); 122 | goto exit; 123 | } 124 | 125 | display_generic_properties((CPhidgetHandle)IFK); 126 | CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle)IFK, &numOutputs); 127 | CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle)IFK, &numInputs); 128 | CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle)IFK, &numSensors); 129 | CPhidgetInterfaceKit_setOutputState((CPhidgetInterfaceKitHandle)IFK, 0, 1); 130 | 131 | printf("Sensors:%d Inputs:%d Outputs:%d\n", numSensors, numInputs, numOutputs); 132 | 133 | 134 | //err = CPhidget_setDeviceLabel((CPhidgetHandle)IFK, "test"); 135 | 136 | 137 | while(1) 138 | { 139 | sleep(1); 140 | } 141 | 142 | while(1) 143 | { 144 | CPhidgetInterfaceKit_setOutputState(IFK, 7, 1); 145 | CPhidgetInterfaceKit_setOutputState(IFK, 7, 0); 146 | } 147 | 148 | 149 | 150 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 1); 151 | sleep(1); 152 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 0); 153 | sleep(1); 154 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 1); 155 | sleep(1); 156 | CPhidgetInterfaceKit_setOutputState(IFK, 0, 0); 157 | 158 | sleep(5); 159 | 160 | exit: 161 | CPhidget_close((CPhidgetHandle)IFK); 162 | CPhidget_delete((CPhidgetHandle)IFK); 163 | 164 | return 0; 165 | } 166 | 167 | int main(int argc, char* argv[]) 168 | { 169 | test_interfacekit(); 170 | return 0; 171 | } 172 | 173 | -------------------------------------------------------------------------------- /cob_phidgets/srv/SetDataRate.srv: -------------------------------------------------------------------------------- 1 | int8 index 2 | uint16 data_rate 3 | -------------------------------------------------------------------------------- /cob_phidgets/srv/SetDigitalSensor.srv: -------------------------------------------------------------------------------- 1 | string uri 2 | int8 state 3 | --- 4 | string uri 5 | int8 state -------------------------------------------------------------------------------- /cob_phidgets/srv/SetTriggerValue.srv: -------------------------------------------------------------------------------- 1 | int8 index 2 | uint16 trigger_value -------------------------------------------------------------------------------- /cob_scan_unifier/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_scan_unifier) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | laser_geometry 6 | roscpp 7 | sensor_msgs 8 | tf 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS roscpp sensor_msgs tf 13 | ) 14 | 15 | ########### 16 | ## Build ## 17 | ########### 18 | 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | add_executable(scan_unifier_node src/scan_unifier_node.cpp) 25 | target_link_libraries(scan_unifier_node ${catkin_LIBRARIES}) 26 | add_dependencies(scan_unifier_node ${catkin_EXPORTED_TARGETS}) 27 | 28 | ############# 29 | ## Install ## 30 | ############# 31 | 32 | install(TARGETS scan_unifier_node 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | -------------------------------------------------------------------------------- /cob_scan_unifier/README.md: -------------------------------------------------------------------------------- 1 | cob\_scan\_unifier 2 | ==================== 3 | 4 | General description 5 | --------------------- 6 | This package implements a node that unifies scan messages from a given numer of laser scanners 7 | 8 | Node: scan\_unifier\_node 9 | --------------------- 10 | 11 | The actual node that unifies a given number of laser scans 12 | #### Parameters 13 | **input\_scans** *(List of std::string)* 14 | The names of the scan topics to subscribe to as list of strings. 15 | 16 | **loop\_rate** *(double, default: 100.0 [hz])* 17 | The loop rate of the ros node. 18 | 19 | #### Published Topics 20 | **scan\_unified** *(sensor_msgs::LaserScan)* 21 | Publishes the unified scans. 22 | 23 | #### Subscribed Topics 24 | **input\_scan\_name** *(sensor_msgs::LaserScan)* 25 | The current scan message from the laser scanner with topic name specified via the parameter **input\_scan\_topics** 26 | 27 | 28 | #### Services 29 | 30 | 31 | #### Services called 32 | -------------------------------------------------------------------------------- /cob_scan_unifier/include/cob_scan_unifier/scan_unifier_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef SCAN_UNIFIER_NODE_H 19 | #define SCAN_UNIFIER_NODE_H 20 | 21 | //################## 22 | //#### includes #### 23 | 24 | // standard includes 25 | #include 26 | #include 27 | #include 28 | 29 | // ROS includes 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | // ROS message includes 41 | #include 42 | #include 43 | 44 | //#################### 45 | //#### node class #### 46 | class ScanUnifierNode 47 | { 48 | private: 49 | /** @struct config_struct 50 | * @brief This structure holds configuration parameters 51 | * @var config_struct::number_input_scans 52 | * Member 'number_input_scans' contains number of scanners to subscribe to 53 | * @var config_struct::loop_rate 54 | * Member 'loop_rate' contains the loop rate of the ros node 55 | * @var config_struct::input_scan_topics 56 | * Member 'input_scan_topics' contains the names of the input scan topics 57 | */ 58 | struct config_struct{ 59 | int number_input_scans; 60 | std::vector input_scan_topics; 61 | bool publish_pointcloud; 62 | }; 63 | 64 | config_struct config_; 65 | 66 | std::string frame_; 67 | 68 | std::vector* > message_filter_subscribers_; 69 | 70 | message_filters::Synchronizer >* synchronizer2_; 72 | message_filters::Synchronizer >* synchronizer3_; 75 | message_filters::Synchronizer >* synchronizer4_; 79 | 80 | void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, 81 | const sensor_msgs::LaserScan::ConstPtr& second_scanner); 82 | void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, 83 | const sensor_msgs::LaserScan::ConstPtr& second_scanner, 84 | const sensor_msgs::LaserScan::ConstPtr& third_scanner); 85 | void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, 86 | const sensor_msgs::LaserScan::ConstPtr& second_scanner, 87 | const sensor_msgs::LaserScan::ConstPtr& third_scanner, 88 | const sensor_msgs::LaserScan::ConstPtr& fourth_scanner); 89 | 90 | public: 91 | 92 | // constructor 93 | ScanUnifierNode(); 94 | 95 | // destructor 96 | ~ScanUnifierNode(); 97 | 98 | /* ----------------------------------- */ 99 | /* --------- ROS Variables ----------- */ 100 | /* ----------------------------------- */ 101 | 102 | // create node handles 103 | ros::NodeHandle nh_, pnh_; 104 | 105 | // declaration of ros publishers 106 | ros::Publisher topicPub_LaserUnified_; 107 | ros::Publisher topicPub_PointCloudUnified_; 108 | 109 | // tf listener 110 | tf::TransformListener listener_; 111 | 112 | // laser geometry projector 113 | laser_geometry::LaserProjection projector_; 114 | 115 | std::vector vec_cloud_; 116 | 117 | /* ----------------------------------- */ 118 | /* ----------- functions ------------- */ 119 | /* ----------------------------------- */ 120 | 121 | /** 122 | * @function getParams 123 | * @brief function to load parameters from ros parameter server 124 | * 125 | * input: - 126 | * output: - 127 | */ 128 | void getParams(); 129 | 130 | /** 131 | * @function unifieLaserScans 132 | * @brief unifie the scan information from all laser scans in vec_laser_struct_ 133 | * 134 | * input: - 135 | * output: 136 | * @param: a laser scan message containing unified information from all scanners 137 | */ 138 | void publish(sensor_msgs::LaserScan& unified_scan); 139 | bool unifyLaserScans(const std::vector& current_scans, 140 | sensor_msgs::LaserScan& unified_scan); 141 | }; 142 | #endif 143 | -------------------------------------------------------------------------------- /cob_scan_unifier/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cob_scan_unifier 4 | 0.8.18 5 | The cob_scan_unifier package holds code to unify two or more laser-scans to one unified scan-message 6 | 7 | Felix Messmer 8 | Benjamin Maidel 9 | Florian Mirus 10 | 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | laser_geometry 16 | roscpp 17 | sensor_msgs 18 | tf 19 | 20 | 21 | -------------------------------------------------------------------------------- /cob_sick_s300/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_sick_s300) 3 | 4 | find_package(catkin REQUIRED COMPONENTS diagnostic_msgs roscpp sensor_msgs std_msgs) 5 | 6 | find_package(Boost REQUIRED COMPONENTS date_time thread) 7 | 8 | catkin_package( 9 | CATKIN_DEPENDS diagnostic_msgs roscpp sensor_msgs std_msgs 10 | ) 11 | 12 | ### BUILD ### 13 | include_directories( 14 | common/include 15 | ${Boost_INCLUDE_DIRS} 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | 19 | add_executable(${PROJECT_NAME} 20 | common/src/ScannerSickS300.cpp 21 | common/src/SerialIO.cpp 22 | ros/src/${PROJECT_NAME}.cpp 23 | ) 24 | 25 | add_executable(cob_scan_filter ros/src/cob_scan_filter.cpp) 26 | 27 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 28 | add_dependencies(cob_scan_filter ${catkin_EXPORTED_TARGETS}) 29 | 30 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 31 | target_link_libraries(cob_scan_filter ${catkin_LIBRARIES}) 32 | 33 | ### INSTALL ### 34 | install(TARGETS cob_scan_filter ${PROJECT_NAME} 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(DIRECTORY ros/test 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 42 | ) 43 | -------------------------------------------------------------------------------- /cob_sick_s300/README.md: -------------------------------------------------------------------------------- 1 | # cob_sick_s300 2 | This package implements a driver for the Sick S300 Safety laser scanners. 3 | It provides an implementation for both, the old (1.40) and the new (2.10) protocol. 4 | Thus, the old Sick S300 Professional CMS as well as the new Sick S300 Expert are supported. 5 | 6 | However, it does not cover the full functionality of the protocol: 7 | - It only handles distance measurements properly 8 | - It only handles no or only one configured measurement range field properly 9 | - It does not handle I/O-data or reflector data 10 | (though it reads the reflector marker field in the distance measurements) 11 | 12 | See http://wiki.ros.org/cob_sick_s300 for more details. 13 | 14 | ## S300 Configuration 15 | Here are a few notes about how to best configure the S300: 16 | - Configure the RS422 output to 500kBaud (otherwise, the scanner only provides a lower frequency) 17 | - Configure the scanner to Continuous Data Output 18 | - Send data via one telegram 19 | - Only configure distances, no I/O or reflector data (otherwise, the scanner only provides a lower frequency). 20 | - Configuration of the measurement ranges 21 | - For protocol 1.40: only configure one measurement range field with the full range (-45° to 225°) with all values. 22 | - For protocol 2.10: do not configure a measurement range field 23 | (otherwise, the scanner only provides a lower frequency). 24 | - If you want to only use certain measurement ranges, do this on the ROS side using e.g. the `cob_scan_filter` 25 | located in this package as well. 26 | -------------------------------------------------------------------------------- /cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef SCANNERSICKS300_INCLUDEDEF_H 19 | #define SCANNERSICKS300_INCLUDEDEF_H 20 | //----------------------------------------------- 21 | 22 | // base classes 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | /** 34 | * Driver class for the laser scanner SICK S300 Professional. 35 | * This driver only supports use with 500KBaud in cont. mode 36 | * 37 | * if the scanner is in standby, the measurements are 0x4004 according to the Sick Support 38 | */ 39 | 40 | class ScannerSickS300 41 | { 42 | public: 43 | 44 | // set of parameters which are specific to the SickS300 45 | struct ParamType 46 | { 47 | int range_field; //measurement range (1 to 5) --> usually 1 (default) 48 | double dScale; // scaling of the scan (multiply with to get scan in meters) 49 | double dStartAngle; // scan start angle 50 | double dStopAngle; // scan stop angle 51 | }; 52 | 53 | // storage container for received scanner data 54 | struct ScanPolarType 55 | { 56 | double dr; // distance //r; 57 | double da; // angle //a; 58 | double di; // intensity; //bool bGlare; 59 | }; 60 | 61 | enum 62 | { 63 | SCANNER_S300_READ_BUF_SIZE = 10000, 64 | READ_BUF_SIZE = 10000, 65 | WRITE_BUF_SIZE = 10000 66 | }; 67 | 68 | // Constructor 69 | ScannerSickS300(); 70 | 71 | // Destructor 72 | ~ScannerSickS300(); 73 | 74 | /** 75 | * Opens serial port. 76 | * @param pcPort used "COMx" or "/dev/tty1" 77 | * @param iBaudRate baud rate 78 | * @param iScanId the scanner id in the data header (7 by default) 79 | */ 80 | bool open(const char* pcPort, int iBaudRate, int iScanId); 81 | 82 | // not implemented 83 | void resetStartup(); 84 | 85 | // not implmented 86 | void startScanner(); 87 | 88 | // not implemented 89 | void stopScanner(); 90 | //sick_lms.Uninitialize(); 91 | 92 | // whether the scanner is currently in Standby or not 93 | bool isInStandby() {return m_bInStandby;} 94 | 95 | void purgeScanBuf(); 96 | 97 | bool getScan(std::vector &vdDistanceM, std::vector &vdAngleRAD, std::vector &vdIntensityAU, unsigned int &iTimestamp, unsigned int &iTimeNow, const bool debug); 98 | 99 | void setRangeField(const int field, const ParamType ¶m) {m_Params[field] = param;} 100 | 101 | private: 102 | 103 | // Constants 104 | static const double c_dPi; 105 | 106 | // Parameters 107 | typedef std::map PARAM_MAP; 108 | PARAM_MAP m_Params; 109 | double m_dBaudMult; 110 | 111 | // Variables 112 | unsigned char m_ReadBuf[READ_BUF_SIZE+10]; 113 | unsigned char m_ReadBuf2[READ_BUF_SIZE+10]; 114 | unsigned int m_uiSumReadBytes; 115 | std::vector m_viScanRaw; 116 | int m_iPosReadBuf2; 117 | static unsigned char m_iScanId; 118 | int m_actualBufferSize; 119 | bool m_bInStandby; 120 | 121 | // Components 122 | SerialIO m_SerialIO; 123 | TelegramParser tp_; 124 | 125 | // Functions 126 | void convertScanToPolar(const PARAM_MAP::const_iterator param, std::vector viScanRaw, 127 | std::vector& vecScanPolar); 128 | 129 | }; 130 | 131 | //----------------------------------------------- 132 | #endif 133 | -------------------------------------------------------------------------------- /cob_sick_s300/common/include/cob_sick_s300/SerialIO.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef _SerialIO_H 19 | #define _SerialIO_H 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | /** 28 | * Wrapper class for serial communication. 29 | */ 30 | class SerialIO 31 | { 32 | public: 33 | // ---------------------- Constants 34 | /// Constants for defining the handshake. 35 | enum HandshakeFlags 36 | { 37 | HS_NONE, 38 | HS_HARDWARE, 39 | HS_XONXOFF 40 | }; 41 | 42 | /// Constants for defining the parity bits. 43 | enum ParityFlags 44 | { 45 | PA_NONE, 46 | PA_EVEN, 47 | PA_ODD, 48 | // UNIX serial drivers only support even, odd, and no parity bit generation. 49 | PA_MARK, 50 | PA_SPACE 51 | }; 52 | 53 | /// Constants for defining the stop bits. 54 | enum StopBits 55 | { 56 | SB_ONE, 57 | SB_ONE_5, // ????? returns an error ????? 58 | SB_TWO 59 | }; 60 | 61 | /// Default constructor 62 | SerialIO(); 63 | 64 | /// Destructor 65 | virtual ~SerialIO(); 66 | 67 | /** 68 | * Sets the device name 69 | * @param Name 'COM1', 'COM2', ... 70 | */ 71 | void setDeviceName(const char *Name) { m_DeviceName = Name; } 72 | 73 | /** 74 | * Sets the baudrate. 75 | * @param BaudRate baudrate. 76 | */ 77 | void setBaudRate(int BaudRate) { m_BaudRate = BaudRate; } 78 | 79 | /** 80 | * Changes the baudrate. 81 | * The serial port is allready open. 82 | * @param BaudRate new baudrate. 83 | */ 84 | void changeBaudRate(int BaudRate); 85 | 86 | /** 87 | * Sets a multiplier for the baudrate. 88 | * Some serial cards need a specific multiplier for the baudrate. 89 | * @param Multiplier default is one. 90 | */ 91 | void setMultiplier(double Multiplier = 1) { m_Multiplier = Multiplier; }; 92 | 93 | /** 94 | * Sets the message format. 95 | */ 96 | void SetFormat(int ByteSize, ParityFlags Parity, int StopBits) 97 | { m_ByteSize = ByteSize; m_Parity = Parity; m_StopBits = StopBits; } 98 | 99 | /** 100 | * Defines the handshake type. 101 | */ 102 | void setHandshake(HandshakeFlags Handshake) { m_Handshake = Handshake; } 103 | 104 | /** 105 | * Sets the buffer sizes. 106 | * @param ReadBufSize number of bytes of the read buffer. 107 | * @param WriteBufSize number of bytes of the write buffer. 108 | */ 109 | void setBufferSize(int ReadBufSize, int WriteBufSize) 110 | { m_ReadBufSize = ReadBufSize; m_WriteBufSize = WriteBufSize; } 111 | 112 | /** 113 | * Sets the timeout. 114 | * @param Timeout in seconds 115 | */ 116 | void setTimeout(double Timeout); 117 | 118 | /** 119 | * Sets the byte period for transmitting bytes. 120 | * If the period is not equal to 0, the transmit will be repeated with the given 121 | * period until all bytes are transmitted. 122 | * @param default is 0. 123 | */ 124 | void setBytePeriod(double Period); 125 | 126 | /** 127 | * Opens serial port. 128 | * The port has to be configured before. 129 | */ 130 | int openIO(); 131 | 132 | /** 133 | * Closes the serial port. 134 | */ 135 | void closeIO(); 136 | 137 | /** 138 | * Reads the serial port blocking. 139 | * The function blocks until the requested number of bytes have been 140 | * read or the timeout occurs. 141 | * @param Buffer pointer to the buffer. 142 | * @param Length number of bytes to read 143 | */ 144 | int readBlocking(char *Buffer, int Length); 145 | 146 | 147 | /** 148 | * Reads the serial port non blocking. 149 | * The function returns all avaiable bytes but not more than requested. 150 | * @param Buffer pointer to the buffer. 151 | * @param Length number of bytes to read 152 | */ 153 | int readNonBlocking(char *Buffer, int Length); 154 | 155 | /** 156 | * Writes bytes to the serial port. 157 | * @param Buffer buffer of the message 158 | * @param Length number of bytes to send 159 | */ 160 | int writeIO(const char *Buffer, int Length); 161 | 162 | /** 163 | * Returns the number of bytes available in the read buffer. 164 | */ 165 | int getSizeRXQueue(); 166 | 167 | 168 | /** Clears the read and transmit buffer. 169 | */ 170 | void purge() 171 | { 172 | ::tcflush(m_Device, TCIOFLUSH); 173 | } 174 | 175 | /** Clears the read buffer. 176 | */ 177 | void purgeRx() { 178 | tcflush(m_Device, TCIFLUSH); 179 | } 180 | 181 | /** 182 | * Clears the transmit buffer. 183 | * The content of the buffer will not be transmitted. 184 | */ 185 | void purgeTx() { 186 | tcflush(m_Device, TCOFLUSH); 187 | } 188 | 189 | /** 190 | * Sends the transmit buffer. 191 | * All bytes of the transmit buffer will be sent. 192 | */ 193 | void flushTx() { 194 | tcdrain(m_Device); 195 | } 196 | 197 | protected: 198 | ::termios m_tio; 199 | std::string m_DeviceName; 200 | int m_Device; 201 | int m_BaudRate; 202 | double m_Multiplier; 203 | int m_ByteSize, m_StopBits; 204 | ParityFlags m_Parity; 205 | HandshakeFlags m_Handshake; 206 | int m_ReadBufSize, m_WriteBufSize; 207 | double m_Timeout; 208 | ::timeval m_BytePeriod; 209 | bool m_ShortBytePeriod; 210 | }; 211 | 212 | 213 | #endif // 214 | 215 | -------------------------------------------------------------------------------- /cob_sick_s300/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_sick_s300 3 | 0.8.18 4 | This package published a laser scan message out of a Sick S300 laser scanner. 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_sick_s300 9 | 10 | 11 | Felix Messmer 12 | Florian Weisshardt 13 | 14 | catkin 15 | 16 | boost 17 | diagnostic_msgs 18 | roscpp 19 | sensor_msgs 20 | std_msgs 21 | 22 | 23 | -------------------------------------------------------------------------------- /cob_sick_s300/ros/test/dummy_config.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyScan0 2 | baud: 500000 3 | scan_duration: 0.025 #no info about that in SICK-docu, but 0.025 is believable and looks good in rviz 4 | scan_cycle_time: 0.040 #SICK-docu says S300 scans every 40ms 5 | inverted: true 6 | scan_id: 7 7 | frame_id: /base_laser_front_link 8 | scan_intervals: [[-1.3526, 1.361357]] #[rad] these intervals are included to the scan 9 | fields: 10 | '1': 11 | scale: 1.0 12 | start_angle: -1.0 13 | stop_angle: 2.0 14 | '2': 15 | scale: 2.0 16 | start_angle: -11.0 17 | stop_angle: 12.0 18 | debug: true 19 | -------------------------------------------------------------------------------- /cob_sick_s300/ros/test/dummy_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /cob_sound/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cob_sound) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs cob_srvs diagnostic_msgs message_generation roscpp std_msgs std_srvs visualization_msgs) 5 | find_package(PkgConfig REQUIRED) 6 | pkg_check_modules(libvlc REQUIRED libvlc) 7 | 8 | add_action_files(DIRECTORY action FILES 9 | Play.action 10 | Say.action 11 | ) 12 | 13 | generate_messages( 14 | DEPENDENCIES actionlib_msgs 15 | ) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS actionlib actionlib_msgs cob_srvs diagnostic_msgs message_runtime roscpp std_msgs std_srvs visualization_msgs 19 | ) 20 | 21 | ### BUILD ### 22 | include_directories(${catkin_INCLUDE_DIRS}) 23 | 24 | add_executable(sound ros/src/sound.cpp) 25 | add_dependencies(sound ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 26 | target_link_libraries(sound ${catkin_LIBRARIES} ${libvlc_LIBRARIES}) 27 | 28 | ### INSTALL ### 29 | install(TARGETS sound 30 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 33 | ) 34 | 35 | catkin_install_python(PROGRAMS ros/src/test_client.py 36 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | ) 38 | 39 | install(PROGRAMS fix_swift_for_precise.sh 40 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | -------------------------------------------------------------------------------- /cob_sound/action/Play.action: -------------------------------------------------------------------------------- 1 | # goal 2 | string filename #the fully qualified filename including file ending, example: "/home/user/sounds/hello.wav" 3 | --- 4 | #result 5 | bool success 6 | string message 7 | --- 8 | #feedback 9 | float32 position #the current position in the media file as percentage between 0.0 and 1.0 10 | int64 time #the current time in the media file (in ms) 11 | -------------------------------------------------------------------------------- /cob_sound/action/Say.action: -------------------------------------------------------------------------------- 1 | # goal 2 | string text 3 | --- 4 | #result 5 | bool success 6 | string message 7 | --- 8 | #feedback 9 | -------------------------------------------------------------------------------- /cob_sound/fix_swift_for_precise.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo touch -f /usr/local/bin/swift_fix 4 | sudo echo "#!/bin/bash" > /usr/local/bin/swift_fix 5 | sudo echo 'if test -x /usr/bin/padsp ; then exec /usr/bin/padsp /opt/swift/bin/swift "$@" ; else exec /opt/swift/bin/swift "$@" ; fi' >> /usr/local/bin/swift_fix 6 | sudo chmod +x /usr/local/bin/swift_fix 7 | sudo ln -sf /usr/local/bin/swift_fix /usr/local/bin/swift 8 | -------------------------------------------------------------------------------- /cob_sound/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cob_sound 3 | 0.8.18 4 | This package implements a sound play module using text2wave and aplay through python. 5 | 6 | Apache 2.0 7 | 8 | http://ros.org/wiki/cob_sound 9 | 10 | 11 | Felix Messmer 12 | Benjamin Maidel 13 | Florian Weisshardt 14 | Benjamin Maidel 15 | 16 | catkin 17 | 18 | message_generation 19 | message_runtime 20 | 21 | actionlib 22 | actionlib_msgs 23 | cob_srvs 24 | diagnostic_msgs 25 | libvlc-dev 26 | roscpp 27 | std_msgs 28 | std_srvs 29 | visualization_msgs 30 | vlc 31 | 32 | alsa-oss 33 | rospy 34 | 35 | 36 | -------------------------------------------------------------------------------- /cob_sound/ros/src/test_client.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 actionlib 20 | from cob_sound.msg import * 21 | 22 | def say_client(): 23 | client = actionlib.SimpleActionClient('say', SayAction) 24 | client.wait_for_server() 25 | 26 | # Creates a goal to send to the action server. 27 | goal = SayGoal() 28 | goal.text = "Hello, how are you? I am fine." 29 | 30 | # Sends the goal to the action server. 31 | client.send_goal(goal) 32 | 33 | # Waits for the server to finish performing the action. 34 | client.wait_for_result() 35 | 36 | # Prints out the result of executing the action 37 | return client.get_result() 38 | rospy.loginfo("Say action finished") 39 | 40 | if __name__ == '__main__': 41 | try: 42 | rospy.init_node('say_client') 43 | result = say_client() 44 | except rospy.ROSInterruptException: 45 | print("program interrupted before completion") 46 | 47 | -------------------------------------------------------------------------------- /laser_scan_densifier/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package laser_scan_densifier 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.17 (2024-04-18) 6 | ------------------- 7 | 8 | 0.8.18 (2024-08-05) 9 | ------------------- 10 | 11 | 0.8.17 (2024-04-30) 12 | ------------------- 13 | * 0.7.17 14 | * update changelogs 15 | * Contributors: fmessmer 16 | 17 | 0.7.16 (2024-02-20) 18 | ------------------- 19 | 20 | 0.7.15 (2023-11-06) 21 | ------------------- 22 | 23 | 0.7.14 (2022-11-17) 24 | ------------------- 25 | 26 | 0.7.13 (2022-07-29) 27 | ------------------- 28 | 29 | 0.7.12 (2022-03-15) 30 | ------------------- 31 | 32 | 0.7.11 (2022-01-12) 33 | ------------------- 34 | 35 | 0.7.10 (2021-12-23) 36 | ------------------- 37 | 38 | 0.7.9 (2021-11-26) 39 | ------------------ 40 | 41 | 0.7.8 (2021-10-19) 42 | ------------------ 43 | 44 | 0.7.7 (2021-08-02) 45 | ------------------ 46 | 47 | 0.7.6 (2021-05-10) 48 | ------------------ 49 | 50 | 0.7.5 (2021-04-06) 51 | ------------------ 52 | 53 | 0.7.4 (2020-10-14) 54 | ------------------ 55 | * Merge pull request `#417 `_ from fmessmer/test_noetic 56 | test noetic 57 | * Bump CMake version to avoid CMP0048 warning 58 | * Contributors: Felix Messmer, fmessmer 59 | 60 | 0.7.3 (2020-03-18) 61 | ------------------ 62 | 63 | 0.7.2 (2020-03-18) 64 | ------------------ 65 | * Merge pull request `#408 `_ from fmessmer/ci_updates 66 | [travis] ci updates 67 | * catkin_lint fixes 68 | * Merge pull request `#403 `_ from fmessmer/fix/compiler_warning 69 | fix format specifiers 70 | * clearer output for misconfiguration 71 | * fix format specifiers 72 | * Contributors: Felix Messmer, fmessmer 73 | 74 | 0.7.1 (2019-11-07) 75 | ------------------ 76 | 77 | 0.7.0 (2019-08-06) 78 | ------------------ 79 | * Merge pull request `#396 `_ from HannesBachter/indigo_dev 80 | 0.6.15 81 | * Contributors: Felix Messmer 82 | 83 | 0.6.15 (2019-07-17) 84 | ------------------- 85 | 86 | 0.6.14 (2019-06-07) 87 | ------------------- 88 | 89 | 0.6.13 (2019-03-14) 90 | ------------------- 91 | * Merge pull request `#383 `_ from fmessmer/laser_densifier 92 | add laser scan densifier 93 | * support copy and interpolation mode 94 | * test interpolated densification 95 | * add laser_scan_densifier 96 | * Contributors: Felix Messmer, fmessmer 97 | -------------------------------------------------------------------------------- /laser_scan_densifier/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(laser_scan_densifier) 3 | 4 | # List C++ dependencies on ros packages 5 | set(ROS_CXX_DEPENDENCIES 6 | nodelet 7 | roscpp 8 | sensor_msgs 9 | ) 10 | 11 | # Find catkin and all required ROS components 12 | find_package(catkin REQUIRED COMPONENTS ${ROS_CXX_DEPENDENCIES}) 13 | 14 | # Set include directories 15 | include_directories(include ${catkin_INCLUDE_DIRS}) 16 | 17 | # Declare info that other packages need to import library generated here 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES ${PROJECT_NAME} 21 | CATKIN_DEPENDS ${ROS_CXX_DEPENDENCIES} 22 | ) 23 | 24 | #Create library 25 | add_library(${PROJECT_NAME} src/${PROJECT_NAME}.cpp) 26 | target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES}) 27 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 28 | 29 | #Create nodelet 30 | add_library(${PROJECT_NAME}_nodelet src/${PROJECT_NAME}_nodelet.cpp) 31 | target_link_libraries(${PROJECT_NAME}_nodelet ${PROJECT_NAME}) 32 | 33 | #Create node 34 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) 35 | target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ) 36 | 37 | #Install library 38 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_nodelet 39 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 41 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 42 | 43 | #Install library includes 44 | install(DIRECTORY include/${PROJECT_NAME}/ 45 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) 46 | 47 | #Install node 48 | install(TARGETS ${PROJECT_NAME}_node 49 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) 50 | 51 | #Install nodelet description 52 | install(FILES ${PROJECT_NAME}_nodelet.xml 53 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) 54 | -------------------------------------------------------------------------------- /laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | /* 17 | * Copyright (c) 2011, Ivan Dryanovski 18 | * All rights reserved. 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions are met: 22 | * 23 | * * Redistributions of source code must retain the above copyright 24 | * notice, this list of conditions and the following disclaimer. 25 | * * Redistributions in binary form must reproduce the above copyright 26 | * notice, this list of conditions and the following disclaimer in the 27 | * documentation and/or other materials provided with the distribution. 28 | * * Neither the name of the CCNY Robotics Lab nor the names of its 29 | * contributors may be used to endorse or promote products derived from 30 | * this software without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 36 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | */ 44 | 45 | #ifndef LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_H 46 | #define LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_H 47 | 48 | #include 49 | #include 50 | 51 | namespace scan_tools { 52 | 53 | class LaserScanDensifier 54 | { 55 | public: 56 | 57 | LaserScanDensifier(ros::NodeHandle nh, ros::NodeHandle nh_private); 58 | virtual ~LaserScanDensifier(); 59 | 60 | private: 61 | 62 | // **** ROS-related 63 | ros::NodeHandle nh_; 64 | ros::NodeHandle nh_private_; 65 | ros::Subscriber scan_subscriber_; 66 | ros::Publisher scan_publisher_; 67 | 68 | // **** paramaters 69 | 70 | int step_; 71 | int mode_; 72 | 73 | // **** member functions 74 | 75 | void scanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg); 76 | }; 77 | 78 | } //namespace scan_tools 79 | 80 | #endif // LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_H 81 | -------------------------------------------------------------------------------- /laser_scan_densifier/include/laser_scan_densifier/laser_scan_densifier_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | /* 17 | * Copyright (c) 2011, Ivan Dryanovski 18 | * All rights reserved. 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions are met: 22 | * 23 | * * Redistributions of source code must retain the above copyright 24 | * notice, this list of conditions and the following disclaimer. 25 | * * Redistributions in binary form must reproduce the above copyright 26 | * notice, this list of conditions and the following disclaimer in the 27 | * documentation and/or other materials provided with the distribution. 28 | * * Neither the name of the CCNY Robotics Lab nor the names of its 29 | * contributors may be used to endorse or promote products derived from 30 | * this software without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 36 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | */ 44 | 45 | #ifndef LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_NODELET_H 46 | #define LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_NODELET_H 47 | 48 | #include 49 | #include 50 | 51 | #include "laser_scan_densifier/laser_scan_densifier.h" 52 | 53 | namespace scan_tools { 54 | 55 | class LaserScanDensifierNodelet : public nodelet::Nodelet 56 | { 57 | public: 58 | virtual void onInit(); 59 | 60 | private: 61 | boost::shared_ptr< LaserScanDensifier > laser_scan_densifier_; 62 | }; 63 | 64 | } //namespace scan_tools 65 | 66 | #endif // LASER_SCAN_DENSIFIER_LASER_SCAN_DENSIFIER_NODELET_H 67 | -------------------------------------------------------------------------------- /laser_scan_densifier/laser_scan_densifier_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | Laser scan densifier nodelet publisher. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /laser_scan_densifier/package.xml: -------------------------------------------------------------------------------- 1 | 2 | laser_scan_densifier 3 | 0.8.18 4 | 5 | The laser_scan_densifier takes in a LaserScan message and densifies it. 6 | Node is inspired by laser_scan_sparsifier (http://wiki.ros.org/laser_scan_sparsifier). 7 | 8 | Felix Messmer 9 | Felix Messmer 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | roscpp 16 | nodelet 17 | sensor_msgs 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /laser_scan_densifier/src/laser_scan_densifier.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 | * Copyright (c) 2011, Ivan Dryanovski, William Morris 18 | * All rights reserved. 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions are met: 22 | * 23 | * * Redistributions of source code must retain the above copyright 24 | * notice, this list of conditions and the following disclaimer. 25 | * * Redistributions in binary form must reproduce the above copyright 26 | * notice, this list of conditions and the following disclaimer in the 27 | * documentation and/or other materials provided with the distribution. 28 | * * Neither the name of the CCNY Robotics Lab nor the names of its 29 | * contributors may be used to endorse or promote products derived from 30 | * this software without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 36 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | */ 44 | 45 | #include "laser_scan_densifier/laser_scan_densifier.h" 46 | 47 | namespace scan_tools { 48 | 49 | LaserScanDensifier::LaserScanDensifier(ros::NodeHandle nh, ros::NodeHandle nh_private): 50 | nh_(nh), 51 | nh_private_(nh_private) 52 | { 53 | ROS_INFO ("Starting LaserScanDensifier"); 54 | 55 | // **** get paramters 56 | 57 | nh_private_.param("step", step_, 2); 58 | nh_private_.param("mode", mode_, 0); 59 | 60 | if (step_ <= 0) 61 | { 62 | ROS_ERROR("LaserScanDensifier has step parameter set to %d, must be > 0! Not initiating subscriptions...", step_); 63 | exit(-1); 64 | } 65 | 66 | switch(mode_) { 67 | case 0: ROS_INFO("LaserScanDensifier started with mode %d: copy data points", mode_); 68 | break; 69 | case 1: ROS_INFO("LaserScanDensifier started with mode %d: interpolate data points", mode_); 70 | break; 71 | default: ROS_WARN("LaserScanDensifier started with unsupported mode %d. Defaulting to mode 0: copy data points", mode_); 72 | mode_ = 0; 73 | break; 74 | } 75 | 76 | // **** advertise topics 77 | 78 | scan_publisher_ = nh_.advertise("scan_dense", 1); 79 | 80 | // **** subscribe to laser scan messages 81 | 82 | scan_subscriber_ = nh_.subscribe("scan", 1, &LaserScanDensifier::scanCallback, this); 83 | } 84 | 85 | LaserScanDensifier::~LaserScanDensifier () 86 | { 87 | ROS_INFO ("Destroying LaserScanDensifier"); 88 | } 89 | 90 | void LaserScanDensifier::scanCallback (const sensor_msgs::LaserScanConstPtr& scan_msg) 91 | { 92 | sensor_msgs::LaserScan::Ptr scan_dense; 93 | scan_dense = boost::make_shared(); 94 | 95 | // copy over equal fields 96 | 97 | scan_dense->header = scan_msg->header; 98 | scan_dense->range_min = scan_msg->range_min; 99 | scan_dense->range_max = scan_msg->range_max; 100 | scan_dense->angle_min = scan_msg->angle_min; 101 | scan_dense->angle_max = scan_msg->angle_max; 102 | scan_dense->angle_increment = scan_msg->angle_increment / step_; 103 | scan_dense->time_increment = scan_msg->time_increment; 104 | scan_dense->scan_time = scan_msg->scan_time; 105 | 106 | scan_dense->ranges.clear(); 107 | scan_dense->intensities.clear(); 108 | 109 | for (size_t i = 0; i < scan_msg->ranges.size()-1; i++) 110 | { 111 | switch (mode_) { 112 | case 0: { //copy data points 113 | scan_dense->ranges.insert(scan_dense->ranges.end(), step_, scan_msg->ranges[i]); 114 | scan_dense->intensities.insert(scan_dense->intensities.end(), step_, scan_msg->intensities[i]); 115 | break; 116 | } 117 | case 1: { //interpolate data points 118 | double delta_range = (scan_msg->ranges[i+1]-scan_msg->ranges[i])/step_; 119 | double delta_intensities = (scan_msg->intensities[i+1]-scan_msg->intensities[i])/step_; 120 | for (int k = 0; k < step_; k++) { 121 | scan_dense->ranges.insert(scan_dense->ranges.end(), 1, scan_msg->ranges[i]+k*delta_range); 122 | scan_dense->intensities.insert(scan_dense->intensities.end(), 1, scan_msg->intensities[i]+k*delta_intensities); 123 | } 124 | break; 125 | } 126 | } 127 | } 128 | // add angle_max data point 129 | scan_dense->ranges.push_back(scan_msg->ranges.back()); 130 | scan_dense->intensities.push_back(scan_msg->intensities.back()); 131 | 132 | scan_publisher_.publish(scan_dense); 133 | } 134 | 135 | } //namespace scan_tools 136 | 137 | -------------------------------------------------------------------------------- /laser_scan_densifier/src/laser_scan_densifier_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | /* 17 | * Copyright (c) 2011, Ivan Dryanovski, William Morris 18 | * All rights reserved. 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions are met: 22 | * 23 | * * Redistributions of source code must retain the above copyright 24 | * notice, this list of conditions and the following disclaimer. 25 | * * Redistributions in binary form must reproduce the above copyright 26 | * notice, this list of conditions and the following disclaimer in the 27 | * documentation and/or other materials provided with the distribution. 28 | * * Neither the name of the CCNY Robotics Lab nor the names of its 29 | * contributors may be used to endorse or promote products derived from 30 | * this software without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 36 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | */ 44 | 45 | #include "laser_scan_densifier/laser_scan_densifier.h" 46 | 47 | int main (int argc, char **argv) 48 | { 49 | ros::init (argc, argv, "LaserScanDensifier"); 50 | ros::NodeHandle nh; 51 | ros::NodeHandle nh_private("~"); 52 | scan_tools::LaserScanDensifier laser_scan_densifier(nh, nh_private); 53 | ros::spin (); 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /laser_scan_densifier/src/laser_scan_densifier_nodelet.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 | * Copyright (c) 2011, Ivan Dryanovski, William Morris 18 | * All rights reserved. 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions are met: 22 | * 23 | * * Redistributions of source code must retain the above copyright 24 | * notice, this list of conditions and the following disclaimer. 25 | * * Redistributions in binary form must reproduce the above copyright 26 | * notice, this list of conditions and the following disclaimer in the 27 | * documentation and/or other materials provided with the distribution. 28 | * * Neither the name of the CCNY Robotics Lab nor the names of its 29 | * contributors may be used to endorse or promote products derived from 30 | * this software without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 35 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 36 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 37 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 38 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 39 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 40 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 41 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | */ 44 | 45 | #include "laser_scan_densifier/laser_scan_densifier_nodelet.h" 46 | 47 | typedef scan_tools::LaserScanDensifierNodelet LaserScanDensifierNodelet; 48 | 49 | PLUGINLIB_EXPORT_CLASS(LaserScanDensifierNodelet, nodelet::Nodelet) 50 | 51 | void LaserScanDensifierNodelet::onInit () 52 | { 53 | NODELET_INFO("Initializing LaserScanDensifier Nodelet"); 54 | 55 | // TODO: Do we want the single threaded or multithreaded NH? 56 | ros::NodeHandle nh = getMTNodeHandle(); 57 | ros::NodeHandle nh_private = getMTPrivateNodeHandle(); 58 | 59 | laser_scan_densifier_ = boost::shared_ptr< LaserScanDensifier >(new LaserScanDensifier(nh, nh_private)); 60 | } 61 | --------------------------------------------------------------------------------