├── .github
    └── workflows
    │   ├── kinetic_xenial.yml
    │   ├── melodic_bionic.yml
    │   ├── noetic_focal.yml
    │   └── ros.yml
├── .gitignore
├── CHANGELOG.rst
├── CMakeLists.txt
├── CMakeLists_catkin.txt
├── LICENSE
├── README.md
├── app
    ├── alrosbridge.pml
    ├── manifest.xml
    └── run.sh
├── doc
    ├── Doxyfile
    └── source
    │   ├── ToggleDirective.py
    │   ├── api.rst
    │   ├── conf.py
    │   ├── index.rst
    │   ├── install.rst
    │   ├── next.rst
    │   ├── other.rst
    │   ├── start.rst
    │   ├── topics.rst
    │   └── trouble.rst
├── include
    └── naoqi_driver
    │   ├── converter
    │       └── converter.hpp
    │   ├── event
    │       └── event.hpp
    │   ├── message_actions.h
    │   ├── naoqi_driver.hpp
    │   ├── publisher
    │       └── publisher.hpp
    │   ├── recorder
    │       ├── globalrecorder.hpp
    │       └── recorder.hpp
    │   ├── service
    │       └── service.hpp
    │   ├── subscriber
    │       └── subscriber.hpp
    │   └── tools.hpp
├── launch
    └── naoqi_driver.launch
├── msg
    └── README
├── package.xml
├── qiproject.xml
├── share
    ├── boot_config.json
    ├── nao.rviz
    ├── pepper.rviz
    └── urdf
    │   ├── nao.urdf
    │   ├── pepper.urdf
    │   └── romeo.urdf
└── src
    ├── autoload_registration.cpp
    ├── converters
        ├── audio.cpp
        ├── audio.hpp
        ├── camera.cpp
        ├── camera.hpp
        ├── camera_info_definitions.hpp
        ├── converter_base.hpp
        ├── diagnostics.cpp
        ├── diagnostics.hpp
        ├── imu.cpp
        ├── imu.hpp
        ├── info.cpp
        ├── info.hpp
        ├── joint_state.cpp
        ├── joint_state.hpp
        ├── laser.cpp
        ├── laser.hpp
        ├── log.cpp
        ├── log.hpp
        ├── memory
        │   ├── bool.cpp
        │   ├── bool.hpp
        │   ├── float.cpp
        │   ├── float.hpp
        │   ├── int.cpp
        │   ├── int.hpp
        │   ├── string.cpp
        │   └── string.hpp
        ├── memory_list.cpp
        ├── memory_list.hpp
        ├── nao_footprint.hpp
        ├── odom.cpp
        ├── odom.hpp
        ├── sonar.cpp
        ├── sonar.hpp
        ├── touch.cpp
        └── touch.hpp
    ├── driver_authenticator.hpp
    ├── event
        ├── audio.cpp
        ├── audio.hpp
        ├── basic.hpp
        ├── basic.hxx
        ├── touch.cpp
        └── touch.hpp
    ├── external_registration.cpp
    ├── helpers
        ├── driver_helpers.cpp
        ├── driver_helpers.hpp
        ├── filesystem_helpers.hpp
        ├── naoqi_helpers.hpp
        ├── recorder_helpers.hpp
        └── transform_helpers.hpp
    ├── naoqi_driver.cpp
    ├── naoqi_env.hpp
    ├── publishers
        ├── basic.hpp
        ├── camera.cpp
        ├── camera.hpp
        ├── info.cpp
        ├── info.hpp
        ├── joint_state.cpp
        ├── joint_state.hpp
        ├── log.cpp
        ├── log.hpp
        ├── memory
        │   ├── bool.cpp
        │   ├── bool.hpp
        │   ├── float.cpp
        │   ├── float.hpp
        │   ├── int.cpp
        │   ├── int.hpp
        │   ├── string.cpp
        │   └── string.hpp
        ├── sonar.cpp
        └── sonar.hpp
    ├── recorder
        ├── basic.hpp
        ├── basic_event.hpp
        ├── camera.cpp
        ├── camera.hpp
        ├── diagnostics.cpp
        ├── diagnostics.hpp
        ├── globalrecorder.cpp
        ├── joint_state.cpp
        ├── joint_state.hpp
        ├── log.cpp
        ├── log.hpp
        ├── sonar.cpp
        └── sonar.hpp
    ├── ros_env.hpp
    ├── services
        ├── get_language.cpp
        ├── get_language.hpp
        ├── robot_config.cpp
        ├── robot_config.hpp
        ├── set_language.cpp
        └── set_language.hpp
    ├── subscribers
        ├── moveto.cpp
        ├── moveto.hpp
        ├── speech.cpp
        ├── speech.hpp
        ├── subscriber_base.hpp
        ├── teleop.cpp
        └── teleop.hpp
    └── tools
        ├── alvisiondefinitions.h
        ├── from_any_value.cpp
        ├── from_any_value.hpp
        ├── naoqi_image.hpp
        ├── robot_description.cpp
        └── robot_description.hpp


/.github/workflows/kinetic_xenial.yml:
--------------------------------------------------------------------------------
 1 | name: ros-kinetic-xenial
 2 | 
 3 | on:
 4 |   push
 5 | 
 6 | jobs:
 7 |   ros_kinetic_xenial_ci:
 8 |     name: kinetic (xenial)
 9 |     uses: ./.github/workflows/ros.yml
10 |     with:
11 |       ROS_DISTRO: kinetic
12 |       ROS_REPO: testing
13 |       OS_NAME: ubuntu
14 |       OS_CODE_NAME: xenial
15 |       ALLOW_FAIL: false


--------------------------------------------------------------------------------
/.github/workflows/melodic_bionic.yml:
--------------------------------------------------------------------------------
 1 | name: ros-melodic-bionic
 2 | 
 3 | on:
 4 |   push
 5 | 
 6 | jobs:
 7 |   ros_melodic_bionic_ci:
 8 |     name: melodic (bionic)
 9 |     uses: ./.github/workflows/ros.yml
10 |     with:
11 |       ROS_DISTRO: melodic
12 |       ROS_REPO: testing
13 |       OS_NAME: ubuntu
14 |       OS_CODE_NAME: bionic
15 |       ALLOW_FAIL: false


--------------------------------------------------------------------------------
/.github/workflows/noetic_focal.yml:
--------------------------------------------------------------------------------
 1 | name: ros-noetic-focal
 2 | 
 3 | on:
 4 |   push
 5 | 
 6 | jobs:
 7 |   ros_noetic_focal_ci:
 8 |     name: noetic (focal)
 9 |     uses: ./.github/workflows/ros.yml
10 |     with:
11 |       ROS_DISTRO: noetic
12 |       ROS_REPO: testing
13 |       OS_NAME: ubuntu
14 |       OS_CODE_NAME: focal
15 |       ALLOW_FAIL: false


--------------------------------------------------------------------------------
/.github/workflows/ros.yml:
--------------------------------------------------------------------------------
 1 | name: ros
 2 | 
 3 | on:
 4 |   workflow_call:
 5 |     inputs:
 6 |       ROS_DISTRO:
 7 |         required: true
 8 |         type: string
 9 |       ROS_REPO:
10 |         required: true
11 |         type: string
12 |       OS_NAME:
13 |         required: true
14 |         type: string
15 |       OS_CODE_NAME:
16 |         required: true
17 |         type: string
18 |       ALLOW_FAIL:
19 |         required: true
20 |         type: boolean
21 | 
22 | jobs:
23 |   ros_ci:
24 |     name: ROS
25 |     runs-on: ubuntu-latest
26 |     continue-on-error: ${{ inputs.ALLOW_FAIL }}
27 |     env:
28 |       CCACHE_DIR: "${{ github.workspace }}/.ccache"
29 |     steps:
30 |       - name: Check out the naoqi_driver repo
31 |         uses: actions/checkout@v2
32 |       - name: Fetch/store directory used by ccache
33 |         uses: actions/cache@v2
34 |         with:
35 |           path: ${{ env.CCACHE_DIR }}
36 |           key: ccache-${{ inputs.ROS_DISTRO }}-${{ inputs.ROS_REPO }}-${{github.run_id}}
37 |           restore-keys: |
38 |             ccache-${{ inputs.ROS_DISTRO }}-${{ inputs.ROS_REPO }}-
39 |       - name: Run industrial CI
40 |         uses: 'ros-industrial/industrial_ci@master'
41 |         env: ${{ inputs }}


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | build*
2 | doc/*
3 | !doc/source/*
4 | !doc/Doxyfile
5 | *.pro
6 | *.pro.user
7 | .vscode/


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | project(naoqi_driver)
  2 | cmake_minimum_required(VERSION 3.0.2)
  3 | 
  4 | # define source files that can be used by qibuild or catkin
  5 | set(
  6 |   CONVERTERS_SRC
  7 |   src/converters/audio.cpp
  8 |   src/converters/touch.cpp
  9 |   src/converters/camera.cpp
 10 |   src/converters/diagnostics.cpp
 11 |   src/converters/imu.cpp
 12 |   src/converters/info.cpp
 13 |   src/converters/joint_state.cpp
 14 |   src/converters/laser.cpp
 15 |   src/converters/memory_list.cpp
 16 |   src/converters/memory/bool.cpp
 17 |   src/converters/memory/int.cpp
 18 |   src/converters/memory/float.cpp
 19 |   src/converters/memory/string.cpp
 20 |   src/converters/sonar.cpp
 21 |   src/converters/log.cpp
 22 |   src/converters/odom.cpp
 23 |   )
 24 | set(
 25 |   TOOLS_SRC
 26 |   src/tools/robot_description.cpp
 27 |   src/tools/from_any_value.cpp
 28 |   )
 29 | 
 30 | set(
 31 |   PUBLISHER_SRC
 32 |   src/publishers/camera.cpp
 33 |   src/publishers/info.cpp
 34 |   src/publishers/joint_state.cpp
 35 |   src/publishers/log.cpp
 36 |   src/publishers/sonar.cpp
 37 |   )
 38 | 
 39 | set(
 40 |   SUBSCRIBER_SRC
 41 |   src/subscribers/teleop.cpp
 42 |   src/subscribers/moveto.cpp
 43 |   src/subscribers/speech.cpp
 44 |   )
 45 | 
 46 | set(
 47 |   SERVICES_SRC
 48 |   src/services/robot_config.cpp
 49 |   src/services/set_language.cpp
 50 |   src/services/get_language.cpp
 51 |   )
 52 | 
 53 | set(
 54 |   RECORDER_SRC
 55 |   src/recorder/camera.cpp
 56 |   src/recorder/diagnostics.cpp
 57 |   src/recorder/joint_state.cpp
 58 |   src/recorder/log.cpp
 59 |   src/recorder/sonar.cpp
 60 |   )
 61 | 
 62 | set(
 63 |   DRIVER_SRC
 64 |   src/naoqi_driver.cpp
 65 |   src/helpers/driver_helpers.cpp
 66 |   src/recorder/globalrecorder.cpp
 67 |   src/event/basic.hxx
 68 |   src/event/basic.hpp
 69 |   src/event/audio.cpp
 70 |   src/event/touch.cpp
 71 |   )
 72 | 
 73 | # use catkin if qibuild is not found
 74 | if(DEFINED qibuild_DIR)
 75 |   find_package(qibuild QUIET)
 76 | else()
 77 |   find_package(catkin QUIET)
 78 |   include(CMakeLists_catkin.txt)
 79 |   return()
 80 | endif()
 81 | 
 82 | find_package(qimodule)
 83 | 
 84 | include_directories( include )
 85 | 
 86 | qi_create_module(
 87 |   naoqi_driver_module
 88 |   src/autoload_registration.cpp
 89 |   SHARED
 90 | )
 91 | qi_stage_lib( naoqi_driver_module )
 92 | 
 93 | qi_create_lib(
 94 |   naoqi_driver
 95 |   SHARED
 96 |   ${DRIVER_SRC}
 97 |   ${CONVERTERS_SRC}
 98 |   ${PUBLISHER_SRC}
 99 |   ${SUBSCRIBER_SRC}
100 |   ${SERVICES_SRC}
101 |   ${RECORDER_SRC}
102 |   ${TOOLS_SRC}
103 | )
104 | qi_use_lib( naoqi_driver QICORE QI ROS )
105 | qi_stage_lib( naoqi_driver )
106 | 
107 | target_link_libraries(
108 |   naoqi_driver_module
109 |   naoqi_driver
110 | )
111 | 
112 | qi_create_bin( naoqi_driver_node src/external_registration.cpp)
113 | target_link_libraries(
114 |   naoqi_driver_node
115 |   naoqi_driver
116 | )
117 | 
118 | # install the urdf for runtime loading
119 | file(COPY "${CMAKE_CURRENT_SOURCE_DIR}/share/" DESTINATION "${QI_SDK_DIR}/${QI_SDK_SHARE}/")
120 | qi_install_data( share/)
121 | 


--------------------------------------------------------------------------------
/CMakeLists_catkin.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | # Add pre-processor compile flag indicating catkin
 3 | add_definitions( -DCATKIN_BUILD )
 4 | 
 5 | # CMake file that uses catkin
 6 | find_package(catkin COMPONENTS
 7 |   cv_bridge
 8 |   diagnostic_msgs
 9 |   diagnostic_updater
10 |   geometry_msgs
11 |   image_transport
12 |   kdl_parser
13 |   naoqi_bridge_msgs
14 |   naoqi_libqi
15 |   naoqi_libqicore
16 |   robot_state_publisher
17 |   rosbag_storage
18 |   rosconsole
19 |   rosgraph_msgs
20 |   sensor_msgs
21 |   tf2_geometry_msgs
22 |   tf2_msgs
23 |   tf2_ros
24 | )
25 | find_package(OpenCV)
26 | find_package(orocos_kdl)
27 | 
28 | add_definitions(-DLIBQI_VERSION=${naoqi_libqi_VERSION_MAJOR}${naoqi_libqi_VERSION_MINOR})
29 | 
30 | catkin_package(LIBRARIES naoqi_driver_module naoqi_driver)
31 | 
32 | include_directories( include ${catkin_INCLUDE_DIRS})
33 | 
34 | # create the different libraries
35 | add_library(
36 |   naoqi_driver_module
37 |   SHARED
38 |   src/autoload_registration.cpp
39 |   )
40 | target_link_libraries(naoqi_driver_module ${naoqi_libqi_LIBRARIES})
41 | install(TARGETS naoqi_driver_module DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
42 | 
43 | add_library(
44 |   naoqi_driver
45 |   SHARED
46 |   ${DRIVER_SRC}
47 |   ${CONVERTERS_SRC}
48 |   ${PUBLISHER_SRC}
49 |   ${SUBSCRIBER_SRC}
50 |   ${SERVICES_SRC}
51 |   ${RECORDER_SRC}
52 |   ${TOOLS_SRC}
53 | )
54 | target_link_libraries(
55 |   naoqi_driver
56 |   ${catkin_LIBRARIES}
57 |   ${OpenCV_LIBRARIES}
58 |   ${orocos_kdl_LIBRARIES}
59 |   )
60 | add_dependencies(naoqi_driver ${catkin_EXPORTED_TARGETS})
61 | install(TARGETS naoqi_driver DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
62 | 
63 | target_link_libraries(
64 |   naoqi_driver_module
65 |   naoqi_driver
66 | )
67 | 
68 | # create the binary of the bridge
69 | add_executable( naoqi_driver_node src/external_registration.cpp)
70 | target_link_libraries(
71 |   naoqi_driver_node
72 |   naoqi_driver
73 |   ${catkin_LIBRARIES}
74 | )
75 | install(TARGETS naoqi_driver_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
76 | 
77 | # install the urdf for runtime loading
78 | file(COPY "${CMAKE_CURRENT_SOURCE_DIR}/share" DESTINATION "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/")
79 | install(DIRECTORY share DESTINATION "${CATKIN_PACKAGE_SHARE_DESTINATION}")
80 | 
81 | # install the launch files
82 | install(DIRECTORY launch DESTINATION "${CATKIN_PACKAGE_SHARE_DESTINATION}")
83 | 
84 | # make sure there is a file describing a naoqi module
85 | file(WRITE ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/qi/module/naoqi_driver_module.mod "cpp")
86 | install(FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/qi/module/naoqi_driver_module.mod DESTINATION ${CATKIN_GLOBAL_SHARE_DESTINATION}/qi/module/)
87 | 


--------------------------------------------------------------------------------
/app/alrosbridge.pml:
--------------------------------------------------------------------------------
 1 | <?xml version='1.0' encoding='UTF-8'?>
 2 | <Package name="naoqi_driver" format_version="4">
 3 |     <Manifest src="manifest.xml" />
 4 |     <Resources>
 5 |         <File name="run" src="run.sh" />
 6 |     </Resources>
 7 |     <qibuild name="naoqi_driver">
 8 |     </qibuild>
 9 | </Package>
10 | 


--------------------------------------------------------------------------------
/app/manifest.xml:
--------------------------------------------------------------------------------
 1 | <?xml version='1.0' encoding='UTF-8'?>
 2 | <package version="0.0.2" uuid="naoqi_driver">
 3 |  <services>
 4 |   <!-- <service autorun="false" execStart="./run.sh" name="alrosbridge"/> -->
 5 |   <service autorun="true" execStart="run.sh" name="naoqi_driver"/>
 6 |  </services>
 7 |  <executableFiles>
 8 |   <file path="./run.sh"/>
 9 |   <!--  <file path="bin/alros_bin"/> -->
10 |  </executableFiles>
11 | </package>
12 | 


--------------------------------------------------------------------------------
/app/run.sh:
--------------------------------------------------------------------------------
1 | touch .catkin
2 | source setup.bash
3 | exec bin/naoqi_driver_node &>> naoqi_driver_node.log
4 | 


--------------------------------------------------------------------------------
/doc/source/ToggleDirective.py:
--------------------------------------------------------------------------------
 1 | # Directive that shows a toggle link between sections.
 2 | # Those sections cannot contain titles
 3 | # The first one is initially showed. Here is the syntax:
 4 | # .. toggle_table::
 5 | #     :arg1: text in button 1
 6 | #     :arg2: text in button 2
 7 | #
 8 | # .. toggle:: text in button 1
 9 | #
10 | #    some RST text
11 | #
12 | #.. toggle:: text in button 2
13 | #
14 | #    some more RST text
15 | import docutils
16 | from docutils import nodes
17 | from docutils.parsers.rst import directives, roles, states
18 | 
19 | def setup(app):
20 |     app.add_directive('toggle', ToggleDirective)
21 |     app.add_directive('toggle_table', ToggleTableDirective)
22 | 
23 | class ToggleDirective(docutils.parsers.rst.Directive):
24 |     """
25 |     Base class that will be used by the two toggle directives
26 |     """
27 |     required_arguments = 1
28 |     optional_arguments = 0
29 |     final_argument_whitespace = True
30 |     option_spec = {}
31 |     has_content = True
32 |     node_class = nodes.container
33 | 
34 |     def run(self):
35 |         self.assert_has_content()
36 |         text = '\n'.join(self.content)
37 |         # Create the node, to be populated by `nested_parse`.
38 |         node = self.node_class(rawsource=text)
39 |         label = self.arguments[0]
40 |         label_strip = label.replace(' ', '')
41 |         node += nodes.raw(self.arguments[0], '<div class="toggleable_div label_%s">' % label_strip, format="html")
42 | 
43 |         # Parse the directive contents.
44 |         self.state.nested_parse(self.content, self.content_offset, node)
45 |         node += nodes.raw(self.arguments[0], '</div>', format="html")
46 |         return [node]
47 | 
48 | class ToggleTableDirective(docutils.parsers.rst.Directive):
49 |     """
50 |     Class used to create a set of buttons to toggle different sections
51 |     """
52 |     required_arguments = 0
53 |     optional_arguments = 10
54 |     final_argument_whitespace = True
55 |     option_spec = {}
56 |     for i in xrange(0, 100):
57 |         option_spec['arg' + str(i)] = str
58 |     has_content = True
59 |     node_class = nodes.container
60 | 
61 |     def run(self):
62 |         js_toggle = """
63 | function toggle(label) {
64 |   $('.toggleable_button').css({border: '2px outset', 'border-radius': '4px'});
65 |   $('.toggleable_button.label_' + label).css({border: '2px inset', 'border-radius': '4px'});
66 |   $('.toggleable_div').css('display', 'none');
67 |   $('.toggleable_div.label_' + label).css('display', 'block');
68 | };
69 | """
70 | 
71 |         js_ready = """
72 | <script>
73 | %s
74 | $(document).ready(function() {
75 |   var classList =$('.toggleable_button').attr('class').split(/\s+/);
76 |   $.each( classList, function(index, item){
77 |     if (item.substring(0, 5) === 'label') {
78 |       toggle(item.substring(6));
79 |     };
80 |   });
81 | });
82 | </script>
83 | """ % js_toggle
84 | 
85 |         # Create the node, to be populated by `nested_parse`.
86 |         node = self.node_class()
87 |         for key in self.options.keys():
88 |             if key not in self.option_spec:
89 |                 raise RuntimeError(key + ' not in the contructor of ToggleTableDirective, use arg0 to arg99')
90 |             label = self.options[key]
91 |             label_strip = label.replace(' ', '')
92 |             str1 = '<button class="toggleable_button label_%s" onclick="' % label_strip
93 |             str2 = js_toggle + "toggle('%s')" % label_strip
94 |             str3 = '">%s</button>' % label
95 |             node += nodes.raw(key, str1 + str2 + str3 + js_ready, format="html")
96 | 
97 |         return [node]
98 | 


--------------------------------------------------------------------------------
/doc/source/api.rst:
--------------------------------------------------------------------------------
  1 | .. _api:
  2 | 
  3 | API description
  4 | ===============
  5 | 
  6 | All the following calls must be performed from the robot. 
  7 | 
  8 | **Environment setup API**
  9 | 
 10 | In order to get the module to connect to your roscore, you should send it your IP.
 11 | 
 12 | * ``void`` ROS-Driver:\:**setMasterURI** ( ``const std::string&`` **uri** )
 13 | 
 14 |   Set current master URI. The IP adress given is from defauth *eth0* network interface.
 15 | 
 16 |   *param:* **uri** - string in form of ``http://<ip>:11311``
 17 | 
 18 | * ``void`` ROS-Driver:\:**setMasterURINet** ( ``const std::string&`` **uri**, ``const std::string&`` **network_interface** )
 19 | 
 20 |   Set current master URI using a given network interface.
 21 | 
 22 |   *param:* **uri** - string in form of ``http://<ip>:11311``
 23 | 
 24 |   *param:* **network_interface** - string. For example ``tether``.
 25 | 
 26 | * ``const std::string&`` ROS-Driver:\:**getMasterURI** ()
 27 | 
 28 |   Get current master URI using a given network interface.
 29 | 
 30 |   *return:* string in form of ``http://<ip>:11311``
 31 | 
 32 | -----------------
 33 | 
 34 | **Converters API**
 35 | 
 36 | The converters are responsible for operating conversion between NAOqi messages and ROS messages, in accordance with given frequency.
 37 | 
 38 | * ``const std::vector< std::string >&`` ROS-Driver:\:**getAvailableConverters** ()
 39 | 
 40 |   Get all registered converters in the module.
 41 | 
 42 |   *return:* vector of string of all converter's topic name
 43 | 
 44 | * ``void`` ROS-Driver:\:**registerMemoryConverter** ( ``const std::string&`` **key**, ``float`` **frequency**, ``int`` **type** )
 45 | 
 46 |   Register a new converter for the memory key given.
 47 | 
 48 |   *param:* **key** - naoqi memory key. For example ``ALMemory/KeyAdded``.
 49 | 
 50 |   *param:* **frequency** - frequency of the converter (in Hz)
 51 | 
 52 |   *param:* **type** - type identifier of the given memory data.
 53 | 
 54 |   ::
 55 | 
 56 |     Available types are:
 57 |     * 0 - None/Undefined
 58 |     * 1 - Int
 59 |     * 2 - Float
 60 |     * 3 - String
 61 |     * 4 - Bool
 62 | 
 63 | * ``void`` ROS-Driver:\:**addMemoryConverters** ( ``std::string`` **filePath** )
 64 | 
 65 |   Add some new converters for memory keys. This call requires as argument the path to a JSON file (stored on the robot) structured as the following one.
 66 |   memKeys and topic must be present and filled. Frequency is optional, and if not there, the default value is 10 Hz.
 67 | 
 68 |   *param:* **filePath** - path of the JSON file
 69 | 
 70 |   ::
 71 | 
 72 |     {
 73 |         "memKeys": [
 74 |                     "KeyName1",
 75 |                     "KeyName2"
 76 |                    ],
 77 |         "topic": "topicName",
 78 |         "frequency": 10
 79 |     }
 80 | 
 81 | -----------------
 82 | 
 83 | **Publishers API**
 84 | 
 85 | * ``void`` ROS-Driver:\:**startPublishing** ()
 86 | 
 87 |   Start/enable publishing all registered publisher
 88 | 
 89 | * ``void`` ROS-Driver:\:**stopPublishing** ()
 90 | 
 91 |   Stop/disable publishing all registered publisher
 92 | 
 93 | * ``const std::vector< std::string >&`` ROS-Driver:\:**getSubscribedPublishers** ()
 94 | 
 95 |   Get all subscribed publishers.
 96 | 
 97 |   *return:* vector of string of publisher's topic name
 98 | 
 99 | -----------------
100 | 
101 | **Recorders API**
102 | 
103 | * ``void`` ROS-Driver:\:**startRecording** ()
104 | 
105 |   Start/enable recording all registered recorder.
106 | 
107 |   This will record all topics in one ROSbag, named after current date & time. The ROSbag is stored in the exact path where the **ROS-Driver** module is launched (meaning that it will be stored on the robot if it's launched from here).
108 | 
109 | * ``void`` ROS-Driver:\:**stopRecording** ()
110 | 
111 |   Stop/disable recording all registered recorder.
112 | 
113 | 
114 | You can now have a look to the :ref:`list of available topics <topic>`, or you can go back to the :ref:`index <main menu>`.
115 | 
116 | If you want to know more about the API, you can look at the doxygen documentation `here <./api/index.html>`_
117 | 


--------------------------------------------------------------------------------
/doc/source/index.rst:
--------------------------------------------------------------------------------
 1 | .. _main menu:
 2 | 
 3 | Description
 4 | ===========
 5 | 
 6 | This is a NAOqi module that bridges with ROS. It is written
 7 | in C++ and gets data straight from the lowest levels of
 8 | NAOqi hence ensuring low latency and CPU usage. It publishes
 9 | several sensor data as well as the robot position and tries
10 | to be as close as possible to the ROS standard by exposing
11 | several standard ROS topics (like /tf or /cmd_vel) as well as
12 | custom ones.
13 | 
14 | Table of content
15 | ================
16 | 
17 | .. toctree::
18 |   :numbered:
19 | 
20 |   install
21 |   start
22 |   api
23 |   topics
24 |   trouble
25 |   next
26 |   other
27 | 


--------------------------------------------------------------------------------
/doc/source/install.rst:
--------------------------------------------------------------------------------
 1 | .. _install:
 2 | 
 3 | How to install it
 4 | =================
 5 | 
 6 | .. toggle_table::
 7 |   :arg1: catkin
 8 |   :arg2: qibuild
 9 | 
10 | .. toggle:: catkin
11 | 
12 |   **Binary:**
13 | 
14 |   If you are in a ROS environment, you followed the official `instructions <http://wiki.ros.org/ROS/Installation>`_.
15 | 
16 |   The easiest therefore is to install the official binary:
17 | 
18 |   .. code-block:: console
19 | 
20 |     sudo apt-get install ros-.*-naoqi-driver
21 | 
22 |   **From source:**
23 | 
24 |   If you want to help develop the bridge or simply want to checkout the latest version, follow the steps below to compile from source. It will use catkin to compile, so in case you are not familiar with, don't forget to follow the official `catkin tutorials <http://wiki.ros.org/catkin/Tutorials>`_. On Ubuntu, it is:
25 | 
26 |   .. code-block:: console
27 | 
28 |     mkdir -p ~/catkin_ws/src
29 |     cd ~/catkin_ws/src
30 |     git clone https://github.com/ros-naoqi/naoqi_driver.git
31 |     # make sure you get all the dependencies installed
32 |     rosdep install -i -y --from-paths ./naoqi_driver
33 |     # build
34 |     source /opt/ros/indigo/setup.sh
35 |     cd ../ && catkin_make
36 | 
37 | .. toggle:: qibuild
38 | 
39 |   **Requirement:**
40 | 
41 |   The naoqi driver module is made to be compiled with qibuild so you first need to install it:
42 | 
43 |   .. code-block:: console
44 | 
45 |     pip install qibuild
46 | 
47 |   To set up your environment and start using qibuild, run the following commands:
48 | 
49 |   .. code-block:: console
50 | 
51 |     mkdir ~/qibuild_ws
52 |     cd ~/qibuild_ws
53 |     qibuild init
54 | 
55 |   If you encounter any issue, please refer to `qibuild documentation <http://doc.aldebaran.com/qibuild/>`_.
56 | 
57 |   When qibuild compiles, it will only look for dependencies within a toolchain, so one needs to be created.
58 | 
59 |   .. code-block:: console
60 | 
61 |     # Create the toolchain
62 |     qitoolchain create <tc_name>
63 |     qibuild add-config <tc_name> -t <tc_name>
64 | 
65 |   By default, qitoolchains don't come with ROS packages. So we need to add them manually.
66 | 
67 |   .. code-block:: console
68 | 
69 |     # Add a package that will find your local ROS installation
70 |     cd ~/qibuild_ws
71 |     git clone https://github.com/ros-naoqi/ros-toolchain.git
72 |     ./ros-toolchain/update_toolchain.sh local_ros <tc_name>
73 | 
74 |   Download the dependencies of the alrosbridge:
75 | 
76 |   .. code-block:: console
77 | 
78 |     cd ~/qibuild_ws
79 |     qisrc add https://github.com/aldebaran/gtest.git
80 |     qisrc add https://github.com/aldebaran/libqiprobes.git
81 |     qisrc add https://github.com/aldebaran/libqicore.git
82 |     qisrc add https://github.com/aldebaran/libqi.git
83 | 
84 |   Download this project and compile it:
85 | 
86 |   .. code-block:: console
87 | 
88 |     cd ~/qibuild_ws
89 |     qisrc add https://github.com/ros-naoqi/naoqi_driver.git
90 |     cd naoqi_driver
91 |     qibuild configure -c <tc_name>
92 |     qibuild make -c <tc_name>
93 | 
94 |   Once compilation is over, the resulting binary will be in *~/qibuild_ws/naoqi_driver/build-<tcname>/sdk/bin/naoqi_driver_node*
95 | 
96 |   If you encounter any compilation issue, (unable to find some dependencies), you might need to install them (through *apt-get install* for instance)
97 | 
98 |   Once you successfully compiled the module, you can learn how to use it on the :ref:`Getting started page <start>` or you can go back to the :ref:`index <main menu>`.
99 | 


--------------------------------------------------------------------------------
/doc/source/next.rst:
--------------------------------------------------------------------------------
 1 | .. _next:
 2 | 
 3 | Next steps
 4 | ==========
 5 | 
 6 | The following services need to be implemented:
 7 | 
 8 |   - ``/naoqi/set_life_status`` to turn the life behavior on or off
 9 |   - ``/naoqi/start_asr`` and ``/naoqi/stop_asr``
10 |   - ``/naoqi/speak`` (TODO: look at audio common)
11 | 
12 | The following items need love:
13 | 
14 |   - info needs to be a proper struct, not fake JSON
15 |   - bumpers are not implemented
16 |   - generic audio
17 |   - octomap
18 |   - LEDs
19 | 
20 | To get things faster:
21 | 
22 |   - check how DCM access is done with B-Human: https://github.com/bhuman/BHumanCodeRelease/blob/master/Src/libbhuman/bhuman.cpp#L768
23 | 
24 | 
25 | Go back to the :ref:`index <main menu>`.


--------------------------------------------------------------------------------
/doc/source/other.rst:
--------------------------------------------------------------------------------
 1 | .. _other:
 2 | 
 3 | Additional Resources
 4 | ====================
 5 | 
 6 | ROS
 7 | ---
 8 | For any ROS question, please refer to the official doc at http://wiki.ros.org .
 9 | 
10 | 
11 | Android Teleop
12 | --------------
13 | 
14 | First, install the teleop app on your Android after installing rosjava and android_apps at http://wiki.ros.org/rosjava
15 | (or ask Karsten for the ``.apk``)
16 | 
17 | When starting your roscore and this naoqi driver, make sure you export your ``ROS_IP`` and ``ROS_MASTER_URI`` to your IP.
18 | 
19 | Then start motion on your robot:
20 | 
21 | .. code-block:: sh
22 | 
23 |   $ qicli call ALMotion.wakeUp
24 |   $ qicli call ALRobotPosture.goToPosture Stand 1
25 | 
26 | You can now control your robot with the app.
27 | 
28 | 
29 | Go back to the :ref:`index <main menu>`.
30 | 


--------------------------------------------------------------------------------
/doc/source/trouble.rst:
--------------------------------------------------------------------------------
 1 | .. _trouble:
 2 | 
 3 | Troubleshooting
 4 | ===============
 5 | 
 6 | The robot cannot connect to the roscore
 7 | ---------------------------------------
 8 | 
 9 | Try out the following solutions:
10 | 
11 | - make sure the roscore is completely started (wait for the "started core service" message)
12 | - check you can ping the roscore IP from the robot
13 | - check the IP you are giving: make sure it is the IP where the roscode run, check that you did not forget http:// and port number
14 | 
15 | 
16 | ROS gets delayed data
17 | ---------------------
18 | 
19 | This is due to a difference of time between your robot and your desktop.
20 | In order to synchronize the two, you need to update the NTP server on both:
21 | 
22 | .. code-block:: sh
23 | 
24 |   TODO
25 | 
26 | 
27 | Go back to the :ref:`index <main menu>`.


--------------------------------------------------------------------------------
/include/naoqi_driver/converter/converter.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 CONVERTER_HPP
 19 | #define CONVERTER_HPP
 20 | 
 21 | #include <string>
 22 | 
 23 | #include <boost/make_shared.hpp>
 24 | #include <boost/shared_ptr.hpp>
 25 | 
 26 | #include <ros/ros.h>
 27 | #include <naoqi_driver/message_actions.h>
 28 | 
 29 | namespace naoqi
 30 | {
 31 | namespace converter
 32 | {
 33 | 
 34 | 
 35 | /**
 36 | * @brief Converter concept interface
 37 | * @note this defines an private concept struct,
 38 | * which each instance has to implement
 39 | * @note a type erasure pattern in implemented here to avoid strict inheritance,
 40 | * thus each possible converter instance has to implement the virtual functions mentioned in the concept
 41 | */
 42 | class Converter
 43 | {
 44 | 
 45 | public:
 46 | 
 47 |   /**
 48 |   * @brief Constructor for converter interface
 49 |   */
 50 |   template<typename T>
 51 |   Converter( T conv ):
 52 |     convPtr_( boost::make_shared<ConverterModel<T> >(conv) )
 53 |   {}
 54 | 
 55 |   /**
 56 |   * @brief getting the descriptive name for this converter instance
 57 |   * @return string with the name
 58 |   */
 59 |   std::string name() const
 60 |   {
 61 |     return convPtr_->name();
 62 |   }
 63 | 
 64 |   /**
 65 |   * @brief getting the assigned frequency of this converter instance
 66 |   * @return float value indicating the frequency
 67 |   */
 68 |   float frequency() const
 69 |   {
 70 |     return convPtr_->frequency();
 71 |   }
 72 | 
 73 |   void reset()
 74 |   {
 75 |     convPtr_->reset();
 76 |   }
 77 | 
 78 |   void callAll( const std::vector<message_actions::MessageAction>& actions )
 79 |   {
 80 |     if ( actions.size() > 0 )
 81 |     {
 82 |       convPtr_->callAll(actions);
 83 |     }
 84 | 
 85 |     ros::Time after = ros::Time::now();
 86 |     lapse_time = after - before;
 87 |     before = after;
 88 |   }
 89 | 
 90 |   ros::Duration lapseTime() const
 91 |   {
 92 |     return lapse_time;
 93 |   }
 94 | 
 95 |   friend bool operator==( const Converter& lhs, const Converter& rhs )
 96 |   {
 97 |     // decision made for OR-comparison since we want to be more restrictive
 98 |     if ( lhs.name() == rhs.name() )
 99 |       return true;
100 |     return false;
101 |   }
102 | 
103 | private:
104 | 
105 |   ros::Time before;
106 |   ros::Duration lapse_time;
107 | 
108 |   /**
109 |   * BASE concept struct
110 |   */
111 |   struct ConverterConcept
112 |   {
113 |     virtual ~ConverterConcept(){}
114 |     virtual std::string name() const = 0;
115 |     virtual float frequency() const = 0;
116 |     virtual void reset() = 0;
117 |     virtual void callAll( const std::vector<message_actions::MessageAction>& actions ) = 0;
118 |   };
119 | 
120 | 
121 |   /**
122 |   * templated instances of base concept
123 |   */
124 |   template<typename T>
125 |   struct ConverterModel : public ConverterConcept
126 |   {
127 |     ConverterModel( const T& other ):
128 |       converter_( other )
129 |     {}
130 | 
131 |     std::string name() const
132 |     {
133 |       return converter_->name();
134 |     }
135 | 
136 |     float frequency() const
137 |     {
138 |       return converter_->frequency();
139 |     }
140 |     
141 |     void reset()
142 |     {
143 |       converter_->reset();
144 |     }
145 | 
146 |     void callAll( const std::vector<message_actions::MessageAction>& actions )
147 |     {
148 |       converter_->callAll( actions );
149 |     }
150 | 
151 |     T converter_;
152 |   };
153 | 
154 |   boost::shared_ptr<ConverterConcept> convPtr_;
155 | 
156 | }; // class converter
157 | 
158 | } //converter
159 | } //naoqi
160 | 
161 | #endif
162 | 


