diff --git a/.github/workflows/iron_jammy.yml b/.github/workflows/iron_jammy.yml new file mode 100644 index 0000000..b289972 --- /dev/null +++ b/.github/workflows/iron_jammy.yml @@ -0,0 +1,15 @@ +name: ros2-iron-jammy + +on: + push + +jobs: + ros2_iron_jammy_ci: + name: iron (jammy) + uses: ./.github/workflows/ros2.yml + with: + ROS_DISTRO: iron + ROS_REPO: testing + OS_NAME: ubuntu + OS_CODE_NAME: jammy + ALLOW_FAIL: false diff --git a/CHANGELOG.rst b/CHANGELOG.rst index bcfab82..3beb0e7 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package naoqi_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.0 (2023-11-23) +------------------ +* New "Listen" action +* Remove usages of BOOST_FOREACH +* New `qi_listen_url` option and update README +* Track `ros2` branch of `nao_meshes` +* Get odom at the same time as joint states +* No need to register the driver as a NAOqi service +* Simplify starting and stopping the node + An extra thread is not required anymore to run the ROS loop. +* Safer unsubscription from ALMemory +* Switch to NAOqi 2 subscribers for touch events +* Fix build warnings + * libqi-related warnings + * Use .value() when getting a Qi service + * Fix lack of return value in `camera.cpp` + * Fix deprecated include to `image_transport.h` + * Remove unused mentions to behavior trees +* Better error messages + * Transform computation +* Experimental running the driver and for dev + Adapted from https://github.com/sea-bass/turtlebot3_behavior_demos +* Support NAOqi 2.8 (NAO v6) + * Repair audio +* Support for humble & iron + * RViz files converted to RViz2 ones + * Update use of placeholders + * Update robot description code + * Remove mentions to catkin +* Update README: + * New details about building meshes + * How to build from source now +* Remove dependence to `orocos-kdl` +* Contributors: Maxime Busy, Victor Paleologue, Victor Paléologue + 2.0.0 (2022-09-13) ------------------ * Update README, add buildfarm status badges diff --git a/CMakeLists.txt b/CMakeLists.txt index 33a3b14..bc6fe7f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,7 @@ project(naoqi_driver) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rosidl_default_generators REQUIRED) find_package(cv_bridge REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) @@ -18,10 +19,8 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(OpenCV REQUIRED) -find_package(orocos_kdl REQUIRED) find_package(Boost QUIET COMPONENTS chrono filesystem program_options regex system thread random) -# define source files that can be used by qibuild or catkin set( CONVERTERS_SRC src/converters/audio.cpp @@ -50,7 +49,6 @@ set( set( PUBLISHER_SRC src/publishers/camera.cpp - src/publishers/info.cpp src/publishers/joint_state.cpp src/publishers/log.cpp src/publishers/sonar.cpp @@ -70,6 +68,11 @@ set( src/services/get_language.cpp ) +set( + ACTIONS_SRC + src/actions/listen.cpp +) + set( RECORDER_SRC src/recorder/camera.cpp @@ -96,23 +99,11 @@ add_definitions(-DLIBQI_VERSION=${naoqi_libqi_VERSION_MAJOR}${naoqi_libqi_VERSIO # Add pre-processor compile flag indicating ament add_definitions( -DAMENT_BUILD ) -include_directories(include +include_directories( + "include" ${naoqi_libqi_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} - ${orocos_kdl_INCLUDE_DIRS}) - -# create the different libraries -# add_library( -# naoqi_driver_module -# SHARED -# src/autoload_registration.cpp -# ) - -# target_link_libraries(naoqi_driver_module ${naoqi_libqi_LIBRARIES}) -# ament_target_dependencies(naoqi_driver_module -# naoqi_libqi) - -# install(TARGETS naoqi_driver_module DESTINATION lib/${PROJECT_NAME}) +) add_library( naoqi_driver @@ -122,12 +113,14 @@ add_library( ${PUBLISHER_SRC} ${SUBSCRIBER_SRC} ${SERVICES_SRC} + ${ACTIONS_SRC} ${RECORDER_SRC} ${TOOLS_SRC} ) ament_target_dependencies(naoqi_driver rclcpp + rclcpp_action cv_bridge diagnostic_msgs diagnostic_updater @@ -143,14 +136,11 @@ ament_target_dependencies(naoqi_driver tf2_msgs tf2_ros OpenCV - orocos_kdl - Boost) + Boost -install(TARGETS naoqi_driver DESTINATION lib/) +) -# target_link_libraries(naoqi_driver_module -# naoqi_driver -# ) +install(TARGETS naoqi_driver DESTINATION lib/) # create the binary of the bridge add_executable(naoqi_driver_node src/external_registration.cpp) @@ -164,19 +154,10 @@ target_link_libraries(naoqi_driver_node install(TARGETS naoqi_driver_node DESTINATION lib/${PROJECT_NAME}) # install the urdf for runtime loading -# file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/share DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/) -# install(DIRECTORY share DESTINATION "${CATKIN_PACKAGE_SHARE_DESTINATION}") install(DIRECTORY share DESTINATION share/${PROJECT_NAME}) # install the launch files -# install(DIRECTORY launch DESTINATION "${CATKIN_PACKAGE_SHARE_DESTINATION}") install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -# make sure there is a file describing a naoqi module -# file(WRITE ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/qi/module/naoqi_driver_module.mod "cpp") -# install(FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/qi/module/naoqi_driver_module.mod DESTINATION ${CATKIN_GLOBAL_SHARE_DESTINATION}/qi/module/) -# file(WRITE share/${PROJECT_NAME}/qi/module/naoqi_driver_module.mod "cpp") - -# ament_export_libraries(naoqi_driver_module naoqi_driver) ament_export_libraries(naoqi_driver) ament_package() \ No newline at end of file diff --git a/README.md b/README.md index 70d2c62..f06fe69 100644 --- a/README.md +++ b/README.md @@ -2,59 +2,208 @@ This repo defines the __naoqi_driver__ package for ROS2. The driver is in charge of providing bridging capabilities between ROS2 and NAOqiOS. -## Dependencies -To run, the driver requires the `naoqi_libqi`, `naoqi_libqicore` and `naoqi_bridge_msgs` packages. Additionally, `pepper_meshes` and/or `nao_meshes` can be useful if you try to display the robot in RViz. -### Using the binaries -To install the binaries, use the following commands: -```sh -sudo apt-get install ros--naoqi-libqi -sudo apt-get install ros--naoqi-libqicore -sudo apt-get install ros--naoqi-bridge-msgs +## How it works -# Optional, if you want to install the Pepper or NAO meshes -sudo apt-get install ros--pepper-meshes -sudo apt-get install ros--nao-meshes -``` +The __naoqi_driver__ is a ROS node. +It connects to a robot running NAOqi using libQi. + +It also listens to a libQi endpoint (specified by the option `qi_listen_url`). +This endpoint should be public (*e.g.* `qi_listen_url:=0.0.0.0:0`) +to enable collecting audio buffers remotely. +It should be set to a local address (*e.g.* `qi_listen_url:=127.0.0.1:0`) +when running on the robot. +It can be disabled by setting `qi_listen_url:=` when audio is not required. + +> Audio can be disabled in [boot_config.json](src/naoqi_driver2/share/boot_config.json). + + +## Installation + +### Dependencies + +To run, the driver requires the [`naoqi_libqi`](https://github.com/ros-naoqi/libqi), +[`naoqi_libqicore`](https://github.com/ros-naoqi/libqicore) +and [`naoqi_bridge_msgs`](https://github.com/ros-naoqi/naoqi_bridge_msgs2) packages. +Those can be installed using apt-get (if they have been released for your ROS distro) or from source. +Additionally, [`pepper_meshes`](https://github.com/ros-naoqi/pepper_meshes2) +and/or [`nao_meshes`](https://github.com/ros-naoqi/nao_meshes2) can be useful to display the robot in RViz. + +On Ubuntu, install them using: -Please not that if the driver has been released, you can also install it using apt-get ```sh -sudo apt-get install ros--naoqi-driver +sudo apt-get install ros--naoqi-libqi ros--naoqi-libqicore ros--naoqi-bridge-msgs ros--pepper-meshes ros--nao-meshes ``` ### Installing from source -Create a workspace, and clone the required repositories. Then, checkout a relevant tag / branch for the cloned repos, and build the packages. This is a cloning example for the ROS2 foxy distro: + +In your ROS2 workspace, clone the repo and its dependencies: + ```sh -git clone --branch v2.5.0-foxy https://github.com/ros-naoqi/libqi.git -git clone --branch v2.5.0-foxy https://github.com/ros-naoqi/libqicore.git -git clone https://github.com/ros-naoqi/naoqi_bridge_msgs2.git +cd /src git clone https://github.com/ros-naoqi/naoqi_driver2.git +vcs import < naoqi_driver2/dependencies.repos +cd +rosdep install --from-paths src --ignore-src --rosdistro -y +``` + +> To install vcs: `sudo apt-get install python3-vcstool` + +Then build the workspace: -# Eventually, if you didn't install the robot meshes. -git clone --branch ros2 https://github.com/ros-naoqi/pepper_meshes.git -git clone --branch ros2 https://github.com/ros-naoqi/nao_meshes.git +```sh +cd +colcon build --symlink-install ``` +> Meshes can only be built on x86(_64) platforms. +> You can skip them by building with these arguments: +> +> ```sh +> colcon build --packages-skip nao_meshes pepper_meshes +> ``` + +#### License of the meshes + +The source repositories include +[`pepper_meshes2`](https://github.com/ros-naoqi/pepper_meshes2) +and [`nao_meshes2`](https://github.com/ros-naoqi/nao_meshes2), +which require an interactive agreement to be provided. +If you agree to their license terms ( +[CC BY-NC-ND 4.0](https://creativecommons.org/licenses/by-nc-nd/4.0/legalcode.en): +[`pepper_meshes2` LICENSE](https://github.com/ros-naoqi/pepper_meshes/blob/master/LICENSE), +[`nao_meshes2` LICENSE](https://github.com/ros-naoqi/nao_meshes/blob/master/LICENSE)). +you may skip the interactive prompt by setting +the `AGREE_TO_NAO_MESHES_LICENSE` and `I_AGREE_TO_PEPPER_MESHES_LICENSE` environment variables: + +```sh +I_AGREE_TO_NAO_MESHES_LICENSE=1 I_AGREE_TO_PEPPER_MESHES_LICENSE=1 colcon build --symlink-install +``` + +Automated jobs on non-interactive environments +(`DEBIAN_FRONTEND=noninteractive`) +defaults to agreeing to the licenses, +assuming the author of the job has agreed to the license terms. + + ## Launch + +To have full control of the robot with ROS, +you may want to disable autonomous behaviors first: + +```sh +ssh nao@ +qicli call ALAutonomousLife.setState disabled +qicli call ALMotion.wakeUp +``` + The driver can be launched using the following command: + +```sh +source /opt/ros//setup.bash # or source /install/setup.bash if built from source +ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:= +``` + +Username and password arguments are required +for robots running NAOqi 2.9 or greater: + ```sh -ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:= nao_port:= network_interface:= username:= password:= +ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:= nao_username:=nao nao_password:= ``` -Note that the username and password arguments are only required for robots running naoqi 2.9 or greater. -## Documentation -For further information, you can consult the documentation (__OUTDATED__) [here](http://ros-naoqi.github.io/naoqi_driver2/) or build it: + +## Check that the node is running correctly + +Check that the driver is connected: ```sh -cd doc -doxygen Doxyfile -sphinx-build -b html ./source/ ./build/ +ros2 node info /naoqi_driver ``` -## Build status +### Hello, world + +Make the robot say hello: + +```sh +ros2 topic pub --once /speech std_msgs/String "data: hello" +``` + +### Listen to words + +You can setup speech recognition and get one result. + +```sh +ros2 action send_goal listen naoqi_bridge_msgs/action/Listen "expected: [\"hello\"] +language: \"en\"" +``` + +### Move the head + +Check that you can move the head by publishing on `/joint_angles`: + +```sh +ros2 topic pub --once /joint_angles naoqi_bridge_msgs/JointAnglesWithSpeed "{header: {stamp: now, frame_id: ''}, joint_names: ['HeadYaw', 'HeadPitch'], joint_angles: [0.5,0.1], speed: 0.1, relative: 0}" +``` + +You can see the published message with `ros2 topic echo /joint_angles` + +### Move around + +Check that you can move the robot by publishing on `cmd_vel` to make the robot move: + +```sh +ros2 topic pub --once /cmd_vel geometry_msgs/Twist "linear: + x: 0.5 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.785" +``` + +> Make some room around the robot! + +To stop the robot, you must send a new message with linear and angular velocities set to 0: + +```sh +ros2 topic pub --once /cmd_vel geometry_msgs/Twist "linear: + x: 0.0 + y: 0.0 + z: 0.0 +angular: + x: 0.0 + y: 0.0 + z: 0.0" +``` + + +## Development + +Check how to [install the driver from source](#installing-from-source), +or use the [`Dockerfile`](Dockerfile) to get setup a reproducible dev environment: + +```sh +docker build -t ros2-naoqi-driver . +``` + +> To get all logs in live and build debuggable binaries, +> you may build with this command: +> +> ```sh +> colcon build --event-handlers console_direct+ --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug +> ``` +> +> It works also when running tests: +> +> ```sh +> colcon test --packages-skip nao_meshes pepper_meshes --event-handlers console_direct+ --ctest-args tests +> ``` + + +## Build Status -ROS Distro| Binary Status | Source Status | Github Build | -|-------------------|-------------------|-------------------|-------------------| -Humble | | | [![ros2-humble-jammy](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/humble_jammy.yml/badge.svg)](https://github.com/naoqi_driver2/actions/workflows/humble_jammy.yml) -Galactic | [![Build Status](https://build.ros2.org/job/Gbin_uF64__naoqi_driver2__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__naoqi_driver2__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Gsrc_uF__naoqi_driver2__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__naoqi_driver2__ubuntu_focal__source/) | [![ros-galactic-focal](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/galactic_focal.yml/badge.svg)](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/galactic_focal.yml) -Foxy | [![Build Status](https://build.ros2.org/job/Gbin_uF64__naoqi_driver2__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__naoqi_driver2__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Gsrc_uF__naoqi_driver2__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__naoqi_driver2__ubuntu_focal__source/) | [![ros-foxy-focal](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/foxy_focal.yml/badge.svg)](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/foxy_focal.yml) | \ No newline at end of file +ROS Distro | Binary Status | Source Status | GitHub Status | +|-------------------|-------------------|-------------------|--------------------| +Humble | [![ros2-humble-jammy-bin-status-badge](https://build.ros2.org/job/Hbin_uJ64__naoqi_driver__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__naoqi_driver__ubuntu_jammy_amd64__binary) | [![ros2-humble-jammy-src-status-badge](https://build.ros2.org/job/Hsrc_uJ__naoqi_driver__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__naoqi_driver__ubuntu_jammy__source) | [![ros2-humble-jammy](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/humble_jammy.yml/badge.svg?branch=main)](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/humble_jammy.yml) +Iron | [![ros2-iron-jammy-bin-status-badge](https://build.ros2.org/job/Ibin_uJ64__naoqi_driver__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__naoqi_driver__ubuntu_jammy_amd64__binary) | [![ros2-iron-jammy-src-status-badge](https://build.ros2.org/job/Isrc_uJ__naoqi_driver__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uJ__naoqi_driver__ubuntu_jammy__source) | [![ros2-iron-jammy](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/iron_jammy.yml/badge.svg?branch=main)](https://github.com/ros-naoqi/naoqi_driver2/actions/workflows/iron_jammy.yml) diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..eaed968 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,22 @@ +repositories: + # NAOqi-specific deps + naoqi_libqi: + type: git + url: https://github.com/ros-naoqi/libqi.git + version: ros2 + naoqi_libqicore: + type: git + url: https://github.com/ros-naoqi/libqicore.git + version: ros2 + naoqi_bridge_msgs: + type: git + url: https://github.com/ros-naoqi/naoqi_bridge_msgs2.git + version: main + nao_meshes: + type: git + url: https://github.com/ros-naoqi/nao_meshes2.git + version: main + pepper_meshes: + type: git + url: https://github.com/ros-naoqi/pepper_meshes2.git + version: main diff --git a/docker-compose.yaml b/docker-compose.yaml new file mode 100644 index 0000000..91424c4 --- /dev/null +++ b/docker-compose.yaml @@ -0,0 +1,32 @@ +# Docker Compose file for TurtleBot3 Behavior Examples +# +# Usage: +# +# To build the images: +# docker compose build +# +# To start up a specific service by name: +# docker compose up +# +# To open an interactive shell to a running container: +# docker exec -it bash + +version: "3.9" +services: + # Developer container + dev: + image: ros2-naoqi-driver:dev + build: + context: . + dockerfile: docker/Dockerfile + target: dev + + # Interactive shell + stdin_open: true + tty: true + # Networking and IPC for ROS 2 + network_mode: host + ipc: host + volumes: + # Mount the source code + - .:/ws/src/naoqi_driver2:rw diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..65b0107 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,35 @@ +ARG ROS_DISTRO=iron +FROM ros:${ROS_DISTRO} as dev +ENV ROS_DISTRO=${ROS_DISTRO} +ENV DEBIAN_FRONTEND=noninteractive +SHELL ["/bin/bash", "-c"] + +# Create Colcon workspace with the project and its deps +ENV WS=/ws +ENV PROJ_SRC=${WS}/src/naoqi_driver2 +COPY . $PROJ_SRC +WORKDIR $WS/src +RUN apt install -y git python3-vcstool +RUN vcs import < $PROJ_SRC/dependencies.repos + +# Build the base Colcon workspace, installing dependencies first. +WORKDIR $WS +RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ + && apt-get update -y \ + && rosdep update \ + && rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y + +# Install extra tools for development +RUN apt-get update && apt-get install -y --no-install-recommends \ + gdb gdbserver nano libgmock-dev + +# Expect to mount the source code from the host. +VOLUME $PROJ_SRC + +# Every bash instance should source the entrypoint +RUN echo "source ${PROJ_SRC}/docker/entrypoint.sh" >> /root/.bashrc +SHELL [ "/bin/bash", "-c" ] +ENTRYPOINT [ "/bin/bash" ] + +FROM dev as dev-prebuilt +RUN colcon build --symlink-install \ No newline at end of file diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh new file mode 100755 index 0000000..a2b1118 --- /dev/null +++ b/docker/entrypoint.sh @@ -0,0 +1,16 @@ +#!/bin/bash +# Basic entrypoint for ROS / Colcon Docker containers + +# Source ROS 2 +source /opt/ros/${ROS_DISTRO}/setup.bash +echo "Sourced ROS 2 ${ROS_DISTRO}" + +# Source the base workspace, if built +if [ -f $WS/install/setup.bash ] +then + source $WS/install/setup.bash + echo "Sourced workspace" +fi + +# Execute the command passed into this entrypoint +exec "$@" diff --git a/docker/run_docker.sh b/docker/run_docker.sh new file mode 100755 index 0000000..b5405d3 --- /dev/null +++ b/docker/run_docker.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# Sample script to run a command in a Docker container +# +# Usage Example: +# ./run_docker.sh turtlebot3_behavior:overlay "ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py" + +# Define Docker volumes and environment variables +DOCKER_VOLUMES=" +--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ +--volume="${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority" \ +" +DOCKER_ENV_VARS=" +--env="DISPLAY" \ +--env="QT_X11_NO_MITSHM=1" \ +--env="NVIDIA_DRIVER_CAPABILITIES=all" \ +" +DOCKER_ARGS=${DOCKER_VOLUMES}" "${DOCKER_ENV_VARS} + +# Run the command +docker run -it --net=host --ipc=host --privileged ${DOCKER_ARGS} "$1" bash -c "$2" diff --git a/include/naoqi_driver/naoqi_driver.hpp b/include/naoqi_driver/naoqi_driver.hpp index fe8fb1d..19dcd80 100644 --- a/include/naoqi_driver/naoqi_driver.hpp +++ b/include/naoqi_driver/naoqi_driver.hpp @@ -26,9 +26,7 @@ * BOOST */ #include -#include #include -#include /* * ALDEB @@ -67,7 +65,7 @@ class Driver : public rclcpp::Node public: /** * @brief Constructor for the naoqi driver - * + * */ Driver(); @@ -77,16 +75,18 @@ class Driver : public rclcpp::Node */ ~Driver(); - void init(); + /** + * @brief Register the ROS components and start the ROS loop. + */ + void run(); /** * @brief Set the Session object - * + * */ void setQiSession(const qi::SessionPtr& session_ptr); - void startRosLoop(); - void stopRosLoop(); + void stop(); /** * @brief Write a ROSbag with the last bufferized data (10s by default) */ @@ -229,8 +229,6 @@ class Driver : public rclcpp::Node void parseJsonFile(std::string filepath, boost::property_tree::ptree& pt); - void stopService(); - std::vector getFilesList(); void removeAllFiles(); @@ -248,7 +246,6 @@ class Driver : public rclcpp::Node bool has_stereo; const size_t freq_; - boost::thread publisherThread_; boost::shared_ptr recorder_; @@ -266,15 +263,14 @@ class Driver : public rclcpp::Node boost::shared_ptr mfp = boost::make_shared( key ); boost::shared_ptr mfr = boost::make_shared( key ); boost::shared_ptr mfc = boost::make_shared( key , frequency, sessionPtr_, key ); - mfc->registerCallback( message_actions::PUBLISH, boost::bind(&T1::publish, mfp, _1) ); - mfc->registerCallback( message_actions::RECORD, boost::bind(&T2::write, mfr, _1) ); - mfc->registerCallback( message_actions::LOG, boost::bind(&T2::bufferize, mfr, _1) ); + mfc->registerCallback( message_actions::PUBLISH, boost::bind(&T1::publish, mfp, boost::placeholders::_1) ); + mfc->registerCallback( message_actions::RECORD, boost::bind(&T2::write, mfr, boost::placeholders::_1) ); + mfc->registerCallback( message_actions::LOG, boost::bind(&T2::bufferize, mfr, boost::placeholders::_1) ); registerConverter( mfc, mfp, mfr ); } - void rosLoop(); + void rosIteration(); - boost::mutex mutex_reinit_; boost::mutex mutex_conv_queue_; boost::mutex mutex_record_; diff --git a/launch/naoqi_driver.launch.py b/launch/naoqi_driver.launch.py index 52fce4b..59eaf65 100644 --- a/launch/naoqi_driver.launch.py +++ b/launch/naoqi_driver.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): launch.actions.DeclareLaunchArgument( 'username', default_value="nao", - description='Username for the connection'), + description='Username for the connection'), launch.actions.DeclareLaunchArgument( 'password', default_value="no_password", @@ -26,18 +26,23 @@ def generate_launch_description(): 'network_interface', default_value="eth0", description='Network interface to be used'), + launch.actions.DeclareLaunchArgument( + 'qi_listen_url', + default_value="tcp://0.0.0.0:0", + description='Endpoint to listen for incoming NAOqi connections (for audio)'), launch.actions.DeclareLaunchArgument( 'namespace', default_value="naoqi_driver", description='Name of the namespace to be used'), launch_ros.actions.Node( package='naoqi_driver', - node_executable='naoqi_driver_node', - node_name=[launch.substitutions.LaunchConfiguration('namespace')], + executable='naoqi_driver_node', parameters=[{ 'nao_ip': launch.substitutions.LaunchConfiguration('nao_ip'), 'nao_port': launch.substitutions.LaunchConfiguration('nao_port'), 'password': launch.substitutions.LaunchConfiguration('password'), - 'network_interface': launch.substitutions.LaunchConfiguration('network_interface')}], + 'network_interface': launch.substitutions.LaunchConfiguration('network_interface'), + 'qi_listen_url': launch.substitutions.LaunchConfiguration('qi_listen_url'), + }], output="screen") ]) \ No newline at end of file diff --git a/package.xml b/package.xml index c086abc..06b2a06 100644 --- a/package.xml +++ b/package.xml @@ -1,10 +1,11 @@ naoqi_driver - 2.0.0 + 2.1.0 Driver module between Aldebaran's NAOqiOS and ROS2. 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. Karsten Knese + Victor Paléologue Marine Chamoux Maxime Busy Surya Ambrose @@ -12,15 +13,17 @@ BSD ament_cmake + rosidl_default_generators rclcpp + rclcpp_action + action_msgs cv_bridge image_transport kdl_parser naoqi_bridge_msgs naoqi_libqi naoqi_libqicore - orocos_kdl robot_state_publisher tf2_ros boost @@ -38,6 +41,8 @@ nao_driver naoqi_rosbridge + rosidl_interface_packages + ament_cmake diff --git a/rviz1_to_rviz2.py b/rviz1_to_rviz2.py new file mode 100644 index 0000000..70fd33b --- /dev/null +++ b/rviz1_to_rviz2.py @@ -0,0 +1,492 @@ +#!/usr/bin/env python3 + +import argparse +import copy +import sys +import warnings +import yaml + + +def del_maybe(dictionary, key): + if key in dictionary: + del dictionary[key] + + +def migrate_panels(panels): + panels_rviz2 = [] + + for panel_dict in panels: + if 'Class' not in panel_dict: + print('Unknown panel format, skipping:' + repr(panel_dict), file=sys.stderr) + continue + + if 'rviz/Displays' == panel_dict['Class']: + panels_rviz2.append(migrate_panel_displays(panel_dict)) + elif 'rviz/Selection' == panel_dict['Class']: + panels_rviz2.append(migrate_panel_selection(panel_dict)) + elif 'rviz/Tool Properties' == panel_dict['Class']: + panels_rviz2.append(migrate_panel_tool_properties(panel_dict)) + elif 'rviz/Views' == panel_dict['Class']: + panels_rviz2.append(migrate_panel_views(panel_dict)) + elif 'rviz/Time' == panel_dict['Class']: + panels_rviz2.append(migrate_panel_time(panel_dict)) + else: + print('Unknown panel type, skipping:' + panel_dict['Class'], file=sys.stderr) + + + return panels_rviz2 + + +def migrate_panel_displays(panel_dict): + panel_rviz2 = copy.deepcopy(panel_dict) + panel_rviz2['Class'] = 'rviz_common/Displays' + # Not sure all display plugins will be present, so clear 'expanded' + panel_rviz2['Property Tree Widget']['Expanded'] = None + return panel_rviz2 + + +def migrate_panel_selection(panel_dict): + panel_rviz2 = copy.deepcopy(panel_dict) + panel_rviz2['Class'] = 'rviz_common/Selection' + return panel_rviz2 + + +def migrate_panel_tool_properties(panel_dict): + panel_rviz2 = copy.deepcopy(panel_dict) + panel_rviz2['Class'] = 'rviz_common/Tool Properties' + # Not sure all tools will be present, so clear 'expanded' + panel_rviz2['Expanded'] = None + return panel_rviz2 + + +def migrate_panel_views(panel_dict): + panel_rviz2 = copy.deepcopy(panel_dict) + panel_rviz2['Class'] = 'rviz_common/Views' + return panel_rviz2 + + +def migrate_panel_time(panel_dict): + panel_rviz2 = copy.deepcopy(panel_dict) + panel_rviz2['Class'] = 'rviz_common/Time' + return panel_rviz2 + + +def migrate_visualization_manager(vm_dict): + vm_rviz2 = { + 'Class': str(vm_dict.get('Class', '')), + 'Displays': migrate_displays(vm_dict['Displays']), + 'Enabled': bool(vm_dict.get('Enabled', True)), + 'Name': str(vm_dict.get('Name', 'root')), + 'Tools': migrate_visualization_manager_tools(vm_dict['Tools']), + 'Value': bool(vm_dict.get('Value', True)), + 'Views': migrate_visualization_manager_views(vm_dict['Views']), + 'Transformation': {'Current': {'Class': 'rviz_default_plugins/TF'}}, + 'Global Options': { + 'Background Color': str(vm_dict.get('Global Options', {}).get('Background Color', '48; 48; 48')), + 'Fixed Frame': str(vm_dict.get('Global Options', {}).get('Fixed Frame', 'map')), + 'Frame Rate': int(vm_dict.get('Global Options', {}).get('Frame Rate', 30)), + } + } + return vm_rviz2 + + +def migrate_displays(displays_list): + migration_functions = { + 'rviz/Grid': auto_migrate_display, + 'rviz/Axes': migrate_display_axes, + 'rviz/Camera': migrate_display_camera, + 'rviz/FluidPressure': auto_migrate_display, + 'rviz/GridCells': auto_migrate_display, + 'rviz/Group': migrate_display_group, + 'rviz/Illuminance': auto_migrate_display, + 'rviz/Image': migrate_display_image, + 'rviz/InteractiveMarkers': migrate_display_interactive_markers, + 'rviz/LaserScan': migrate_display_laser_scan, + 'rviz/Map': migrate_display_map, + 'rviz/Marker': migrate_display_marker, + 'rviz/MarkerArray': migrate_display_marker_array, + 'rviz/Odometry': auto_migrate_display, + 'rviz/Path': auto_migrate_display, + 'rviz/PointCloud': migrate_display_point_cloud, + 'rviz/PointCloud2': migrate_display_point_cloud2, + 'rviz/PointStamped': auto_migrate_display, + 'rviz/Polygon': auto_migrate_display, + 'rviz/Pose': auto_migrate_display, + 'rviz/PoseArray': auto_migrate_display, + 'rviz/PoseWithCovariance': auto_migrate_display, + 'rviz/Range': auto_migrate_display, + 'rviz/RelativeHumidity': auto_migrate_display, + 'rviz/RobotModel': migrate_display_robot_model, + 'rviz/TF': migrate_display_tf, + 'rviz/Temperature': auto_migrate_display, + 'rviz/WrenchStamped': migrate_display_wrench_stamped, + } + + displays_rviz2 = [] + for display_dict in displays_list: + if display_dict['Class'] in migration_functions: + displays_rviz2.append(migration_functions[display_dict['Class']](display_dict)) + else: + print(f"Cannot migrate display {display_dict['Class']} - skipping", file=sys.stderr) + return displays_rviz2 if displays_rviz2 else None + + +def auto_migrate_display(display_dict): + rviz2 = copy.deepcopy(display_dict) + assert display_dict['Class'].startswith('rviz/') + rviz2['Class'] = 'rviz_default_plugins' + display_dict['Class'][4:] + if 'Topic' in display_dict: + kwargs = {'name': display_dict['Topic']} + if 'Queue Size' in display_dict: + kwargs['depth'] = display_dict['Queue Size'] + del rviz2['Queue Size'] + if 'Unreliable' in display_dict: + kwargs['reliable'] = not display_dict['Unreliable'] + del rviz2['Unreliable'] + + rviz2['Topic'] = migrate_topic(**kwargs) + return rviz2 + + +def migrate_display_axes(display_dict): + rviz2 = auto_migrate_display(display_dict) + del_maybe(rviz2, 'Alpha') + return rviz2 + + +def migrate_display_camera(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/Camera' + rviz2['Topic'] = migrate_topic( + name=display_dict['Image Topic'], + depth=display_dict['Queue Size'], + reliable=not display_dict['Unreliable']) + del rviz2['Image Topic'] + del rviz2['Queue Size'] + del rviz2['Unreliable'] + # TODO(sloretz) what is the ROS 2 equivalent to Transport Hint? + del_maybe(rviz2, 'Transport Hint') + del rviz2['Topic']['Filter size'] + rviz2['Far Plane Distance'] = 100 + # Not sure all plugins that wil be migrated, so clear visibility + rviz2['Visibility'] = {} + return rviz2 + + +def migrate_display_group(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_common/Group' + rviz2['Displays'] = migrate_displays(display_dict['Displays']) + return rviz2 + + +def migrate_display_image(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/Image' + rviz2['Topic'] = migrate_topic( + name=display_dict['Image Topic'], + depth=display_dict['Queue Size'], + reliable=not display_dict.get('Unreliable', False)) + del rviz2['Image Topic'] + del rviz2['Queue Size'] + rviz2.pop('Unreliable', None) + # TODO(sloretz) what is the ROS 2 equivalent to Transport Hint? + del_maybe(rviz2, 'Transport Hint') + del rviz2['Topic']['Filter size'] + return rviz2 + + +def migrate_display_interactive_markers(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/InteractiveMarkers' + rviz2['Interactive Markers Namespace'] = display_dict['Update Topic'] + del rviz2['Update Topic'] + return rviz2 + + +def migrate_display_laser_scan(display_dict): + rviz2 = auto_migrate_display(display_dict) + rviz2['Max Intensity'] = 4096 + rviz2['Min Intensity'] = 0 + return rviz2 + + +def migrate_display_map(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/Map' + rviz2['Topic'] = migrate_topic( + name=display_dict['Topic'], + reliable=not display_dict['Unreliable']) + rviz2['Update Topic'] = migrate_topic( + name=display_dict['Topic'] + '_updates', + reliable=not display_dict['Unreliable']) + del rviz2['Update Topic']['Filter size'] + del rviz2['Unreliable'] + return rviz2 + + +def migrate_display_marker(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/Marker' + rviz2['Topic'] = migrate_topic( + name=display_dict['Marker Topic'], + depth=display_dict['Queue Size']) + del rviz2['Marker Topic'] + del rviz2['Queue Size'] + return rviz2 + + +def migrate_display_marker_array(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/MarkerArray' + rviz2['Topic'] = migrate_topic( + name=display_dict['Marker Topic'], + depth=display_dict['Queue Size']) + del rviz2['Marker Topic'] + del rviz2['Queue Size'] + del rviz2['Topic']['Filter size'] + return rviz2 + + +def migrate_display_point_cloud(display_dict): + rviz2 = auto_migrate_display(display_dict) + rviz2['Max Intensity'] = 4096 + rviz2['Min Intensity'] = 0 + return rviz2 + + +def migrate_display_point_cloud2(display_dict): + rviz2 = auto_migrate_display(display_dict) + rviz2['Max Intensity'] = 4096 + rviz2['Min Intensity'] = 0 + return rviz2 + + +def migrate_display_robot_model(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/RobotModel' + del rviz2['Robot Description'] + rviz2['Description File'] = "" + rviz2['Description Source'] = "Topic" + rviz2['Description Topic'] = migrate_topic(name="robot_description") + del rviz2['Description Topic']['Filter size'] + rviz2['Mass Properties'] = {'Inertia': False, 'Mass': False} + return rviz2 + + +def migrate_display_tf(display_dict): + rviz2 = auto_migrate_display(display_dict) + del_maybe(rviz2, 'Marker Alpha') + return rviz2 + + +def migrate_display_wrench_stamped(display_dict): + rviz2 = copy.deepcopy(display_dict) + rviz2['Class'] = 'rviz_default_plugins/Wrench' + rviz2['Topic'] = migrate_topic( + name=display_dict['Topic'], + depth=display_dict['Queue Size'], + reliable=not display_dict['Unreliable']) + del rviz2['Queue Size'] + del rviz2['Unreliable'] + rviz2['Accept NaN Values'] = False + del_maybe(rviz2, 'Hide Small Values') + return rviz2 + + +def migrate_visualization_manager_tools(tools_list): + tools_rviz2 = [] + for tool_dict in tools_list: + name = tool_dict['Class'] + if name in ('rviz/Interact', 'rviz/MoveCamera', 'rviz/Select', 'rviz/FocusCamera'): + rviz2_dict = copy.deepcopy(tool_dict) + rviz2_dict['Class'] = 'rviz_default_plugins' + name[4:] + tools_rviz2.append(rviz2_dict) + elif name == 'rviz/Measure': + tools_rviz2.append({ + 'Class': 'rviz_default_plugins/Measure', + 'Line color': '128; 128; 0' + }) + elif name == 'rviz/SetInitialPose': + rviz2 = { + 'Class': 'rviz_default_plugins/SetInitialPose', + 'Topic': migrate_topic(name = tool_dict.get('Topic', '/initialpose')), + } + if 'X std deviation' in tool_dict: + rviz2['Covariance x'] = float(tool_dict['X std deviation'])**2 + if 'Y std deviation' in tool_dict: + rviz2['Covariance y'] = float(tool_dict['Y std deviation'])**2 + if 'Theta std deviation' in tool_dict: + rviz2['Covariance yaw'] = float(tool_dict['Theta std deviation'])**2 + del rviz2['Topic']['Filter size'] + tools_rviz2.append(rviz2) + elif name == 'rviz/SetGoal': + rviz2 = { + 'Class': 'rviz_default_plugins/SetGoal', + 'Topic': migrate_topic(name = tool_dict.get('Topic', '/goal_pose')), + } + del rviz2['Topic']['Filter size'] + tools_rviz2.append(rviz2) + elif name == 'rviz/PublishPoint': + rviz2 = { + 'Class': 'rviz_default_plugins/PublishPoint', + 'Single click': bool(tool_dict.get('Single click', True)), + 'Topic': migrate_topic(name = tool_dict.get('Topic', '/clicked_point')), + } + del rviz2['Topic']['Filter size'] + tools_rviz2.append(rviz2) + else: + print(f"Unknown tool {tool_dict['Class']}, skipping", file=sys.stderr) + + return tools_rviz2 + + +def migrate_visualization_manager_views(view_dict): + rviz2 = { + 'Current': migrate_view(view_dict['Current']), + 'Saved': [], + } + if view_dict['Saved']: + for saved_view in view_dict['Saved']: + rviz2_view = migrate_view(saved_view) + if rviz2_view: + rviz2['Saved'].append(rviz2_view) + if not rviz2['Saved']: + rviz2['Saved'] = None + return rviz2 + + +def migrate_view(view_dict): + rviz2_view = {} + if 'rviz/Orbit' == view_dict['Class']: + return migrate_view_orbit(view_dict) + elif 'rviz/FPS' == view_dict['Class']: + return migrate_view_fps(view_dict) + elif 'rviz/ThirdPersonFollower' == view_dict['Class']: + return migrate_view_third_person_follower(view_dict) + elif 'rviz/TopDownOrtho' == view_dict['Class']: + return migrate_view_top_down_ortho(view_dict) + elif 'rviz/XYOrbit' == view_dict['Class']: + return migrate_view_xy_orbit(view_dict) + print(f"Unable to migrate view type {view_dict['Class']} - skipping", file=sys.stderr) + + +def migrate_view_orbit(view_dict): + rviz2_view = copy.deepcopy(view_dict) + rviz2_view['Class'] = 'rviz_default_plugins/Orbit' + rviz2_view['Value'] = 'Orbit (rviz)' + del_maybe(rviz2_view, 'Field of View') + return rviz2_view + + +def migrate_view_fps(view_dict): + rviz2_view = copy.deepcopy(view_dict) + rviz2_view['Class'] = 'rviz_default_plugins/FPS' + rviz2_view['Value'] = 'FPS (rviz_default_plugins)' + del_maybe(rviz2_view, 'Roll') + return rviz2_view + + +def migrate_view_third_person_follower(view_dict): + rviz2_view = copy.deepcopy(view_dict) + rviz2_view['Class'] = 'rviz_default_plugins/ThirdPersonFollower' + rviz2_view['Value'] = 'ThirdPersonFollower (rviz_default_plugins)' + del_maybe(rviz2_view, 'Field of View') + return rviz2_view + + +def migrate_view_top_down_ortho(view_dict): + rviz2_view = copy.deepcopy(view_dict) + rviz2_view['Class'] = 'rviz_default_plugins/TopDownOrtho' + rviz2_view['Value'] = 'TopDownOrtho (rviz_default_plugins)' + return rviz2_view + + +def migrate_view_xy_orbit(view_dict): + rviz2_view = copy.deepcopy(view_dict) + rviz2_view['Class'] = 'rviz_default_plugins/XYOrbit' + rviz2_view['Value'] = 'XYOrbit (rviz_default_plugins)' + del_maybe(rviz2_view, 'Field of View') + return rviz2_view + + +def migrate_topic(name, depth=5, reliable=False): + return { + 'Depth': depth, + 'Durability Policy': 'Volatile', + 'Filter size': 10, + 'History Policy': 'Keep Last', + 'Reliability Policy': 'Reliable' if reliable else 'Best Effort', + 'Value': str(name), + } + + +def migrate_window_geometry(window_geometry): + # Return unmodified + return copy.deepcopy(window_geometry) + + +def migrate_to_rviz2(rviz1_config): + """Given an RViz 1 config dictionary, return an RViz 2 config dictionary.""" + + rviz1_sections = { + 'Panels', + 'Preferences', + 'Toolbars', + 'Visualization Manager', + 'Window Geometry'} + + unknown_top_level = set(rviz1_config.keys()).difference(rviz1_sections) + if unknown_top_level: + print( + "I don't know how to convert these sections - skipping" + repr( + unknown_top_level), file=sys.stderr) + + rviz2_config = {} + + p_key = 'Panels' + assert p_key in rviz1_sections + rviz2_config[p_key] = migrate_panels(rviz1_config[p_key]) + + vm_key = 'Visualization Manager' + assert vm_key in rviz1_sections + rviz2_config[vm_key] = migrate_visualization_manager(rviz1_config[vm_key]) + + wg_key = 'Window Geometry' + assert wg_key in rviz1_sections + rviz2_config[wg_key] = migrate_window_geometry(rviz1_config[wg_key]) + + return rviz2_config + + +def parse_arguments(): + parser = argparse.ArgumentParser(description='Convert RViz 1 files to 2') + parser.add_argument('input', metavar='INPUT', help='Path to RViz 1 config') + parser.add_argument( + 'output', metavar='OUTPUT', help='Path to write RViz 2 config') + + return parser.parse_args() + + +def parse_yaml_file(path): + with open(path, 'r') as fin: + return yaml.safe_load(fin) + + +def write_yaml_file(output, yaml_dict): + if '-' == output: + yaml.dump(yaml_dict, stream=sys.stdout) + else: + with open(output, 'w') as fout: + yaml.dump(yaml_dict, stream=fout) + + +def main(): + args = parse_arguments() + rviz1_config = parse_yaml_file(args.input) + rviz2_config = migrate_to_rviz2(rviz1_config) + write_yaml_file(args.output, rviz2_config) + + +if __name__ == '__main__': + main() diff --git a/share/nao.rviz b/share/nao.rviz index aece9cd..2543b1e 100644 --- a/share/nao.rviz +++ b/share/nao.rviz @@ -1,767 +1,293 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Grid1/Offset1 - - /Sonar_Left1 - - /Sonar_Right1 - - /Image1 - - /Imu1 - Splitter Ratio: 0.476667 - Tree Height: 517 - - Class: rviz/Selection + - /Status1 + - /RobotModel1/Description Topic1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 647 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: true + - Class: rviz_common/Time + Experimental: false Name: Time - SyncMode: 2 - SyncSource: Image + SyncMode: 0 + SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 0.1 - Class: rviz/Grid - Color: 141; 141; 141 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: - X: 0.5 + X: 0 Y: 0 Z: 0 Plane: XY - Plane Cell Count: 36 + Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true - CameraBottom_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - CameraBottom_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - CameraTop_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - CameraTop_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - ChestButton_frame: - Alpha: 1 - Show Axes: false - Show Trail: false Expand Joint Details: false Expand Link Details: false Expand Tree: false - Head: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - HeadTouchFront_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - HeadTouchMiddle_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - HeadTouchRear_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - ImuTorsoAccelerometer_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - ImuTorsoGyrometer_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LAnklePitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LBicep: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LElbow: - Alpha: 1 - Show Axes: false - Show Trail: false - LFinger11_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFinger12_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFinger13_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFinger21_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFinger22_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFinger23_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFootBumperLeft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LFootBumperRight_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LForeArm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LFsrFL_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LFsrFR_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LFsrRL_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LFsrRR_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LHandTouchBack_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LHandTouchLeft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LHandTouchRight_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LHip: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LInfraRed_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LPelvis: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LShoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LSonar_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - LThigh: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LThumb1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LThumb2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LTibia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Link Tree Style: Links in Alphabetic Order - Neck: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RAnklePitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RBicep: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RElbow: - Alpha: 1 - Show Axes: false - Show Trail: false - RFinger11_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFinger12_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFinger13_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFinger21_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFinger22_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFinger23_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFootBumperLeft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RFootBumperRight_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RForeArm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RFsrFL_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RFsrFR_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RFsrRL_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RFsrRR_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RHandTouchBack_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RHandTouchLeft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RHandTouchRight_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RHip: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RInfraRed_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RPelvis: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RShoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RSonar_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - RThigh: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RThumb1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RThumb2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - RTibia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - gaze: - Alpha: 1 - Show Axes: false - Show Trail: false - l_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - l_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - l_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - r_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - r_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true - Frame Timeout: 1500 + Frame Timeout: 15 Frames: - All Enabled: false - CameraBottom_frame: - Value: true - CameraBottom_optical_frame: - Value: true - CameraTop_frame: - Value: true - CameraTop_optical_frame: - Value: true - ChestButton_frame: - Value: true - Head: - Value: true - HeadTouchFront_frame: - Value: true - HeadTouchMiddle_frame: - Value: true - HeadTouchRear_frame: - Value: true - ImuTorsoAccelerometer_frame: - Value: true - ImuTorsoGyrometer_frame: - Value: true - LAnklePitch: - Value: true - LBicep: - Value: true - LElbow: - Value: true - LFootBumperLeft_frame: - Value: true - LFootBumperRight_frame: - Value: true - LForeArm: - Value: true - LFsrFL_frame: - Value: true - LFsrFR_frame: - Value: true - LFsrRL_frame: - Value: true - LFsrRR_frame: - Value: true - LHandTouchBack_frame: - Value: true - LHandTouchLeft_frame: - Value: true - LHandTouchRight_frame: - Value: true - LHip: - Value: true - LInfraRed_frame: - Value: true - LPelvis: - Value: true - LShoulder: - Value: true - LSonar_frame: - Value: true - LThigh: - Value: true - LTibia: - Value: true - Neck: - Value: true - RAnklePitch: - Value: true - RBicep: - Value: true - RElbow: - Value: true - RFootBumperLeft_frame: - Value: true - RFootBumperRight_frame: - Value: true - RForeArm: - Value: true - RFsrFL_frame: - Value: true - RFsrFR_frame: - Value: true - RFsrRL_frame: - Value: true - RFsrRR_frame: - Value: true - RHandTouchBack_frame: - Value: true - RHandTouchLeft_frame: - Value: true - RHandTouchRight_frame: - Value: true - RHip: - Value: true - RInfraRed_frame: - Value: true - RPelvis: - Value: true - RShoulder: - Value: true - RSonar_frame: - Value: true - RThigh: - Value: true - RTibia: - Value: true - base_footprint: - Value: true - base_link: - Value: true - gaze: - Value: true - l_ankle: - Value: true - l_gripper: - Value: true - l_sole: - Value: true - l_wrist: - Value: true - odom: - Value: true - r_ankle: - Value: true - r_gripper: - Value: true - r_sole: - Value: true - r_wrist: - Value: true - torso: - Value: true - Marker Scale: 0.1 + All Enabled: true + Marker Scale: 1 Name: TF - Show Arrows: true - Show Axes: true - Show Names: true + Show Arrows: false + Show Axes: false + Show Names: false Tree: - odom: - base_link: - base_footprint: - {} - torso: - ChestButton_frame: - {} - ImuTorsoAccelerometer_frame: - {} - ImuTorsoGyrometer_frame: - {} - LPelvis: - LHip: - LThigh: - LTibia: - LAnklePitch: - l_ankle: - LFootBumperLeft_frame: - {} - LFootBumperRight_frame: - {} - LFsrFL_frame: - {} - LFsrFR_frame: - {} - LFsrRL_frame: - {} - LFsrRR_frame: - {} - l_sole: - {} - LShoulder: - LBicep: - LElbow: - LForeArm: - l_wrist: - LHandTouchBack_frame: - {} - LHandTouchLeft_frame: - {} - LHandTouchRight_frame: - {} - l_gripper: - {} - LSonar_frame: - {} - Neck: - Head: - CameraBottom_frame: - CameraBottom_optical_frame: - {} - CameraTop_frame: - CameraTop_optical_frame: - {} - HeadTouchFront_frame: - {} - HeadTouchMiddle_frame: - {} - HeadTouchRear_frame: - {} - LInfraRed_frame: - {} - RInfraRed_frame: - {} - gaze: - {} - RPelvis: - RHip: - RThigh: - RTibia: - RAnklePitch: - r_ankle: - RFootBumperLeft_frame: - {} - RFootBumperRight_frame: - {} - RFsrFL_frame: - {} - RFsrFR_frame: - {} - RFsrRL_frame: - {} - RFsrRR_frame: - {} - r_sole: - {} - RShoulder: - RBicep: - RElbow: - RForeArm: - r_wrist: - RHandTouchBack_frame: - {} - RHandTouchLeft_frame: - {} - RHandTouchRight_frame: - {} - r_gripper: - {} - RSonar_frame: - {} + {} Update Interval: 0 Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Top Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/front/image_raw + Value: true + Visibility: + Bottom Camera: true + Grid: true + Left Sonar: true + Odometry: true + Right Sonar: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Bottom Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/bottom/image_raw + Value: true + Visibility: + Grid: true + Left Sonar: true + Odometry: true + Right Sonar: true + RobotModel: true + TF: true + Top Camera: true + Value: true + Zoom Factor: 1 - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true - Name: Sonar_Left - Queue Size: 100 - Topic: /nao_robot/sonar/left + Name: Left Sonar + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sonar/left Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true - Name: Sonar_Right - Queue Size: 100 - Topic: /nao_robot/sonar/right - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /nao_robot/camera/front/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Value: true - - Alpha: 0.2 - Class: rviz_plugin_tutorials/Imu - Color: 204; 51; 204 - Enabled: true - History Length: 1 - Name: Imu - Topic: /nao_robot/imu/torso + Name: Right Sonar + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sonar/right Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false Enabled: true Global Options: - Background Color: 170; 170; 170 - Fixed Frame: base_footprint + Background Color: 48; 48; 48 + Fixed Frame: odom Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/XYOrbit - Distance: 1.29287 + Class: rviz_default_plugins/Orbit + Distance: 2.8185791969299316 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.411995 - Y: 0.032867 - Z: -1.41923e-06 + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.129797 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.42039817571640015 Target Frame: - Value: XYOrbit (rviz) - Yaw: 6.23044 + Value: Orbit (rviz) + Yaw: 0.4953904151916504 Saved: ~ Window Geometry: + Bottom Camera: + collapsed: false Displays: collapsed: false - Height: 1059 + Height: 944 Hide Left Dock: false - Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002830000038ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003600000290000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002cc000000f90000001600fffffffb0000000c00430061006d00650072006101000003af000000160000000000000000fb0000000a0049006d00610067006501000001ea000001db0000000000000000000000010000010f0000038ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000360000038f0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002c700fffffffb0000000800540069006d00650100000000000004500000000000000000000004f50000038f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000312fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000312000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001df000000a70000000000000000fb0000000c00430061006d006500720061000000028c000000c30000000000000000000000010000010f00000312fc0200000006fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003d000000a50000005c00fffffffb000000140054006f0070002000430061006d006500720061010000003d000001860000002800fffffffb0000001a0042006f00740074006f006d002000430061006d00650072006101000001c9000001860000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000022800000127000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005560000003efc0100000002fb0000000800540069006d0065010000000000000556000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e50000031200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false + Top Camera: + collapsed: false Views: - collapsed: true - Width: 1918 - X: 1920 - Y: 24 + collapsed: false + Width: 1366 + X: 70 + Y: 60 diff --git a/src/actions/listen.cpp b/src/actions/listen.cpp new file mode 100644 index 0000000..e0a73e1 --- /dev/null +++ b/src/actions/listen.cpp @@ -0,0 +1,256 @@ +#include "listen.hpp" +#include + +using Listen = naoqi_bridge_msgs::action::Listen; +using ListenGoalHandle = rclcpp_action::ServerGoalHandle; + +namespace naoqi +{ +namespace action +{ +namespace +{ + // Trivial Qi Object with a callback to receive ALMemory events + struct MemoryReceiver + { + void callback(const std::string& key, const qi::AnyValue& value) + { + if (key == "Dialog/Answered") + { + std::cout << "Received input: " << value.toString() << std::endl; + } + } + }; + + QI_REGISTER_OBJECT(MemoryReceiver, callback); + + std::string qichatLanguageFromIso(const std::string& iso_language) + { + static const std::unordered_map iso_to_qichat = { + {"en", "English"}, + {"fr", "French"}, + {"de", "German"}, + {"it", "Italian"}, + {"ja", "Japanese"}, + {"ko", "Korean"}, + {"pt", "Portuguese"}, + {"es", "Spanish"}, + {"zh", "Chinese"} + }; + + return iso_to_qichat.at(iso_language); + } + + struct ListenState { + ListenState(rclcpp::Node* node, qi::SessionPtr session) : + node(node), + session(std::move(session)) + {} + + rclcpp::Node* node; + qi::SessionPtr session; + rclcpp::Logger logger = node->get_logger(); + std::shared_ptr current_goal; + qi::AnyObject memory_subscriber; + }; + + std::string topic_name_for_goal_id(const rclcpp_action::GoalUUID& goal_id) + { + std::string topic_name = rclcpp_action::to_string(goal_id); + boost::algorithm::replace_all(topic_name, "-", ""); + return std::string("ros_") + topic_name; + } + + rclcpp_action::GoalResponse handle_goal( + std::shared_ptr state, + const rclcpp_action::GoalUUID & uuid, + const std::shared_ptr goal) + { + std::string goal_id = rclcpp_action::to_string(uuid); + RCLCPP_INFO(state->logger, "Received goal request %s", goal_id.c_str()); + + if (auto current_goal = state->current_goal) { + RCLCPP_INFO( + state->logger, + "Rejected request %s because the robot is already listening for request %s", + goal_id.c_str(), + rclcpp_action::to_string(current_goal->get_goal_id()).c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(state->logger, "Accepted request %s", goal_id.c_str()); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + void cleanup(ListenState &state); + + void handle_accepted( + std::shared_ptr state, + const std::shared_ptr goal_handle) + { + auto& logger = state->logger; + std::string goal_id = rclcpp_action::to_string(goal_handle->get_goal_id()); + RCLCPP_INFO(logger, "Listen %s starts", goal_id.c_str()); + auto result = std::make_shared(); + + const auto& session = state->session; + boost::algorithm::replace_all(goal_id, "-", ""); + auto topic_name = std::string("ros_") + goal_id; + + try + { + auto dialog = session->service("ALDialog").value(); + + try + { + session->service("ALSpeechRecognition").value().call("_enableFreeSpeechToText"); + } + catch (const std::exception& e) + { + RCLCPP_INFO(logger, "Failed to enable free speech to text: %s", e.what()); + } + + // Get language from goal or from dialog + const auto& iso_language = goal_handle->get_goal()->language; + const std::string language = [&] + { + if (iso_language.empty()) + { + return dialog.call("getLanguage"); + } + else + { + return qichatLanguageFromIso(iso_language); + } + }(); + + // Setup topic + std::stringstream pattern_ss; + pattern_ss << "[ "; + for (const auto& utterance: goal_handle->get_goal()->expected) + { + pattern_ss << "\"" << utterance << "\" "; + } + pattern_ss << "]"; + + std::stringstream topic_ss; + topic_ss << "topic: ~" << topic_name << " ()" << std::endl + << "language: " << language << std::endl + << "u:(_" << pattern_ss.str() << ") $" << topic_name << "/result=$1" << std::endl + << "u:(_*) $" << topic_name << "/result=$1" << std::endl; + std::string qichat_topic = topic_ss.str(); + RCLCPP_INFO(logger, "Loading topic:\n%s", qichat_topic.c_str()); + dialog.call("loadTopicContent", qichat_topic); + dialog.call("activateTopic", topic_name); + dialog.call("subscribe", topic_name); + dialog.call("setFocus", topic_name); + + // auto memory_receiver = boost::make_shared(); + // auto service_id = session->registerService(topic_name, memory_receiver); + auto memory = session->service("ALMemory").value(); + // memory.call("subscribeToEvent", "Dialog/Answered", topic_name, "callback"); + state->memory_subscriber = memory.call("subscriber", topic_name + "/result"); + state->memory_subscriber.connect("signal", [=](const qi::AnyValue& value) + { + auto utterance = value.toString(); + RCLCPP_INFO(state->logger, "Received input: %s", utterance.c_str()); + result->result = std::vector{utterance}; + cleanup(*state); + goal_handle->succeed(result); + }); + + state->current_goal = goal_handle; + } + catch (const std::exception& e) + { + RCLCPP_ERROR(logger, "Failed to start listen %s: %s", goal_id.c_str(), e.what()); + cleanup(*state); + goal_handle->abort(result); + } + } + + rclcpp_action::CancelResponse handle_cancel( + std::shared_ptr state, + const std::shared_ptr goal_handle) + { + std::string goal_id = rclcpp_action::to_string(goal_handle->get_goal_id()); + RCLCPP_INFO(state->logger, "Received goal cancellation request %s", goal_id.c_str()); + cleanup(*state); + + // ROS2 action API requires that we set the canceled state after returning. + (void)std::async( + std::launch::async, + [state, goal_handle]{ goal_handle->canceled(std::make_shared()); } + ); + return rclcpp_action::CancelResponse::ACCEPT; + } + + void cleanup(ListenState& state) + { + if (!state.current_goal) { + return; + } + + auto goal_id = rclcpp_action::to_string(state.current_goal->get_goal_id()); + const auto topic_name = topic_name_for_goal_id(state.current_goal->get_goal_id()); + + state.memory_subscriber.reset(); + + auto& logger = state.logger; + const auto& session = state.session; + try + { + auto dialog = session->service("ALDialog").value(); + + // try + // { + // dialog.call("unsubscribe", topic_name); + // } + // catch (const std::exception& e) + // { + // RCLCPP_WARN(logger, "Failed to unsubscribe from topic %s: %s", topic_name.c_str(), e.what()); + // } + + try + { + dialog.call("deactivateTopic", topic_name); + } + catch (const std::exception& e) + { + RCLCPP_WARN(logger, "Failed to deactivate topic %s: %s", topic_name.c_str(), e.what()); + } + + try + { + dialog.call("unloadTopic", topic_name); + } + catch (const std::exception& e) + { + RCLCPP_WARN(logger, "Failed to unload topic %s: %s", topic_name.c_str(), e.what()); + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR(logger, "Failed to get ALDialog service while cleaning up: %s", e.what()); + } + + RCLCPP_INFO(logger, "Listen %s stopped and cleaned up", goal_id.c_str()); + state.current_goal.reset(); + } +} + +rclcpp_action::Server::SharedPtr +createListenServer(rclcpp::Node* node, qi::SessionPtr session) +{ + namespace ph = std::placeholders; + auto task = std::make_shared(node, std::move(session)); + return rclcpp_action::create_server( + node, "listen", + std::bind(handle_goal, task, ph::_1, ph::_2), + std::bind(handle_cancel, task, ph::_1), + std::bind(handle_accepted, task, ph::_1) + ); +} + +} // ends namespace action +} // ends namespace naoqi \ No newline at end of file diff --git a/src/actions/listen.hpp b/src/actions/listen.hpp new file mode 100644 index 0000000..10759a9 --- /dev/null +++ b/src/actions/listen.hpp @@ -0,0 +1,13 @@ +#include +#include +#include + +namespace naoqi +{ +namespace action { + +rclcpp_action::Server::SharedPtr +createListenServer(rclcpp::Node* node, qi::SessionPtr sesssion); + +} // ends namespace action +} // ends namespace naoqi \ No newline at end of file diff --git a/src/converters/audio.cpp b/src/converters/audio.cpp index 8dad28c..0250c66 100644 --- a/src/converters/audio.cpp +++ b/src/converters/audio.cpp @@ -20,11 +20,6 @@ */ #include "audio.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi{ @@ -47,10 +42,15 @@ void AudioEventConverter::registerCallback( const message_actions::MessageAction callbacks_[action] = cb; } +void AudioEventConverter::unregisterCallback( const message_actions::MessageAction action ) +{ + callbacks_.erase(action); +} + void AudioEventConverter::callAll(const std::vector& actions, naoqi_bridge_msgs::msg::AudioBuffer& msg) { msg_ = msg; - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action](msg_); } diff --git a/src/converters/audio.hpp b/src/converters/audio.hpp index be740d2..14f10f8 100644 --- a/src/converters/audio.hpp +++ b/src/converters/audio.hpp @@ -50,7 +50,8 @@ class AudioEventConverter : public BaseConverter virtual void reset(); - void registerCallback( const message_actions::MessageAction action, Callback_t cb ); + void registerCallback(const message_actions::MessageAction action, Callback_t cb); + void unregisterCallback(const message_actions::MessageAction action); void callAll(const std::vector& actions, naoqi_bridge_msgs::msg::AudioBuffer& msg); diff --git a/src/converters/camera.cpp b/src/converters/camera.cpp index 3ad592f..c4c5e5b 100644 --- a/src/converters/camera.cpp +++ b/src/converters/camera.cpp @@ -26,18 +26,13 @@ /* * ROS includes */ -#include +#include /* * CV includes */ #include -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -171,10 +166,9 @@ const sensor_msgs::msg::CameraInfo& getCameraInfo( int camera_source, int resolu return cam_info_msg; } } - else{ - std::cout << "no camera information found for camera_source " << camera_source << " and res: " << resolution << std::endl; - return getEmptyInfo(); - } + + std::cout << "no camera information found for camera_source " << camera_source << " and res: " << resolution << std::endl; + return getEmptyInfo(); } } // camera_info_definitions @@ -186,7 +180,7 @@ CameraConverter::CameraConverter( const int& camera_source, const int& resolution, const bool& has_stereo) : BaseConverter( name, frequency, session ), - p_video_( session->service("ALVideoDevice") ), + p_video_( session->service("ALVideoDevice").value()), camera_source_(camera_source), resolution_(resolution), // change in case of depth camera @@ -291,7 +285,7 @@ void CameraConverter::callAll( const std::vector //msg_->header.stamp.nsec = image.timestamp_us*1000; camera_info_.header.stamp = msg_->header.stamp; - for_each( const message_actions::MessageAction& action, actions ) + for( const message_actions::MessageAction& action: actions ) { callbacks_[action]( msg_, camera_info_ ); } diff --git a/src/converters/camera.hpp b/src/converters/camera.hpp index 367a372..2fc91ae 100644 --- a/src/converters/camera.hpp +++ b/src/converters/camera.hpp @@ -28,7 +28,7 @@ /* * ROS includes */ -#include +#include namespace naoqi { diff --git a/src/converters/diagnostics.cpp b/src/converters/diagnostics.cpp index e718db3..89fc040 100644 --- a/src/converters/diagnostics.cpp +++ b/src/converters/diagnostics.cpp @@ -26,11 +26,6 @@ */ #include -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace { @@ -53,16 +48,16 @@ namespace converter DiagnosticsConverter::DiagnosticsConverter( const std::string& name, float frequency, const qi::SessionPtr& session ): BaseConverter( name, frequency, session ), - p_memory_(session->service("ALMemory")), + p_memory_(session->service("ALMemory").value()), temperature_warn_level_(68), temperature_error_level_(74) { // Allow for temperature reporting (for CPU) if ((robot_ == robot::PEPPER) || (robot_ == robot::NAO)) { - p_body_temperature_ = session->service("ALBodyTemperature"); - + p_body_temperature_ = session->service("ALBodyTemperature").value(); + // Only call setEnableNotifications if NAOqi < 2.9 - if (helpers::driver::isNaoqiVersionLesser(naoqi_version_, 2, 9)) + if (helpers::driver::isNaoqiVersionLesser(naoqi_version_, 2, 8)) { p_body_temperature_.call("setEnableNotifications", true); } @@ -72,7 +67,7 @@ DiagnosticsConverter::DiagnosticsConverter( const std::string& name, float frequ qi::AnyValue qi_joint_limits; // Get all the joint names - this->p_motion_ = session->service("ALMotion"); + this->p_motion_ = session->service("ALMotion").value(); joint_names_ = this->p_motion_.call >("getBodyNames", "JointActuators"); for(std::vector::const_iterator it = joint_names_.begin(); it != joint_names_.end(); ++it) { @@ -306,7 +301,7 @@ void DiagnosticsConverter::callAll( const std::vector -#include +#include -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -40,7 +35,7 @@ namespace converter { ImuConverter::ImuConverter(const std::string& name, const IMU::Location& location, const float& frequency, const qi::SessionPtr& session): BaseConverter(name, frequency, session), - p_memory_(session->service("ALMemory")) + p_memory_(session->service("ALMemory").value()) { if(location == IMU::TORSO){ msg_imu_.header.frame_id = "base_link"; @@ -126,7 +121,7 @@ namespace converter { msg_imu_.linear_acceleration_covariance[0] = -1; - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action]( msg_imu_ ); } diff --git a/src/converters/info.cpp b/src/converters/info.cpp index 345b407..4a36326 100644 --- a/src/converters/info.cpp +++ b/src/converters/info.cpp @@ -25,8 +25,6 @@ * BOOST includes */ #include -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -35,7 +33,7 @@ namespace converter InfoConverter::InfoConverter( const std::string& name, float frequency, const qi::SessionPtr& session ) : BaseConverter( name, frequency, session ), - p_memory_( session->service("ALMemory") ) + p_memory_( session->service("ALMemory").value() ) { keys_.push_back("RobotConfig/Head/FullHeadId"); keys_.push_back("Device/DeviceList/ChestBoard/BodyId"); @@ -86,7 +84,7 @@ void InfoConverter::callAll( const std::vector& if (i != keys_.size()-1) msg.data += " ; "; } - for_each( const message_actions::MessageAction& action, actions ) + for( const message_actions::MessageAction& action: actions ) { callbacks_[action](msg); } diff --git a/src/converters/joint_state.cpp b/src/converters/joint_state.cpp index 35911a9..1a61f72 100644 --- a/src/converters/joint_state.cpp +++ b/src/converters/joint_state.cpp @@ -21,18 +21,12 @@ #include "joint_state.hpp" #include "nao_footprint.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH - /* * ROS includes */ #include #include -#include +#include namespace naoqi { @@ -42,11 +36,10 @@ namespace converter JointStateConverter::JointStateConverter( const std::string& name, const float& frequency, const BufferPtr& tf2_buffer, const qi::SessionPtr& session ): BaseConverter( name, frequency, session ), - p_motion_( session->service("ALMotion") ), - p_memory_( session->service("ALMemory") ), + p_motion_( session->service("ALMotion").value() ), + p_memory_( session->service("ALMemory").value() ), tf2_buffer_(tf2_buffer) { - robot_desc_ = tools::getRobotDescription( robot_ ); } JointStateConverter::~JointStateConverter() @@ -56,13 +49,15 @@ JointStateConverter::~JointStateConverter() void JointStateConverter::reset() { - if ( robot_desc_.empty() ) + std::string robot_desc = naoqi::tools::getRobotDescription(robot_); + if ( robot_desc.empty() ) { std::cout << "error in loading robot description" << std::endl; return; } + urdf::Model model; - model.initString( robot_desc_ ); + model.initString(robot_desc); KDL::Tree tree; kdl_parser::treeFromUrdfModel( model, tree ); @@ -86,11 +81,20 @@ void JointStateConverter::registerCallback( const message_actions::MessageAction void JointStateConverter::callAll( const std::vector& actions ) { + /* + * get torso position for odometry at the same time as joint states + * can be called via getRobotPosture + * but this would require a proper URDF + * with a base_link and base_footprint in the base + */ + auto getting_odometry_data = p_motion_.async >( "getPosition", "Torso", 1, true ); + // get joint state values std::vector al_joint_angles = p_motion_.call >("getAngles", "Body", true ); std::vector al_joint_velocities; std::vector al_joint_torques; + std::vector al_odometry_data = getting_odometry_data.value(); const rclcpp::Time& stamp = helpers::Time::now(); /** @@ -153,15 +157,8 @@ void JointStateConverter::callAll( const std::vector al_odometry_data = p_motion_.call >( "getPosition", "Torso", 1, true ); - const rclcpp::Time& odom_stamp = helpers::Time::now(); - const float& odomX = al_odometry_data[0]; + const rclcpp::Time &odom_stamp = stamp; + const float &odomX = al_odometry_data[0]; const float& odomY = al_odometry_data[1]; const float& odomZ = al_odometry_data[2]; const float& odomWX = al_odometry_data[3]; @@ -196,7 +193,7 @@ void JointStateConverter::callAll( const std::vector /** Registered Callbacks **/ std::map callbacks_; - /** Robot Description in xml format **/ - std::string robot_desc_; - /** MimicJoint List **/ MimicMap mimic_; diff --git a/src/converters/laser.cpp b/src/converters/laser.cpp index edabedd..2ed3734 100644 --- a/src/converters/laser.cpp +++ b/src/converters/laser.cpp @@ -21,12 +21,6 @@ #include "laser.hpp" #include "../tools/from_any_value.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH - namespace naoqi { namespace converter @@ -130,7 +124,7 @@ static const char* laserMemoryKeys[] = { LaserConverter::LaserConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ): BaseConverter( name, frequency, session ), - p_memory_(session->service("ALMemory")) + p_memory_(session->service("ALMemory").value()) { } @@ -204,7 +198,7 @@ void LaserConverter::callAll( const std::vector& msg_.ranges[pos] = dist; } - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action](msg_); } @@ -224,12 +218,12 @@ void LaserConverter::reset( ) void LaserConverter::setLaserRanges( const float &range_min, const float &range_max) { - + if (range_min > 0) this->range_min_ = range_min; else this->range_min_ = 0.1; - + if (range_max > this->range_min_) this->range_max_ = range_max; else diff --git a/src/converters/log.cpp b/src/converters/log.cpp index c0840ee..0e7f087 100644 --- a/src/converters/log.cpp +++ b/src/converters/log.cpp @@ -24,9 +24,6 @@ #include #include -#include - -#define for_each BOOST_FOREACH namespace naoqi { @@ -43,7 +40,7 @@ std::queue LOGS; * libqi and ROS log levels. The severity variable refers to the * RCUTILS_LOG_SEVERITY level of a specific logger, while the ros_msg variable * refers to the level of a rcl_interfaces/Log message. - * + * */ class LogLevel { @@ -56,23 +53,26 @@ class LogLevel static const LogLevel& get_from_qi(qi::LogLevel qi) { - for_each(const LogLevel& log_level, all_) + for(const LogLevel& log_level: all_) if (log_level.qi_ == qi) return log_level; + throw std::invalid_argument("unknown qi log level"); } static const LogLevel& get_from_ros_msg(rcl_interfaces::msg::Log::_level_type ros_msg) { - for_each(const LogLevel& log_level, all_) + for(const LogLevel& log_level: all_) if (log_level.ros_msg_ == ros_msg) return log_level; + throw std::invalid_argument("unknown ROS message log level type"); } static const LogLevel& get_from_log_severity(int severity) { - for_each(const LogLevel& log_level, all_) + for(const LogLevel& log_level: all_) if (log_level.severity_ == severity) return log_level; + throw std::invalid_argument("unknown log severity"); } qi::LogLevel qi_; @@ -114,7 +114,7 @@ void logCallback(const qi::LogMessage& msg) LogConverter::LogConverter( const std::string& name, float frequency, const qi::SessionPtr& session ) : BaseConverter( name, frequency, session ), - logger_( session->service("LogManager") ), + logger_( session->service("LogManager").value() ), // Default log level is info log_level_(qi::LogLevel_Info) { @@ -126,14 +126,14 @@ LogConverter::LogConverter( const std::string& name, float frequency, const qi:: LogLevel(qi::LogLevel_Info, rcl_interfaces::msg::Log::INFO, RCUTILS_LOG_SEVERITY_INFO); LogLevel(qi::LogLevel_Verbose, rcl_interfaces::msg::Log::DEBUG, RCUTILS_LOG_SEVERITY_DEBUG); LogLevel(qi::LogLevel_Debug, rcl_interfaces::msg::Log::DEBUG, RCUTILS_LOG_SEVERITY_DEBUG); - + // TEMPORARY CODE, WEIRD BUG - qi::AnyObject p_manager = session->service("LogManager"); + qi::AnyObject p_manager = session->service("LogManager").value(); auto test_obj = p_manager.call("getListener"); qi::LogListenerPtr test = static_cast(test_obj); test->onLogMessage.connect(logCallback); // END - + // listener_ = logger_->getListener(); set_qi_logger_level(); // listener_->onLogMessage.connect(logCallback); @@ -149,7 +149,7 @@ void LogConverter::callAll( const std::vector& a while ( !LOGS.empty() ) { rcl_interfaces::msg::Log& log_msg = LOGS.front(); - for_each( const message_actions::MessageAction& action, actions) + for( const message_actions::MessageAction& action: actions) { callbacks_[action](log_msg); } diff --git a/src/converters/memory/bool.cpp b/src/converters/memory/bool.cpp index 7f61dc5..ad2f51a 100644 --- a/src/converters/memory/bool.cpp +++ b/src/converters/memory/bool.cpp @@ -20,11 +20,6 @@ */ #include "bool.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -34,7 +29,7 @@ namespace converter MemoryBoolConverter::MemoryBoolConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key ) : BaseConverter( name, frequency, session ), memory_key_(memory_key), - p_memory_( session->service("ALMemory") ) + p_memory_( session->service("ALMemory").value() ) {} void MemoryBoolConverter::registerCallback( message_actions::MessageAction action, Callback_t cb ) @@ -59,7 +54,7 @@ bool MemoryBoolConverter::convert() void MemoryBoolConverter::callAll( const std::vector& actions ) { if (convert()) { - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action]( msg_ ); } diff --git a/src/converters/memory/float.cpp b/src/converters/memory/float.cpp index a846f6d..36b908a 100644 --- a/src/converters/memory/float.cpp +++ b/src/converters/memory/float.cpp @@ -20,12 +20,6 @@ */ #include "float.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH - namespace naoqi { namespace converter @@ -34,7 +28,7 @@ namespace converter MemoryFloatConverter::MemoryFloatConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key ) : BaseConverter( name, frequency, session ), memory_key_(memory_key), - p_memory_( session->service("ALMemory") ) + p_memory_( session->service("ALMemory").value() ) {} void MemoryFloatConverter::registerCallback( message_actions::MessageAction action, Callback_t cb ) @@ -63,7 +57,7 @@ bool MemoryFloatConverter::convert() void MemoryFloatConverter::callAll( const std::vector& actions ) { if (convert()) { - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action]( msg_ ); } diff --git a/src/converters/memory/int.cpp b/src/converters/memory/int.cpp index 3f87a61..b24751a 100644 --- a/src/converters/memory/int.cpp +++ b/src/converters/memory/int.cpp @@ -20,11 +20,6 @@ */ #include "int.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -34,7 +29,7 @@ namespace converter MemoryIntConverter::MemoryIntConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key ) : BaseConverter( name, frequency, session ), memory_key_(memory_key), - p_memory_( session->service("ALMemory") ) + p_memory_( session->service("ALMemory").value() ) {} void MemoryIntConverter::registerCallback( message_actions::MessageAction action, Callback_t cb ) @@ -63,7 +58,7 @@ bool MemoryIntConverter::convert() void MemoryIntConverter::callAll( const std::vector& actions ) { if (convert()) { - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action]( msg_ ); } diff --git a/src/converters/memory/string.cpp b/src/converters/memory/string.cpp index 7e5db83..bbe6bf7 100644 --- a/src/converters/memory/string.cpp +++ b/src/converters/memory/string.cpp @@ -20,11 +20,6 @@ */ #include "string.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -34,7 +29,7 @@ namespace converter MemoryStringConverter::MemoryStringConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session, const std::string& memory_key ) : BaseConverter( name, frequency, session ), memory_key_(memory_key), - p_memory_( session->service("ALMemory") ) + p_memory_( session->service("ALMemory").value() ) {} void MemoryStringConverter::registerCallback( message_actions::MessageAction action, Callback_t cb ) @@ -63,7 +58,7 @@ bool MemoryStringConverter::convert() void MemoryStringConverter::callAll( const std::vector& actions ) { if (convert()) { - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action]( msg_ ); } diff --git a/src/converters/memory_list.cpp b/src/converters/memory_list.cpp index aed6143..59ed034 100644 --- a/src/converters/memory_list.cpp +++ b/src/converters/memory_list.cpp @@ -20,11 +20,6 @@ */ #include "memory_list.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -32,7 +27,7 @@ namespace converter { MemoryListConverter::MemoryListConverter(const std::vector& key_list, const std::string &name, const float &frequency, const qi::SessionPtr &session): BaseConverter(name, frequency, session), - p_memory_(session->service("ALMemory")), + p_memory_(session->service("ALMemory").value()), _key_list(key_list) {} @@ -76,7 +71,7 @@ void MemoryListConverter::callAll(const std::vector #include -#include +#include /* * loca includes @@ -45,7 +45,7 @@ inline void addBaseFootprint( boost::shared_ptr tf2_buffer, std bool canTransform = tf2_buffer->canTransform("odom", "l_sole", tf2::timeFromSec(time.seconds()), tf2::durationFromSec(0.1) ); if (!canTransform) { - RCLCPP_ERROR(helpers::Node::get_logger(), "Do not calculate NAO Footprint, no transform possible (%d seconds)", time.seconds()); + RCLCPP_ERROR(helpers::Node::get_logger(), "Could not compute NAO Footprint: no transform is possible (%f seconds)", time.seconds()); return; } diff --git a/src/converters/odom.cpp b/src/converters/odom.cpp index 6714d61..d138341 100644 --- a/src/converters/odom.cpp +++ b/src/converters/odom.cpp @@ -21,24 +21,18 @@ #include "odom.hpp" #include "../tools/from_any_value.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH - -#include +#include namespace naoqi { namespace converter { - + OdomConverter::OdomConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ): BaseConverter( name, frequency, session ), - p_motion_( session->service("ALMotion") ) - + p_motion_( session->service("ALMotion").value() ) + { } @@ -49,22 +43,22 @@ void OdomConverter::registerCallback( message_actions::MessageAction action, Cal void OdomConverter::callAll( const std::vector& actions ) { - + int FRAME_WORLD = 1; bool use_sensor = true; // documentation of getPosition available here: http://doc.aldebaran.com/2-1/naoqi/motion/control-cartesian.html std::vector al_odometry_data = p_motion_.call >( "getPosition", "Torso", FRAME_WORLD, use_sensor ); - + const rclcpp::Time& odom_stamp = helpers::Time::now(); std::vector al_speed_data = p_motion_.call >( "getRobotVelocity" ); - + const float& odomX = al_odometry_data[0]; const float& odomY = al_odometry_data[1]; const float& odomZ = al_odometry_data[2]; const float& odomWX = al_odometry_data[3]; const float& odomWY = al_odometry_data[4]; const float& odomWZ = al_odometry_data[5]; - + const float& dX = al_speed_data[0]; const float& dY = al_speed_data[1]; const float& dWZ = al_speed_data[2]; @@ -83,19 +77,19 @@ void OdomConverter::callAll( const std::vector& msg_odom.pose.pose.position.x = odomX; msg_odom.pose.pose.position.y = odomY; msg_odom.pose.pose.position.z = odomZ; - + msg_odom.twist.twist.linear.x = dX; msg_odom.twist.twist.linear.y = dY; msg_odom.twist.twist.linear.z = 0; - + msg_odom.twist.twist.angular.x = 0; msg_odom.twist.twist.angular.y = 0; msg_odom.twist.twist.angular.z = dWZ; - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action](msg_odom); - + } } diff --git a/src/converters/sonar.cpp b/src/converters/sonar.cpp index 5feca30..45cc948 100644 --- a/src/converters/sonar.cpp +++ b/src/converters/sonar.cpp @@ -21,11 +21,6 @@ #include "sonar.hpp" #include "../tools/from_any_value.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi { @@ -34,13 +29,13 @@ namespace converter SonarConverter::SonarConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ) : BaseConverter( name, frequency, session ), - p_memory_( session->service("ALMemory") ), + p_memory_( session->service("ALMemory").value() ), is_subscribed_(false) { // Only create a sonar proxy if NAOqi < 2.9 if (helpers::driver::isNaoqiVersionLesser(naoqi_version_, 2, 9)) { - p_sonar_ = session->service("ALSonar"); + p_sonar_ = session->service("ALSonar").value(); } std::vector keys; @@ -116,7 +111,7 @@ void SonarConverter::callAll( const std::vector& msgs_[i].range = float(values[i]); } - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action]( msgs_ ); } diff --git a/src/converters/touch.cpp b/src/converters/touch.cpp index 13b79dc..b9dec26 100644 --- a/src/converters/touch.cpp +++ b/src/converters/touch.cpp @@ -20,11 +20,6 @@ */ #include "touch.hpp" -/* -* BOOST includes -*/ -#include -#define for_each BOOST_FOREACH namespace naoqi{ @@ -55,7 +50,7 @@ template void TouchEventConverter::callAll(const std::vector& actions, T& msg) { msg_ = msg; - for_each( message_actions::MessageAction action, actions ) + for( message_actions::MessageAction action: actions ) { callbacks_[action](msg_); } diff --git a/src/event/audio.cpp b/src/event/audio.cpp index 8fef58b..53911b4 100644 --- a/src/event/audio.cpp +++ b/src/event/audio.cpp @@ -18,8 +18,6 @@ #include #include -#include - #include #include @@ -31,13 +29,15 @@ namespace naoqi { +static const std::string AUDIO_EXTRACTOR_NAME = "ROS-Driver-Audio"; AudioEventRegister::AudioEventRegister( const std::string& name, const float& frequency, const qi::SessionPtr& session ) - : serviceId(0), - p_audio_( session->service("ALAudioDevice")), - p_robot_model_(session->service("ALRobotModel")), - session_(session), - naoqi_version_(helpers::driver::getNaoqiVersion(session)), + : session_(session), + publisher_(name), + recorder_(name), + converter_(name, frequency, session), + p_audio_( session->service("ALAudioDevice").value()), + serviceId(0), isStarted_(false), isPublishing_(false), isRecording_(false), @@ -45,18 +45,16 @@ AudioEventRegister::AudioEventRegister( const std::string& name, const float& fr { // _getMicrophoneConfig is used for NAOqi < 2.9, _getConfigMap for NAOqi > 2.9 int micConfig; - - if (helpers::driver::isNaoqiVersionLesser(naoqi_version_, 2, 9)) + auto robotModel = session->service("ALRobotModel").value(); + const auto &naoqiVersion = helpers::driver::getNaoqiVersion(session); + if (helpers::driver::isNaoqiVersionLesser(naoqiVersion, 2, 8)) { - micConfig = p_robot_model_.call("_getMicrophoneConfig"); + micConfig = robotModel.call("_getMicrophoneConfig"); } else { - std::map config_map =\ - p_robot_model_.call >("_getConfigMap"); - - micConfig = std::atoi( - config_map["RobotConfig/Head/Device/Micro/Version"].c_str()); + auto config_map = robotModel.call >("_getConfigMap"); + micConfig = std::atoi(config_map["RobotConfig/Head/Device/Micro/Version"].c_str()); } if(micConfig){ @@ -71,29 +69,27 @@ AudioEventRegister::AudioEventRegister( const std::string& name, const float& fr channelMap.push_back(1); channelMap.push_back(4); } - publisher_ = boost::make_shared >( name ); - recorder_ = boost::make_shared >( name ); - converter_ = boost::make_shared( name, frequency, session ); - - converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, publisher_, _1) ); - converter_->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicEventRecorder::write, recorder_, _1) ); - converter_->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicEventRecorder::bufferize, recorder_, _1) ); - + converter_.registerCallback( message_actions::PUBLISH, [&](auto msg){ publisher_.publish(msg); }); + converter_.registerCallback( message_actions::RECORD, [&](auto msg){ recorder_.write(msg); }); + converter_.registerCallback( message_actions::LOG, [&](auto msg){ recorder_.bufferize(msg); }); } AudioEventRegister::~AudioEventRegister() { stopProcess(); + converter_.unregisterCallback(message_actions::PUBLISH); + converter_.unregisterCallback(message_actions::RECORD); + converter_.unregisterCallback(message_actions::LOG); } void AudioEventRegister::resetPublisher(rclcpp::Node* node) { - publisher_->reset(node); + publisher_.reset(node); } void AudioEventRegister::resetRecorder( boost::shared_ptr gr ) { - recorder_->reset(gr, converter_->frequency()); + recorder_.reset(gr, converter_.frequency()); } void AudioEventRegister::startProcess() @@ -103,15 +99,15 @@ void AudioEventRegister::startProcess() { if(!serviceId) { - serviceId = session_->registerService("ROS-Driver-Audio", shared_from_this()); + serviceId = session_->registerService(AUDIO_EXTRACTOR_NAME, shared_from_this()).value(); p_audio_.call( "setClientPreferences", - "ROS-Driver-Audio", + AUDIO_EXTRACTOR_NAME, 48000, 0, 0 ); - p_audio_.call("subscribe","ROS-Driver-Audio"); + p_audio_.call("subscribe", AUDIO_EXTRACTOR_NAME); std::cout << "Audio Extractor: Start" << std::endl; } isStarted_ = true; @@ -124,7 +120,7 @@ void AudioEventRegister::stopProcess() if (isStarted_) { if(serviceId){ - p_audio_.call("unsubscribe", "ROS-Driver-Audio"); + p_audio_.call("unsubscribe", AUDIO_EXTRACTOR_NAME); session_->unregisterService(serviceId); serviceId = 0; } @@ -137,13 +133,13 @@ void AudioEventRegister::writeDump(const rclcpp::Time& time) { if (isStarted_) { - recorder_->writeDump(time); + recorder_.writeDump(time); } } void AudioEventRegister::setBufferDuration(float duration) { - recorder_->setBufferDuration(duration); + recorder_.setBufferDuration(duration); } void AudioEventRegister::isRecording(bool state) @@ -189,7 +185,7 @@ void AudioEventRegister::processRemote(int nbOfChannels, int samplesByChannel, q boost::mutex::scoped_lock callback_lock(processing_mutex_); if (isStarted_) { // CHECK FOR PUBLISH - if ( isPublishing_ && publisher_->isSubscribed() ) + if ( isPublishing_ && publisher_.isSubscribed() ) { actions.push_back(message_actions::PUBLISH); } @@ -204,7 +200,7 @@ void AudioEventRegister::processRemote(int nbOfChannels, int samplesByChannel, q } if (actions.size() >0) { - converter_->callAll( actions, msg ); + converter_.callAll( actions, msg ); } } diff --git a/src/event/audio.hpp b/src/event/audio.hpp index 96eaec5..346e6c8 100644 --- a/src/event/audio.hpp +++ b/src/event/audio.hpp @@ -83,17 +83,14 @@ class AudioEventRegister: public boost::enable_shared_from_this converter_; - boost::shared_ptr > publisher_; - boost::shared_ptr > recorder_; - qi::SessionPtr session_; + publisher::BasicPublisher publisher_; + recorder::BasicEventRecorder recorder_; + converter::AudioEventConverter converter_; qi::AnyObject p_audio_; - qi::AnyObject p_robot_model_; qi::FutureSync p_audio_extractor_request; std::vector channelMap; unsigned int serviceId; - const robot::NaoqiVersion& naoqi_version_; boost::mutex subscription_mutex_; boost::mutex processing_mutex_; diff --git a/src/event/basic.hxx b/src/event/basic.hxx index 3e74bd5..c4cebdc 100644 --- a/src/event/basic.hxx +++ b/src/event/basic.hxx @@ -38,7 +38,7 @@ EventRegister::EventRegister() template EventRegister::EventRegister( const std::string& key, const qi::SessionPtr& session ) : key_(key), - p_memory_( session->service("ALMemory") ), + p_memory_( session->service("ALMemory").value()), isStarted_(false), isPublishing_(false), isRecording_(false), @@ -48,9 +48,10 @@ EventRegister::EventRegister( const std::string& recorder_ = boost::make_shared( key_ ); converter_ = boost::make_shared( key_, 0, session, key_ ); - converter_->registerCallback( message_actions::PUBLISH, boost::bind(&Publisher::publish, publisher_, _1) ); - converter_->registerCallback( message_actions::RECORD, boost::bind(&Recorder::write, recorder_, _1) ); - converter_->registerCallback( message_actions::LOG, boost::bind(&Recorder::bufferize, recorder_, _1) ); + namespace ph = boost::placeholders; + converter_->registerCallback( message_actions::PUBLISH, boost::bind(&Publisher::publish, publisher_, ph::_1) ); + converter_->registerCallback( message_actions::RECORD, boost::bind(&Recorder::write, recorder_, ph::_1) ); + converter_->registerCallback( message_actions::LOG, boost::bind(&Recorder::bufferize, recorder_, ph::_1) ); signal_ = p_memory_.call("subscriber", key_); } @@ -133,8 +134,7 @@ void EventRegister::isDumping(bool state) template void EventRegister::registerCallback() { - signalID_ = signal_.connect("signal", (boost::function(boost::bind(&EventRegister::onEvent, - this)))); + signalID_ = signal_.connect("signal", [&](qi::AnyValue value) { onEvent(); }).value(); } template diff --git a/src/event/touch.cpp b/src/event/touch.cpp index 871f90f..d48f16c 100644 --- a/src/event/touch.cpp +++ b/src/event/touch.cpp @@ -39,9 +39,7 @@ TouchEventRegister::TouchEventRegister() template TouchEventRegister::TouchEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) - : serviceId(0), - p_memory_( session->service("ALMemory")), - session_(session), + : p_memory_( session->service("ALMemory").value()), isStarted_(false), isPublishing_(false), isRecording_(false), @@ -51,7 +49,7 @@ TouchEventRegister::TouchEventRegister( const std::string& name, const std::v //recorder_ = boost::make_shared >( name ); converter_ = boost::make_shared >( name, frequency, session ); - converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, publisher_, _1) ); + converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, publisher_, boost::placeholders::_1) ); //converter_->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicEventRecorder::write, recorder_, _1) ); //converter_->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicEventRecorder::bufferize, recorder_, _1) ); @@ -87,16 +85,13 @@ void TouchEventRegister::startProcess() boost::mutex::scoped_lock start_lock(mutex_); if (!isStarted_) { - if(!serviceId) + if (subscriptions_.empty()) { - //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name(); - std::string serviceName = std::string("ROS-Driver-") + keys_[0]; - serviceId = session_->registerService(serviceName, this->shared_from_this()); - for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) { - std::cerr << *it << std::endl; - p_memory_.call("subscribeToEvent",it->c_str(), serviceName, "touchCallback"); + for (const auto& key : keys_) { + auto subscriber = p_memory_.call("subscriber", key); + qi::SignalLink subscription = subscriber.connect("signal", [=](const qi::AnyValue& v){ touchCallback(key, v); }).value(); + subscriptions_.emplace_back(std::move(subscriber), std::move(subscription)); } - std::cout << serviceName << " : Start" << std::endl; } isStarted_ = true; } @@ -108,16 +103,16 @@ void TouchEventRegister::stopProcess() boost::mutex::scoped_lock stop_lock(mutex_); if (isStarted_) { - //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name(); - std::string serviceName = std::string("ROS-Driver-") + keys_[0]; - if(serviceId){ - for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) { - p_memory_.call("unsubscribeToEvent",it->c_str(), serviceName); + if (!subscriptions_.empty()){ + for (const auto& subscription : subscriptions_) { + try { + subscription.subscriber.disconnect(subscription.link).value(); + } catch (const std::exception& e) { + std::cerr << "Error attempting to clean-up ALMemory subscription: " << e.what() << std::endl; + } } - session_->unregisterService(serviceId); - serviceId = 0; + subscriptions_.clear(); } - std::cout << serviceName << " : Stop" << std::endl; isStarted_ = false; } } @@ -169,14 +164,11 @@ void TouchEventRegister::unregisterCallback() } template -void TouchEventRegister::touchCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message) +void TouchEventRegister::touchCallback(const std::string &key, const qi::AnyValue& value) { T msg = T(); - bool state = value.toFloat() > 0.5f; - //std::cerr << key << " " << state << std::endl; - touchCallbackMessage(key, state, msg); std::vector actions; @@ -204,7 +196,7 @@ void TouchEventRegister::touchCallback(std::string &key, qi::AnyValue &value, } template -void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::msg::Bumper &msg) +void TouchEventRegister::touchCallbackMessage(const std::string &key, bool &state, naoqi_bridge_msgs::msg::Bumper &msg) { int i = 0; for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i) @@ -217,7 +209,7 @@ void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, } template -void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::msg::HandTouch &msg) +void TouchEventRegister::touchCallbackMessage(const std::string &key, bool &state, naoqi_bridge_msgs::msg::HandTouch &msg) { int i = 0; for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i) @@ -230,7 +222,7 @@ void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, } template -void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::msg::HeadTouch &msg) +void TouchEventRegister::touchCallbackMessage(const std::string &key, bool &state, naoqi_bridge_msgs::msg::HeadTouch &msg) { int i = 0; for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i) diff --git a/src/event/touch.hpp b/src/event/touch.hpp index 2f876d5..5276ce9 100644 --- a/src/event/touch.hpp +++ b/src/event/touch.hpp @@ -19,8 +19,8 @@ #define TOUCH_EVENT_REGISTER_HPP #include +#include -#include #include #include #include @@ -78,13 +78,12 @@ class TouchEventRegister: public boost::enable_shared_from_this subscriptions_; + std::string name_; boost::mutex mutex_; @@ -129,33 +138,6 @@ class HandTouchEventRegister: public TouchEventRegister keys, const float& frequency, const qi::SessionPtr& session ) : TouchEventRegister(name, keys, frequency, session) {} }; -//QI_REGISTER_OBJECT(BumperEventRegister, touchCallback) -//QI_REGISTER_OBJECT(HeadTouchEventRegister, touchCallback) - -static bool _qiregisterTouchEventRegisterBumper() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, TouchEventRegister, touchCallback) - b.registerType(); - return true; - } -static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterTouchEventRegisterBumper(); - -static bool _qiregisterTouchEventRegisterHandTouch() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, TouchEventRegister, touchCallback) - b.registerType(); - return true; - } -static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterTouchEventRegisterHandTouch(); - -static bool _qiregisterTouchEventRegisterHeadTouch() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, TouchEventRegister, touchCallback) - b.registerType(); - return true; - } -static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterTouchEventRegisterHeadTouch(); - } //naoqi #endif diff --git a/src/external_registration.cpp b/src/external_registration.cpp index db39624..a4e74d5 100644 --- a/src/external_registration.cpp +++ b/src/external_registration.cpp @@ -15,6 +15,8 @@ * */ +#include + #include #include #include @@ -26,47 +28,59 @@ #include -#include #include "naoqi_driver/tools.hpp" #include "naoqi_driver/ros_helpers.hpp" -int main(int argc, char** argv) { +boost::weak_ptr driver_weak; + +void sigint_handler(int sig) +{ + if (auto driver = driver_weak.lock()) { + driver->stop(); + } +} + +int main(int argc, char** argv) +{ const std::string no_password = "no_password"; std::string protocol = "tcp://"; - + std::string nao_ip; int nao_port; std::string user; std::string password; std::string network_interface; + std::string listen_url; // Initialize ROS and create the driver Node rclcpp::init(argc, argv); - boost::shared_ptr bs = boost::make_shared(); + auto bs = boost::make_shared(); // Setup the time helper naoqi::helpers::Node::setNode(bs); - + // Retrieve the parameters bs->declare_parameter("nao_ip", "127.0.0.1"); bs->declare_parameter("nao_port", 9559); bs->declare_parameter("user", "nao"); bs->declare_parameter("password", no_password); bs->declare_parameter("network_interface", "eth0"); - + bs->declare_parameter("qi_listen_url", "tcp://0.0.0.0:0"); + bs->get_parameter("nao_ip", nao_ip); bs->get_parameter("nao_port", nao_port); bs->get_parameter("user", user); bs->get_parameter("password", password); bs->get_parameter("network_interface", network_interface); + bs->get_parameter("qi_listen_url", listen_url); if (password.compare(no_password) != 0) { #if LIBQI_VERSION >= 29 protocol = "tcps://"; #else - std::cout << BOLDRED - << "No need to set a password" - << RESETCOLOR + std::cout << BOLDRED + << "No need to set a password" + << RESETCOLOR << std::endl; #endif } @@ -86,30 +100,28 @@ int main(int argc, char** argv) { } #endif - qi::Future connection = app.session()->connect(url); - + auto session = app.session(); + qi::Future connection = session->connect(url); + if (connection.hasError()) { std::cout << BOLDRED << connection.error() << RESETCOLOR << std::endl; - return 0; + return EXIT_FAILURE; } - - // Register the ROS-Driver service and init the driver node - app.session()->registerService("ROS-Driver", bs); + // The session needs to listen on the network to process audio callbacks. + if (!listen_url.empty()) + session->listen(listen_url); // Pass the create session to the driver node, and init the node - bs->setQiSession(app.session()); - bs->init(); - std::cout << BOLDYELLOW - << "naoqi_driver initialized" - << RESETCOLOR - << std::endl; - - // Run the qi::ApplicationSession - app.run(); - - // Stop the driver service, close the qi::Session and stop ROS - bs->stopService(); + bs->setQiSession(session); + + // Run the driver node. Stop it on SIGINT. + driver_weak = bs; + signal(SIGINT, sigint_handler); + bs->run(); + driver_weak.reset(); + + // Close the qi::Session and shutdown ROS. app.session()->close(); rclcpp::shutdown(); - return 0; + return EXIT_SUCCESS; } diff --git a/src/helpers/driver_helpers.cpp b/src/helpers/driver_helpers.cpp index b35e715..4419a89 100644 --- a/src/helpers/driver_helpers.cpp +++ b/src/helpers/driver_helpers.cpp @@ -40,14 +40,14 @@ static naoqi_bridge_msgs::msg::RobotInfo& getRobotInfoLocal( const qi::SessionPt // Get the robot type std::cout << "Receiving information about robot model" << std::endl; - qi::AnyObject p_memory = session->service("ALMemory"); + qi::AnyObject p_memory = session->service("ALMemory").value(); std::string robot = p_memory.call("getData", "RobotConfig/Body/Type" ); std::string hardware_version = p_memory.call("getData", "RobotConfig/Body/BaseVersion" ); robot::NaoqiVersion naoqi_version = getNaoqiVersion(session); std::transform(robot.begin(), robot.end(), robot.begin(), ::tolower); - + std::cout << BOLDYELLOW << "Robot detected/NAOqi version: " << RESETCOLOR; - + if (std::string(robot) == "nao") { info.type = naoqi_bridge_msgs::msg::RobotInfo::NAO; @@ -67,7 +67,7 @@ static naoqi_bridge_msgs::msg::RobotInfo& getRobotInfoLocal( const qi::SessionPt std::cout << BOLDCYAN << " / " << naoqi_version.text << RESETCOLOR << std::endl; // Get the data from RobotConfig - qi::AnyObject p_motion = session->service("ALMotion"); + qi::AnyObject p_motion = session->service("ALMotion").value(); std::vector > config = p_motion.call > >("getRobotConfig"); // TODO, fill with the proper string matches from http://doc.aldebaran.com/2-1/naoqi/motion/tools-general-api.html#ALMotionProxy::getRobotConfig @@ -199,23 +199,23 @@ const robot::Robot& getRobot( const qi::SessionPtr& session ) /** * @brief Function that retrieves the NAOqi version of the robot - * - * @param session - * @return const robot::NaoqiVersion& + * + * @param session + * @return const robot::NaoqiVersion& */ const robot::NaoqiVersion& getNaoqiVersion( const qi::SessionPtr& session ) { static robot::NaoqiVersion naoqi_version; try { - qi::AnyObject p_system = session->service("ALSystem"); + qi::AnyObject p_system = session->service("ALSystem").value(); naoqi_version.text = p_system.call("systemVersion"); } catch (const std::exception& e) { std::cerr << "Could not retrieve the version of NAOqi: " << e.what() << std::endl; - + naoqi_version.text = "unknown"; return naoqi_version; } @@ -266,31 +266,24 @@ const naoqi_bridge_msgs::msg::RobotInfo& getRobotInfo( const qi::SessionPtr& ses /** Function that sets language for a robot */ -bool& setLanguage( const qi::SessionPtr& session, const std::shared_ptr request) +bool setLanguage( const qi::SessionPtr& session, const std::shared_ptr request) { - static bool success; - std::cout << "Receiving service call of setting speech language" << std::endl; try{ - qi::AnyObject p_text_to_speech = session->service("ALTextToSpeech"); - p_text_to_speech.call("setLanguage", request->data); - success = true; - return success; + qi::AnyObject dialog = session->service("ALDialog").value(); + dialog.call("setLanguage", request->data); + return true; } catch(const std::exception& e){ - success = false; - return success; + return false; } } /** Function that gets language set to a robot */ -std::string& getLanguage( const qi::SessionPtr& session ) +std::string getLanguage( const qi::SessionPtr& session ) { - static std::string language; - std::cout << "Receiving service call of getting speech language" << std::endl; - qi::AnyObject p_text_to_speech = session->service("ALTextToSpeech"); - language = p_text_to_speech.call("getLanguage"); - return language; + qi::AnyObject dialog = session->service("ALDialog").value(); + return dialog.call("getLanguage"); } /** @@ -300,7 +293,7 @@ bool isDepthStereo(const qi::SessionPtr &session) { std::vector sensor_names; try { - qi::AnyObject p_motion = session->service("ALMotion"); + qi::AnyObject p_motion = session->service("ALMotion").value(); sensor_names = p_motion.call >("getSensorNames"); if (std::find(sensor_names.begin(), @@ -322,14 +315,14 @@ bool isDepthStereo(const qi::SessionPtr &session) { /** * @brief Function that returns true if the provided naoqi_version is * (strictly) lesser than the specified one (major.minor.patch.build). - * - * @param naoqi_version - * @param major - * @param minor - * @param patch - * @param build - * @return true - * @return false + * + * @param naoqi_version + * @param major + * @param minor + * @param patch + * @param build + * @return true + * @return false */ bool isNaoqiVersionLesser( const robot::NaoqiVersion& naoqi_version, diff --git a/src/helpers/driver_helpers.hpp b/src/helpers/driver_helpers.hpp index df8363b..640ef2e 100644 --- a/src/helpers/driver_helpers.hpp +++ b/src/helpers/driver_helpers.hpp @@ -40,9 +40,9 @@ const robot::NaoqiVersion& getNaoqiVersion(const qi::SessionPtr& session); const naoqi_bridge_msgs::msg::RobotInfo& getRobotInfo( const qi::SessionPtr& session ); -bool& setLanguage( const qi::SessionPtr& session, const std::shared_ptr request ); +bool setLanguage( const qi::SessionPtr& session, const std::shared_ptr request ); -std::string& getLanguage( const qi::SessionPtr& session ); +std::string getLanguage( const qi::SessionPtr& session ); bool isDepthStereo(const qi::SessionPtr &session); diff --git a/src/helpers/filesystem_helpers.hpp b/src/helpers/filesystem_helpers.hpp index d394344..2e7244a 100644 --- a/src/helpers/filesystem_helpers.hpp +++ b/src/helpers/filesystem_helpers.hpp @@ -100,7 +100,7 @@ inline std::string& getBootConfigFile() { #ifdef AMENT_BUILD static std::string path = ament_index_cpp::get_package_share_directory("naoqi_driver") + "/share/" + boot_config_file_name; - std::cout << "found a catkin prefix " << path << std::endl; + std::cout << "found an ament prefix " << path << std::endl; return path; #else static std::string path = qi::path::findData( "/", boot_config_file_name ); @@ -114,7 +114,7 @@ inline std::string& getURDF( std::string filename ) { #ifdef AMENT_BUILD static std::string path = ament_index_cpp::get_package_share_directory("naoqi_driver") + "/share/urdf/"+filename; - std::cout << "found a catkin URDF " << path << std::endl; + std::cout << "found a ament URDF " << path << std::endl; return path; #else static std::string path = qi::path::findData( "/urdf/", filename ); diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 322a332..b8f24df 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -15,6 +15,16 @@ * */ +/* + * BOOST + */ +#include + +/* + * ROS + */ +#include + /* * PUBLIC INTERFACE */ @@ -89,6 +99,11 @@ #include "event/audio.hpp" #include "event/touch.hpp" +/* + * ACTIONS + */ +#include "actions/listen.hpp" + /* * STATIC FUNCTIONS INCLUDE */ @@ -98,19 +113,9 @@ #include "helpers/naoqi_helpers.hpp" #include "helpers/driver_helpers.hpp" -/* - * ROS - */ -#include -/* - * BOOST - */ -#include -#include -#define for_each BOOST_FOREACH -#define DEBUG 0 +namespace ph = boost::placeholders; namespace naoqi { @@ -126,91 +131,92 @@ Driver::Driver() : rclcpp::Node("naoqi_driver"), Driver::~Driver() { - std::cout << BOLDCYAN - << "naoqi driver is shutting down.." - << RESETCOLOR + std::cout << BOLDCYAN + << "naoqi driver is shutting down.." + << RESETCOLOR << std::endl; } -void Driver::init() +void Driver::run() { loadBootConfig(); + auto robot_desc_pub = tools::publishRobotDescription(this, robot_); registerDefaultConverter(); registerDefaultSubscriber(); registerDefaultServices(); - startRosLoop(); - /* TEMPORARY CODE, TO ORGANIZE, REPLACING MASTER URI METHOD */ - // To avoid two calls to this function happening at the same time - boost::mutex::scoped_lock lock( mutex_conv_queue_ ); + // Setting up action servers. + auto listen_server = action::createListenServer(this, sessionPtr_); - // Stopping the loop if there is any - stopRosLoop(); + // A single iteration will propagate registrations, etc... + rosIteration(); - // Reinitializing ROS Node - // { - // nhPtr_.reset(); - // std::cout << "nodehandle reset " << std::endl; - // ros_env::setMasterURI( uri, network_interface ); - // nhPtr_.reset( new ros::NodeHandle("~") ); - // } + std::cout << BOLDYELLOW + << "naoqi_driver initialized" + << RESETCOLOR + << std::endl; - if(converters_.empty()) { - // If there is no converters, create them - // (converters only depends on Naoqi, resetting the - // Ros node has no impact on them) - std::cout << BOLDRED << "going to register converters" << RESETCOLOR << std::endl; - registerDefaultConverter(); - registerDefaultSubscriber(); -// startRosLoop(); - } - else - { - std::cout << "NOT going to re-register the converters" << std::endl; - // If some converters are already there, then - // we just need to reset the registered publisher - // using the ROS node - typedef std::map< std::string, publisher::Publisher > publisher_map; - for_each( publisher_map::value_type &pub, pub_map_ ) - { - pub.second.reset(this); - } + boost::mutex::scoped_lock lock( mutex_conv_queue_ ); - for_each( subscriber::Subscriber& sub, subscribers_ ) + if(converters_.empty()) { - sub.reset(this); + // If there is no converters, create them + // (converters only depends on Naoqi, resetting the + // Ros node has no impact on them) + std::cout << BOLDRED << "going to register converters" << RESETCOLOR << std::endl; + registerDefaultConverter(); + registerDefaultSubscriber(); } - - for_each( service::Service& srv, services_ ) + else { - srv.reset(this); + std::cout << "NOT going to re-register the converters" << std::endl; + // If some converters are already there, then + // we just need to reset the registered publisher + // using the ROS node + typedef std::map< std::string, publisher::Publisher > publisher_map; + for( publisher_map::value_type &pub: pub_map_ ) + { + pub.second.reset(this); + } + + for (subscriber::Subscriber &sub: subscribers_) + { + sub.reset(this); + } + + for( service::Service& srv: services_ ) + { + srv.reset(this); + } } - } - if (!event_map_.empty()) { - typedef std::map< std::string, event::Event > event_map; - for_each( event_map::value_type &event, event_map_ ) - { - event.second.resetPublisher(this); + if (!event_map_.empty()) { + typedef std::map< std::string, event::Event > event_map; + for( event_map::value_type &event: event_map_ ) + { + event.second.resetPublisher(this); + } } + // Start publishing again + startPublishing(); } - // Start publishing again - startPublishing(); - if ( !keep_looping ) + std::cout << BOLDYELLOW + << "naoqi_driver initialized" + << RESETCOLOR + << std::endl; + std::cout << "Starting ROS loop" << std::endl; + + while ( keep_looping ) { - std::cout << "going to start ROS loop" << std::endl; - startRosLoop(); + this->rosIteration(); } - /* END */ } -// } - /** * @brief Sets the Driver sessionPtr, robot and has_stereo objects - * + * * @param sessionPtr */ void Driver::setQiSession(const qi::SessionPtr& sessionPtr) @@ -230,93 +236,78 @@ void Driver::loadBootConfig() } } -void Driver::stopService() { - stopRosLoop(); - converters_.clear(); - subscribers_.clear(); - event_map_.clear(); -} - - -void Driver::rosLoop() -{ - static std::vector actions; +void Driver::rosIteration() { + std::vector actions; -// ros::Time::init(); - while( keep_looping ) { - // clear the callback triggers - actions.clear(); + boost::mutex::scoped_lock lock( mutex_conv_queue_ ); + if (!conv_queue_.empty()) { - boost::mutex::scoped_lock lock( mutex_conv_queue_ ); - if (!conv_queue_.empty()) + // Wait for the next Publisher to be ready + size_t conv_index = conv_queue_.top().conv_index_; + converter::Converter& conv = converters_[conv_index]; + rclcpp::Time schedule = conv_queue_.top().schedule_; + + // check the publishing condition + // 1. publishing enabled + // 2. has to be registered + // 3. has to be subscribed + PubConstIter pub_it = pub_map_.find( conv.name() ); + if ( publish_enabled_ && pub_it != pub_map_.end() && pub_it->second.isSubscribed() ) { - // Wait for the next Publisher to be ready - size_t conv_index = conv_queue_.top().conv_index_; - converter::Converter& conv = converters_[conv_index]; - rclcpp::Time schedule = conv_queue_.top().schedule_; - - // check the publishing condition - // 1. publishing enabled - // 2. has to be registered - // 3. has to be subscribed - PubConstIter pub_it = pub_map_.find( conv.name() ); - if ( publish_enabled_ && pub_it != pub_map_.end() && pub_it->second.isSubscribed() ) - { - actions.push_back(message_actions::PUBLISH); - } - - // check the recording condition - // 1. recording enabled - // 2. has to be registered - // 3. has to be subscribed (configured to be recorded) - RecConstIter rec_it = rec_map_.find( conv.name() ); - { - boost::mutex::scoped_lock lock_record( mutex_record_, boost::try_to_lock ); - if ( lock_record && record_enabled_ && rec_it != rec_map_.end() && rec_it->second.isSubscribed() ) - { - actions.push_back(message_actions::RECORD); - } - } - - // bufferize data in recorder - if ( log_enabled_ && rec_it != rec_map_.end() && conv.frequency() != 0) - { - actions.push_back(message_actions::LOG); - } + actions.push_back(message_actions::PUBLISH); + } - // only call when we have at least one action to perform - if (actions.size() >0) + // check the recording condition + // 1. recording enabled + // 2. has to be registered + // 3. has to be subscribed (configured to be recorded) + RecConstIter rec_it = rec_map_.find( conv.name() ); + { + boost::mutex::scoped_lock lock_record( mutex_record_, boost::try_to_lock ); + if ( lock_record && record_enabled_ && rec_it != rec_map_.end() && rec_it->second.isSubscribed() ) { - conv.callAll( actions ); + actions.push_back(message_actions::RECORD); } + } - rclcpp::Duration d((schedule - this->now()).nanoseconds()); - if ( d > rclcpp::Duration(0, 0)) - { - rclcpp::sleep_for(d.to_chrono()); - } + // bufferize data in recorder + if ( log_enabled_ && rec_it != rec_map_.end() && conv.frequency() != 0) + { + actions.push_back(message_actions::LOG); + } - // Schedule for a future time or not - conv_queue_.pop(); - if ( conv.frequency() != 0 ) - { - conv_queue_.push(ScheduledConverter(schedule + rclcpp::Duration(0, (1.0f / conv.frequency())*1e9), conv_index)); - } + // only call when we have at least one action to perform + if (actions.size() >0) + { + conv.callAll( actions ); + } + rclcpp::Duration d(schedule - this->now()); + if ( d > rclcpp::Duration(0, 0)) + { + rclcpp::sleep_for(d.to_chrono()); } - else // conv_queue is empty. + + // Schedule for a future time or not + conv_queue_.pop(); + if ( conv.frequency() != 0 ) { - // sleep one second - rclcpp::sleep_for(rclcpp::Duration(1, 0).to_chrono()); + conv_queue_.push(ScheduledConverter(schedule + rclcpp::Duration(0, (1.0f / conv.frequency())*1e9), conv_index)); } - } // mutex scope - if ( publish_enabled_ ) + } + else // conv_queue is empty. { - rclcpp::spin_some(this->get_node_base_interface()); + // sleep one second + rclcpp::sleep_for(rclcpp::Duration(1, 0).to_chrono()); } - } // while loop + } // mutex scope + + if ( publish_enabled_ ) + { + rclcpp::spin_some(this->get_node_base_interface()); + } } std::string Driver::minidump(const std::string& prefix) @@ -418,7 +409,7 @@ std::string Driver::minidumpConverters(const std::string& prefix, const std::vec boost::mutex::scoped_lock lock_record( mutex_record_ ); bool is_started = false; - for_each( const std::string& name, names) + for( const std::string& name: names) { RecIter it = rec_map_.find(name); if ( it != rec_map_.end() ) @@ -538,7 +529,7 @@ bool Driver::registerMemoryConverter( const std::string& key, float frequency, c dataType::DataType data_type; qi::AnyValue value; try { - qi::AnyObject p_memory = sessionPtr_->service("ALMemory"); + qi::AnyObject p_memory = sessionPtr_->service("ALMemory").value(); value = p_memory.call("getData", key); } catch (const std::exception& e) { std::cout << BOLDRED << "Could not get data in memory for the key: " @@ -688,13 +679,14 @@ void Driver::registerDefaultConverter() /** Info publisher **/ if ( info_enabled ) { - boost::shared_ptr inp = boost::make_shared( "info" , robot_); - boost::shared_ptr > inr = boost::make_shared >( "info" ); - boost::shared_ptr inc = boost::make_shared( "info", 0, sessionPtr_ ); - inc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::InfoPublisher::publish, inp, _1) ); - inc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, inr, _1) ); - inc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, inr, _1) ); - registerConverter( inc, inp, inr ); + static const auto topic = "info"; + auto inp = boost::make_shared(topic); + auto inr = boost::make_shared >(topic); + boost::shared_ptr inc = boost::make_shared(topic, 0, sessionPtr_); + inc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::InfoPublisher::publish, inp, ph::_1)); + inc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, inr, ph::_1)); + inc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, inr, ph::_1)); + registerConverter(inc, inp, inr); } @@ -703,7 +695,7 @@ void Driver::registerDefaultConverter() { boost::shared_ptr lc = boost::make_shared( "log", logs_frequency, sessionPtr_); boost::shared_ptr lp = boost::make_shared( "/rosout" ); - lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::LogPublisher::publish, lp, _1) ); + lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::LogPublisher::publish, lp, ph::_1) ); registerPublisher( lc, lp ); } @@ -713,9 +705,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr dc = boost::make_shared( "diag", diag_frequency, sessionPtr_); boost::shared_ptr > dp = boost::make_shared >( "/diagnostics" ); boost::shared_ptr dr = boost::make_shared( "/diagnostics" ); - dc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, dp, _1) ); - dc->registerCallback( message_actions::RECORD, boost::bind(&recorder::DiagnosticsRecorder::write, dr, _1) ); - dc->registerCallback( message_actions::LOG, boost::bind(&recorder::DiagnosticsRecorder::bufferize, dr, _1) ); + dc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, dp, ph::_1) ); + dc->registerCallback( message_actions::RECORD, boost::bind(&recorder::DiagnosticsRecorder::write, dr, ph::_1) ); + dc->registerCallback( message_actions::LOG, boost::bind(&recorder::DiagnosticsRecorder::bufferize, dr, ph::_1) ); registerConverter( dc, dp, dr ); } @@ -725,9 +717,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr > imutp = boost::make_shared >( "imu/torso" ); boost::shared_ptr > imutr = boost::make_shared >( "imu/torso" ); boost::shared_ptr imutc = boost::make_shared( "imu_torso", converter::IMU::TORSO, imu_torso_frequency, sessionPtr_); - imutc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, imutp, _1) ); - imutc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, imutr, _1) ); - imutc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, imutr, _1) ); + imutc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, imutp, ph::_1) ); + imutc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, imutr, ph::_1) ); + imutc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, imutr, ph::_1) ); registerConverter( imutc, imutp, imutr ); } @@ -739,9 +731,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr > imubp = boost::make_shared >( "imu/base" ); boost::shared_ptr > imubr = boost::make_shared >( "imu/base" ); boost::shared_ptr imubc = boost::make_shared( "imu_base", converter::IMU::BASE, imu_base_frequency, sessionPtr_); - imubc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, imubp, _1) ); - imubc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, imubr, _1) ); - imubc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, imubr, _1) ); + imubc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, imubp, ph::_1) ); + imubc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, imubr, ph::_1) ); + imubc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, imubr, ph::_1) ); registerConverter( imubc, imubp, imubr ); } } // endif PEPPER @@ -752,9 +744,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr fcp = boost::make_shared( "camera/front/image_raw", AL::kTopCamera ); boost::shared_ptr fcr = boost::make_shared( "camera/front", camera_front_recorder_fps ); boost::shared_ptr fcc = boost::make_shared( "front_camera", camera_front_fps, sessionPtr_, AL::kTopCamera, camera_front_resolution ); - fcc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, fcp, _1, _2) ); - fcc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, fcr, _1, _2) ); - fcc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, fcr, _1, _2) ); + fcc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, fcp, ph::_1, ph::_2) ); + fcc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, fcr, ph::_1, ph::_2) ); + fcc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, fcr, ph::_1, ph::_2) ); registerConverter( fcc, fcp, fcr ); } @@ -764,9 +756,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr bcp = boost::make_shared( "camera/bottom/image_raw", AL::kBottomCamera ); boost::shared_ptr bcr = boost::make_shared( "camera/bottom", camera_bottom_recorder_fps ); boost::shared_ptr bcc = boost::make_shared( "bottom_camera", camera_bottom_fps, sessionPtr_, AL::kBottomCamera, camera_bottom_resolution ); - bcc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, bcp, _1, _2) ); - bcc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, bcr, _1, _2) ); - bcc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, bcr, _1, _2) ); + bcc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, bcp, ph::_1, ph::_2) ); + bcc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, bcr, ph::_1, ph::_2) ); + bcc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, bcr, ph::_1, ph::_2) ); registerConverter( bcc, bcp, bcr ); } @@ -786,9 +778,9 @@ void Driver::registerDefaultConverter() camera_depth_resolution, this->has_stereo); - dcc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, dcp, _1, _2) ); - dcc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, dcr, _1, _2) ); - dcc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, dcr, _1, _2) ); + dcc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, dcp, ph::_1, ph::_2) ); + dcc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, dcr, ph::_1, ph::_2) ); + dcc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, dcr, ph::_1, ph::_2) ); registerConverter( dcc, dcp, dcr ); } @@ -806,9 +798,9 @@ void Driver::registerDefaultConverter() camera_stereo_resolution, this->has_stereo); - scc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, scp, _1, _2) ); - scc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, scr, _1, _2) ); - scc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, scr, _1, _2) ); + scc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, scp, ph::_1, ph::_2) ); + scc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, scr, ph::_1, ph::_2) ); + scc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, scr, ph::_1, ph::_2) ); registerConverter( scc, scp, scr ); } @@ -818,9 +810,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr icp = boost::make_shared( "camera/ir/image_raw", AL::kInfraredOrStereoCamera ); boost::shared_ptr icr = boost::make_shared( "camera/ir", camera_ir_recorder_fps ); boost::shared_ptr icc = boost::make_shared( "infrared_camera", camera_ir_fps, sessionPtr_, AL::kInfraredOrStereoCamera, camera_ir_resolution); - icc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, icp, _1, _2) ); - icc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, icr, _1, _2) ); - icc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, icr, _1, _2) ); + icc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::CameraPublisher::publish, icp, ph::_1, ph::_2) ); + icc->registerCallback( message_actions::RECORD, boost::bind(&recorder::CameraRecorder::write, icr, ph::_1, ph::_2) ); + icc->registerCallback( message_actions::LOG, boost::bind(&recorder::CameraRecorder::bufferize, icr, ph::_1, ph::_2) ); registerConverter( icc, icp, icr ); } } // endif PEPPER @@ -831,9 +823,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr jsp = boost::make_shared( "/joint_states" ); boost::shared_ptr jsr = boost::make_shared( "/joint_states" ); boost::shared_ptr jsc = boost::make_shared( "joint_states", joint_states_frequency, tf2_buffer_, sessionPtr_ ); - jsc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::JointStatePublisher::publish, jsp, _1, _2) ); - jsc->registerCallback( message_actions::RECORD, boost::bind(&recorder::JointStateRecorder::write, jsr, _1, _2) ); - jsc->registerCallback( message_actions::LOG, boost::bind(&recorder::JointStateRecorder::bufferize, jsr, _1, _2) ); + jsc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::JointStatePublisher::publish, jsp, ph::_1, ph::_2) ); + jsc->registerCallback( message_actions::RECORD, boost::bind(&recorder::JointStateRecorder::write, jsr, ph::_1, ph::_2) ); + jsc->registerCallback( message_actions::LOG, boost::bind(&recorder::JointStateRecorder::bufferize, jsr, ph::_1, ph::_2) ); registerConverter( jsc, jsp, jsr ); // registerRecorder(jsc, jsr); } @@ -848,9 +840,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr lc = boost::make_shared( "laser", laser_frequency, sessionPtr_ ); lc->setLaserRanges(laser_range_min, laser_range_max); - lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, lp, _1) ); - lc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, lr, _1) ); - lc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, lr, _1) ); + lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, lp, ph::_1) ); + lc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, lr, ph::_1) ); + lc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, lr, ph::_1) ); registerConverter( lc, lp, lr ); } } @@ -872,9 +864,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr usp = boost::make_shared( sonar_topics ); boost::shared_ptr usr = boost::make_shared( sonar_topics ); boost::shared_ptr usc = boost::make_shared( "sonar", sonar_frequency, sessionPtr_ ); - usc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::SonarPublisher::publish, usp, _1) ); - usc->registerCallback( message_actions::RECORD, boost::bind(&recorder::SonarRecorder::write, usr, _1) ); - usc->registerCallback( message_actions::LOG, boost::bind(&recorder::SonarRecorder::bufferize, usr, _1) ); + usc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::SonarPublisher::publish, usp, ph::_1) ); + usc->registerCallback( message_actions::RECORD, boost::bind(&recorder::SonarRecorder::write, usr, ph::_1) ); + usc->registerCallback( message_actions::LOG, boost::bind(&recorder::SonarRecorder::bufferize, usr, ph::_1) ); registerConverter( usc, usp, usr ); } @@ -955,9 +947,9 @@ void Driver::registerDefaultConverter() boost::shared_ptr > lp = boost::make_shared >( "odom" ); boost::shared_ptr > lr = boost::make_shared >( "odom" ); boost::shared_ptr lc = boost::make_shared( "odom", odom_frequency, sessionPtr_ ); - lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, lp, _1) ); - lc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, lr, _1) ); - lc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, lr, _1) ); + lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, lp, ph::_1) ); + lc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, lr, ph::_1) ); + lc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, lr, ph::_1) ); registerConverter( lc, lp, lr ); } @@ -1011,7 +1003,7 @@ void Driver::registerDefaultServices() std::vector Driver::getAvailableConverters() { std::vector conv_list; - for_each( const converter::Converter& conv, converters_ ) + for( const converter::Converter& conv: converters_ ) { conv_list.push_back(conv.name()); } @@ -1023,86 +1015,6 @@ std::vector Driver::getAvailableConverters() return conv_list; } -/* -* EXPOSED FUNCTIONS -*/ - -// std::string Driver::getMasterURI() const -// { -// return ros_env::getMasterURI(); -// } - -// void Driver::setMasterURI( const std::string& uri) -// { -// setMasterURINet(uri, "eth0"); -// } - -// void Driver::setMasterURINet( const std::string& uri, const std::string& network_interface) -// { -// // To avoid two calls to this function happening at the same time -// boost::mutex::scoped_lock lock( mutex_conv_queue_ ); - -// // Stopping the loop if there is any -// //stopRosLoop(); - -// // Reinitializing ROS Node -// { -// nhPtr_.reset(); -// std::cout << "nodehandle reset " << std::endl; -// ros_env::setMasterURI( uri, network_interface ); -// nhPtr_.reset( new ros::NodeHandle("~") ); -// } - -// if(converters_.empty()) -// { -// // If there is no converters, create them -// // (converters only depends on Naoqi, resetting the -// // Ros node has no impact on them) -// std::cout << BOLDRED << "going to register converters" << RESETCOLOR << std::endl; -// registerDefaultConverter(); -// registerDefaultSubscriber(); -// // startRosLoop(); -// } -// else -// { -// std::cout << "NOT going to re-register the converters" << std::endl; -// // If some converters are already there, then -// // we just need to reset the registered publisher -// // using the ROS node -// typedef std::map< std::string, publisher::Publisher > publisher_map; -// for_each( publisher_map::value_type &pub, pub_map_ ) -// { -// pub.second.reset(this); -// } - -// for_each( subscriber::Subscriber& sub, subscribers_ ) -// { -// sub.reset(this); -// } - -// for_each( service::Service& srv, services_ ) -// { -// srv.reset(this); -// } -// } - -// if (!event_map_.empty()) { -// typedef std::map< std::string, event::Event > event_map; -// for_each( event_map::value_type &event, event_map_ ) -// { -// event.second.resetPublisher(this); -// } -// } -// // Start publishing again -// startPublishing(); - -// if ( !keep_looping ) -// { -// std::cout << "going to start ROS loop" << std::endl; -// startRosLoop(); -// } -// } - void Driver::startPublishing() { publish_enabled_ = true; @@ -1141,7 +1053,7 @@ void Driver::startRecording() { boost::mutex::scoped_lock lock_record( mutex_record_ ); recorder_->startRecord(); - for_each( converter::Converter& conv, converters_ ) + for( converter::Converter& conv: converters_ ) { RecIter it = rec_map_.find(conv.name()); if ( it != rec_map_.end() ) @@ -1167,7 +1079,7 @@ void Driver::startRecordingConverters(const std::vector& names) boost::mutex::scoped_lock lock_record( mutex_record_ ); bool is_started = false; - for_each( const std::string& name, names) + for( const std::string& name: names) { RecIter it_rec = rec_map_.find(name); EventIter it_ev = event_map_.find(name); @@ -1220,7 +1132,7 @@ std::string Driver::stopRecording() { boost::mutex::scoped_lock lock_record( mutex_record_ ); record_enabled_ = false; - for_each( converter::Converter& conv, converters_ ) + for( converter::Converter& conv: converters_ ) { RecIter it = rec_map_.find(conv.name()); if ( it != rec_map_.end() ) @@ -1245,28 +1157,18 @@ void Driver::stopLogging() log_enabled_ = false; } -void Driver::startRosLoop() -{ - keep_looping = true; - if (publisherThread_.get_id() == boost::thread::id()) - publisherThread_ = boost::thread( &Driver::rosLoop, this ); - for(EventIter iterator = event_map_.begin(); iterator != event_map_.end(); iterator++) - { - iterator->second.startProcess(); - } - // Create the publishing thread if needed - // keep_looping = true; -} - -void Driver::stopRosLoop() +void Driver::stop() { keep_looping = false; - if (publisherThread_.get_id() != boost::thread::id()) - publisherThread_.join(); for(EventIter iterator = event_map_.begin(); iterator != event_map_.end(); iterator++) { iterator->second.stopProcess(); } + + converters_.clear(); + subscribers_.clear(); + event_map_.clear(); + rclcpp::spin_some(this->get_node_base_interface()); } void Driver::parseJsonFile(std::string filepath, boost::property_tree::ptree &pt){ @@ -1311,7 +1213,7 @@ void Driver::addMemoryConverters(std::string filepath){ std::vector list; try{ - BOOST_FOREACH(boost::property_tree::ptree::value_type &v, pt.get_child("memKeys")) + for(boost::property_tree::ptree::value_type &v: pt.get_child("memKeys")) { std::string topic = v.second.get_value(); list.push_back(topic); @@ -1332,9 +1234,9 @@ void Driver::addMemoryConverters(std::string filepath){ boost::shared_ptr > mlp = boost::make_shared >( topic ); boost::shared_ptr > mlr = boost::make_shared >( topic ); boost::shared_ptr mlc = boost::make_shared(list, topic, frequency, sessionPtr_ ); - mlc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, mlp, _1) ); - mlc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, mlr, _1) ); - mlc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, mlr, _1) ); + mlc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, mlp, ph::_1) ); + mlc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, mlr, ph::_1) ); + mlc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, mlr, ph::_1) ); registerConverter( mlc, mlp, mlr ); } @@ -1343,7 +1245,7 @@ bool Driver::registerEventConverter(const std::string& key, const dataType::Data dataType::DataType data_type; qi::AnyValue value; try { - qi::AnyObject p_memory = sessionPtr_->service("ALMemory"); + qi::AnyObject p_memory = sessionPtr_->service("ALMemory").value(); value = p_memory.call("getData", key); } catch (const std::exception& e) { std::cout << BOLDRED << "Could not get data in memory for the key: " diff --git a/src/publishers/basic.hpp b/src/publishers/basic.hpp index d327a89..3ee0566 100644 --- a/src/publishers/basic.hpp +++ b/src/publishers/basic.hpp @@ -55,9 +55,9 @@ class BasicPublisher virtual inline bool isSubscribed() const { - if (is_initialized_ == false){ + if (is_initialized_ == false) { return false; - } else{ + } else { return helpers::Node::count_subscribers(topic_) > 0; } } diff --git a/src/publishers/camera.hpp b/src/publishers/camera.hpp index fc0a4f7..f77a6bb 100644 --- a/src/publishers/camera.hpp +++ b/src/publishers/camera.hpp @@ -22,7 +22,7 @@ * ROS includes */ #include -#include +#include #include #include diff --git a/src/publishers/info.cpp b/src/publishers/info.cpp deleted file mode 100644 index 2b6aa46..0000000 --- a/src/publishers/info.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright 2015 Aldebaran - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -/* -* LOCAL includes -*/ -#include "info.hpp" -#include "../tools/robot_description.hpp" - -namespace naoqi -{ -namespace publisher -{ - -InfoPublisher::InfoPublisher(const std::string& topic , const robot::Robot& robot_type) - : BasicPublisher( topic ), - robot_(robot_type) -{ -} - -void InfoPublisher::reset( rclcpp::Node* node ) -{ - // We latch as we only publish once - pub_ = node->create_publisher( topic_, 1); - - std::string robot_desc = naoqi::tools::getRobotDescription(robot_); - std::string parameter_name = "/robot_description"; - rclcpp::ParameterValue value = node->declare_parameter(parameter_name); - node->set_parameter(rclcpp::Parameter(parameter_name, robot_desc)); - std::cout << "load robot description from file" << std::endl; - - is_initialized_ = true; -} - -} // publisher -} //naoqi diff --git a/src/publishers/info.hpp b/src/publishers/info.hpp index 5e9b400..6b2fbe1 100644 --- a/src/publishers/info.hpp +++ b/src/publishers/info.hpp @@ -22,12 +22,10 @@ * LOCAL includes */ #include "basic.hpp" -#include /* * ROS includes */ -#include #include namespace naoqi @@ -38,17 +36,14 @@ namespace publisher class InfoPublisher : public BasicPublisher { public: - InfoPublisher( const std::string& topic, const robot::Robot& robot_type ); - - void reset( rclcpp::Node* node ); + InfoPublisher(const std::string& topic) : BasicPublisher(topic) + { + } virtual inline bool isSubscribed() const { return true; } - -protected: - const robot::Robot& robot_; }; } //publisher diff --git a/src/recorder/basic_event.hpp b/src/recorder/basic_event.hpp index 04fde1d..b6fc74d 100644 --- a/src/recorder/basic_event.hpp +++ b/src/recorder/basic_event.hpp @@ -24,6 +24,7 @@ #include #include #include "../helpers/recorder_helpers.hpp" +#include /* * STANDARD includes @@ -119,7 +120,7 @@ class BasicEventRecorder protected: bool isTooOld(const T& msg) { - rclcpp::Duration d((helpers::Time::now() - msg.header.stamp).nanoseconds()); + rclcpp::Duration d(helpers::Time::now() - msg.header.stamp); if (static_cast(d.seconds()) > buffer_duration_) { return true; @@ -129,7 +130,7 @@ class BasicEventRecorder bool isOlderThan(const T& msg, const rclcpp::Time& time) { - rclcpp::Duration d((time - msg.header.stamp).nanoseconds()); + rclcpp::Duration d(time - msg.header.stamp); if (static_cast(d.seconds()) > buffer_duration_) { return true; diff --git a/src/services/get_language.cpp b/src/services/get_language.cpp index 70aca92..b2b8291 100644 --- a/src/services/get_language.cpp +++ b/src/services/get_language.cpp @@ -38,6 +38,7 @@ void GetLanguageService::reset( rclcpp::Node* node ) void GetLanguageService::callback( const std::shared_ptr req, std::shared_ptr resp ) { + std::cout << "Receiving service call of getting language" << std::endl; resp->data = helpers::driver::getLanguage(session_); } diff --git a/src/services/set_language.cpp b/src/services/set_language.cpp index 3df7695..bfedffb 100644 --- a/src/services/set_language.cpp +++ b/src/services/set_language.cpp @@ -16,6 +16,7 @@ */ #include "set_language.hpp" +#include #include "../helpers/driver_helpers.hpp" namespace naoqi @@ -38,9 +39,9 @@ void SetLanguageService::reset( rclcpp::Node* node ) void SetLanguageService::callback( const std::shared_ptr req, std::shared_ptr resp ) { + std::cout << "Receiving service call of setting language" << std::endl; resp->success = helpers::driver::setLanguage(session_, req); } - } } diff --git a/src/subscribers/moveto.cpp b/src/subscribers/moveto.cpp index b828a11..88225a8 100644 --- a/src/subscribers/moveto.cpp +++ b/src/subscribers/moveto.cpp @@ -24,7 +24,7 @@ * ROS includes */ //#include -#include +#include #include "../helpers/transform_helpers.hpp" @@ -36,7 +36,7 @@ namespace subscriber MovetoSubscriber::MovetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer): BaseSubscriber( name, topic, session ), - p_motion_( session->service("ALMotion") ), + p_motion_( session->service("ALMotion").value() ), tf2_buffer_( tf2_buffer ) {} diff --git a/src/subscribers/speech.cpp b/src/subscribers/speech.cpp index 9e69061..c95b4b2 100644 --- a/src/subscribers/speech.cpp +++ b/src/subscribers/speech.cpp @@ -29,7 +29,7 @@ namespace subscriber SpeechSubscriber::SpeechSubscriber( const std::string& name, const std::string& speech_topic, const qi::SessionPtr& session ): speech_topic_(speech_topic), BaseSubscriber( name, speech_topic, session ), - p_tts_( session->service("ALTextToSpeech") ) + p_tts_(session->service("ALTextToSpeech").value()) {} void SpeechSubscriber::reset(rclcpp::Node* node ) diff --git a/src/subscribers/teleop.cpp b/src/subscribers/teleop.cpp index 7a0e58a..73e6faa 100644 --- a/src/subscribers/teleop.cpp +++ b/src/subscribers/teleop.cpp @@ -30,7 +30,7 @@ TeleopSubscriber::TeleopSubscriber( const std::string& name, const std::string& cmd_vel_topic_(cmd_vel_topic), joint_angles_topic_(joint_angles_topic), BaseSubscriber( name, cmd_vel_topic, session ), - p_motion_( session->service("ALMotion") ) + p_motion_( session->service("ALMotion").value() ) {} void TeleopSubscriber::reset( rclcpp::Node* node ) diff --git a/src/tools/robot_description.cpp b/src/tools/robot_description.cpp index ddaefda..d1f42ba 100644 --- a/src/tools/robot_description.cpp +++ b/src/tools/robot_description.cpp @@ -15,6 +15,8 @@ * */ +#include + /* * LOCAL includes */ @@ -59,6 +61,26 @@ std::string getRobotDescription( const robot::Robot& robot){ return robot_desc; } +rclcpp::Publisher::SharedPtr +publishRobotDescription(rclcpp::Node* node, const robot::Robot& robot_type) { + static const auto topic = "robot_description"; + rclcpp::Publisher::SharedPtr description_pub = + node->create_publisher( + topic, + // Transient local is similar to latching in ROS 1. + rclcpp::QoS(1).transient_local() + ); + + std::string robot_desc = getRobotDescription(robot_type); + auto msg = std::make_unique(); + msg->data = robot_desc; + + // Publish the robot description + description_pub->publish(std::move(msg)); + std::cout << "published robot description to /" << topic << std::endl; + return description_pub; +} + } // tools } // naoqi diff --git a/src/tools/robot_description.hpp b/src/tools/robot_description.hpp index 66278c4..685dfb1 100644 --- a/src/tools/robot_description.hpp +++ b/src/tools/robot_description.hpp @@ -18,17 +18,23 @@ #ifndef ROBOT_DESCRIPTION_HPP #define ROBOT_DESCRIPTION_HPP -/* -* LOCAL includes -*/ -#include - /* * STANDARD includes */ #include #include +// ROS includes +#include +#include + +/* +* LOCAL includes +*/ +#include +namespace rclcpp { + class Node; +} namespace naoqi { @@ -36,8 +42,13 @@ namespace tools { std::string getRobotDescription( const robot::Robot& robot); +/** + * Publishes the robot description on the conventional topic /robot_description. + * Returns the publisher that must be kept alive to keep the robot description published. + */ +rclcpp::Publisher::SharedPtr +publishRobotDescription(rclcpp::Node *node, const robot::Robot &robot_type); } - } #endif // ROBOT_DESCRIPTION_HPP