--------------------------------------------------------------------------------
/include/naoqi_driver/message_actions.h:
--------------------------------------------------------------------------------
 1 | #ifndef MESSAGE_ACTIONS_H
 2 | #define MESSAGE_ACTIONS_H
 3 | 
 4 | namespace naoqi
 5 | {
 6 | namespace message_actions
 7 | {
 8 | 
 9 | enum MessageAction
10 | {
11 |   PUBLISH,
12 |   RECORD,
13 |   LOG
14 | };
15 | 
16 | }
17 | 
18 | }
19 | 
20 | 
21 | #endif
22 | 


--------------------------------------------------------------------------------
/include/naoqi_driver/recorder/globalrecorder.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 GLOBALRECORDER_HPP
 19 | #define GLOBALRECORDER_HPP
 20 | 
 21 | /*
 22 | * LOCAL includes
 23 | */
 24 | #include <naoqi_driver/tools.hpp>
 25 | 
 26 | /*
 27 | * STANDARD includes
 28 | */
 29 | #include <string>
 30 | 
 31 | /*
 32 | * BOOST includes
 33 | */
 34 | # include <boost/thread/mutex.hpp>
 35 | 
 36 | /*
 37 | * ROS includes
 38 | */
 39 | #include <ros/ros.h>
 40 | #include <rosbag/bag.h>
 41 | #include <geometry_msgs/TransformStamped.h>
 42 | 
 43 | 
 44 | 
 45 | namespace naoqi
 46 | {
 47 | namespace recorder
 48 | {
 49 | 
 50 | /**
 51 | * @brief GlobalRecorder concept interface
 52 | * @note this defines an private concept struct,
 53 | * which each instance has to implement
 54 | * @note a type erasure pattern in implemented here to avoid strict inheritance,
 55 | * thus each possible publisher instance has to implement the virtual functions mentioned in the concept
 56 | */
 57 | class GlobalRecorder
 58 | {
 59 | 
 60 | public:
 61 | 
 62 |   /**
 63 |   * @brief Constructor for recorder interface
 64 |   */
 65 |   GlobalRecorder(const std::string& prefix_topic);
 66 | 
 67 |   /**
 68 |   * @brief Initialize the recording of the ROSbag
 69 |   */
 70 |   void startRecord(const std::string& prefix_bag = "");
 71 | 
 72 |   /**
 73 |   * @brief Terminate the recording of the ROSbag
 74 |   */
 75 |   std::string stopRecord(const std::string& robot_ip = "<ROBOT_IP>");
 76 | 
 77 |   /**
 78 |   * @brief Insert data into the ROSbag
 79 |   */
 80 |   template <class T>
 81 |   void write(const std::string& topic, const T& msg, const ros::Time& time = ros::Time::now() ) {
 82 |     std::string ros_topic;
 83 |     if (topic[0]!='/')
 84 |     {
 85 |       ros_topic = _prefix_topic+topic;
 86 |     }
 87 |     else
 88 |     {
 89 |       ros_topic = topic;
 90 |     }
 91 |     ros::Time time_msg = time;
 92 |     boost::mutex::scoped_lock writeLock( _processMutex );
 93 |     if (_isStarted) {
 94 |       _bag.write(ros_topic, time_msg, msg);
 95 |     }
 96 |   }
 97 | 
 98 |   void write(const std::string& topic, const std::vector<geometry_msgs::TransformStamped>& msgtf);
 99 | 
100 |   /**
101 |   * @brief Check if the ROSbag is opened
102 |   */
103 |   bool isStarted();
104 | 
105 | private:
106 |   std::string _prefix_topic;
107 |   boost::mutex _processMutex;
108 |   rosbag::Bag _bag;
109 |   std::string _nameBag;
110 |   bool _isStarted;
111 | 
112 |   // TOPICS
113 |   std::vector<Topics> _topics;
114 | 
115 | }; // class globalrecorder
116 | } // recorder
117 | } //naoqi
118 | 
119 | #endif
120 | 


--------------------------------------------------------------------------------
/include/naoqi_driver/service/service.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 SERVICE_HPP
 19 | #define SERVICE_HPP
 20 | 
 21 | #include <string>
 22 | 
 23 | #include <boost/make_shared.hpp>
 24 | #include <boost/shared_ptr.hpp>
 25 | 
 26 | #include <ros/ros.h>
 27 | 
 28 | namespace naoqi
 29 | {
 30 | namespace service
 31 | {
 32 | 
 33 | 
 34 | /**
 35 | * @brief Service concept interface
 36 | * @note this defines an private concept struct,
 37 | * which each instance has to implement
 38 | * @note a type erasure pattern in implemented here to avoid strict inheritance,
 39 | * thus each possible service instance has to implement the virtual functions mentioned in the concept
 40 | */
 41 | class Service
 42 | {
 43 | 
 44 | public:
 45 | 
 46 |   /**
 47 |   * @brief Constructor for service interface
 48 |   */
 49 |   template<typename T>
 50 |   Service( T srv ):
 51 |     srvPtr_( boost::make_shared<ServiceModel<T> >(srv) )
 52 |   {}
 53 | 
 54 |   /**
 55 |   * @brief initializes/resets the service into ROS with a given nodehandle,
 56 |   * this will be called at first for initialization or again when master uri has changed
 57 |   * @param ros NodeHandle to register the service on
 58 |   */
 59 |   void reset( ros::NodeHandle& nh )
 60 |   {
 61 |     std::cout << name() << " is resetting" << std::endl;
 62 |     srvPtr_->reset( nh );
 63 |     std::cout << name() << " reset" << std::endl;
 64 |   }
 65 | 
 66 |   /**
 67 |   * @brief getting the descriptive name for this service instance
 68 |   * @return string with the name
 69 |   */
 70 |   std::string name() const
 71 |   {
 72 |     return srvPtr_->name();
 73 |   }
 74 | 
 75 |   /**
 76 |   * @brief getting the topic to service on
 77 |   * @return string indicating the topic
 78 |   */
 79 |   std::string topic() const
 80 |   {
 81 |     return srvPtr_->topic();
 82 |   }
 83 | 
 84 | private:
 85 | 
 86 |   /**
 87 |   * BASE concept struct
 88 |   */
 89 |   struct ServiceConcept
 90 |   {
 91 |     virtual ~ServiceConcept(){}
 92 |     virtual void reset( ros::NodeHandle& nh ) = 0;
 93 |     virtual std::string name() const = 0;
 94 |     virtual std::string topic() const = 0;
 95 |   };
 96 | 
 97 | 
 98 |   /**
 99 |   * templated instances of base concept
100 |   */
101 |   template<typename T>
102 |   struct ServiceModel : public ServiceConcept
103 |   {
104 |     ServiceModel( const T& other ):
105 |       service_( other )
106 |     {}
107 | 
108 |     std::string name() const
109 |     {
110 |       return service_->name();
111 |     }
112 | 
113 |     std::string topic() const
114 |     {
115 |       return service_->topic();
116 |     }
117 | 
118 |     bool isInitialized() const
119 |     {
120 |       return service_->isInitialized();
121 |     }
122 | 
123 |     void reset( ros::NodeHandle& nh )
124 |     {
125 |       service_->reset( nh );
126 |     }
127 | 
128 |     T service_;
129 |   };
130 | 
131 |   boost::shared_ptr<ServiceConcept> srvPtr_;
132 | 
133 | }; // class service
134 | 
135 | } //service
136 | } //naoqi
137 | 
138 | #endif
139 | 


--------------------------------------------------------------------------------
/include/naoqi_driver/subscriber/subscriber.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 SUBSCRIBER_HPP
 19 | #define SUBSCRIBER_HPP
 20 | 
 21 | #include <string>
 22 | 
 23 | #include <boost/make_shared.hpp>
 24 | #include <boost/shared_ptr.hpp>
 25 | 
 26 | #include <ros/ros.h>
 27 | 
 28 | namespace naoqi
 29 | {
 30 | namespace subscriber
 31 | {
 32 | 
 33 | 
 34 | /**
 35 | * @brief Subscriber concept interface
 36 | * @note this defines an private concept struct,
 37 | * which each instance has to implement
 38 | * @note a type erasure pattern in implemented here to avoid strict inheritance,
 39 | * thus each possible subscriber instance has to implement the virtual functions mentioned in the concept
 40 | */
 41 | class Subscriber
 42 | {
 43 | 
 44 | public:
 45 | 
 46 |   /**
 47 |   * @brief Constructor for subscriber interface
 48 |   */
 49 |   template<typename T>
 50 |   Subscriber( T sub ):
 51 |     subPtr_( boost::make_shared<SubscriberModel<T> >(sub) )
 52 |   {}
 53 | 
 54 |   /**
 55 |   * @brief checks if the subscriber is correctly initialized on the ros-master
 56 |   @ @return bool value indicating true for success
 57 |   */
 58 |   bool isInitialized() const
 59 |   {
 60 |     return subPtr_->isInitialized();
 61 |   }
 62 | 
 63 |   /**
 64 |   * @brief initializes/resets the subscriber into ROS with a given nodehandle,
 65 |   * this will be called at first for initialization or again when master uri has changed
 66 |   * @param ros NodeHandle to register the subscriber on
 67 |   */
 68 |   void reset( ros::NodeHandle& nh )
 69 |   {
 70 |     std::cout << name() << " is resetting" << std::endl;
 71 |     subPtr_->reset( nh );
 72 |     std::cout << name() << " reset" << std::endl;
 73 |   }
 74 | 
 75 |   /**
 76 |   * @brief getting the descriptive name for this subscriber instance
 77 |   * @return string with the name
 78 |   */
 79 |   std::string name() const
 80 |   {
 81 |     return subPtr_->name();
 82 |   }
 83 | 
 84 |   /**
 85 |   * @brief getting the topic to subscriber on
 86 |   * @return string indicating the topic
 87 |   */
 88 |   std::string topic() const
 89 |   {
 90 |     return subPtr_->topic();
 91 |   }
 92 | 
 93 |   friend bool operator==( const Subscriber& lhs, const Subscriber& rhs )
 94 |   {
 95 |     // decision made for OR-comparison since we want to be more restrictive
 96 |     if ( lhs.name() == rhs.name() || lhs.topic() == rhs.topic() )
 97 |       return true;
 98 |     return false;
 99 |   }
100 | 
101 | private:
102 | 
103 |   /**
104 |   * BASE concept struct
105 |   */
106 |   struct SubscriberConcept
107 |   {
108 |     virtual ~SubscriberConcept(){}
109 |     virtual bool isInitialized() const = 0;
110 |     virtual void reset( ros::NodeHandle& nh ) = 0;
111 |     virtual std::string name() const = 0;
112 |     virtual std::string topic() const = 0;
113 |   };
114 | 
115 | 
116 |   /**
117 |   * templated instances of base concept
118 |   */
119 |   template<typename T>
120 |   struct SubscriberModel : public SubscriberConcept
121 |   {
122 |     SubscriberModel( const T& other ):
123 |       subscriber_( other )
124 |     {}
125 | 
126 |     std::string name() const
127 |     {
128 |       return subscriber_->name();
129 |     }
130 | 
131 |     std::string topic() const
132 |     {
133 |       return subscriber_->topic();
134 |     }
135 | 
136 |     bool isInitialized() const
137 |     {
138 |       return subscriber_->isInitialized();
139 |     }
140 | 
141 |     void reset( ros::NodeHandle& nh )
142 |     {
143 |       subscriber_->reset( nh );
144 |     }
145 | 
146 |     T subscriber_;
147 |   };
148 | 
149 |   boost::shared_ptr<SubscriberConcept> subPtr_;
150 | 
151 | }; // class subscriber
152 | 
153 | } //subscriber
154 | } //naoqi
155 | 
156 | #endif
157 | 


--------------------------------------------------------------------------------
/include/naoqi_driver/tools.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef ALROS_TOOLS_HPP
20 | #define ALROS_TOOLS_HPP
21 | 
22 | #define RESETCOLOR "\033[0m"
23 | #define GREEN "\033[32m"
24 | #define HIGHGREEN "\033[92m"
25 | #define BOLDRED "\033[1m\033[31m"
26 | #define YELLOW "\033[33m"
27 | #define BOLDYELLOW "\033[1m\033[33m"
28 | #define BOLDCYAN "\033[1m\033[36m"
29 | 
30 | # include <qi/anyobject.hpp>
31 | 
32 | namespace naoqi
33 | {
34 | 
35 | namespace robot
36 | {
37 | enum Robot
38 | {
39 |   UNIDENTIFIED,
40 |   NAO,
41 |   PEPPER,
42 |   ROMEO
43 | };
44 | 
45 | struct NaoqiVersion
46 | {
47 |   int major;
48 |   int minor;
49 |   int patch;
50 |   int build;
51 |   std::string text;
52 | };
53 | }
54 | 
55 | enum Topics {
56 |   Laser = 0,
57 |   Camera,
58 |   Sonar
59 | };
60 | 
61 | namespace dataType {
62 | enum DataType
63 | {
64 |   None = 0,
65 |   Float,
66 |   Int,
67 |   String,
68 |   Bool
69 | };
70 | }
71 | 
72 | } // naoqi
73 | 
74 | #if LIBQI_VERSION>24
75 | QI_TYPE_ENUM(naoqi::Topics);
76 | QI_TYPE_ENUM(naoqi::dataType::DataType);
77 | #else
78 | QI_TYPE_ENUM_REGISTER(naoqi::Topics);
79 | QI_TYPE_ENUM_REGISTER(naoqi::dataType::DataType);
80 | #endif
81 | 
82 | #endif
83 | 


--------------------------------------------------------------------------------
/launch/naoqi_driver.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="nao_ip"            default="$(optenv NAO_IP)" />
 4 |   <arg name="nao_port"          default="$(optenv NAO_PORT 9559)" />
 5 |   <arg name="roscore_ip"        default="127.0.0.1" />
 6 |   <arg name="network_interface" default="eth0" />
 7 |   <arg name="namespace"         default="naoqi_driver" />
 8 |   <arg name="user"              default="nao" />
 9 |   <arg name="password"          default="no_password" />
10 | 
11 |   <node pkg="naoqi_driver" type="naoqi_driver_node" name="$(arg namespace)" required="true" args="--qi-url=tcp://$(arg nao_ip):$(arg nao_port) --nao_ip=$(arg nao_ip) --nao_port=$(arg nao_port) --user=$(arg user) --password=$(arg password) --roscore_ip=$(arg roscore_ip) --network_interface=$(arg network_interface) --namespace=$(arg namespace)" output="screen" />
12 | 
13 | </launch>
14 | 


--------------------------------------------------------------------------------
/msg/README:
--------------------------------------------------------------------------------
 1 | this folder is just meant to be run as follows:
 2 | 
 3 | git clone --depth=1 https://github.com/ros-naoqi/naoqi_bridge.git
 4 | mv naoqi_bridge/naoqi_msgs ./
 5 | rm -fr naoqi_bridge
 6 | catkin_make --build ./build --source ./
 7 | mv devel/include/* ../include/
 8 | rm -fr build devel naoqi_msgs
 9 | rm CMakeLists.txt
10 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="3">
 3 |   <name>naoqi_driver</name>
 4 |   <version>0.6.0</version>
 5 |   <description>Driver module between Aldebaran's NAOqiOS and ROS. It publishes all sensor and actuator data as well as basic diagnostic for battery, temperature. It subscribes also to RVIZ simple goal and cmd_vel for teleop.</description>
 6 | 
 7 |   <author>Karsten Knese</author>
 8 |   <maintainer email="mchamoux@softbankrobotics.com">Marine Chamoux</maintainer>
 9 |   <maintainer email="mbusy@softbankrobotics.com">Maxime Busy</maintainer>
10 |   <maintainer email="surya.ambrose@gmail.com">Surya Ambrose</maintainer>
11 |   <maintainer email="karsten.knese@gmail.com">Karsten Knese</maintainer>
12 |   <license>BSD</license>
13 | 
14 |   <build_depend>boost</build_depend>
15 |   <build_depend>cv_bridge</build_depend>
16 |   <build_depend>diagnostic_msgs</build_depend>
17 |   <build_depend>diagnostic_updater</build_depend>
18 |   <build_depend>geometry_msgs</build_depend>
19 |   <build_depend>image_transport</build_depend>
20 |   <build_depend>kdl_parser</build_depend>
21 |   <build_depend version_gte="0.0.4">naoqi_bridge_msgs</build_depend>
22 |   <build_depend>naoqi_libqi</build_depend>
23 |   <build_depend>naoqi_libqicore</build_depend>
24 |   <build_depend condition="$ROS_DISTRO != noetic">orocos_kdl</build_depend>
25 |   <build_depend condition="$ROS_DISTRO == noetic">liborocos-kdl-dev</build_depend>
26 |   <build_depend>robot_state_publisher</build_depend>
27 |   <build_depend>rosbag_storage</build_depend>
28 |   <build_depend>rosconsole</build_depend>
29 |   <build_depend>rosgraph_msgs</build_depend>
30 |   <build_depend>sensor_msgs</build_depend>
31 |   <build_depend>tf2_geometry_msgs</build_depend>
32 |   <build_depend>tf2_msgs</build_depend>
33 |   <build_depend>tf2_ros</build_depend>
34 | 
35 |   <buildtool_depend>catkin</buildtool_depend>
36 | 
37 |   <exec_depend>boost</exec_depend>
38 |   <exec_depend>cv_bridge</exec_depend>
39 |   <exec_depend>image_transport</exec_depend>
40 |   <exec_depend>kdl_parser</exec_depend>
41 |   <exec_depend version_gte="0.0.4">naoqi_bridge_msgs</exec_depend>
42 |   <exec_depend>naoqi_libqi</exec_depend>
43 |   <exec_depend>naoqi_libqicore</exec_depend>
44 |   <exec_depend condition="$ROS_DISTRO != noetic">orocos_kdl</exec_depend>
45 |   <exec_depend condition="$ROS_DISTRO == noetic">liborocos-kdl-dev</exec_depend>
46 |   <exec_depend>robot_state_publisher</exec_depend>
47 |   <exec_depend>rosbag_storage</exec_depend>
48 |   <exec_depend>rosconsole</exec_depend>
49 |   <exec_depend>tf2_ros</exec_depend>
50 | 
51 |   <conflict>nao_driver</conflict>
52 |   <conflict>naoqi_rosbridge</conflict>
53 | </package>
54 | 


--------------------------------------------------------------------------------
/qiproject.xml:
--------------------------------------------------------------------------------
1 | <!-- in qiproject.xml -->
2 | <project version="3">
3 |   <qibuild name="naoqi_driver" >
4 |     <depends buildtime="true" runtime="true" names="libqicore libqi ros" />
5 |   </qibuild>
6 | </project>
7 | 


--------------------------------------------------------------------------------
/share/boot_config.json:
--------------------------------------------------------------------------------
  1 | {
  2 |   "_comment": "QQVGA = 0, QVGA = 1, VGA = 2",
  3 |   "converters":
  4 |   {
  5 |     "front_camera":
  6 |     {
  7 |       "enabled"       : true,
  8 |       "resolution"    : 1,
  9 |       "fps"           : 10,
 10 |       "recorder_fps"  : 5
 11 |     },
 12 |     "bottom_camera":
 13 |     {
 14 |       "enabled"       : true,
 15 |       "resolution"    : 1,
 16 |       "fps"           : 10,
 17 |       "recorder_fps"  : 5
 18 |     },
 19 |     "depth_camera":
 20 |     {
 21 |       "enabled"       : true,
 22 |       "xtion_resolution": 1,
 23 |       "stereo_resolution": 9,
 24 |       "fps"           : 10,
 25 |       "recorder_fps"  : 5
 26 |     },
 27 |     "stereo_camera":
 28 |     {
 29 |       "enabled"       : true,
 30 |       "resolution"    : 15,
 31 |       "fps"           : 10,
 32 |       "recorder_fps"  : 5
 33 |     },
 34 |     "ir_camera":
 35 |     {
 36 |       "enabled"       : true,
 37 |       "resolution"    : 1,
 38 |       "fps"           : 10,
 39 |       "recorder_fps"  : 5
 40 |     },
 41 |     "info":
 42 |     {
 43 |       "enabled"       : true,
 44 |       "frequency"     : 1
 45 |     },
 46 |     "logs":
 47 |     {
 48 |       "enabled"       : false,
 49 |       "frequency"     : 1
 50 |     },
 51 |     "diag":
 52 |     {
 53 |       "enabled"       : true,
 54 |       "frequency"     : 1
 55 |     },
 56 |     "imu_torso":
 57 |     {
 58 |       "enabled"       : true,
 59 |       "frequency"     : 10
 60 |     },
 61 |     "imu_base":
 62 |     {
 63 |       "enabled"       : true,
 64 |       "frequency"     : 10
 65 |     },
 66 |     "joint_states":
 67 |     {
 68 |       "enabled"       : true,
 69 |       "frequency"     : 50
 70 |     },
 71 |     "laser":
 72 |     {
 73 |       "enabled"       : true,
 74 |       "frequency"     : 10,
 75 |       "range_min"     : 0.1,
 76 |       "range_max"     : 3.0
 77 |     },
 78 |     "sonar":
 79 |     {
 80 |       "enabled"       : true,
 81 |       "frequency"     : 10
 82 |     },
 83 |     "audio":
 84 |     {
 85 |       "enabled"       : true
 86 |     },
 87 |     "bumper":
 88 |     {
 89 |       "enabled"       : true
 90 |     },
 91 |     "touch_hand":
 92 |     {
 93 |       "enabled"       : true
 94 |     },
 95 |     "touch_head":
 96 |     {
 97 |       "enabled"       : true
 98 |     },
 99 |     "odom":
100 |     {
101 |       "enabled"       : true,
102 |       "frequency"     : 15
103 |     }
104 |   }
105 | }
106 | 


--------------------------------------------------------------------------------
/src/autoload_registration.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 |  * LOCAL includes
20 |  */
21 | #include <naoqi_driver/naoqi_driver.hpp>
22 | 
23 | /*
24 |  * ALDEBARAN includes
25 |  */
26 | #include <qi/anymodule.hpp>
27 | 
28 | /**
29 | * @brief starter code for registrating the naoqi_driver module via the autoload.ini.
30 | */
31 | void registerRosDriver(qi::ModuleBuilder* mb) {
32 |   mb->advertiseFactory<naoqi::Driver, qi::SessionPtr, std::string>("ROS-Driver");
33 | }
34 | QI_REGISTER_MODULE("naoqi_driver_module", &registerRosDriver);
35 | 


--------------------------------------------------------------------------------
/src/converters/audio.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "audio.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi{
30 | 
31 | namespace converter{
32 | 
33 | AudioEventConverter::AudioEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session)
34 |     : BaseConverter(name, frequency, session)
35 | {
36 | }
37 | 
38 | AudioEventConverter::~AudioEventConverter() {
39 | }
40 | 
41 | void AudioEventConverter::reset()
42 | {
43 | }
44 | 
45 | void AudioEventConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
46 | {
47 |   callbacks_[action] = cb;
48 | }
49 | 
50 | void AudioEventConverter::callAll(const std::vector<message_actions::MessageAction>& actions, naoqi_bridge_msgs::AudioBuffer& msg)
51 | {
52 |   msg_ = msg;
53 |   for_each( message_actions::MessageAction action, actions )
54 |   {
55 |     callbacks_[action](msg_);
56 |   }
57 | }
58 | 
59 | }
60 | 
61 | }
62 | 


--------------------------------------------------------------------------------
/src/converters/audio.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 AUDIO_EVENT_CONVERTER_HPP
19 | #define AUDIO_EVENT_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/AudioBuffer.h>
31 | 
32 | /*
33 | * ALDEBARAN includes
34 | */
35 | #include <qi/anymodule.hpp>
36 | 
37 | namespace naoqi{
38 | 
39 | namespace converter{
40 | 
41 | class AudioEventConverter : public BaseConverter<AudioEventConverter>
42 | {
43 | 
44 |   typedef boost::function<void(naoqi_bridge_msgs::AudioBuffer&) > Callback_t;
45 | 
46 | public:
47 |   AudioEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session);
48 | 
49 |   ~AudioEventConverter();
50 | 
51 |   virtual void reset();
52 | 
53 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
54 | 
55 |   void callAll(const std::vector<message_actions::MessageAction>& actions, naoqi_bridge_msgs::AudioBuffer& msg);
56 | 
57 | private:
58 |   /** Registered Callbacks **/
59 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
60 |   naoqi_bridge_msgs::AudioBuffer msg_;
61 | };
62 | 
63 | }
64 | 
65 | }
66 | 
67 | #endif // AUDIO_CONVERTER_HPP
68 | 


--------------------------------------------------------------------------------
/src/converters/camera.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 CONVERTER_CAMERA_HPP
19 | #define CONVERTER_CAMERA_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <image_transport/image_transport.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class CameraConverter : public BaseConverter<CameraConverter>
38 | {
39 | 
40 |   typedef boost::function<void(sensor_msgs::ImagePtr, sensor_msgs::CameraInfo)> Callback_t;
41 | 
42 | public:
43 |   CameraConverter(
44 |     const std::string& name,
45 |     const float& frequency,
46 |     const qi::SessionPtr& session,
47 |     const int& camera_source,
48 |     const int& resolution,
49 |     const bool& has_stereo=false);
50 | 
51 |   ~CameraConverter();
52 | 
53 |   void reset();
54 | 
55 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
56 | 
57 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
58 | 
59 | private:
60 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
61 | 
62 |   /** VideoDevice (Proxy) configurations */
63 |   qi::AnyObject p_video_;
64 |   int camera_source_;
65 |   int resolution_;
66 |   int colorspace_;
67 |   std::string handle_;
68 | 
69 |   // string indicating image transport encoding
70 |   // goes along with colorspace_
71 |   std::string msg_colorspace_;
72 |   int cv_mat_type_;
73 |   // msg frame id
74 |   std::string msg_frameid_;
75 |   sensor_msgs::CameraInfo camera_info_;
76 |   sensor_msgs::ImagePtr msg_;
77 | };
78 | 
79 | } //publisher
80 | } //naoqi
81 | 
82 | 
83 | #endif
84 | 


--------------------------------------------------------------------------------
/src/converters/converter_base.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 BASE_CONVERTER_HPP
19 | #define BASE_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include <naoqi_driver/tools.hpp>
25 | #include "../helpers/driver_helpers.hpp"
26 | 
27 | /*
28 | * ALDEBARAN includes
29 | */
30 | #include <qi/session.hpp>
31 | #include <qi/anyobject.hpp>
32 | 
33 | namespace naoqi
34 | {
35 | namespace converter
36 | {
37 | 
38 | // CRTP
39 | template<class T>
40 | class BaseConverter
41 | {
42 | 
43 | public:
44 |   BaseConverter( const std::string& name, float frequency, qi::SessionPtr session ):
45 |     name_( name ),
46 |     frequency_( frequency ),
47 |     robot_( helpers::driver::getRobot(session) ),
48 |     naoqi_version_( helpers::driver::getNaoqiVersion(session) ),
49 |     session_(session),
50 |     record_enabled_(false)
51 |   {}
52 | 
53 |   virtual ~BaseConverter() {}
54 | 
55 |   inline std::string name() const
56 |   {
57 |     return name_;
58 |   }
59 | 
60 |   inline float frequency() const
61 |   {
62 |     return frequency_;
63 |   }
64 | 
65 | protected:
66 |   std::string name_;
67 | 
68 |   /** Frequency at which the converter should turn. This is informative */
69 |   float frequency_;
70 |   /** The type of the robot */
71 |   const robot::Robot& robot_;
72 |   /* The version of the Naoqi Software on the robot */
73 |   const robot::NaoqiVersion& naoqi_version_;
74 | 
75 |   /** Pointer to a session from which we can create proxies */
76 |   qi::SessionPtr session_;
77 | 
78 |   /** Enable recording */
79 |   bool record_enabled_;
80 | }; // class
81 | 
82 | } // converter
83 | } // naoqi
84 | 
85 | #endif
86 | 


--------------------------------------------------------------------------------
/src/converters/diagnostics.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 DIAGNOSTICS_CONVERTER_HPP
19 | #define DIAGNOSTICS_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <diagnostic_msgs/DiagnosticArray.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | /**
38 |  * @brief This class defines a Diagnostic converter
39 |  * It does not use the DiagnostricsUpdater for optimization.
40 |  * A full diagnostic_msgs/DiagnosticArray is built and sent to requesting nodes
41 |  */
42 | class DiagnosticsConverter : public BaseConverter<DiagnosticsConverter>
43 | {
44 | 
45 |   typedef boost::function<void(diagnostic_msgs::DiagnosticArray&) > Callback_t;
46 | 
47 | public:
48 |   DiagnosticsConverter(const std::string& name, float frequency, const qi::SessionPtr &session );
49 | 
50 |   void reset();
51 | 
52 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
53 | 
54 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
55 | 
56 | private:
57 |   /** The names of the joints in the order given by the motion proxy */
58 |   std::vector<std::string> joint_names_;
59 |   /** all the keys to check. It is a concatenation of joint_temperatures_keys_, battery_keys_ */
60 |   std::vector<std::string> all_keys_;
61 |   /** Keys for the battery status */
62 |   std::vector<std::string> battery_status_keys_;
63 |   /** Map storing the joints informations */
64 |   std::map<std::string, std::vector<double> > joint_limit_map_;
65 |   /** Proxy to ALMemory */
66 |   qi::AnyObject p_memory_;
67 |   /** Proxy to ALMotion */
68 |   qi::AnyObject p_motion_;
69 |   /** Proxy to ALBodyTemperature */
70 |   qi::AnyObject p_body_temperature_;
71 | 
72 |   float temperature_warn_level_;
73 |   float temperature_error_level_;
74 | 
75 |   /** Registered Callbacks **/
76 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
77 | };
78 | 
79 | } //converter
80 | } // naoqi
81 | 
82 | #endif
83 | 


--------------------------------------------------------------------------------
/src/converters/imu.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 IMU_CONVERTER_HPP
19 | #define IMU_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <sensor_msgs/Imu.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter {
35 | 
36 | namespace IMU{
37 | 
38 | enum Location{
39 |     TORSO,
40 |     BASE
41 | };
42 | 
43 | }
44 | 
45 | class ImuConverter : public BaseConverter<ImuConverter>
46 | {
47 | 
48 |   typedef boost::function<void(sensor_msgs::Imu&) > Callback_t;
49 | 
50 | public:
51 |   ImuConverter(const std::string& name, const IMU::Location& location, const float& frequency, const qi::SessionPtr& session);
52 | 
53 |   ~ImuConverter();
54 | 
55 |   virtual void reset();
56 | 
57 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
58 | 
59 |   virtual void callAll(const std::vector<message_actions::MessageAction>& actions);
60 | 
61 | private:
62 |   sensor_msgs::Imu msg_imu_;
63 |   qi::AnyObject p_memory_;
64 |   std::vector<std::string> data_names_list_;
65 | 
66 |   /** Registered Callbacks **/
67 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
68 | };
69 | 
70 | }
71 | 
72 | }
73 | 
74 | #endif // IMU_CONVERTER_HPP
75 | 


--------------------------------------------------------------------------------
/src/converters/info.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "info.hpp"
22 | #include "../tools/from_any_value.hpp"
23 | 
24 | /*
25 | * BOOST includes
26 | */
27 | #include <boost/assign/list_of.hpp>
28 | #include <boost/foreach.hpp>
29 | #define for_each BOOST_FOREACH
30 | 
31 | namespace naoqi
32 | {
33 | namespace converter
34 | {
35 | 
36 | InfoConverter::InfoConverter( const std::string& name, float frequency, const qi::SessionPtr& session )
37 |   : BaseConverter( name, frequency, session ),
38 |     p_memory_( session->service("ALMemory").value() )
39 | {
40 |   keys_.push_back("RobotConfig/Head/FullHeadId");
41 |   keys_.push_back("Device/DeviceList/ChestBoard/BodyId");
42 |   keys_.push_back("RobotConfig/Body/Type");
43 |   keys_.push_back("RobotConfig/Body/BaseVersion");
44 |   keys_.push_back("RobotConfig/Body/Device/LeftArm/Version");
45 |   keys_.push_back("RobotConfig/Body/Device/RightArm/Version");
46 |   keys_.push_back("RobotConfig/Body/Device/Hand/Left/Version");
47 |   keys_.push_back("RobotConfig/Body/Version");
48 |   keys_.push_back("RobotConfig/Body/SoftwareRequirement");
49 |   keys_.push_back("RobotConfig/Body/Device/Legs/Version");
50 |   if(robot_ == robot::PEPPER)
51 |   {
52 |     keys_.push_back("Device/DeviceList/BatteryFuelGauge/SerialNumber");
53 |     keys_.push_back("Device/DeviceList/BatteryFuelGauge/FirmwareVersion");
54 |     keys_.push_back("RobotConfig/Body/Device/Platform/Version");
55 |     keys_.push_back("RobotConfig/Body/Device/Brakes/Version");
56 |     keys_.push_back("RobotConfig/Body/Device/Wheel/Version");
57 |   }
58 | }
59 | 
60 | void InfoConverter::reset()
61 | {
62 | }
63 | 
64 | void InfoConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
65 | {
66 |   callbacks_[action] = cb;
67 | }
68 | 
69 | void InfoConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
70 | {
71 |   std::vector<std::string> values;
72 |   try {
73 |       qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("getListData", keys_);
74 |       tools::fromAnyValueToStringVector(anyvalues, values);
75 |   } catch (const std::exception& e) {
76 |     std::cerr << "Exception caught in InfoConverter: " << e.what() << std::endl;
77 |     return;
78 |   }
79 | 
80 |   naoqi_bridge_msgs::StringStamped msg;
81 | 
82 |   msg.header.stamp = ros::Time::now();
83 |   for(size_t i = 0; i < keys_.size(); ++i)
84 |   {
85 |     msg.data += keys_[i] + ": " + values[i];
86 |     if (i != keys_.size()-1)
87 |     msg.data += " ; ";
88 |   }
89 |   for_each( const message_actions::MessageAction& action, actions )
90 |   {
91 |     callbacks_[action](msg);
92 |   }
93 | }
94 | 
95 | } // converter
96 | } //naoqi
97 | 


--------------------------------------------------------------------------------
/src/converters/info.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 INFO_CONVERTER_HPP
19 | #define INFO_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/StringStamped.h>
31 | 
32 | 
33 | namespace naoqi
34 | {
35 | namespace converter
36 | {
37 | 
38 | class InfoConverter : public BaseConverter<InfoConverter>
39 | {
40 | 
41 |   typedef boost::function<void(naoqi_bridge_msgs::StringStamped)> Callback_t;
42 | 
43 | public:
44 |   InfoConverter( const std::string& name, float frequency, const qi::SessionPtr& sessions );
45 | 
46 |   void reset();
47 | 
48 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
49 | 
50 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
51 | 
52 | private:
53 |   /** Memory (Proxy) configurations */
54 |   qi::AnyObject p_memory_;
55 | 
56 |   /** The keys to get from ALMemory */
57 |   std::vector<std::string> keys_;
58 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
59 | 
60 |   naoqi_bridge_msgs::StringStamped msg_;
61 | };
62 | 
63 | } //converter
64 | } //naoqi
65 | 
66 | #endif
67 | 


--------------------------------------------------------------------------------
/src/converters/joint_state.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 JOINT_STATES_CONVERTER_HPP
19 | #define JOINT_STATES_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include "../tools/robot_description.hpp"
26 | #include <naoqi_driver/message_actions.h>
27 | 
28 | /*
29 | * ROS includes
30 | */
31 | #include <urdf/model.h>
32 | #include <sensor_msgs/JointState.h>
33 | #include <tf2_ros/buffer.h>
34 | #include <robot_state_publisher/robot_state_publisher.h>
35 | 
36 | namespace naoqi
37 | {
38 | namespace converter
39 | {
40 | 
41 | class JointStateConverter : public BaseConverter<JointStateConverter>
42 | {
43 | 
44 |   typedef boost::function<void(sensor_msgs::JointState&, std::vector<geometry_msgs::TransformStamped>&) > Callback_t;
45 | 
46 |   typedef boost::shared_ptr<tf2_ros::Buffer> BufferPtr;
47 | 
48 |   typedef std::map<std::string, urdf::JointMimicSharedPtr> MimicMap;
49 | 
50 | public:
51 |   JointStateConverter( const std::string& name, const float& frequency, const BufferPtr& tf2_buffer, const qi::SessionPtr& session );
52 | 
53 |   ~JointStateConverter();
54 | 
55 |   virtual void reset( );
56 | 
57 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
58 | 
59 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
60 | 
61 | private:
62 | 
63 |   /** blatently copied from robot state publisher */
64 |   void addChildren(const KDL::SegmentMap::const_iterator segment);
65 |   std::map<std::string, robot_state_publisher::SegmentPair> segments_, segments_fixed_;
66 |   void setTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time, const std::string& tf_prefix);
67 |   void setFixedTransforms(const std::string& tf_prefix, const ros::Time& time);
68 | 
69 |   /** Global Shared tf2 buffer **/
70 |   BufferPtr tf2_buffer_;
71 | 
72 |   /** Motion Proxy **/
73 |   qi::AnyObject p_motion_;
74 |   qi::AnyObject p_memory_;
75 | 
76 |   /** Registered Callbacks **/
77 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
78 | 
79 |   /** Robot Description in xml format **/
80 |   std::string robot_desc_;
81 | 
82 |   /** MimicJoint List **/
83 |   MimicMap mimic_;
84 | 
85 |   /** JointState Message **/
86 |   sensor_msgs::JointState msg_joint_states_;
87 | 
88 |   /** Transform Messages **/
89 |   std::vector<geometry_msgs::TransformStamped> tf_transforms_;
90 | 
91 | }; // class
92 | 
93 | } //publisher
94 | } // naoqi
95 | 
96 | #endif
97 | 


--------------------------------------------------------------------------------
/src/converters/laser.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 LASER_CONVERTER_HPP
19 | #define LASER_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <sensor_msgs/LaserScan.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class LaserConverter : public BaseConverter<LaserConverter>
38 | {
39 | 
40 |   typedef boost::function<void(sensor_msgs::LaserScan&)> Callback_t;
41 | 
42 | public:
43 |   LaserConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session );
44 | 
45 |   void registerCallback( message_actions::MessageAction action, Callback_t cb );
46 | 
47 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
48 | 
49 |   void reset( );
50 | 
51 |   void setLaserRanges(const float &range_min, const float &range_max);
52 | 
53 | private:
54 | 
55 |   qi::AnyObject p_memory_;
56 |   float range_min_;
57 |   float range_max_;
58 | 
59 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
60 |   sensor_msgs::LaserScan msg_;
61 | }; // class
62 | 
63 | } //publisher
64 | } // naoqi
65 | 
66 | #endif
67 | 


--------------------------------------------------------------------------------
/src/converters/log.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 CONVERTERS_LOG_HPP
19 | #define CONVERTERS_LOG_HPP
20 | 
21 | #include <rosgraph_msgs/Log.h>
22 | 
23 | #include <naoqi_driver/message_actions.h>
24 | #include "converter_base.hpp"
25 | 
26 | #include <qicore/logmanager.hpp>
27 | #include <qicore/loglistener.hpp>
28 | 
29 | namespace naoqi
30 | {
31 | namespace converter
32 | {
33 | 
34 | class LogConverter : public BaseConverter<LogConverter>
35 | {
36 | 
37 |   typedef boost::function<void(rosgraph_msgs::Log&) > Callback_t;
38 | 
39 | public:
40 |   LogConverter( const std::string& name, float frequency, const qi::SessionPtr& sessions );
41 | 
42 |   void reset( );
43 | 
44 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
45 | 
46 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
47 | 
48 | private:
49 |   /** Function that sets the NAOqi log level to the ROS one */
50 |   void set_qi_logger_level();
51 | 
52 |   qi::LogManagerPtr logger_;
53 |   /** Log level that is currently translated to ROS */
54 |   qi::LogLevel log_level_;
55 |   qi::LogListenerPtr listener_;
56 | 
57 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
58 | };
59 | 
60 | } //publisher
61 | } //naoqi
62 | 
63 | #endif
64 | 


--------------------------------------------------------------------------------
/src/converters/memory/bool.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "bool.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi
30 | {
31 | namespace converter
32 | {
33 | 
34 | MemoryBoolConverter::MemoryBoolConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key )
35 |   : BaseConverter( name, frequency, session ),
36 |     memory_key_(memory_key),
37 |     p_memory_(session->service("ALMemory").value())
38 | {}
39 | 
40 | void MemoryBoolConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
41 | {
42 |   callbacks_[action] = cb;
43 | }
44 | 
45 | bool MemoryBoolConverter::convert()
46 | {
47 |   bool success = false;
48 |   try {
49 |     bool value = p_memory_.call<bool>("getData", memory_key_);
50 |     msg_.header.stamp = ros::Time::now();
51 |     msg_.data = value;
52 |     success = true;
53 |   } catch (const std::exception& e) {
54 |     std::cerr << "Exception caught in MemoryBoolConverter: " << e.what() << std::endl;
55 |   }
56 |   return success;
57 | }
58 | 
59 | void MemoryBoolConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
60 | {
61 |   if (convert()) {
62 |     for_each( message_actions::MessageAction action, actions )
63 |     {
64 |       callbacks_[action]( msg_ );
65 |     }
66 |   }
67 | }
68 | 
69 | void MemoryBoolConverter::reset( )
70 | {}
71 | 
72 | } // publisher
73 | } //naoqi
74 | 


--------------------------------------------------------------------------------
/src/converters/memory/bool.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_BOOL_CONVERTER_HPP
19 | #define MEMORY_BOOL_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/BoolStamped.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class MemoryBoolConverter : public BaseConverter<MemoryBoolConverter>
38 | {
39 |   typedef boost::function<void(naoqi_bridge_msgs::BoolStamped&)> Callback_t;
40 | 
41 | public:
42 | 
43 |   MemoryBoolConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key );
44 | 
45 |   void reset();
46 | 
47 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
48 | 
49 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
50 | 
51 | private:
52 |   bool convert();
53 | 
54 | private:
55 |   /** Memory key to retrieve data */
56 |   std::string memory_key_;
57 |   /** Memory (Proxy) configurations */
58 |   qi::AnyObject p_memory_;
59 | 
60 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
61 |   naoqi_bridge_msgs::BoolStamped msg_;
62 | 
63 | }; // class
64 | 
65 | } //publisher
66 | } // naoqi
67 | 
68 | #endif
69 | 


--------------------------------------------------------------------------------
/src/converters/memory/float.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "float.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi
30 | {
31 | namespace converter
32 | {
33 | 
34 | MemoryFloatConverter::MemoryFloatConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key )
35 |   : BaseConverter( name, frequency, session ),
36 |     memory_key_(memory_key),
37 |     p_memory_(session->service("ALMemory").value())
38 | {}
39 | 
40 | void MemoryFloatConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
41 | {
42 |   callbacks_[action] = cb;
43 | }
44 | 
45 | bool MemoryFloatConverter::convert()
46 | {
47 |   bool success = false;
48 |   try
49 |   {
50 |     float value = p_memory_.call<float>("getData", memory_key_);
51 |     msg_.header.stamp = ros::Time::now();
52 |     msg_.data = value;
53 |     success = true;
54 |   }
55 |   catch( const std::exception& e )
56 |   {
57 |     std::cerr << "Exception caught in MemoryFloatConverter " << e.what() << std::endl;
58 |     success = false;
59 |   }
60 |   return success;
61 | }
62 | 
63 | void MemoryFloatConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
64 | {
65 |   if (convert()) {
66 |     for_each( message_actions::MessageAction action, actions )
67 |     {
68 |       callbacks_[action]( msg_ );
69 |     }
70 |   }
71 | }
72 | 
73 | void MemoryFloatConverter::reset( )
74 | {}
75 | 
76 | } // publisher
77 | } //naoqi
78 | 


--------------------------------------------------------------------------------
/src/converters/memory/float.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_FLOAT_CONVERTER_HPP
19 | #define MEMORY_FLOAT_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/FloatStamped.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class MemoryFloatConverter : public BaseConverter<MemoryFloatConverter>
38 | {
39 |   typedef boost::function<void(naoqi_bridge_msgs::FloatStamped&)> Callback_t;
40 | 
41 | public:
42 | 
43 |   MemoryFloatConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key );
44 | 
45 |   void reset();
46 | 
47 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
48 | 
49 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
50 | 
51 | private:
52 |   bool convert();
53 | 
54 | private:
55 |   /** Memory key to retrieve data */
56 |   std::string memory_key_;
57 |   /** Memory (Proxy) configurations */
58 |   qi::AnyObject p_memory_;
59 | 
60 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
61 |   naoqi_bridge_msgs::FloatStamped msg_;
62 | 
63 | }; // class
64 | 
65 | } //publisher
66 | } // naoqi
67 | 
68 | #endif
69 | 


--------------------------------------------------------------------------------
/src/converters/memory/int.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "int.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi
30 | {
31 | namespace converter
32 | {
33 | 
34 | MemoryIntConverter::MemoryIntConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key )
35 |   : BaseConverter( name, frequency, session ),
36 |     memory_key_(memory_key),
37 |     p_memory_( session->service("ALMemory").value() )
38 | {}
39 | 
40 | void MemoryIntConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
41 | {
42 |   callbacks_[action] = cb;
43 | }
44 | 
45 | bool MemoryIntConverter::convert()
46 | {
47 |   bool success = false;
48 |   try
49 |   {
50 |     int value = p_memory_.call<int>("getData", memory_key_);
51 |     msg_.header.stamp = ros::Time::now();
52 |     msg_.data = value;
53 |     success = true;
54 |   }
55 |   catch( const std::exception& e)
56 |   {
57 |     std::cerr << "Exception caught in MemoryIntConverter " << e.what() << std::endl;
58 |     success = false;
59 |   }
60 |   return success;
61 | }
62 | 
63 | void MemoryIntConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
64 | {
65 |   if (convert()) {
66 |     for_each( message_actions::MessageAction action, actions )
67 |     {
68 |       callbacks_[action]( msg_ );
69 |     }
70 |   }
71 | }
72 | 
73 | void MemoryIntConverter::reset( )
74 | {}
75 | 
76 | } // publisher
77 | } //naoqi
78 | 


--------------------------------------------------------------------------------
/src/converters/memory/int.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_INT_CONVERTER_HPP
19 | #define MEMORY_INT_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/IntStamped.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class MemoryIntConverter : public BaseConverter<MemoryIntConverter>
38 | {
39 |   typedef boost::function<void(naoqi_bridge_msgs::IntStamped&)> Callback_t;
40 | 
41 | public:
42 | 
43 |   MemoryIntConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key );
44 | 
45 |   void reset();
46 | 
47 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
48 | 
49 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
50 | 
51 | private:
52 |   bool convert();
53 | 
54 | private:
55 |   /** Memory key to retrieve data */
56 |   std::string memory_key_;
57 |   /** Memory (Proxy) configurations */
58 |   qi::AnyObject p_memory_;
59 | 
60 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
61 |   naoqi_bridge_msgs::IntStamped msg_;
62 | 
63 | }; // class
64 | 
65 | } //publisher
66 | } // naoqi
67 | 
68 | #endif
69 | 


--------------------------------------------------------------------------------
/src/converters/memory/string.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "string.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi
30 | {
31 | namespace converter
32 | {
33 | 
34 | MemoryStringConverter::MemoryStringConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key )
35 |   : BaseConverter( name, frequency, session ),
36 |     memory_key_(memory_key),
37 |     p_memory_(session->service("ALMemory").value())
38 | {}
39 | 
40 | void MemoryStringConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
41 | {
42 |   callbacks_[action] = cb;
43 | }
44 | 
45 | bool MemoryStringConverter::convert()
46 | {
47 |   bool success = false;
48 |   try
49 |   {
50 |     std::string value = p_memory_.call<std::string>("getData", memory_key_);
51 |     msg_.header.stamp = ros::Time::now();
52 |     msg_.data = value;
53 |     success = true;
54 |   }
55 |   catch( const std::exception& e )
56 |   {
57 |     std::cerr << "Exception caught in MemoryStringConverter " << e.what() << std::endl;
58 |     success = false;
59 |   }
60 |   return success;
61 | }
62 | 
63 | void MemoryStringConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
64 | {
65 |   if (convert()) {
66 |     for_each( message_actions::MessageAction action, actions )
67 |     {
68 |       callbacks_[action]( msg_ );
69 |     }
70 |   }
71 | }
72 | 
73 | void MemoryStringConverter::reset( )
74 | {}
75 | 
76 | } // publisher
77 | } //naoqi
78 | 


--------------------------------------------------------------------------------
/src/converters/memory/string.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_STRING_CONVERTER_HPP
19 | #define MEMORY_STRING_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/StringStamped.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class MemoryStringConverter : public BaseConverter<MemoryStringConverter>
38 | {
39 |   typedef boost::function<void(naoqi_bridge_msgs::StringStamped&)> Callback_t;
40 | 
41 | public:
42 | 
43 |   MemoryStringConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key );
44 | 
45 |   void reset();
46 | 
47 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
48 | 
49 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
50 | 
51 | private:
52 |   bool convert();
53 | 
54 | private:
55 |   /** Memory key to retrieve data */
56 |   std::string memory_key_;
57 |   /** Memory (Proxy) configurations */
58 |   qi::AnyObject p_memory_;
59 | 
60 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
61 |   naoqi_bridge_msgs::StringStamped msg_;
62 | 
63 | }; // class
64 | 
65 | } //publisher
66 | } // naoqi
67 | 
68 | #endif
69 | 


--------------------------------------------------------------------------------
/src/converters/memory_list.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "memory_list.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi {
30 | 
31 | namespace converter {
32 | 
33 | MemoryListConverter::MemoryListConverter(const std::vector<std::string>& key_list, const std::string &name, const float &frequency, const qi::SessionPtr &session):
34 |     BaseConverter(name, frequency, session),
35 |     p_memory_(session->service("ALMemory").value()),
36 |     _key_list(key_list)
37 | {}
38 | 
39 | void MemoryListConverter::reset(){
40 | 
41 | }
42 | 
43 | void MemoryListConverter::callAll(const std::vector<message_actions::MessageAction> &actions){
44 |   // Get inertial data
45 |   qi::AnyValue memData_anyvalue = p_memory_.call<qi::AnyValue>("getListData", _key_list);
46 | 
47 |   // Reset message
48 |   _msg = naoqi_bridge_msgs::MemoryList();
49 |   ros::Time now = ros::Time::now();
50 |   _msg.header.stamp = now;
51 | 
52 |   qi::AnyReferenceVector memData_anyref = memData_anyvalue.asListValuePtr();
53 | 
54 |   for(int i=0; i<memData_anyref.size();i++)
55 |   {
56 |     if(memData_anyref[i].content().kind() == qi::TypeKind_Int)
57 |     {
58 |       naoqi_bridge_msgs::MemoryPairInt tmp_msg;
59 |       tmp_msg.memoryKey = _key_list[i];
60 |       tmp_msg.data = memData_anyref[i].content().asInt32();
61 |       _msg.ints.push_back(tmp_msg);
62 |     }
63 |     else if(memData_anyref[i].content().kind() == qi::TypeKind_Float)
64 |     {
65 |         naoqi_bridge_msgs::MemoryPairFloat tmp_msg;
66 |         tmp_msg.memoryKey = _key_list[i];
67 |         tmp_msg.data = memData_anyref[i].content().asFloat();
68 |         _msg.floats.push_back(tmp_msg);
69 |     }
70 |     else if(memData_anyref[i].content().kind() == qi::TypeKind_String)
71 |     {
72 |       naoqi_bridge_msgs::MemoryPairString tmp_msg;
73 |       tmp_msg.memoryKey = _key_list[i];
74 |       tmp_msg.data = memData_anyref[i].content().asString();
75 |       _msg.strings.push_back(tmp_msg);
76 |     }
77 |   }
78 | 
79 |   for_each( message_actions::MessageAction action, actions )
80 |   {
81 |     callbacks_[action]( _msg);
82 |   }
83 | }
84 | 
85 | void MemoryListConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
86 | {
87 |   callbacks_[action] = cb;
88 | }
89 | 
90 | }
91 | 
92 | }
93 | 


--------------------------------------------------------------------------------
/src/converters/memory_list.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_LIST_CONVERTER_HPP
19 | #define MEMORY_LIST_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/MemoryList.h>
31 | 
32 | namespace naoqi {
33 | 
34 | namespace converter {
35 | 
36 | class MemoryListConverter : public BaseConverter<MemoryListConverter>
37 | {
38 |   typedef boost::function<void(naoqi_bridge_msgs::MemoryList&) > Callback_t;
39 | 
40 | public:
41 |   MemoryListConverter(const std::vector<std::string>& key_list, const std::string& name, const float& frequency, const qi::SessionPtr& session);
42 | 
43 |   virtual void reset();
44 | 
45 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
46 | 
47 |   virtual void callAll(const std::vector<message_actions::MessageAction>& actions );
48 | 
49 | private:
50 |   std::vector<std::string> _key_list;
51 |   naoqi_bridge_msgs::MemoryList _msg;
52 |   qi::AnyObject p_memory_;
53 |   std::vector<std::string> data_names_list_;
54 | 
55 |   /** Registered Callbacks **/
56 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
57 | };
58 | 
59 | }
60 | 
61 | }
62 | 
63 | #endif // MEMORY_LIST_CONVERTER_HPP
64 | 


--------------------------------------------------------------------------------
/src/converters/odom.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | /*
 19 | * LOCAL includes
 20 | */
 21 | #include "odom.hpp"
 22 | #include "../tools/from_any_value.hpp"
 23 | 
 24 | /*
 25 | * BOOST includes
 26 | */
 27 | #include <boost/foreach.hpp>
 28 | #define for_each BOOST_FOREACH
 29 | 
 30 | 
 31 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 32 | 
 33 | namespace naoqi
 34 | {
 35 | namespace converter
 36 | {
 37 | 
 38 | OdomConverter::OdomConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ):
 39 |   BaseConverter( name, frequency, session ),
 40 |   p_motion_(session->service("ALMotion").value())
 41 | 
 42 | {
 43 | }
 44 | 
 45 | void OdomConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
 46 | {
 47 |   callbacks_[action] = cb;
 48 | }
 49 | 
 50 | void OdomConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
 51 | {
 52 | 
 53 |   int FRAME_WORLD = 1;
 54 |   bool use_sensor = true;
 55 |   // documentation of getPosition available here: http://doc.aldebaran.com/2-1/naoqi/motion/control-cartesian.html
 56 |   std::vector<float> al_odometry_data = p_motion_.call<std::vector<float> >( "getPosition", "Torso", FRAME_WORLD, use_sensor );
 57 | 
 58 |   const ros::Time& odom_stamp = ros::Time::now();
 59 |   std::vector<float> al_speed_data = p_motion_.call<std::vector<float> >( "getRobotVelocity" );
 60 | 
 61 |   const float& odomX  =  al_odometry_data[0];
 62 |   const float& odomY  =  al_odometry_data[1];
 63 |   const float& odomZ  =  al_odometry_data[2];
 64 |   const float& odomWX =  al_odometry_data[3];
 65 |   const float& odomWY =  al_odometry_data[4];
 66 |   const float& odomWZ =  al_odometry_data[5];
 67 | 
 68 |   const float& dX = al_speed_data[0];
 69 |   const float& dY = al_speed_data[1];
 70 |   const float& dWZ = al_speed_data[2];
 71 | 
 72 |   //since all odometry is 6DOF we'll need a quaternion created from yaw
 73 |   tf2::Quaternion tf_quat;
 74 |   tf_quat.setRPY( odomWX, odomWY, odomWZ );
 75 |   geometry_msgs::Quaternion odom_quat = tf2::toMsg( tf_quat );
 76 | 
 77 |   static nav_msgs::Odometry msg_odom;
 78 |   msg_odom.header.frame_id = "odom";
 79 |   msg_odom.child_frame_id = "base_link";
 80 |   msg_odom.header.stamp = odom_stamp;
 81 | 
 82 |   msg_odom.pose.pose.orientation = odom_quat;
 83 |   msg_odom.pose.pose.position.x = odomX;
 84 |   msg_odom.pose.pose.position.y = odomY;
 85 |   msg_odom.pose.pose.position.z = odomZ;
 86 | 
 87 |   msg_odom.twist.twist.linear.x = dX;
 88 |   msg_odom.twist.twist.linear.y = dY;
 89 |   msg_odom.twist.twist.linear.z = 0;
 90 | 
 91 |   msg_odom.twist.twist.angular.x = 0;
 92 |   msg_odom.twist.twist.angular.y = 0;
 93 |   msg_odom.twist.twist.angular.z = dWZ;
 94 | 
 95 |   for_each( message_actions::MessageAction action, actions )
 96 |   {
 97 |     callbacks_[action](msg_odom);
 98 | 
 99 |   }
100 | }
101 | 
102 | void OdomConverter::reset( )
103 | {
104 | }
105 | 
106 | } //converter
107 | } // naoqi
108 | 


--------------------------------------------------------------------------------
/src/converters/odom.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 ODOM_CONVERTER_HPP
19 | #define ODOM_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <nav_msgs/Odometry.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class OdomConverter : public BaseConverter<OdomConverter>
38 | {
39 | 
40 |   typedef boost::function<void(nav_msgs::Odometry&)> Callback_t;
41 | 
42 | public:
43 |   OdomConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session );
44 | 
45 |   void registerCallback( message_actions::MessageAction action, Callback_t cb );
46 | 
47 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
48 | 
49 |   void reset( );
50 | 
51 | private:
52 | 
53 |   /** Motion Proxy **/
54 |   qi::AnyObject p_motion_;
55 | 
56 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
57 |   nav_msgs::Odometry msg_;
58 | }; // class
59 | 
60 | } //publisher
61 | } // naoqi
62 | 
63 | #endif
64 | 


--------------------------------------------------------------------------------
/src/converters/sonar.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 SONAR_CONVERTER_HPP
19 | #define SONAR_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <sensor_msgs/Range.h>
31 | 
32 | namespace naoqi
33 | {
34 | namespace converter
35 | {
36 | 
37 | class SonarConverter : public BaseConverter<SonarConverter>
38 | {
39 | 
40 |   typedef boost::function<void(std::vector<sensor_msgs::Range>&)> Callback_t;
41 | 
42 | 
43 | public:
44 |   SonarConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session );
45 | 
46 |   ~SonarConverter();
47 | 
48 |   void reset( );
49 | 
50 |   void registerCallback( message_actions::MessageAction action, Callback_t cb );
51 | 
52 |   void callAll( const std::vector<message_actions::MessageAction>& actions );
53 | 
54 | 
55 | private:
56 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
57 | 
58 |   /** Sonar (Proxy) configurations */
59 |   qi::AnyObject p_sonar_;
60 |   /** Memory (Proxy) configurations */
61 |   qi::AnyObject p_memory_;
62 |   /** Key describeing whether we are subscribed to the ALSonar module */
63 |   bool is_subscribed_;
64 | 
65 |   /** The memory keys of the sonars */
66 |   std::vector<std::string> keys_;
67 |   /** The frames of the sonars */
68 |   std::vector<std::string> frames_;
69 |   /** Pre-filled messges that are sent */
70 |   std::vector<sensor_msgs::Range> msgs_;
71 | };
72 | 
73 | } //publisher
74 | } //naoqi
75 | 
76 | #endif
77 | 


--------------------------------------------------------------------------------
/src/converters/touch.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "touch.hpp"
22 | 
23 | /*
24 | * BOOST includes
25 | */
26 | #include <boost/foreach.hpp>
27 | #define for_each BOOST_FOREACH
28 | 
29 | namespace naoqi{
30 | 
31 | namespace converter{
32 | 
33 | template <class T>
34 | TouchEventConverter<T>::TouchEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session)
35 |   : BaseConverter<TouchEventConverter<T> >(name, frequency, session)
36 | {
37 | }
38 | 
39 | template <class T>
40 | TouchEventConverter<T>::~TouchEventConverter() {
41 | }
42 | 
43 | template <class T>
44 | void TouchEventConverter<T>::reset()
45 | {
46 | }
47 | 
48 | template <class T>
49 | void TouchEventConverter<T>::registerCallback( const message_actions::MessageAction action, Callback_t cb )
50 | {
51 |   callbacks_[action] = cb;
52 | }
53 | 
54 | template <class T>
55 | void TouchEventConverter<T>::callAll(const std::vector<message_actions::MessageAction>& actions, T& msg)
56 | {
57 |   msg_ = msg;
58 |   for_each( message_actions::MessageAction action, actions )
59 |   {
60 |     callbacks_[action](msg_);
61 |   }
62 | }
63 | 
64 | // http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
65 | template class TouchEventConverter<naoqi_bridge_msgs::Bumper>;
66 | template class TouchEventConverter<naoqi_bridge_msgs::HandTouch>;
67 | template class TouchEventConverter<naoqi_bridge_msgs::HeadTouch>;
68 | }
69 | 
70 | }
71 | 


--------------------------------------------------------------------------------
/src/converters/touch.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 TOUCH_EVENT_CONVERTER_HPP
19 | #define TOUCH_EVENT_CONVERTER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "converter_base.hpp"
25 | #include <naoqi_driver/message_actions.h>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <naoqi_bridge_msgs/Bumper.h>
31 | #include <naoqi_bridge_msgs/HandTouch.h>
32 | #include <naoqi_bridge_msgs/HeadTouch.h>
33 | 
34 | /*
35 | * ALDEBARAN includes
36 | */
37 | #include <qi/anymodule.hpp>
38 | 
39 | namespace naoqi{
40 | 
41 | namespace converter{
42 | 
43 | template <class T>
44 | class TouchEventConverter : public BaseConverter<TouchEventConverter<T> >
45 | {
46 | 
47 |   typedef boost::function<void(T&) > Callback_t;
48 | 
49 | public:
50 |   TouchEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session);
51 | 
52 |   ~TouchEventConverter();
53 | 
54 |   virtual void reset();
55 | 
56 |   void registerCallback( const message_actions::MessageAction action, Callback_t cb );
57 | 
58 |   void callAll(const std::vector<message_actions::MessageAction>& actions, T& msg);
59 | 
60 | private:
61 |   /** Registered Callbacks **/
62 |   std::map<message_actions::MessageAction, Callback_t> callbacks_;
63 |   T msg_;
64 | };
65 | 
66 | }
67 | 
68 | }
69 | 
70 | #endif // TOUCH_CONVERTER_HPP
71 | 


--------------------------------------------------------------------------------
/src/driver_authenticator.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 DRIVER_AUTHENTICATOR_HPP
19 | #define DRIVER_AUTHENTICATOR_HPP
20 | 
21 | #include <qi/messaging/authprovider.hpp>
22 | 
23 | namespace naoqi {
24 | 
25 | class DriverAuthenticator : public qi::ClientAuthenticator {
26 | public:
27 |   static const std::string user_key;
28 |   static const std::string pass_key;
29 | 
30 |   DriverAuthenticator(
31 |     const std::string& user, 
32 |     const std::string& pass) : _u(user), _p(pass) {}
33 | 
34 |   qi::CapabilityMap initialAuthData() {
35 |     qi::CapabilityMap result;
36 |     result[DriverAuthenticator::user_key] = qi::AnyValue::from(_u);
37 |     result[DriverAuthenticator::pass_key] = qi::AnyValue::from(_p);
38 |     return result;
39 |   }
40 | 
41 |   qi::CapabilityMap _processAuth(const qi::CapabilityMap&) {
42 |     return qi::CapabilityMap();
43 |   }
44 | private:
45 |   std::string _u;
46 |   std::string _p;
47 | };
48 | 
49 | const std::string DriverAuthenticator::user_key = "user";
50 | const std::string DriverAuthenticator::pass_key = "token";
51 | 
52 | class DriverAuthenticatorFactory : public qi::ClientAuthenticatorFactory {
53 | public:
54 |   std::string user;
55 |   std::string pass;
56 | 
57 |   qi::ClientAuthenticatorPtr newAuthenticator() {
58 |     return boost::make_shared<DriverAuthenticator>(user, pass);
59 |   }
60 | };
61 | 
62 | }
63 | 
64 | #endif // DRIVER_AUTHENTICATOR


--------------------------------------------------------------------------------
/src/event/audio.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 AUDIO_EVENT_REGISTER_HPP
 19 | #define AUDIO_EVENT_REGISTER_HPP
 20 | 
 21 | #include <string>
 22 | 
 23 | #include <boost/make_shared.hpp>
 24 | #include <boost/shared_ptr.hpp>
 25 | #include <boost/thread/mutex.hpp>
 26 | #include <boost/enable_shared_from_this.hpp>
 27 | 
 28 | #include <qi/session.hpp>
 29 | 
 30 | #include <ros/ros.h>
 31 | #include <naoqi_bridge_msgs/AudioBuffer.h>
 32 | 
 33 | #include <naoqi_driver/tools.hpp>
 34 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 35 | 
 36 | // Converter
 37 | #include "../src/converters/audio.hpp"
 38 | // Publisher
 39 | #include "../src/publishers/basic.hpp"
 40 | // Recorder
 41 | #include "../recorder/basic_event.hpp"
 42 | 
 43 | namespace naoqi
 44 | {
 45 | 
 46 | /**
 47 | * @brief GlobalRecorder concept interface
 48 | * @note this defines an private concept struct,
 49 | * which each instance has to implement
 50 | * @note a type erasure pattern in implemented here to avoid strict inheritance,
 51 | * thus each possible publisher instance has to implement the virtual functions mentioned in the concept
 52 | */
 53 | class AudioEventRegister: public boost::enable_shared_from_this<AudioEventRegister>
 54 | {
 55 | 
 56 | public:
 57 | 
 58 |   /**
 59 |   * @brief Constructor for recorder interface
 60 |   */
 61 |   AudioEventRegister( const std::string& name, const float& frequency, const qi::SessionPtr& session );
 62 |   ~AudioEventRegister();
 63 | 
 64 |   void resetPublisher( ros::NodeHandle& nh );
 65 |   void resetRecorder( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr );
 66 | 
 67 |   void startProcess();
 68 |   void stopProcess();
 69 | 
 70 |   void writeDump(const ros::Time& time);
 71 |   void setBufferDuration(float duration);
 72 | 
 73 |   void isRecording(bool state);
 74 |   void isPublishing(bool state);
 75 |   void isDumping(bool state);
 76 | 
 77 |   void processRemote(int nbOfChannels, int samplesByChannel, qi::AnyValue altimestamp, qi::AnyValue buffer);
 78 | 
 79 | private:
 80 |   void registerCallback();
 81 |   void unregisterCallback();
 82 |   void onEvent();
 83 | 
 84 | private:
 85 |   boost::shared_ptr<converter::AudioEventConverter> converter_;
 86 |   boost::shared_ptr<publisher::BasicPublisher<naoqi_bridge_msgs::AudioBuffer> > publisher_;
 87 |   boost::shared_ptr<recorder::BasicEventRecorder<naoqi_bridge_msgs::AudioBuffer> > recorder_;
 88 | 
 89 |   qi::SessionPtr session_;
 90 |   qi::AnyObject p_audio_;
 91 |   qi::AnyObject p_robot_model_;
 92 |   qi::FutureSync<qi::AnyObject> p_audio_extractor_request;
 93 |   std::vector<uint8_t> channelMap;
 94 |   unsigned int serviceId;
 95 |   const robot::NaoqiVersion& naoqi_version_;
 96 | 
 97 |   boost::mutex subscription_mutex_;
 98 |   boost::mutex processing_mutex_;
 99 | 
100 |   bool isStarted_;
101 |   bool isPublishing_;
102 |   bool isRecording_;
103 |   bool isDumping_;
104 | 
105 | }; // class
106 | 
107 | QI_REGISTER_OBJECT(AudioEventRegister, processRemote)
108 | 
109 | } //naoqi
110 | 
111 | #endif
112 | 


--------------------------------------------------------------------------------
/src/event/basic.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 EVENT_REGISTER_HPP
19 | #define EVENT_REGISTER_HPP
20 | 
21 | #include <string>
22 | 
23 | #include <boost/make_shared.hpp>
24 | #include <boost/shared_ptr.hpp>
25 | #include <boost/thread/mutex.hpp>
26 | 
27 | #include <qi/session.hpp>
28 | 
29 | #include <ros/ros.h>
30 | 
31 | #include <naoqi_driver/tools.hpp>
32 | #include <naoqi_driver/recorder/globalrecorder.hpp>
33 | 
34 | namespace naoqi
35 | {
36 | 
37 | /**
38 | * @brief GlobalRecorder concept interface
39 | * @note this defines an private concept struct,
40 | * which each instance has to implement
41 | * @note a type erasure pattern in implemented here to avoid strict inheritance,
42 | * thus each possible publisher instance has to implement the virtual functions mentioned in the concept
43 | */
44 | template <typename Converter, typename Publisher, typename Recorder>
45 | class EventRegister
46 | {
47 | 
48 | public:
49 | 
50 |   /**
51 |   * @brief Constructor for recorder interface
52 |   */
53 |   EventRegister();
54 |   EventRegister( const std::string& key, const qi::SessionPtr& session );
55 |   ~EventRegister();
56 | 
57 |   void resetPublisher( ros::NodeHandle& nh );
58 |   void resetRecorder( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr );
59 | 
60 |   void startProcess();
61 |   void stopProcess();
62 | 
63 |   void writeDump(const ros::Time& time);
64 |   void setBufferDuration(float duration);
65 | 
66 |   void isRecording(bool state);
67 |   void isPublishing(bool state);
68 |   void isDumping(bool state);
69 | 
70 | private:
71 |   void registerCallback();
72 |   void unregisterCallback();
73 |   void onEvent();
74 | 
75 | private:
76 |   boost::shared_ptr<Converter> converter_;
77 |   boost::shared_ptr<Publisher> publisher_;
78 |   boost::shared_ptr<Recorder> recorder_;
79 | 
80 |   qi::AnyObject p_memory_;
81 |   qi::AnyObject signal_;
82 |   qi::SignalLink signalID_;
83 |   std::string key_;
84 | 
85 |   boost::mutex mutex_;
86 | 
87 |   bool isStarted_;
88 |   bool isPublishing_;
89 |   bool isRecording_;
90 |   bool isDumping_;
91 | 
92 | }; // class globalrecorder
93 | } //naoqi
94 | #include "basic.hxx"
95 | 
96 | #endif
97 | 


--------------------------------------------------------------------------------
/src/helpers/driver_helpers.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef DRIVER_HELPERS_HPP
20 | #define DRIVER_HELPERS_HPP
21 | 
22 | #include <naoqi_driver/tools.hpp>
23 | 
24 | #include <naoqi_bridge_msgs/RobotInfo.h>
25 | 
26 | #include <naoqi_bridge_msgs/SetString.h>
27 | 
28 | #include <qi/applicationsession.hpp>
29 | 
30 | namespace naoqi
31 | {
32 | namespace helpers
33 | {
34 | namespace driver
35 | {
36 | 
37 | const robot::Robot& getRobot( const qi::SessionPtr& session );
38 | 
39 | const robot::NaoqiVersion& getNaoqiVersion( const qi::SessionPtr& session );
40 | 
41 | const naoqi_bridge_msgs::RobotInfo& getRobotInfo( const qi::SessionPtr& session );
42 | 
43 | bool& setLanguage( const qi::SessionPtr& session, naoqi_bridge_msgs::SetStringRequest req );
44 | 
45 | std::string& getLanguage( const qi::SessionPtr& session );
46 | 
47 | bool isDepthStereo(const qi::SessionPtr &session);
48 | 
49 | bool isNaoqiVersionLesser(
50 |   const robot::NaoqiVersion& naoqi_version,
51 |   const int& major,
52 |   const int& minor=0,
53 |   const int& patch=0,
54 |   const int& build=0);
55 | 
56 | } // driver
57 | } // helpers
58 | } // naoqi
59 | 
60 | #endif
61 | 


--------------------------------------------------------------------------------
/src/helpers/filesystem_helpers.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | 
 19 | #ifndef FILESYSTEM_HELPERS_HPP
 20 | #define FILESYSTEM_HELPERS_HPP
 21 | 
 22 | #include <iostream>
 23 | #include <qi/session.hpp>
 24 | 
 25 | #include <boost/filesystem.hpp>
 26 | #include <boost/filesystem/operations.hpp>
 27 | #include <boost/algorithm/string/replace.hpp>
 28 | 
 29 | #ifdef CATKIN_BUILD
 30 | #include <ros/package.h>
 31 | #endif
 32 | 
 33 | namespace naoqi
 34 | {
 35 | namespace helpers
 36 | {
 37 | namespace filesystem
 38 | {
 39 | 
 40 | static const long folderMaximumSize = 2000000000;
 41 | 
 42 | inline void getFoldersize(std::string rootFolder, long& file_size){
 43 |   boost::algorithm::replace_all(rootFolder, "\\\\", "\\");
 44 |   boost::filesystem::path folderPath(rootFolder);
 45 |   if (boost::filesystem::exists(folderPath)){
 46 |     boost::filesystem::directory_iterator end_itr;
 47 | 
 48 |     for (boost::filesystem::directory_iterator dirIte(rootFolder); dirIte != end_itr; ++dirIte )
 49 |     {
 50 |       boost::filesystem::path filePath(dirIte->path());
 51 |       try{
 52 |         if (!boost::filesystem::is_directory(dirIte->status()) )
 53 |         {
 54 |           file_size = file_size + boost::filesystem::file_size(filePath);
 55 |         }else{
 56 |           getFoldersize(filePath.string(),file_size);
 57 |         }
 58 |       }catch(std::exception& e){
 59 |         std::cout << e.what() << std::endl;
 60 |       }
 61 |     }
 62 |   }
 63 | }
 64 | 
 65 | inline void getFiles(const boost::filesystem::path& root, const std::string& ext, std::vector<boost::filesystem::path>& ret)
 66 | {
 67 |   if(!boost::filesystem::exists(root) || !boost::filesystem::is_directory(root)) return;
 68 | 
 69 |   boost::filesystem::recursive_directory_iterator it(root);
 70 |   boost::filesystem::recursive_directory_iterator endit;
 71 | 
 72 |   while(it != endit)
 73 |   {
 74 |     if(boost::filesystem::is_regular_file(*it) && it->path().extension() == ext)
 75 |     {
 76 |       ret.push_back(it->path().filename());
 77 |     }
 78 |     ++it;
 79 |   }
 80 | }
 81 | 
 82 | inline void getFilesSize(const boost::filesystem::path& root, long& file_size)
 83 | {
 84 |   std::vector<boost::filesystem::path> files_path;
 85 |   getFiles(root, ".bag", files_path);
 86 |   for (std::vector<boost::filesystem::path>::const_iterator it=files_path.begin();
 87 |        it!=files_path.end(); it++)
 88 |   {
 89 |     try{
 90 |       file_size = file_size + boost::filesystem::file_size(*it);
 91 |     }catch(std::exception& e){
 92 |       std::cout << e.what() << std::endl;
 93 |     }
 94 |   }
 95 | }
 96 | 
 97 | /** Boot config loader */
 98 | static const std::string boot_config_file_name = "boot_config.json";
 99 | inline std::string& getBootConfigFile()
100 | {
101 | #ifdef CATKIN_BUILD
102 |   static std::string path = ros::package::getPath("naoqi_driver")+"/share/"+boot_config_file_name;
103 |   std::cout << "found a catkin prefix " << path << std::endl;
104 |   return path;
105 | #else
106 |   static std::string path = qi::path::findData( "/", boot_config_file_name );
107 |   std::cout << "found a qibuild path " << path << std::endl;
108 |   return path;
109 | #endif
110 | }
111 | 
112 | /* URDF loader */
113 | inline std::string& getURDF( std::string filename )
114 | {
115 | #ifdef CATKIN_BUILD
116 |   static std::string path = ros::package::getPath("naoqi_driver")+"/share/urdf/"+filename;
117 |   std::cout << "found a catkin URDF " << path << std::endl;
118 |   return path;
119 | #else
120 |   static std::string path = qi::path::findData( "/urdf/", filename );
121 |   std::cout << "found a qibuild URDF " << path << std::endl;
122 |   return path;
123 | #endif
124 | }
125 | 
126 | } // filesystem
127 | } //helpers
128 | } // naoqi
129 | 
130 | #endif
131 | 


--------------------------------------------------------------------------------
/src/helpers/naoqi_helpers.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef NAOQI_HELPERS_HPP
20 | #define NAOQI_HELPERS_HPP
21 | 
22 | namespace naoqi
23 | {
24 | namespace helpers
25 | {
26 | namespace naoqi
27 | {
28 | 
29 | static dataType::DataType getDataType(qi::AnyValue value)
30 | {
31 |   dataType::DataType type;
32 |   if (value.kind() == qi::TypeKind_Int) {
33 |     type = dataType::Int;
34 |   }
35 |   else if (value.kind() == qi::TypeKind_Float) {
36 |     type = dataType::Float;
37 |   }
38 |   else if (value.kind() == qi::TypeKind_String) {
39 |     type = dataType::String;
40 |   }
41 |   else {
42 |     throw std::runtime_error("Cannot get a valid type.");
43 |   }
44 |   return type;
45 | }
46 | 
47 | } // naoqi
48 | } // helpers
49 | } // naoqi
50 | 
51 | #endif
52 | 


--------------------------------------------------------------------------------
/src/helpers/recorder_helpers.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef RECORDER_HELPERS_HPP
20 | #define RECORDER_HELPERS_HPP
21 | 
22 | namespace naoqi
23 | {
24 | namespace helpers
25 | {
26 | namespace recorder
27 | {
28 | 
29 | static const float bufferDefaultDuration = 10.f;
30 | 
31 | } // recorder
32 | } // helpers
33 | } // naoqi
34 | 
35 | #endif
36 | 


--------------------------------------------------------------------------------
/src/helpers/transform_helpers.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef TRANSFORM_HELPERS_HPP
20 | #define TRANSFORM_HELPERS_HPP
21 | 
22 | #include <geometry_msgs/Transform.h>
23 | #include <geometry_msgs/Pose.h>
24 | #include <tf2/LinearMath/Matrix3x3.h>
25 | 
26 | namespace naoqi
27 | {
28 | namespace helpers
29 | {
30 | namespace transform
31 | {
32 | 
33 | inline double getYaw(const geometry_msgs::Pose& pose)
34 | {
35 |   double yaw, _pitch, _roll;
36 |   tf2::Matrix3x3(tf2::Quaternion(pose.orientation.x, pose.orientation.y,
37 |                                 pose.orientation.z, pose.orientation.w)).getEulerYPR(yaw, _pitch, _roll);
38 |   return yaw;
39 | }
40 | 
41 | inline double getYaw( const geometry_msgs::Transform& pose)
42 | {
43 |   double yaw, _pitch, _roll;
44 |   tf2::Matrix3x3(tf2::Quaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w)).getEulerYPR(yaw, _pitch, _roll);
45 |   return yaw;
46 | }
47 | 
48 | } //transform
49 | } //helpers
50 | } // naoqi
51 | 
52 | #endif
53 | 


--------------------------------------------------------------------------------
/src/naoqi_env.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef NAOQI_ENV_HPP
20 | #define NAOQI_ENV_HPP
21 | 
22 | /*
23 | * ALDEBARAN includes
24 | */
25 | #include <qi/os.hpp>
26 | #include <boost/algorithm/string/split.hpp>
27 | #include <boost/algorithm/string/classification.hpp>
28 | 
29 | namespace naoqi
30 | {
31 | namespace naoqi_env
32 | {
33 | 
34 | static std::string getCMakePrefixPath()
35 | {
36 |   char *cMakePrefixPath = getenv( "CMAKE_PREFIX_PATH" );
37 |   if (cMakePrefixPath != NULL) {
38 |     return getenv( "CMAKE_PREFIX_PATH" );
39 |   }
40 |   return "";
41 | }
42 | 
43 | static void adjustSDKPrefix()
44 | {
45 |   std::string cmake_prefix = getCMakePrefixPath();
46 |   std::vector<std::string> prefixes;
47 |   boost::split( prefixes, cmake_prefix, boost::is_any_of(":") );
48 | 
49 |   for (size_t i=0; i<prefixes.size(); i++)
50 |   {
51 |     std::cout << "going to add: " << prefixes[i] << std::endl;
52 |     qi::path::detail::addOptionalSdkPrefix( prefixes[i].c_str() );
53 |   }
54 | }
55 | 
56 | } // naoqi_env
57 | } // naoqi
58 | #endif
59 | 


--------------------------------------------------------------------------------
/src/publishers/basic.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 BASIC_PUBLISHER_HPP
19 | #define BASIC_PUBLISHER_HPP
20 | 
21 | #include <string>
22 | 
23 | /*
24 | * ROS includes
25 | */
26 | #include <ros/ros.h>
27 | 
28 | namespace naoqi
29 | {
30 | namespace publisher
31 | {
32 | 
33 | template<class T>
34 | class BasicPublisher
35 | {
36 | 
37 | public:
38 |   BasicPublisher( const std::string& topic ):
39 |     topic_( topic ),
40 |     is_initialized_( false )
41 |   {}
42 | 
43 |   virtual ~BasicPublisher() {}
44 | 
45 |   inline std::string topic() const
46 |   {
47 |     return topic_;
48 |   }
49 | 
50 |   inline bool isInitialized() const
51 |   {
52 |     return is_initialized_;
53 |   }
54 | 
55 |   virtual inline bool isSubscribed() const
56 |   {
57 |     if (is_initialized_ == false) return false;
58 |       return pub_.getNumSubscribers() > 0;
59 |   }
60 | 
61 |   virtual void publish( const T& msg )
62 |   {
63 |     pub_.publish( msg );
64 |   }
65 | 
66 |   virtual void reset( ros::NodeHandle& nh )
67 |   {
68 |     pub_ = nh.advertise<T>( this->topic_, 10 );
69 |     is_initialized_ = true;
70 |   }
71 | 
72 | protected:
73 |   std::string topic_;
74 | 
75 |   bool is_initialized_;
76 | 
77 |   /** Publisher */
78 |   ros::Publisher pub_;
79 | }; // class
80 | 
81 | } // publisher
82 | } // naoqi
83 | 
84 | #endif
85 | 


--------------------------------------------------------------------------------
/src/publishers/camera.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "camera.hpp"
22 | 
23 | /*
24 | * ALDEBARAN includes
25 | */
26 | #include "../tools/alvisiondefinitions.h" // for kTop...
27 | 
28 | namespace naoqi
29 | {
30 | namespace publisher
31 | {
32 | 
33 | CameraPublisher::CameraPublisher( const std::string& topic, int camera_source ):
34 |   topic_( topic ),
35 |   is_initialized_(false),
36 |   camera_source_( camera_source )
37 | {
38 | }
39 | 
40 | CameraPublisher::~CameraPublisher()
41 | {
42 | }
43 | 
44 | void CameraPublisher::publish( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info )
45 | {
46 |   pub_.publish( *img, camera_info );
47 | }
48 | 
49 | void CameraPublisher::reset( ros::NodeHandle& nh )
50 | {
51 | 
52 |   image_transport::ImageTransport it( nh );
53 |   pub_ = it.advertiseCamera( topic_, 1 );
54 | 
55 |   // Unregister compressedDepth topics for non depth cameras
56 |   if (camera_source_!=AL::kDepthCamera)
57 |   {
58 |     // Get our URI as a caller
59 |     std::string node_name = ros::this_node::getName();
60 |     XmlRpc::XmlRpcValue args, result, payload;
61 |     args[0] = node_name;
62 |     args[1] = node_name;
63 |     ros::master::execute("lookupNode", args, result, payload, false);
64 |     args[2] = result[2];
65 | 
66 |     // List the topics to remove
67 |     std::vector<std::string> topic_list;
68 |     topic_list.push_back(std::string("/") + node_name + "/" + topic_ + std::string("/compressedDepth"));
69 |     topic_list.push_back(std::string("/") + node_name + "/" + topic_ + std::string("/compressedDepth/parameter_updates"));
70 |     topic_list.push_back(std::string("/") + node_name + "/" + topic_ + std::string("/compressedDepth/parameter_descriptions"));
71 | 
72 |     // Remove undesirable topics
73 |     for(std::vector<std::string>::const_iterator topic = topic_list.begin(); topic != topic_list.end(); ++topic)
74 |     {
75 |       args[1] = *topic;
76 |       ros::master::execute("unregisterPublisher", args, result, payload, false);
77 |     }
78 |   }
79 | 
80 |   is_initialized_ = true;
81 | }
82 | 
83 | } // publisher
84 | } //naoqi
85 | 


--------------------------------------------------------------------------------
/src/publishers/camera.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 PUBLISHER_CAMERA_HPP
19 | #define PUBLISHER_CAMERA_HPP
20 | 
21 | /*
22 | * ROS includes
23 | */
24 | #include <ros/ros.h>
25 | #include <image_transport/image_transport.h>
26 | 
27 | namespace naoqi
28 | {
29 | namespace publisher
30 | {
31 | 
32 | class CameraPublisher
33 | {
34 | public:
35 |   CameraPublisher( const std::string& topic, int camera_source );
36 | 
37 |   ~CameraPublisher();
38 | 
39 |   inline std::string topic() const
40 |   {
41 |     return topic_;
42 |   }
43 | 
44 |   inline bool isInitialized() const
45 |   {
46 |     return is_initialized_;
47 |   }
48 | 
49 |   void publish( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info );
50 | 
51 |   void reset( ros::NodeHandle& nh );
52 | 
53 |   inline bool isSubscribed() const
54 |   {
55 |     if (is_initialized_ == false) return false;
56 |     return pub_.getNumSubscribers() > 0;
57 |   }
58 | 
59 | private:
60 |   std::string topic_;
61 | 
62 |   bool is_initialized_;
63 | 
64 |   //image_transport::ImageTransport it_;
65 |   image_transport::CameraPublisher pub_;
66 | 
67 |   int camera_source_;
68 | };
69 | 
70 | } //publisher
71 | } //naoqi
72 | 
73 | 
74 | #endif
75 | 


--------------------------------------------------------------------------------
/src/publishers/info.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "info.hpp"
22 | #include "../tools/robot_description.hpp"
23 | 
24 | namespace naoqi
25 | {
26 | namespace publisher
27 | {
28 | 
29 | InfoPublisher::InfoPublisher(const std::string& topic , const robot::Robot& robot_type)
30 |   : BasicPublisher( topic ),
31 |     robot_(robot_type)
32 | {
33 | }
34 | 
35 | void InfoPublisher::reset( ros::NodeHandle& nh )
36 | {
37 |   // We latch as we only publish once
38 |   pub_ = nh.advertise<naoqi_bridge_msgs::StringStamped>( topic_, 1, true );
39 | 
40 |   std::string robot_desc = naoqi::tools::getRobotDescription(robot_);
41 |   nh.setParam("/robot_description", robot_desc);
42 |   std::cout << "load robot description from file" << std::endl;
43 | 
44 |   is_initialized_ = true;
45 | }
46 | 
47 | } // publisher
48 | } //naoqi
49 | 


--------------------------------------------------------------------------------
/src/publishers/info.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 PUBLISHER_INFO_HPP
19 | #define PUBLISHER_INFO_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "basic.hpp"
25 | #include <naoqi_driver/tools.hpp>
26 | 
27 | /*
28 | * ROS includes
29 | */
30 | #include <ros/ros.h>
31 | #include <naoqi_bridge_msgs/StringStamped.h>
32 | 
33 | namespace naoqi
34 | {
35 | namespace publisher
36 | {
37 | 
38 | class InfoPublisher : public BasicPublisher<naoqi_bridge_msgs::StringStamped>
39 | {
40 | public:
41 |   InfoPublisher( const std::string& topic, const robot::Robot& robot_type );
42 | 
43 |   void reset( ros::NodeHandle& nh );
44 | 
45 |   virtual inline bool isSubscribed() const
46 |   {
47 |     return true;
48 |   }
49 | 
50 | protected:
51 |   const robot::Robot& robot_;
52 | };
53 | 
54 | } //publisher
55 | } //naoqi
56 | 
57 | #endif
58 | 


--------------------------------------------------------------------------------
/src/publishers/joint_state.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "joint_state.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | JointStatePublisher::JointStatePublisher( const std::string& topic ):
29 |   topic_( topic ),
30 |   is_initialized_( false )
31 | {}
32 | 
33 | void JointStatePublisher::publish( const sensor_msgs::JointState& js_msg,
34 |                                    const std::vector<geometry_msgs::TransformStamped>& tf_transforms )
35 | {
36 |   pub_joint_states_.publish( js_msg );
37 | 
38 |   /**
39 |    * ROBOT STATE PUBLISHER
40 |    */
41 |   tf_broadcasterPtr_->sendTransform(tf_transforms);
42 | }
43 | 
44 | 
45 | void JointStatePublisher::reset( ros::NodeHandle& nh )
46 | {
47 |   pub_joint_states_ = nh.advertise<sensor_msgs::JointState>( topic_, 10 );
48 | 
49 |   tf_broadcasterPtr_ = boost::make_shared<tf2_ros::TransformBroadcaster>();
50 | 
51 |   is_initialized_ = true;
52 | }
53 | 
54 | bool JointStatePublisher::isSubscribed() const
55 | {
56 |   // assume JS and TF as essential, so publish always
57 |   return true;
58 | }
59 | 
60 | } //publisher
61 | } // naoqi
62 | 


--------------------------------------------------------------------------------
/src/publishers/joint_state.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 JOINT_STATES_PUBLISHER_HPP
19 | #define JOINT_STATES_PUBLISHER_HPP
20 | 
21 | /*
22 | * ROS includes
23 | */
24 | #include <ros/ros.h>
25 | #include <geometry_msgs/Transform.h>
26 | #include <sensor_msgs/JointState.h>
27 | #include <tf2_ros/transform_broadcaster.h>
28 | 
29 | namespace naoqi
30 | {
31 | namespace publisher
32 | {
33 | 
34 | class JointStatePublisher
35 | {
36 | 
37 | public:
38 |   JointStatePublisher( const std::string& topic = "/joint_states" );
39 | 
40 |   inline std::string topic() const
41 |   {
42 |   return topic_;
43 |   }
44 | 
45 |   inline bool isInitialized() const
46 |   {
47 |   return is_initialized_;
48 |   }
49 | 
50 |   virtual void publish( const sensor_msgs::JointState& js_msg,
51 |                         const std::vector<geometry_msgs::TransformStamped>& tf_transforms );
52 | 
53 |   virtual void reset( ros::NodeHandle& nh );
54 | 
55 |   virtual bool isSubscribed() const;
56 | 
57 | private:
58 |   boost::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcasterPtr_;
59 | 
60 |   /** initialize separate publishers for js and odom */
61 |   ros::Publisher pub_joint_states_;
62 | 
63 |   std::string topic_;
64 | 
65 |   bool is_initialized_;
66 | 
67 | }; // class
68 | 
69 | } //publisher
70 | } // naoqi
71 | 
72 | #endif
73 | 


--------------------------------------------------------------------------------
/src/publishers/log.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "log.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | LogPublisher::LogPublisher(const std::string &topic )
29 |   : BasicPublisher( topic )
30 | {
31 | }
32 | 
33 | } // publisher
34 | } //naoqi
35 | 


--------------------------------------------------------------------------------
/src/publishers/log.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 PUBLISHER_LOG_HPP
19 | #define PUBLISHER_LOG_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "basic.hpp"
25 | 
26 | /*
27 | * ROS includes
28 | */
29 | #include <ros/ros.h>
30 | #include <rosgraph_msgs/Log.h>
31 | #include <ros/serialization.h>
32 | #include <std_msgs/String.h>
33 | 
34 | /*
35 | * BOOST includes
36 | */
37 | #include <boost/algorithm/string.hpp>
38 | #include <boost/thread/mutex.hpp>
39 | 
40 | /*
41 | * ALDEBARAN includes
42 | */
43 | #include <qi/anyobject.hpp>
44 | #include <qicore/logmessage.hpp>
45 | #include <qicore/logmanager.hpp>
46 | #include <qicore/loglistener.hpp>
47 | 
48 | namespace naoqi
49 | {
50 | namespace publisher
51 | {
52 | 
53 | class LogPublisher : public BasicPublisher<rosgraph_msgs::Log>
54 | {
55 | public:
56 |   LogPublisher(const std::string& topic);
57 | 
58 |   // check whether a real copy of the log message should be more safe
59 |   // remove const ref here
60 | 
61 |   inline bool isSubscribed() const
62 |   {
63 |     // We assume it is essential
64 |     return true;
65 |   }
66 | 
67 | private:
68 |   ros::Publisher pub_;
69 | 
70 | };
71 | 
72 | } //publisher
73 | } //naoqi
74 | 
75 | #endif
76 | 


--------------------------------------------------------------------------------
/src/publishers/memory/bool.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "bool.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | MemoryBoolPublisher::MemoryBoolPublisher( const std::string& topic ):
29 |   BasePublisher( topic )
30 | {}
31 | 
32 | void MemoryBoolPublisher::publish(const naoqi_bridge_msgs::BoolStamped& msg )
33 | {
34 |   pub_.publish( msg );
35 | }
36 | 
37 | void MemoryBoolPublisher::reset( ros::NodeHandle& nh )
38 | {
39 |   pub_ = nh.advertise< naoqi_bridge_msgs::BoolStamped >( topic_, 10 );
40 |   is_initialized_ = true;
41 | }
42 | 
43 | } //publisher
44 | } // naoqi
45 | 


--------------------------------------------------------------------------------
/src/publishers/memory/bool.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_BOOL_PUBLISHER_HPP
19 | #define MEMORY_BOOL_PUBLISHER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../publisher_base.hpp"
25 | 
26 | /*
27 | * ROS includes
28 | */
29 | #include <naoqi_bridge_msgs/BoolStamped.h>
30 | 
31 | namespace naoqi
32 | {
33 | namespace publisher
34 | {
35 | 
36 | class MemoryBoolPublisher : public BasePublisher<naoqi_bridge_msgs::BoolStamped>
37 | {
38 | 
39 | public:
40 |   MemoryBoolPublisher( const std::string& topic );
41 | 
42 |   void publish( const naoqi_bridge_msgs::BoolStamped& msg );
43 | 
44 |   void reset( ros::NodeHandle& nh );
45 | 
46 | }; // class
47 | 
48 | } //publisher
49 | } // naoqi
50 | 
51 | #endif
52 | 


--------------------------------------------------------------------------------
/src/publishers/memory/float.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "float.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | MemoryFloatPublisher::MemoryFloatPublisher( const std::string& topic ):
29 |   BasePublisher( topic )
30 | {}
31 | 
32 | void MemoryFloatPublisher::publish( const naoqi_bridge_msgs::FloatStamped& msg )
33 | {
34 |   pub_.publish( msg );
35 | }
36 | 
37 | void MemoryFloatPublisher::reset( ros::NodeHandle& nh )
38 | {
39 |   pub_ = nh.advertise< naoqi_bridge_msgs::FloatStamped >( topic_, 10 );
40 |   is_initialized_ = true;
41 | }
42 | 
43 | } //publisher
44 | } // naoqi
45 | 


--------------------------------------------------------------------------------
/src/publishers/memory/float.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_FLOAT_PUBLISHER_HPP
19 | #define MEMORY_FLOAT_PUBLISHER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../publisher_base.hpp"
25 | 
26 | /*
27 | * ROS includes
28 | */
29 | #include <naoqi_bridge_msgs/FloatStamped.h>
30 | 
31 | namespace naoqi
32 | {
33 | namespace publisher
34 | {
35 | 
36 | class MemoryFloatPublisher : public BasePublisher<naoqi_bridge_msgs::FloatStamped>
37 | {
38 | 
39 | public:
40 |   MemoryFloatPublisher( const std::string& topic );
41 | 
42 |   void publish( const naoqi_bridge_msgs::FloatStamped& msg );
43 | 
44 |   void reset( ros::NodeHandle& nh );
45 | 
46 | }; // class
47 | 
48 | } //publisher
49 | } // naoqi
50 | 
51 | #endif
52 | 


--------------------------------------------------------------------------------
/src/publishers/memory/int.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "int.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | MemoryIntPublisher::MemoryIntPublisher( const std::string& topic ):
29 |   BasePublisher( topic )
30 | {}
31 | 
32 | void MemoryIntPublisher::publish(const naoqi_bridge_msgs::IntStamped& msg )
33 | {
34 |   pub_.publish( msg );
35 | }
36 | 
37 | void MemoryIntPublisher::reset( ros::NodeHandle& nh )
38 | {
39 |   pub_ = nh.advertise< naoqi_bridge_msgs::IntStamped >( topic_, 10 );
40 |   is_initialized_ = true;
41 | }
42 | 
43 | } //publisher
44 | } // naoqi
45 | 


--------------------------------------------------------------------------------
/src/publishers/memory/int.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_INT_PUBLISHER_HPP
19 | #define MEMORY_INT_PUBLISHER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../publisher_base.hpp"
25 | 
26 | /*
27 | * ROS includes
28 | */
29 | #include <naoqi_bridge_msgs/IntStamped.h>
30 | 
31 | namespace naoqi
32 | {
33 | namespace publisher
34 | {
35 | 
36 | class MemoryIntPublisher : public BasePublisher<naoqi_bridge_msgs::IntStamped>
37 | {
38 | 
39 | public:
40 |   MemoryIntPublisher( const std::string& topic );
41 | 
42 |   void publish( const naoqi_bridge_msgs::IntStamped& msg );
43 | 
44 |   void reset( ros::NodeHandle& nh );
45 | 
46 | }; // class
47 | 
48 | } //publisher
49 | } // naoqi
50 | 
51 | #endif
52 | 


--------------------------------------------------------------------------------
/src/publishers/memory/string.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "string.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | MemoryStringPublisher::MemoryStringPublisher( const std::string& topic ):
29 |     BasePublisher( topic )
30 | {
31 | }
32 | 
33 | void MemoryStringPublisher::publish( const naoqi_bridge_msgs::StringStamped& msg )
34 | {
35 |   pub_.publish( msg );
36 | }
37 | 
38 | void MemoryStringPublisher::reset( ros::NodeHandle& nh )
39 | {
40 |   pub_ = nh.advertise<naoqi_bridge_msgs::StringStamped>( topic_, 10 );
41 |   is_initialized_ = true;
42 | }
43 | 
44 | } //publisher
45 | } // naoqi
46 | 


--------------------------------------------------------------------------------
/src/publishers/memory/string.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 MEMORY_STRING_PUBLISHER_HPP
19 | #define MEMORY_STRING_PUBLISHER_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include "../publisher_base.hpp"
25 | 
26 | /*
27 | * ROS includes
28 | */
29 | #include <naoqi_bridge_msgs/StringStamped.h>
30 | 
31 | namespace naoqi
32 | {
33 | namespace publisher
34 | {
35 | 
36 | class MemoryStringPublisher : public BasePublisher<naoqi_bridge_msgs::StringStamped>
37 | {
38 | 
39 | public:
40 |   MemoryStringPublisher( const std::string& topic );
41 | 
42 |   void publish( const naoqi_bridge_msgs::StringStamped& msg );
43 | 
44 |   void reset( ros::NodeHandle& nh );
45 | }; // class
46 | 
47 | } //publisher
48 | } // naoqi
49 | 
50 | #endif
51 | 


--------------------------------------------------------------------------------
/src/publishers/sonar.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "sonar.hpp"
22 | 
23 | namespace naoqi
24 | {
25 | namespace publisher
26 | {
27 | 
28 | SonarPublisher::SonarPublisher( const std::vector<std::string>& topics )
29 |   : is_initialized_(false),
30 |   topics_(topics)
31 | {
32 | }
33 | 
34 | void SonarPublisher::publish( const std::vector<sensor_msgs::Range>& sonar_msgs )
35 | {
36 |   if ( pubs_.size() != sonar_msgs.size() )
37 |   {
38 |     std::cerr << "Incorrect number of sonar range messages in sonar publisher. " << sonar_msgs.size() << "/" << pubs_.size() << std::endl;
39 |     return;
40 |   }
41 | 
42 |   for( size_t i=0; i<sonar_msgs.size(); ++i)
43 |   {
44 |     pubs_[i].publish( sonar_msgs[i] );
45 |   }
46 | }
47 | 
48 | void SonarPublisher::reset( ros::NodeHandle& nh )
49 | {
50 |   pubs_ = std::vector<ros::Publisher>();
51 |   for( size_t i=0; i<topics_.size(); ++i)
52 |   {
53 |     pubs_.push_back( nh.advertise<sensor_msgs::Range>(topics_[i], 1) );
54 |   }
55 | 
56 |   is_initialized_ = true;
57 | }
58 | 
59 | } // publisher
60 | } //naoqi
61 | 


--------------------------------------------------------------------------------
/src/publishers/sonar.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 PUBLISHER_SONAR_HPP
19 | #define PUBLISHER_SONAR_HPP
20 | 
21 | /*
22 | * ROS includes
23 | */
24 | #include <ros/ros.h>
25 | #include <sensor_msgs/Range.h>
26 | 
27 | namespace naoqi
28 | {
29 | namespace publisher
30 | {
31 | 
32 | class SonarPublisher
33 | {
34 | public:
35 |   SonarPublisher( const std::vector<std::string>& topics );
36 | 
37 |   inline std::string topic() const
38 |   {
39 |     return "sonar";
40 |   }
41 | 
42 |   inline bool isInitialized() const
43 |   {
44 |     return is_initialized_;
45 |   }
46 | 
47 |   void publish( const std::vector<sensor_msgs::Range>& sonar_msgs );
48 | 
49 |   void reset( ros::NodeHandle& nh );
50 | 
51 |   inline bool isSubscribed() const
52 |   {
53 |     if (is_initialized_ == false) return false;
54 |     for(std::vector<ros::Publisher>::const_iterator it = pubs_.begin(); it != pubs_.end(); ++it)
55 |       if (it->getNumSubscribers())
56 |         return true;
57 |     return false;
58 |   }
59 | 
60 | private:
61 |   std::vector<std::string> topics_;
62 |   std::vector<ros::Publisher> pubs_;
63 |   bool is_initialized_;
64 | 
65 | };
66 | 
67 | } //publisher
68 | } //naoqi
69 | 
70 | #endif
71 | 


--------------------------------------------------------------------------------
/src/recorder/basic.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 BASIC_RECORDER_HPP
 19 | #define BASIC_RECORDER_HPP
 20 | 
 21 | /*
 22 | * LOCAL includes
 23 | */
 24 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 25 | #include "../helpers/recorder_helpers.hpp"
 26 | 
 27 | /*
 28 | * STANDARD includes
 29 | */
 30 | #include <string>
 31 | #include <boost/circular_buffer.hpp>
 32 | 
 33 | namespace naoqi
 34 | {
 35 | namespace recorder
 36 | {
 37 | 
 38 | template<class T>
 39 | class BasicRecorder
 40 | {
 41 | 
 42 | public:
 43 |   BasicRecorder( const std::string& topic, float buffer_frequency = 0 ):
 44 |     topic_( topic ),
 45 |     buffer_duration_( helpers::recorder::bufferDefaultDuration ),
 46 |     is_initialized_( false ),
 47 |     is_subscribed_( false ),
 48 |     buffer_frequency_(buffer_frequency),
 49 |     counter_(1)
 50 |   {}
 51 | 
 52 |   virtual ~BasicRecorder() {}
 53 | 
 54 |   inline std::string topic() const
 55 |   {
 56 |     return topic_;
 57 |   }
 58 | 
 59 |   inline bool isInitialized() const
 60 |   {
 61 |     return is_initialized_;
 62 |   }
 63 | 
 64 |   inline void subscribe( bool state)
 65 |   {
 66 |     is_subscribed_ = state;
 67 |   }
 68 | 
 69 |   inline bool isSubscribed() const
 70 |   {
 71 |     return is_subscribed_;
 72 |   }
 73 | 
 74 |   virtual void write(const T& msg)
 75 |   {
 76 |     if (!msg.header.stamp.isZero()) {
 77 |       gr_->write(topic_, msg, msg.header.stamp);
 78 |     }
 79 |     else {
 80 |       gr_->write(topic_, msg);
 81 |     }
 82 |   }
 83 | 
 84 |   virtual void writeDump(const ros::Time& time)
 85 |   {
 86 |     boost::mutex::scoped_lock lock_write_buffer( mutex_ );
 87 |     typename boost::circular_buffer<T>::iterator it;
 88 |     for (it = buffer_.begin(); it != buffer_.end(); it++)
 89 |     {
 90 |       if (!it->header.stamp.isZero()) {
 91 |         gr_->write(topic_, *it, it->header.stamp);
 92 |       }
 93 |       else {
 94 |         gr_->write(topic_, *it);
 95 |       }
 96 |     }
 97 |   }
 98 | 
 99 |   virtual void bufferize(const T& msg)
100 |   {
101 |     boost::mutex::scoped_lock lock_bufferize( mutex_ );
102 |     if (counter_ < max_counter_)
103 |     {
104 |       counter_++;
105 |     }
106 |     else
107 |     {
108 |       counter_ = 1;
109 |       buffer_.push_back(msg);
110 |     }
111 |   }
112 | 
113 |   virtual void reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
114 |   {
115 |     gr_ = gr;
116 |     conv_frequency_ = conv_frequency;
117 |     if (buffer_frequency_ != 0)
118 |     {
119 |       max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
120 |       buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
121 |     }
122 |     else
123 |     {
124 |       max_counter_ = 1;
125 |       buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
126 |     }
127 |     buffer_.resize(buffer_size_);
128 |     is_initialized_ = true;
129 |   }
130 | 
131 |   virtual void setBufferDuration(float duration)
132 |   {
133 |     boost::mutex::scoped_lock lock_bufferize( mutex_ );
134 |     buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
135 |     buffer_duration_ = duration;
136 |     buffer_.set_capacity(buffer_size_);
137 |   }
138 | 
139 | protected:
140 |   std::string topic_;
141 | 
142 |   boost::circular_buffer<T> buffer_;
143 |   size_t buffer_size_;
144 |   float buffer_duration_;
145 |   boost::mutex mutex_;
146 | 
147 |   bool is_initialized_;
148 |   bool is_subscribed_;
149 | 
150 |   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
151 | 
152 |   float buffer_frequency_;
153 |   float conv_frequency_;
154 |   int counter_;
155 |   int max_counter_;
156 | 
157 | }; // class
158 | 
159 | } // publisher
160 | } // naoqi
161 | 
162 | #endif
163 | 


--------------------------------------------------------------------------------
/src/recorder/camera.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | /*
 19 | * LOCAL includes
 20 | */
 21 | #include "camera.hpp"
 22 | 
 23 | namespace naoqi
 24 | {
 25 | namespace recorder
 26 | {
 27 | 
 28 | CameraRecorder::CameraRecorder( const std::string& topic_, float buffer_frequency ):
 29 |   buffer_frequency_(buffer_frequency),
 30 |   buffer_duration_( helpers::recorder::bufferDefaultDuration ),
 31 |   counter_(1)
 32 | {
 33 |   topic_info_ = topic_ + "/camera_info";
 34 |   topic_img_ = topic_ + "/image_raw";
 35 | }
 36 | 
 37 | void CameraRecorder::write(const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info)
 38 | {
 39 |   if (!img->header.stamp.isZero()) {
 40 |     gr_->write(topic_img_, *img, img->header.stamp);
 41 |   }
 42 |   else {
 43 |     gr_->write(topic_img_, *img);
 44 |   }
 45 |   if (!camera_info.header.stamp.isZero()) {
 46 |     gr_->write(topic_info_, camera_info, camera_info.header.stamp);
 47 |   }
 48 |   else {
 49 |     gr_->write(topic_info_, camera_info);
 50 |   }
 51 | }
 52 | 
 53 | void CameraRecorder::writeDump(const ros::Time& time)
 54 | {
 55 |   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
 56 |   boost::circular_buffer< std::pair<sensor_msgs::ImagePtr, sensor_msgs::CameraInfo> >::iterator it;
 57 |   for (it = buffer_.begin(); it != buffer_.end(); it++)
 58 |   {
 59 |     if (it->first != NULL)
 60 |     {
 61 |       write(it->first, it->second);
 62 |     }
 63 |   }
 64 | }
 65 | 
 66 | void CameraRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
 67 | {
 68 |   gr_ = gr;
 69 |   conv_frequency_ = conv_frequency;
 70 |   max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
 71 |   buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
 72 |   buffer_.resize(buffer_size_);
 73 |   is_initialized_ = true;
 74 | }
 75 | 
 76 | void CameraRecorder::bufferize( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info )
 77 | {
 78 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 79 |   if (counter_ < max_counter_)
 80 |   {
 81 |     counter_++;
 82 |   }
 83 |   else
 84 |   {
 85 |     counter_ = 1;
 86 |     buffer_.push_back(std::make_pair(img, camera_info));
 87 |   }
 88 | }
 89 | 
 90 | void CameraRecorder::setBufferDuration(float duration)
 91 | {
 92 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 93 |   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
 94 |   buffer_duration_ = duration;
 95 |   buffer_.set_capacity(buffer_size_);
 96 | }
 97 | 
 98 | } //publisher
 99 | } // naoqi
100 | 


--------------------------------------------------------------------------------
/src/recorder/camera.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 CAMERA_RECORDER_HPP
 19 | #define CAMERA_RECORDER_HPP
 20 | 
 21 | /*
 22 | * BOOST includes
 23 | */
 24 | #include <boost/circular_buffer.hpp>
 25 | 
 26 | /*
 27 | * LOCAL includes
 28 | */
 29 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 30 | #include "../helpers/recorder_helpers.hpp"
 31 | 
 32 | /*
 33 | * ROS includes
 34 | */
 35 | #include <sensor_msgs/CameraInfo.h>
 36 | #include <sensor_msgs/Image.h>
 37 | 
 38 | namespace naoqi
 39 | {
 40 | namespace recorder
 41 | {
 42 | 
 43 | class CameraRecorder
 44 | {
 45 | 
 46 | public:
 47 |   CameraRecorder(const std::string& topic, float buffer_frequency );
 48 | 
 49 |   void write( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info );
 50 | 
 51 |   void reset(boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr, float conv_frequency );
 52 | 
 53 |   void bufferize( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info );
 54 | 
 55 |   void writeDump(const ros::Time& time);
 56 | 
 57 |   void setBufferDuration(float duration);
 58 | 
 59 |   inline std::string topic() const
 60 |   {
 61 |     return topic_img_;
 62 |   }
 63 | 
 64 |   inline bool isInitialized() const
 65 |   {
 66 |     return is_initialized_;
 67 |   }
 68 | 
 69 |   inline void subscribe( bool state)
 70 |   {
 71 |     is_subscribed_ = state;
 72 |   }
 73 | 
 74 |   inline bool isSubscribed() const
 75 |   {
 76 |     return is_subscribed_;
 77 |   }
 78 | 
 79 | protected:
 80 |   bool is_initialized_;
 81 |   bool is_subscribed_;
 82 | 
 83 |   boost::circular_buffer< std::pair<sensor_msgs::ImagePtr, sensor_msgs::CameraInfo> > buffer_;
 84 |   size_t buffer_size_;
 85 |   float buffer_duration_;
 86 | 
 87 |   boost::mutex mutex_;
 88 | 
 89 |   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
 90 |   std::string topic_info_;
 91 |   std::string topic_img_;
 92 | 
 93 |   float buffer_frequency_;
 94 |   float conv_frequency_;
 95 |   int counter_;
 96 |   int max_counter_;
 97 | 
 98 | }; // class
 99 | 
100 | } //publisher
101 | } // naoqi
102 | 
103 | #endif
104 | 


--------------------------------------------------------------------------------
/src/recorder/diagnostics.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | /*
 19 | * LOCAL includes
 20 | */
 21 | #include "diagnostics.hpp"
 22 | 
 23 | namespace naoqi
 24 | {
 25 | namespace recorder
 26 | {
 27 | 
 28 | DiagnosticsRecorder::DiagnosticsRecorder( const std::string& topic, float buffer_frequency ):
 29 |   topic_( topic ),
 30 |   buffer_duration_( helpers::recorder::bufferDefaultDuration ),
 31 |   is_initialized_( false ),
 32 |   is_subscribed_( false ),
 33 |   buffer_frequency_(buffer_frequency),
 34 |   counter_(1)
 35 | {}
 36 | 
 37 | void DiagnosticsRecorder::write(diagnostic_msgs::DiagnosticArray& msg)
 38 | {
 39 |   if (!msg.header.stamp.isZero()) {
 40 |     gr_->write(topic_, msg, msg.header.stamp);
 41 |   }
 42 |   else {
 43 |     gr_->write(topic_, msg);
 44 |   }
 45 | }
 46 | 
 47 | void DiagnosticsRecorder::writeDump(const ros::Time& time)
 48 | {
 49 |   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
 50 |   boost::circular_buffer<diagnostic_msgs::DiagnosticArray>::iterator it;
 51 |   for (it = buffer_.begin(); it != buffer_.end(); it++)
 52 |   {
 53 |     if (!it->header.stamp.isZero()) {
 54 |       gr_->write(topic_, *it, it->header.stamp);
 55 |     }
 56 |     else {
 57 |       gr_->write(topic_, *it);
 58 |     }
 59 |   }
 60 | }
 61 | 
 62 | void DiagnosticsRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
 63 | {
 64 |   gr_ = gr;
 65 |   conv_frequency_ = conv_frequency;
 66 |   if (buffer_frequency_ != 0)
 67 |   {
 68 |     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
 69 |     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
 70 |   }
 71 |   else
 72 |   {
 73 |     max_counter_ = 1;
 74 |     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
 75 |   }
 76 |   buffer_.resize(buffer_size_);
 77 |   is_initialized_ = true;
 78 | }
 79 | 
 80 | void DiagnosticsRecorder::bufferize(diagnostic_msgs::DiagnosticArray& msg )
 81 | {
 82 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 83 |   if (counter_ < max_counter_)
 84 |   {
 85 |     counter_++;
 86 |   }
 87 |   else
 88 |   {
 89 |     counter_ = 1;
 90 |     buffer_.push_back(msg);
 91 |   }
 92 | }
 93 | 
 94 | void DiagnosticsRecorder::setBufferDuration(float duration)
 95 | {
 96 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 97 |   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
 98 |   buffer_duration_ = duration;
 99 |   buffer_.set_capacity(buffer_size_);
100 | }
101 | 
102 | } //publisher
103 | } // naoqi
104 | 


--------------------------------------------------------------------------------
/src/recorder/diagnostics.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 DIAGNOSTICS_RECORDER_HPP
 19 | #define DIAGNOSTICS_RECORDER_HPP
 20 | 
 21 | /*
 22 | * BOOST includes
 23 | */
 24 | #include <boost/circular_buffer.hpp>
 25 | 
 26 | /*
 27 | * LOCAL includes
 28 | */
 29 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 30 | #include "../helpers/recorder_helpers.hpp"
 31 | 
 32 | /*
 33 | * ROS includes
 34 | */
 35 | #include <diagnostic_msgs/DiagnosticArray.h>
 36 | 
 37 | namespace naoqi
 38 | {
 39 | namespace recorder
 40 | {
 41 | 
 42 | class DiagnosticsRecorder
 43 | {
 44 | 
 45 | public:
 46 |   DiagnosticsRecorder( const std::string& topic, float buffer_frequency = 0 );
 47 | 
 48 |   void write(diagnostic_msgs::DiagnosticArray& msg );
 49 | 
 50 |   void reset( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr, float conv_frequency );
 51 | 
 52 |   void bufferize(diagnostic_msgs::DiagnosticArray& msg );
 53 | 
 54 |   void writeDump(const ros::Time& time);
 55 | 
 56 |   void setBufferDuration(float duration);
 57 | 
 58 |   inline std::string topic() const
 59 |   {
 60 |     return topic_;
 61 |   }
 62 | 
 63 |   inline bool isInitialized() const
 64 |   {
 65 |     return is_initialized_;
 66 |   }
 67 | 
 68 |   inline void subscribe( bool state)
 69 |   {
 70 |     is_subscribed_ = state;
 71 |   }
 72 | 
 73 |   inline bool isSubscribed() const
 74 |   {
 75 |     return is_subscribed_;
 76 |   }
 77 | 
 78 | protected:
 79 |   std::string topic_;
 80 | 
 81 |   boost::circular_buffer<diagnostic_msgs::DiagnosticArray> buffer_;
 82 |   size_t buffer_size_;
 83 |   float buffer_duration_;
 84 | 
 85 |   boost::mutex mutex_;
 86 | 
 87 |   bool is_initialized_;
 88 |   bool is_subscribed_;
 89 | 
 90 |   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
 91 | 
 92 |   float buffer_frequency_;
 93 |   float conv_frequency_;
 94 |   int counter_;
 95 |   int max_counter_;
 96 | 
 97 | }; // class
 98 | 
 99 | } //publisher
100 | } // naoqi
101 | 
102 | #endif
103 | 


--------------------------------------------------------------------------------
/src/recorder/joint_state.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | /*
 19 | * LOCAL includes
 20 | */
 21 | #include "joint_state.hpp"
 22 | 
 23 | namespace naoqi
 24 | {
 25 | namespace recorder
 26 | {
 27 | 
 28 | JointStateRecorder::JointStateRecorder( const std::string& topic, float buffer_frequency ):
 29 |   topic_( topic ),
 30 |   buffer_duration_(helpers::recorder::bufferDefaultDuration),
 31 |   is_initialized_( false ),
 32 |   is_subscribed_( false ),
 33 |   buffer_frequency_(buffer_frequency),
 34 |   counter_(1)
 35 | {}
 36 | 
 37 | void JointStateRecorder::write( const sensor_msgs::JointState& js_msg,
 38 |                                 const std::vector<geometry_msgs::TransformStamped>& tf_transforms )
 39 | {
 40 |   if (!js_msg.header.stamp.isZero()) {
 41 |     gr_->write(topic_, js_msg, js_msg.header.stamp);
 42 |   }
 43 |   else {
 44 |     gr_->write(topic_, js_msg);
 45 |   }
 46 |   gr_->write("/tf", tf_transforms);
 47 | }
 48 | 
 49 | void JointStateRecorder::writeDump(const ros::Time& time)
 50 | {
 51 |   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
 52 |   boost::circular_buffer< std::vector<geometry_msgs::TransformStamped> >::iterator it_tf;
 53 |   for (it_tf = bufferTF_.begin(); it_tf != bufferTF_.end(); it_tf++)
 54 |   {
 55 |     gr_->write("/tf", *it_tf);
 56 |   }
 57 |   for (boost::circular_buffer<sensor_msgs::JointState>::iterator it_js = bufferJoinState_.begin();
 58 |        it_js != bufferJoinState_.end(); it_js++)
 59 |   {
 60 |     if (!it_js->header.stamp.isZero()) {
 61 |       gr_->write(topic_, *it_js, it_js->header.stamp);
 62 |     }
 63 |     else {
 64 |       gr_->write(topic_, *it_js);
 65 |     }
 66 |   }
 67 | }
 68 | 
 69 | void JointStateRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
 70 | {
 71 |   gr_ = gr;
 72 |   conv_frequency_ = conv_frequency;
 73 |   if (buffer_frequency_ != 0)
 74 |   {
 75 |     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
 76 |     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
 77 |   }
 78 |   else
 79 |   {
 80 |     max_counter_ = 1;
 81 |     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
 82 |   }
 83 |   bufferJoinState_.resize(buffer_size_);
 84 |   bufferTF_.resize(buffer_size_);
 85 |   is_initialized_ = true;
 86 | }
 87 | 
 88 | void JointStateRecorder::bufferize( const sensor_msgs::JointState& js_msg,
 89 |                 const std::vector<geometry_msgs::TransformStamped>& tf_transforms )
 90 | {
 91 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 92 |   if (counter_ < max_counter_)
 93 |   {
 94 |     counter_++;
 95 |   }
 96 |   else
 97 |   {
 98 |     counter_ = 1;
 99 |     bufferJoinState_.push_back(js_msg);
100 |     bufferTF_.push_back(tf_transforms);
101 |   }
102 | }
103 | 
104 | void JointStateRecorder::setBufferDuration(float duration)
105 | {
106 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
107 |   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
108 |   buffer_duration_ = duration;
109 |   bufferJoinState_.set_capacity(buffer_size_);
110 |   bufferTF_.set_capacity(buffer_size_);
111 | }
112 | 
113 | } //publisher
114 | } // naoqi
115 | 


--------------------------------------------------------------------------------
/src/recorder/joint_state.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 JOINT_STATE_RECORDER_HPP
 19 | #define JOINT_STATE_RECORDER_HPP
 20 | 
 21 | /*
 22 | * BOOST includes
 23 | */
 24 | #include <boost/circular_buffer.hpp>
 25 | 
 26 | /*
 27 | * LOCAL includes
 28 | */
 29 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 30 | #include "../helpers/recorder_helpers.hpp"
 31 | 
 32 | /*
 33 | * ROS includes
 34 | */
 35 | #include <sensor_msgs/JointState.h>
 36 | 
 37 | namespace naoqi
 38 | {
 39 | namespace recorder
 40 | {
 41 | 
 42 | class JointStateRecorder
 43 | {
 44 | 
 45 | public:
 46 |   JointStateRecorder( const std::string& topic, float buffer_frequency = 0 );
 47 | 
 48 |   void write( const sensor_msgs::JointState& js_msg,
 49 |               const std::vector<geometry_msgs::TransformStamped>& tf_transforms );
 50 | 
 51 |   void reset( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr, float conv_frequency );
 52 | 
 53 |   void bufferize( const sensor_msgs::JointState& js_msg,
 54 |                   const std::vector<geometry_msgs::TransformStamped>& tf_transforms );
 55 | 
 56 |   void writeDump(const ros::Time& time);
 57 | 
 58 |   void setBufferDuration(float duration);
 59 | 
 60 |   inline std::string topic() const
 61 |   {
 62 |     return topic_;
 63 |   }
 64 | 
 65 |   inline bool isInitialized() const
 66 |   {
 67 |     return is_initialized_;
 68 |   }
 69 | 
 70 |   inline void subscribe( bool state)
 71 |   {
 72 |     is_subscribed_ = state;
 73 |   }
 74 | 
 75 |   inline bool isSubscribed() const
 76 |   {
 77 |     return is_subscribed_;
 78 |   }
 79 | 
 80 | protected:
 81 |   std::string topic_;
 82 | 
 83 |   boost::circular_buffer<sensor_msgs::JointState> bufferJoinState_;
 84 |   boost::circular_buffer< std::vector<geometry_msgs::TransformStamped> > bufferTF_;
 85 |   size_t buffer_size_;
 86 |   float buffer_duration_;
 87 | 
 88 |   boost::mutex mutex_;
 89 | 
 90 |   bool is_initialized_;
 91 |   bool is_subscribed_;
 92 | 
 93 |   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
 94 | 
 95 |   float buffer_frequency_;
 96 |   float conv_frequency_;
 97 |   int counter_;
 98 |   int max_counter_;
 99 | 
100 | }; // class
101 | 
102 | } //publisher
103 | } // naoqi
104 | 
105 | #endif
106 | 


--------------------------------------------------------------------------------
/src/recorder/log.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 "log.hpp"
 19 | 
 20 | namespace naoqi
 21 | {
 22 | namespace recorder
 23 | {
 24 | 
 25 | LogRecorder::LogRecorder(const std::string& topic , float buffer_frequency):
 26 |   topic_( topic ),
 27 |   buffer_duration_(helpers::recorder::bufferDefaultDuration),
 28 |   buffer_frequency_(buffer_frequency),
 29 |   counter_(1)
 30 | {}
 31 | 
 32 | void LogRecorder::write(std::list<rosgraph_msgs::Log>& log_msgs)
 33 | {
 34 |   while ( !log_msgs.empty() )
 35 |   {
 36 |     if (!log_msgs.front().header.stamp.isZero()) {
 37 |       gr_->write(topic_, log_msgs.front(), log_msgs.front().header.stamp);
 38 |     }
 39 |     else {
 40 |       gr_->write(topic_, log_msgs.front());
 41 |     }
 42 |     {
 43 |       log_msgs.pop_front();
 44 |     }
 45 |   }
 46 | }
 47 | 
 48 | void LogRecorder::writeDump(const ros::Time& time)
 49 | {
 50 |   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
 51 |   boost::circular_buffer< std::list<rosgraph_msgs::Log> >::iterator it;
 52 |   for (it = buffer_.begin(); it != buffer_.end(); it++)
 53 |   {
 54 |     write(*it);
 55 |   }
 56 | }
 57 | 
 58 | void LogRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
 59 | {
 60 |   gr_ = gr;
 61 |   conv_frequency_ = conv_frequency;
 62 |   if (buffer_frequency_ != 0)
 63 |   {
 64 |     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
 65 |     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
 66 |   }
 67 |   else
 68 |   {
 69 |     max_counter_ = 1;
 70 |     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
 71 |   }
 72 |   buffer_.resize(buffer_size_);
 73 |   is_initialized_ = true;
 74 | }
 75 | 
 76 | void LogRecorder::bufferize( std::list<rosgraph_msgs::Log>& log_msgs )
 77 | {
 78 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 79 |   if (counter_ < max_counter_)
 80 |   {
 81 |     counter_++;
 82 |   }
 83 |   else
 84 |   {
 85 |     counter_ = 1;
 86 |     buffer_.push_back(log_msgs);
 87 |   }
 88 | }
 89 | 
 90 | void LogRecorder::setBufferDuration(float duration)
 91 | {
 92 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 93 |   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
 94 |   buffer_duration_ = duration;
 95 |   buffer_.set_capacity(buffer_size_);
 96 | }
 97 | 
 98 | } //publisher
 99 | } // naoqi
100 | 


--------------------------------------------------------------------------------
/src/recorder/log.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 LOG_RECORDER_HPP
 19 | #define LOG_RECORDER_HPP
 20 | 
 21 | /*
 22 | * BOOST includes
 23 | */
 24 | #include <boost/circular_buffer.hpp>
 25 | 
 26 | /*
 27 | * LOCAL includes
 28 | */
 29 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 30 | #include "../helpers/recorder_helpers.hpp"
 31 | 
 32 | /*
 33 | * ROS includes
 34 | */
 35 | #include <ros/ros.h>
 36 | #include <rosgraph_msgs/Log.h>
 37 | 
 38 | namespace naoqi
 39 | {
 40 | namespace recorder
 41 | {
 42 | 
 43 | class LogRecorder
 44 | {
 45 | 
 46 | public:
 47 |   LogRecorder( const std::string& topic, float buffer_frequency = 0 );
 48 | 
 49 |   void write( std::list<rosgraph_msgs::Log>& log_msgs );
 50 | 
 51 |   void reset(boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr, float conv_frequency );
 52 | 
 53 |   void bufferize( std::list<rosgraph_msgs::Log>& log_msgs );
 54 | 
 55 |   void writeDump(const ros::Time& time);
 56 | 
 57 |   void setBufferDuration(float duration);
 58 | 
 59 |   inline std::string topic() const
 60 |   {
 61 |     return topic_;
 62 |   }
 63 | 
 64 |   inline bool isInitialized() const
 65 |   {
 66 |     return is_initialized_;
 67 |   }
 68 | 
 69 |   inline void subscribe( bool state)
 70 |   {
 71 |     is_subscribed_ = state;
 72 |   }
 73 | 
 74 |   inline bool isSubscribed() const
 75 |   {
 76 |     return is_subscribed_;
 77 |   }
 78 | 
 79 | protected:
 80 |   std::string topic_;
 81 | 
 82 |   boost::circular_buffer< std::list<rosgraph_msgs::Log> > buffer_;
 83 |   size_t buffer_size_;
 84 |   float buffer_duration_;
 85 | 
 86 |   boost::mutex mutex_;
 87 | 
 88 |   bool is_initialized_;
 89 |   bool is_subscribed_;
 90 | 
 91 |   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
 92 | 
 93 |   float buffer_frequency_;
 94 |   float conv_frequency_;
 95 |   int counter_;
 96 |   int max_counter_;
 97 | 
 98 | }; // class
 99 | 
100 | } //publisher
101 | } // naoqi
102 | 
103 | #endif
104 | 


--------------------------------------------------------------------------------
/src/recorder/sonar.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | /*
 19 | * LOCAL includes
 20 | */
 21 | #include "sonar.hpp"
 22 | 
 23 | namespace naoqi
 24 | {
 25 | namespace recorder
 26 | {
 27 | 
 28 | SonarRecorder::SonarRecorder(const std::vector<std::string>& topics, float buffer_frequency ):
 29 |   topics_(topics),
 30 |   buffer_duration_(helpers::recorder::bufferDefaultDuration),
 31 |   buffer_frequency_(buffer_frequency),
 32 |   counter_(1)
 33 | {}
 34 | 
 35 | void SonarRecorder::write(const std::vector<sensor_msgs::Range>& sonar_msgs)
 36 | {
 37 |   if ( topics_.size() != sonar_msgs.size() )
 38 |   {
 39 |     std::cerr << "Incorrect number of sonar range messages in sonar recorder. " << sonar_msgs.size() << "/" << topics_.size() << std::endl;
 40 |     return;
 41 |   }
 42 | 
 43 |   for( size_t i=0; i<sonar_msgs.size(); ++i)
 44 |   {
 45 |     if (!sonar_msgs[i].header.stamp.isZero()) {
 46 |       gr_->write(topics_[i], sonar_msgs[i], sonar_msgs[i].header.stamp);
 47 |     }
 48 |     else {
 49 |       gr_->write(topics_[i], sonar_msgs[i]);
 50 |     }
 51 |   }
 52 | }
 53 | 
 54 | void SonarRecorder::writeDump(const ros::Time& time)
 55 | {
 56 |   boost::mutex::scoped_lock lock_write_buffer( mutex_ );
 57 |   boost::circular_buffer< std::vector<sensor_msgs::Range> >::iterator it;
 58 |   for (it = buffer_.begin(); it != buffer_.end(); it++)
 59 |   {
 60 |     write(*it);
 61 |   }
 62 | }
 63 | 
 64 | void SonarRecorder::reset(boost::shared_ptr<GlobalRecorder> gr, float conv_frequency)
 65 | {
 66 |   gr_ = gr;
 67 |   conv_frequency_ = conv_frequency;
 68 |   if (buffer_frequency_ != 0)
 69 |   {
 70 |     max_counter_ = static_cast<int>(conv_frequency/buffer_frequency_);
 71 |     buffer_size_ = static_cast<size_t>(buffer_duration_*(conv_frequency/max_counter_));
 72 |   }
 73 |   else
 74 |   {
 75 |     max_counter_ = 1;
 76 |     buffer_size_ = static_cast<size_t>(buffer_duration_*conv_frequency);
 77 |   }
 78 |   buffer_.resize(buffer_size_);
 79 |   is_initialized_ = true;
 80 | }
 81 | 
 82 | void SonarRecorder::bufferize(const std::vector<sensor_msgs::Range>& sonar_msgs )
 83 | {
 84 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 85 |   if (counter_ < max_counter_)
 86 |   {
 87 |     counter_++;
 88 |   }
 89 |   else
 90 |   {
 91 |     counter_ = 1;
 92 |     buffer_.push_back(sonar_msgs);
 93 |   }
 94 | }
 95 | 
 96 | void SonarRecorder::setBufferDuration(float duration)
 97 | {
 98 |   boost::mutex::scoped_lock lock_bufferize( mutex_ );
 99 |   buffer_size_ = static_cast<size_t>(duration*(conv_frequency_/max_counter_));
100 |   buffer_duration_ = duration;
101 |   buffer_.set_capacity(buffer_size_);
102 | }
103 | 
104 | } //publisher
105 | } // naoqi
106 | 


--------------------------------------------------------------------------------
/src/recorder/sonar.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 SONAR_RECORDER_HPP
 19 | #define SONAR_RECORDER_HPP
 20 | 
 21 | /*
 22 | * BOOST includes
 23 | */
 24 | #include <boost/circular_buffer.hpp>
 25 | 
 26 | /*
 27 | * LOCAL includes
 28 | */
 29 | #include <naoqi_driver/recorder/globalrecorder.hpp>
 30 | #include "../helpers/recorder_helpers.hpp"
 31 | 
 32 | /*
 33 | * ROS includes
 34 | */
 35 | #include <sensor_msgs/Range.h>
 36 | 
 37 | namespace naoqi
 38 | {
 39 | namespace recorder
 40 | {
 41 | 
 42 | class SonarRecorder
 43 | {
 44 | 
 45 | public:
 46 |   SonarRecorder( const std::vector<std::string>& topics, float buffer_frequency = 0 );
 47 | 
 48 |   void write(const std::vector<sensor_msgs::Range>& sonar_msgs );
 49 | 
 50 |   void reset( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr, float conv_frequency );
 51 | 
 52 |   void bufferize(const std::vector<sensor_msgs::Range>& sonar_msgs );
 53 | 
 54 |   void writeDump(const ros::Time& time);
 55 | 
 56 |   void setBufferDuration(float duration);
 57 | 
 58 |   inline std::string topic() const
 59 |   {
 60 |     return "sonar";
 61 |   }
 62 | 
 63 |   inline bool isInitialized() const
 64 |   {
 65 |     return is_initialized_;
 66 |   }
 67 | 
 68 |   inline void subscribe( bool state)
 69 |   {
 70 |     is_subscribed_ = state;
 71 |   }
 72 | 
 73 |   inline bool isSubscribed() const
 74 |   {
 75 |     return is_subscribed_;
 76 |   }
 77 | 
 78 | protected:
 79 |   std::string topic_;
 80 | 
 81 |   boost::circular_buffer< std::vector<sensor_msgs::Range> > buffer_;
 82 |   size_t buffer_size_;
 83 |   float buffer_duration_;
 84 | 
 85 |   boost::mutex mutex_;
 86 | 
 87 |   bool is_initialized_;
 88 |   bool is_subscribed_;
 89 | 
 90 |   boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr_;
 91 |   std::vector<std::string> topics_;
 92 | 
 93 |   float buffer_frequency_;
 94 |   float conv_frequency_;
 95 |   int counter_;
 96 |   int max_counter_;
 97 | 
 98 | }; // class
 99 | 
100 | } //publisher
101 | } // naoqi
102 | 
103 | #endif
104 | 


--------------------------------------------------------------------------------
/src/ros_env.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2015 Aldebaran
  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 | 
 19 | #ifndef ROS_ENV_HPP
 20 | #define ROS_ENV_HPP
 21 | 
 22 | /*
 23 | * ROS includes
 24 | */
 25 | #include <ros/ros.h>
 26 | 
 27 | 
 28 | /*
 29 | * ALDEBARAN includes
 30 | */
 31 | #include <qi/os.hpp>
 32 | 
 33 | #include <stdlib.h>
 34 | 
 35 | #include <boost/algorithm/string.hpp>
 36 | 
 37 | namespace naoqi
 38 | {
 39 | namespace ros_env
 40 | {
 41 | 
 42 | /** Queries NAOqi to get the IP
 43 |  * @param network_interface the name of the network interface to use. "eth0" by default. If you put your NAO in
 44 |  * tethering mode, you will need to put "tether"
 45 |  */
 46 | static std::string getROSIP(std::string network_interface)
 47 | {
 48 |   if (network_interface.empty())
 49 |     network_interface = "eth0";
 50 | 
 51 |   typedef std::map< std::string, std::vector<std::string> > Map_IP;
 52 |   Map_IP map_ip = static_cast<Map_IP>(qi::os::hostIPAddrs());
 53 |   if ( map_ip.find(network_interface) == map_ip.end() ) {
 54 |     std::cerr << "Could not find network interface named " << network_interface << ", possible interfaces are ... ";
 55 |     for (Map_IP::iterator it=map_ip.begin(); it!=map_ip.end(); ++it) std::cerr << it->first <<  " ";
 56 |     std::cerr << std::endl;
 57 |     exit(1);
 58 |   }
 59 | 
 60 |   static const std::string ip = map_ip[network_interface][0];
 61 |   return ip;
 62 | }
 63 | 
 64 | static std::string prefix = "";
 65 | 
 66 | static void setPrefix( std::string s )
 67 | {
 68 |   prefix = s;
 69 |   std::cout << "set prefix successfully to " << prefix << std::endl;
 70 | }
 71 | 
 72 | static std::string getPrefix()
 73 | {
 74 |   return prefix;
 75 | }
 76 | 
 77 | static void setMasterURI( const std::string& uri, const std::string& network_interface )
 78 | {
 79 |   if (ros::isInitialized() )
 80 |   {
 81 |     std::cout << "stopping ros init" << std::endl;
 82 |     ros::shutdown();
 83 |   }
 84 | 
 85 |   setenv("ROS_MASTER_URI", uri.c_str(), 1);
 86 | 
 87 |   std::string my_master = "__master="+uri;
 88 |   std::map< std::string, std::string > remap;
 89 |   remap["__master"] = uri;
 90 |   remap["__ip"] = ::naoqi::ros_env::getROSIP(network_interface);
 91 |   // init ros without a sigint-handler in order to shutdown correctly by naoqi
 92 |   const char* ns_env = std::getenv("ROS_NAMESPACE");
 93 |   ros::init( remap, (::naoqi::ros_env::getPrefix()), ros::init_options::NoSigintHandler );
 94 |   // to prevent shutdown based on no existing nodehandle
 95 |   ros::start();
 96 | 
 97 |   std::cout << "using master ip: " <<  ros::master::getURI() << std::endl;
 98 | }
 99 | 
100 | static std::string getMasterURI( )
101 | {
102 |   return getenv("ROS_MASTER_URI");
103 | }
104 | 
105 | 
106 | } // ros_env
107 | } // naoqi
108 | #endif
109 | 


--------------------------------------------------------------------------------
/src/services/get_language.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2017 Aldebaran
 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 "get_language.hpp"
19 | #include "../helpers/driver_helpers.hpp"
20 | 
21 | namespace naoqi
22 | {
23 | namespace service
24 | {
25 | 
26 | GetLanguageService::GetLanguageService( const std::string& name, const std::string& topic, const qi::SessionPtr& session )
27 |   : name_(name),
28 |   topic_(topic),
29 |   session_(session)
30 | {}
31 | 
32 | void GetLanguageService::reset( ros::NodeHandle& nh )
33 | {
34 |   service_ = nh.advertiseService(topic_, &GetLanguageService::callback, this);
35 | }
36 | 
37 | bool GetLanguageService::callback( naoqi_bridge_msgs::GetStringRequest& req, naoqi_bridge_msgs::GetStringResponse& resp )
38 | {
39 |   resp.data = helpers::driver::getLanguage(session_);
40 |   return true;
41 | }
42 | 
43 | 
44 | }
45 | }
46 | 


--------------------------------------------------------------------------------
/src/services/get_language.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2017 Aldebaran
 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 | 
19 | #ifndef GET_LANGUAGE_SERVICE_HPP
20 | #define GET_LANGUAGE_SERVICE_HPP
21 | 
22 | #include <iostream>
23 | 
24 | #include <ros/node_handle.h>
25 | #include <ros/service_server.h>
26 | 
27 | #include <naoqi_bridge_msgs/GetString.h>
28 | #include <qi/session.hpp>
29 | 
30 | namespace naoqi
31 | {
32 | namespace service
33 | {
34 | 
35 | class GetLanguageService
36 | {
37 | public:
38 |   GetLanguageService( const std::string& name, const std::string& topic, const qi::SessionPtr& session );
39 | 
40 |   ~GetLanguageService(){};
41 | 
42 |   std::string name() const
43 |   {
44 |     return name_;
45 |   }
46 | 
47 |   std::string topic() const
48 |   {
49 |     return topic_;
50 |   }
51 | 
52 |   void reset( ros::NodeHandle& nh );
53 | 
54 |   bool callback( naoqi_bridge_msgs::GetStringRequest& req, naoqi_bridge_msgs::GetStringResponse& resp );
55 | 
56 | 
57 | private:
58 |   const std::string name_;
59 |   const std::string topic_;
60 | 
61 |   const qi::SessionPtr& session_;
62 |   ros::ServiceServer service_;
63 | };
64 | 
65 | } // service
66 | } // naoqi
67 | #endif
68 | 


--------------------------------------------------------------------------------
/src/services/robot_config.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 "robot_config.hpp"
19 | #include "../helpers/driver_helpers.hpp"
20 | 
21 | namespace naoqi
22 | {
23 | namespace service
24 | {
25 | 
26 | RobotConfigService::RobotConfigService( const std::string& name, const std::string& topic, const qi::SessionPtr& session )
27 |   : name_(name),
28 |   topic_(topic),
29 |   session_(session)
30 | {}
31 | 
32 | void RobotConfigService::reset( ros::NodeHandle& nh )
33 | {
34 |   service_ = nh.advertiseService(topic_, &RobotConfigService::callback, this);
35 | }
36 | 
37 | bool RobotConfigService::callback( naoqi_bridge_msgs::GetRobotInfoRequest& req, naoqi_bridge_msgs::GetRobotInfoResponse& resp )
38 | {
39 |   resp.info = helpers::driver::getRobotInfo(session_);
40 |   return true;
41 | }
42 | 
43 | 
44 | }
45 | }
46 | 


--------------------------------------------------------------------------------
/src/services/robot_config.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef ROBOT_CONFIG_SERVICE_HPP
20 | #define ROBOT_CONFIG_SERVICE_HPP
21 | 
22 | #include <iostream>
23 | 
24 | #include <ros/node_handle.h>
25 | #include <ros/service_server.h>
26 | 
27 | #include <naoqi_bridge_msgs/GetRobotInfo.h>
28 | #include <qi/session.hpp>
29 | 
30 | namespace naoqi
31 | {
32 | namespace service
33 | {
34 | 
35 | class RobotConfigService
36 | {
37 | public:
38 |   RobotConfigService( const std::string& name, const std::string& topic, const qi::SessionPtr& session );
39 | 
40 |   ~RobotConfigService(){};
41 | 
42 |   std::string name() const
43 |   {
44 |     return name_;
45 |   }
46 | 
47 |   std::string topic() const
48 |   {
49 |     return topic_;
50 |   }
51 | 
52 |   void reset( ros::NodeHandle& nh );
53 | 
54 |   bool callback( naoqi_bridge_msgs::GetRobotInfoRequest& req, naoqi_bridge_msgs::GetRobotInfoResponse& resp );
55 | 
56 | 
57 | private:
58 |   const std::string name_;
59 |   const std::string topic_;
60 | 
61 |   const qi::SessionPtr& session_;
62 |   ros::ServiceServer service_;
63 | };
64 | 
65 | } // service
66 | } // naoqi
67 | #endif
68 | 


--------------------------------------------------------------------------------
/src/services/set_language.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2017 Aldebaran
 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 "set_language.hpp"
19 | #include "../helpers/driver_helpers.hpp"
20 | 
21 | namespace naoqi
22 | {
23 | namespace service
24 | {
25 | 
26 | SetLanguageService::SetLanguageService( const std::string& name, const std::string& topic, const qi::SessionPtr& session )
27 |   : name_(name),
28 |   topic_(topic),
29 |   session_(session)
30 | {}
31 | 
32 | void SetLanguageService::reset( ros::NodeHandle& nh )
33 | {
34 |   service_ = nh.advertiseService(topic_, &SetLanguageService::callback, this);
35 | }
36 | 
37 | bool SetLanguageService::callback( naoqi_bridge_msgs::SetStringRequest& req, naoqi_bridge_msgs::SetStringResponse& resp )
38 | {
39 |   resp.success = helpers::driver::setLanguage(session_, req);
40 |   return true;
41 | }
42 | 
43 | 
44 | }
45 | }
46 | 


--------------------------------------------------------------------------------
/src/services/set_language.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2017 Aldebaran
 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 | 
19 | #ifndef SET_LANGUAGE_SERVICE_HPP
20 | #define SET_LANGUAGE_SERVICE_HPP
21 | 
22 | #include <iostream>
23 | 
24 | #include <ros/node_handle.h>
25 | #include <ros/service_server.h>
26 | 
27 | #include <naoqi_bridge_msgs/SetString.h>
28 | #include <qi/session.hpp>
29 | 
30 | namespace naoqi
31 | {
32 | namespace service
33 | {
34 | 
35 | class SetLanguageService
36 | {
37 | public:
38 |   SetLanguageService( const std::string& name, const std::string& topic, const qi::SessionPtr& session );
39 | 
40 |   ~SetLanguageService(){};
41 | 
42 |   std::string name() const
43 |   {
44 |     return name_;
45 |   }
46 | 
47 |   std::string topic() const
48 |   {
49 |     return topic_;
50 |   }
51 | 
52 |   void reset( ros::NodeHandle& nh );
53 | 
54 |   bool callback( naoqi_bridge_msgs::SetStringRequest& req, naoqi_bridge_msgs::SetStringResponse& resp );
55 | 
56 | 
57 | private:
58 |   const std::string name_;
59 |   const std::string topic_;
60 | 
61 |   const qi::SessionPtr& session_;
62 |   ros::ServiceServer service_;
63 | };
64 | 
65 | } // service
66 | } // naoqi
67 | #endif
68 | 


--------------------------------------------------------------------------------
/src/subscribers/moveto.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef MOVETO_SUBSCRIBER_HPP
20 | #define MOVETO_SUBSCRIBER_HPP
21 | 
22 | /*
23 |  * LOCAL includes
24 |  */
25 | #include "subscriber_base.hpp"
26 | 
27 | /*
28 |  * ROS includes
29 |  */
30 | #include <ros/ros.h>
31 | #include <geometry_msgs/PoseStamped.h>
32 | #include <tf2_ros/buffer.h>
33 | 
34 | namespace naoqi
35 | {
36 | namespace subscriber
37 | {
38 | 
39 | class MovetoSubscriber: public BaseSubscriber<MovetoSubscriber>
40 | {
41 | public:
42 |   MovetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr<tf2_ros::Buffer>& tf2_buffer );
43 |   ~MovetoSubscriber(){}
44 | 
45 |   void reset( ros::NodeHandle& nh );
46 |   void callback( const geometry_msgs::PoseStampedConstPtr& pose_msg );
47 | 
48 | private:
49 |   qi::AnyObject p_motion_;
50 |   ros::Subscriber sub_moveto_;
51 |   boost::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
52 | }; // class Teleop
53 | 
54 | } // subscriber
55 | }// naoqi
56 | #endif
57 | 


--------------------------------------------------------------------------------
/src/subscribers/speech.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 |  * LOCAL includes
20 |  */
21 | #include "speech.hpp"
22 | 
23 | 
24 | namespace naoqi
25 | {
26 | namespace subscriber
27 | {
28 | 
29 | SpeechSubscriber::SpeechSubscriber( const std::string& name, const std::string& speech_topic, const qi::SessionPtr& session ):
30 |   speech_topic_(speech_topic),
31 |   BaseSubscriber( name, speech_topic, session ),
32 |   p_tts_( session->service("ALTextToSpeech").value())
33 | {}
34 | 
35 | void SpeechSubscriber::reset( ros::NodeHandle& nh )
36 | {
37 |   sub_speech_ = nh.subscribe( speech_topic_, 10, &SpeechSubscriber::speech_callback, this );
38 | 
39 |   is_initialized_ = true;
40 | }
41 | 
42 | void SpeechSubscriber::speech_callback( const std_msgs::StringConstPtr& string_msg )
43 | {
44 |   p_tts_.async<void>("say", string_msg->data);
45 | }
46 | 
47 | } //publisher
48 | } // naoqi
49 | 


--------------------------------------------------------------------------------
/src/subscribers/speech.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef SPEECH_SUBSCRIBER_HPP
20 | #define SPEECH_SUBSCRIBER_HPP
21 | 
22 | /*
23 |  * LOCAL includes
24 |  */
25 | #include "subscriber_base.hpp"
26 | 
27 | /*
28 |  * ROS includes
29 |  */
30 | #include <ros/ros.h>
31 | #include <std_msgs/String.h>
32 | 
33 | namespace naoqi
34 | {
35 | namespace subscriber
36 | {
37 | 
38 | class SpeechSubscriber: public BaseSubscriber<SpeechSubscriber>
39 | {
40 | public:
41 |   SpeechSubscriber( const std::string& name, const std::string& speech_topic, const qi::SessionPtr& session );
42 |   ~SpeechSubscriber(){}
43 | 
44 |   void reset( ros::NodeHandle& nh );
45 |   void speech_callback( const std_msgs::StringConstPtr& speech_msg );
46 | 
47 | private:
48 | 
49 |   std::string speech_topic_;
50 | 
51 |   qi::AnyObject p_tts_;
52 |   ros::Subscriber sub_speech_;
53 | 
54 | 
55 | 
56 | }; // class Speech
57 | 
58 | } // subscriber
59 | }// naoqi
60 | #endif
61 | 


--------------------------------------------------------------------------------
/src/subscribers/subscriber_base.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 BASE_SUBSCRIBER_HPP
19 | #define BASE_SUBSCRIBER_HPP
20 | 
21 | /*
22 |  * ALDEBARAN includes
23 |  */
24 | #include <qi/session.hpp>
25 | 
26 | /*
27 |  * LOCAL includes
28 |  */
29 | #include <naoqi_driver/tools.hpp>
30 | #include "../helpers/driver_helpers.hpp"
31 | 
32 | namespace naoqi
33 | {
34 | namespace subscriber
35 | {
36 | 
37 | // CRTP
38 | template<class T>
39 | class BaseSubscriber
40 | {
41 | 
42 | public:
43 |   BaseSubscriber( const std::string& name, const std::string& topic, qi::SessionPtr session ):
44 |     name_( name ),
45 |     topic_( topic ),
46 |     is_initialized_( false ),
47 |     robot_( helpers::driver::getRobot(session) ),
48 |     session_(session)
49 |   {}
50 | 
51 |   virtual ~BaseSubscriber() {}
52 | 
53 |   inline std::string name() const
54 |   {
55 |     return name_;
56 |   }
57 | 
58 |   inline std::string topic() const
59 |   {
60 |     return topic_;
61 |   }
62 | 
63 |   inline bool isInitialized() const
64 |   {
65 |     return is_initialized_;
66 |   }
67 | 
68 | protected:
69 |   std::string name_, topic_;
70 | 
71 |   bool is_initialized_;
72 | 
73 |   /** The type of the robot */
74 |   const robot::Robot& robot_;
75 | 
76 |   /** Pointer to a session from which we can create proxies */
77 |   qi::SessionPtr session_;
78 | }; // class
79 | 
80 | } // subscriber
81 | } // naoqi
82 | 
83 | #endif
84 | 


--------------------------------------------------------------------------------
/src/subscribers/teleop.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 |  * LOCAL includes
20 |  */
21 | #include "teleop.hpp"
22 | 
23 | 
24 | namespace naoqi
25 | {
26 | namespace subscriber
27 | {
28 | 
29 | TeleopSubscriber::TeleopSubscriber( const std::string& name, const std::string& cmd_vel_topic, const std::string& joint_angles_topic, const qi::SessionPtr& session ):
30 |   cmd_vel_topic_(cmd_vel_topic),
31 |   joint_angles_topic_(joint_angles_topic),
32 |   BaseSubscriber( name, cmd_vel_topic, session ),
33 |   p_motion_( session->service("ALMotion").value())
34 | {}
35 | 
36 | void TeleopSubscriber::reset( ros::NodeHandle& nh )
37 | {
38 |   sub_cmd_vel_ = nh.subscribe( cmd_vel_topic_, 10, &TeleopSubscriber::cmd_vel_callback, this );
39 |   sub_joint_angles_ = nh.subscribe( joint_angles_topic_, 10, &TeleopSubscriber::joint_angles_callback, this );
40 | 
41 |   is_initialized_ = true;
42 | }
43 | 
44 | void TeleopSubscriber::cmd_vel_callback( const geometry_msgs::TwistConstPtr& twist_msg )
45 | {
46 |   // no need to check for max velocity since motion clamps the velocities internally
47 |   const float& vel_x = twist_msg->linear.x;
48 |   const float& vel_y = twist_msg->linear.y;
49 |   const float& vel_th = twist_msg->angular.z;
50 | 
51 |   std::cout << "going to move x: " << vel_x << " y: " << vel_y << " th: " << vel_th << std::endl;
52 |   p_motion_.async<void>("move", vel_x, vel_y, vel_th );
53 | }
54 | 
55 | void TeleopSubscriber::joint_angles_callback( const naoqi_bridge_msgs::JointAnglesWithSpeedConstPtr& js_msg )
56 | {
57 |   if ( js_msg->relative==0 )
58 |   {
59 |     p_motion_.async<void>("setAngles", js_msg->joint_names, js_msg->joint_angles, js_msg->speed);
60 |   }
61 |   else
62 |   {
63 |     p_motion_.async<void>("changeAngles", js_msg->joint_names, js_msg->joint_angles, js_msg->speed);
64 |   }
65 | }
66 | 
67 | } //publisher
68 | } // naoqi
69 | 


--------------------------------------------------------------------------------
/src/subscribers/teleop.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | 
19 | #ifndef TELEOP_SUBSCRIBER_HPP
20 | #define TELEOP_SUBSCRIBER_HPP
21 | 
22 | /*
23 |  * LOCAL includes
24 |  */
25 | #include "subscriber_base.hpp"
26 | 
27 | /*
28 |  * ROS includes
29 |  */
30 | #include <ros/ros.h>
31 | #include <geometry_msgs/Twist.h>
32 | #include <naoqi_bridge_msgs/JointAnglesWithSpeed.h>
33 | 
34 | namespace naoqi
35 | {
36 | namespace subscriber
37 | {
38 | 
39 | class TeleopSubscriber: public BaseSubscriber<TeleopSubscriber>
40 | {
41 | public:
42 |   TeleopSubscriber( const std::string& name, const std::string& cmd_vel_topic, const std::string& joint_angles_topic, const qi::SessionPtr& session );
43 |   ~TeleopSubscriber(){}
44 | 
45 |   void reset( ros::NodeHandle& nh );
46 |   void cmd_vel_callback( const geometry_msgs::TwistConstPtr& twist_msg );
47 |   void joint_angles_callback( const naoqi_bridge_msgs::JointAnglesWithSpeedConstPtr& js_msg );
48 | 
49 | private:
50 | 
51 |   std::string cmd_vel_topic_;
52 |   std::string joint_angles_topic_;
53 | 
54 |   qi::AnyObject p_motion_;
55 |   ros::Subscriber sub_cmd_vel_;
56 |   ros::Subscriber sub_joint_angles_;
57 | 
58 | 
59 | 
60 | }; // class Teleop
61 | 
62 | } // subscriber
63 | }// naoqi
64 | #endif
65 | 


--------------------------------------------------------------------------------
/src/tools/from_any_value.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 FROM_ANY_VALUE_HPP
19 | #define FROM_ANY_VALUE_HPP
20 | 
21 | #include <iostream>
22 | 
23 | /*
24 | * LOCAL includes
25 | */
26 | #include "naoqi_image.hpp"
27 | 
28 | /*
29 | * ALDEBARAN includes
30 | */
31 | #include <qi/anyvalue.hpp>
32 | 
33 | namespace naoqi {
34 | 
35 | namespace tools {
36 | 
37 | NaoqiImage fromAnyValueToNaoqiImage(qi::AnyValue& value);
38 | 
39 | std::vector<std::string> fromAnyValueToStringVector(qi::AnyValue& value, std::vector<std::string>& result);
40 | 
41 | std::vector<float> fromAnyValueToFloatVector(qi::AnyValue& value, std::vector<float>& result);
42 | 
43 | void fromAnyValueToFloatVectorVector(
44 |         qi::AnyValue &value,
45 |         std::vector< std::vector<float> > &result);
46 | 
47 | }
48 | }
49 | 
50 | #endif // FROM_ANY_VALUE_HPP
51 | 


--------------------------------------------------------------------------------
/src/tools/naoqi_image.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 NAOQI_IMAGE_HPP
19 | #define NAOQI_IMAGE_HPP
20 | 
21 | namespace naoqi{
22 | 
23 | namespace tools {
24 | 
25 | /**
26 |  * @brief The struct describing an image retrieved by ALVideoDevice.
27 |  *        This specification can be found here:
28 |  *        http://doc.aldebaran.com/2-1/naoqi/vision/alvideodevice-tuto.html
29 |  */
30 | struct NaoqiImage{
31 |   int width;
32 |   int height;
33 |   int number_of_layers;
34 |   int colorspace;
35 |   int timestamp_s;
36 |   int timestamp_us;
37 |   void* buffer;
38 |   int cam_id;
39 |   float fov_left;
40 |   float fov_top;
41 |   float fov_right;
42 |   float fov_bottom;
43 | };
44 | 
45 | }
46 | 
47 | }
48 | 
49 | #endif // NAOQI_IMAGE_HPP
50 | 


--------------------------------------------------------------------------------
/src/tools/robot_description.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 | /*
19 | * LOCAL includes
20 | */
21 | #include "robot_description.hpp"
22 | #include "../helpers/filesystem_helpers.hpp"
23 | 
24 | namespace naoqi{
25 | 
26 | namespace tools{
27 | 
28 | std::string getRobotDescription( const robot::Robot& robot){
29 |     std::string urdf_path;
30 |     static std::string robot_desc;
31 |     if(!robot_desc.empty())
32 |       return robot_desc;
33 | 
34 |     if ( robot == robot::PEPPER)
35 |     {
36 |       urdf_path = helpers::filesystem::getURDF("pepper.urdf");
37 |     }
38 |     else if ( robot == robot::NAO )
39 |     {
40 |       urdf_path = helpers::filesystem::getURDF("nao.urdf");
41 |     }
42 |     else if ( robot == robot::ROMEO )
43 |     {
44 |       urdf_path = helpers::filesystem::getURDF("romeo.urdf");
45 |     }
46 |     else
47 |     {
48 |       std::cerr << " could not load urdf file from disk " << std::endl;
49 |       return std::string();
50 |     }
51 | 
52 |     std::ifstream stream( (urdf_path).c_str() );
53 |     if (!stream)
54 |     {
55 |       std::cerr << "failed to load robot description in joint_state_publisher: " << urdf_path << std::endl;
56 |       return std::string();
57 |     }
58 |     robot_desc = std::string( (std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
59 |     return robot_desc;
60 | }
61 | 
62 | } // tools
63 | 
64 | } // naoqi
65 | 


--------------------------------------------------------------------------------
/src/tools/robot_description.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2015 Aldebaran
 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 ROBOT_DESCRIPTION_HPP
19 | #define ROBOT_DESCRIPTION_HPP
20 | 
21 | /*
22 | * LOCAL includes
23 | */
24 | #include <naoqi_driver/tools.hpp>
25 | 
26 | /*
27 | * STANDARD includes
28 | */
29 | #include <string>
30 | #include <fstream>
31 | 
32 | 
33 | namespace naoqi {
34 | 
35 | namespace tools {
36 | 
37 | std::string getRobotDescription( const robot::Robot& robot);
38 | 
39 | }
40 | 
41 | }
42 | 
43 | #endif // ROBOT_DESCRIPTION_HPP
44 | 


--------------------------------------------------------------------------------