ros-naoqi / naoqi_driver2 Goto Github PK

ROS2 driver for NAO and Pepper robots

License: Apache License 2.0

CMake 0.88% Promela 0.06% Shell 0.26% C++ 93.88% Python 4.66% Dockerfile 0.26%

naoqi_driver2's Introduction

naoqi_driver2

This repo defines the naoqi_driver package for ROS2. The driver is in charge of providing bridging capabilities between ROS2 and NAOqiOS.

How it works

The naoqi_driver is a ROS node. It connects to a robot running NAOqi using libQi. To support the audio features, naoqi_driver opens a public endpoint, on a random port by default.

To avoid opening a public endpoint, you would need to disable the audio features by setting the audio argument to false in boot_config.json, and then set qi_listen_url to listen to a local endpoint, e.g. qi_listen_url:=tcp://127.0.0.1:12345.

Installation

Dependencies

To run, the driver requires the naoqi_libqi, naoqi_libqicore and naoqi_bridge_msgs packages. Those can be installed using apt-get (if they have been released for your ROS distro) or from source. Additionally, pepper_meshes and/or nao_meshes can be useful to display the robot in RViz.

On Ubuntu, install them using:

sudo apt-get install ros-<distro>-naoqi-libqi ros-<distro>-naoqi-libqicore ros-<distro>-naoqi-bridge-msgs ros-<distro>-pepper-meshes ros-<distro>-nao-meshes

Installing from source

In your ROS2 workspace, clone the repo and its dependencies:

cd <ws>/src
git clone https://github.com/ros-naoqi/naoqi_driver2.git
vcs import < naoqi_driver2/dependencies.repos
cd <ws>
rosdep install --from-paths src --ignore-src --rosdistro <distro> -y

To install vcs: sudo apt-get install python3-vcstool

Then build the workspace:

cd <ws>
colcon build --symlink-install

Meshes can only be built on x86(_64) platforms. You can skip them by building with these arguments:
colcon build --packages-skip nao_meshes pepper_meshes

License of the meshes

The source repositories include pepper_meshes2 and nao_meshes2, which require an interactive agreement to be provided. If you agree to their license terms ( CC BY-NC-ND 4.0: pepper_meshes2 LICENSE, nao_meshes2 LICENSE). you may skip the interactive prompt by setting the AGREE_TO_NAO_MESHES_LICENSE and I_AGREE_TO_PEPPER_MESHES_LICENSE environment variables:

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

Avoid interference with autonomous life

To have full control of the robot with ROS, you may want to disable autonomous behaviors first:

ssh nao@<robot_host>
qicli call ALAutonomousLife.setState disabled
qicli call ALMotion.wakeUp

NAOqi 2.8 and lower

The driver can be launched from a remote machine this way:

source /opt/ros/<distro>/setup.bash # or source <ws>/install/setup.bash if built from source
ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=<robot_host> qi_listen_url:=tcp://0.0.0.0:0

NAOqi 2.9 and higher

Username and password arguments are required for robots running NAOqi 2.9 or greater:

ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=<robot_host> nao_username:=nao nao_password:=<robot_password> qi_listen_url:=tcp://0.0.0.0:0

From a Docker container

If you run naoqi_driver from a Docker container with audio features enabled, you must specify the libQi endpoint with, e.g. for port 56000:

source /opt/ros/<distro>/setup.bash # or source <ws>/install/setup.bash if built from source
ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=<robot_host> qi_listen_url:=tcp://0.0.0.0:56000

Then you must expose it from the container.

On the robot or on the same machine

If you run the driver directly from the robot (or your machine running a virtual robot), you can omit the options:

ros2 launch naoqi_driver naoqi_driver.launch.py

Check that the node is running correctly

Check that the driver is connected:

ros2 node info /naoqi_driver

Hello, world

Make the robot say hello:

ros2 topic pub --once /speech std_msgs/String "data: hello"

Listen to words

You can setup speech recognition and get one result.

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:

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:

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:

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, or use docker compose to get a more reproducible dev environment:

ROS_DISTRO=iron docker compose up dev --build

To get all logs in live and build debuggable binaries, you may build with this command:
colcon build --event-handlers console_direct+ --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug
It works also when running tests:
colcon test --event-handlers console_direct+ --ctest-args tests

Build Status

ROS Distro	Binary Status	Source Status	GitHub Status
Humble
Iron

naoqi_driver2's People

Contributors

Stargazers

Watchers

Forkers

kjs26 sinfoniauniandes robobreizh-robocup-home victorpaleologue k4belbr4nd los-urubots-del-norte

naoqi_driver2's Issues

How is it possible to control the actions of the robot through Python code (not ros2 commands)

How to write Python to control your driver?

ros2 topic pub --once /speech std_msgs/String "data: hello" how to do that in Python?
where to look for the entire API (topics) listing documentation?
Also how to debug the driver (in the above command) in PyCharm ?

Document audio format

170ms of unsigned 16-bits samples at 48kHz, of all 4 channels, interleaved

qi::FutureUserException Invalid signature

I'm running into the following error when running ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=192.168.1.101

I have tried to install the naoqi_driver for ROS 2 in various ways. Ultimately, I want to run the setup in a Docker container. I have tested the installation in the following ways:

Setup in Docker with ROS Iron
Setup in Docker with ROS Humble
Setup on the host system with ROS Iron

On all platforms, I have attempted the installation both with apt and by building from source. However, I consistently encounter the same error:

[naoqi_driver_node-1] terminate called after throwing an instance of 'qi::FutureUserException'
[naoqi_driver_node-1]   what():  Invalid signature

Output:

(base) stefan@stefan-ROS2:~/Downloads$ ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=192.168.1.101 
[INFO] [launch]: All log files can be found below /home/stefan/.ros/log/2024-01-26-19-11-54-262748-stefan-ROS2-770229
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [naoqi_driver_node-1]: process started with pid [770239]
[naoqi_driver_node-1] 2024-01-26 19:11:54.389 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port7415: open_and_lock_file failed -> Function open_port_internal
[naoqi_driver_node-1] 2024-01-26 19:11:54.389 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port7417: open_and_lock_file failed -> Function open_port_internal
[naoqi_driver_node-1] 2024-01-26 19:11:54.389 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port7419: open_and_lock_file failed -> Function open_port_internal
[naoqi_driver_node-1] Receiving information about robot model
[naoqi_driver_node-1] Robot detected/NAOqi version: Pepper 1.8A / 2.5.10.7
[naoqi_driver_node-1] found an ament prefix /opt/ros/iron/share/naoqi_driver/share/boot_config.json
[naoqi_driver_node-1] load boot config from /opt/ros/iron/share/naoqi_driver/share/boot_config.json
[naoqi_driver_node-1] found a ament URDF /opt/ros/iron/share/naoqi_driver/share/urdf/pepper.urdf
[naoqi_driver_node-1] published robot description to /robot_description
[naoqi_driver_node-1] Audio Extractor: Start
[naoqi_driver_node-1] terminate called after throwing an instance of 'qi::FutureUserException'
[naoqi_driver_node-1]   what():  Invalid signature
[ERROR] [naoqi_driver_node-1]: process has died [pid 770239, exit code -6, cmd '/opt/ros/iron/lib/naoqi_driver/naoqi_driver_node --ros-args --params-file /tmp/launch_params_r9z25r93'].

Connection refused when trying to connect to pepper robot running naoqi 2.9

Hello there,

I am experiencing issues connecting to Pepper robots running NAOqi 2.9 using ROS Humble. The error message [naoqi_driver_node-1] Connect error: 111: Connection refused is displayed, and the naoqi driver node shuts down immediately.

I've tried the provided dev dockerfile from the repository as well as my own dockerfile with the release packages using docker compose and running it directly on my host machine with both the source build and the release packages.

Any help would be much appreciated.

Log / Error Message

root@linuxkit-525400123456:/ws# ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=192.168.0.108 password:=xxxx qi_listen_url:=0.0.0.0:56000
[INFO] [launch]: All log files can be found below /root/.ros/log/2024-01-31-08-18-17-235989-linuxkit-525400123456-282
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [naoqi_driver_node-1]: process started with pid [283]
[naoqi_driver_node-1] Connect error: 111: Connection refused
[naoqi_driver_node-1] naoqi driver is shutting down..
[naoqi_driver_node-1] cannot publish data, at ./src/rmw_publish.cpp:62 during '__function__'
[naoqi_driver_node-1] [ERROR] [1706689100.345726297] [naoqi_driver.rclcpp]: Error in destruction of rcl subscription handle: Failed to delete datareader, at ./src/subscription.cpp:52, at ./src/rcl/subscription.c:184
[naoqi_driver_node-1] cannot publish data, at ./src/rmw_publish.cpp:62 during '__function__'
[naoqi_driver_node-1] Fail in delete datareader, at ./src/rmw_service.cpp:104 during '__function__'
[ERROR] [naoqi_driver_node-1]: process has died [pid 283, exit code -11, cmd '/opt/ros/humble/lib/naoqi_driver/naoqi_driver_node --ros-args --params-file /tmp/launch_params_cpsefmdf'].

Environment

OS: Ubuntu 22.04 (Jammy)
ROS Version: humble
NAOqi Version: 2.9
Pepper Models: 1.8 and 1.8a
Docker Container:

# my dockerfile using release packages
ARG ROS_DISTRO=humble
FROM ros:${ROS_DISTRO} as dev
ENV ROS_DISTRO=${ROS_DISTRO}
ENV DEBIAN_FRONTEND=noninteractive
SHELL ["/bin/bash", "-c"]

# install basic dependencies
RUN apt-get update -y \
  && apt-get install -y git python3-vcstool

# install naoqi driver libraries
RUN apt-get install -y ros-${ROS_DISTRO}-naoqi-libqi \
  ros-${ROS_DISTRO}-naoqi-libqicore ros-${ROS_DISTRO}-naoqi-bridge-msgs \
  ros-${ROS_DISTRO}-pepper-meshes \
  ros-${ROS_DISTRO}-nao-meshes \
  ros-${ROS_DISTRO}-naoqi-driver
  
# create workspace
ENV WS=/ws
COPY . $WS
RUN apt install -y git python3-vcstool

# Build the base Colcon workspace, installing dependencies first.
WORKDIR $WS


# Install extra tools for development
RUN apt-get update && apt-get install -y --no-install-recommends \
 gdb gdbserver vim libgmock-dev

# Every bash instance should source the entrypoint
RUN echo "source ${WS}/docker/entrypoint.sh" >> /root/.bashrc
SHELL [ "/bin/bash", "-c" ]
ENTRYPOINT [ "/bin/bash" ]

FROM dev as dev-prebuilt
RUN source /opt/ros/${ROS_DISTRO}/setup.bash \
  && colcon build --symlink-install

version: "3.9"
services:
  dev:
    image: naoqi-driver:dev-$ROS_DISTRO
    build:
      context: .
      dockerfile: docker/Dockerfile
      target: dev-prebuilt
      args:
        - ROS_DISTRO=$ROS_DISTRO
    ports:
      - "9559:9559"
      - "56000:56000"

    # Interactive shell
    stdin_open: true
    tty: true

    # Networking and IPC for ROS 2
    network_mode: host
    ipc: host

Steps to reproduce

ssh into robot
qicli call ALAutonomouslife.setState disable
qicli call ALMotion.wakeUp
exit ssh connection
docker compose up --build
enter docker container in new terminal via docker exec -it <container_name> bash
launching naoqi_driver via ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=192.168.0.108 password:=xxxx qi_listen_url:=0.0.0.0:56000

Additional Information

I can SSH into the robots and disable autonomous life.
I can wake up the robots via qicli.
I've tried both IP addresses of the robot and attached tablet.
I've tried using two Docker containers (dev-prebuilt from your repository and fresh humble container using release packages).
I've also tried the source build and released packages directly on my machine.
Audio feature has been disabled inside the bootconfig.
I've used the entrypoint.sh provided in the repository also for my container
The issue does not occur with NAOqi 2.8., tested with NAOv6
I've noticed the python launch file uses the launch arguments username and password without the `nao_' prefix in contrast to what's written in the README.

"disconnected" Error

Hey,

We are facing this "disconnected" error without anymore clue of why it's hapenning.
Here is the command we are executing:

ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=169.254.153.200 nao_port:=9503 nao_username:=nao nao_password:=<pass> qi_listen_url:=0.0.0.0:0

And it is being run on Ubuntu 22.04 with ROS Humble. The same setup was working when trying on Ubuntu 20.04 and ROS Noetic, even though there we had to also set the ROS_MASTER_URI.

[INFO] [naoqi_driver_node-1]: process started with pid [15992]
[naoqi_driver_node-1] disconnected
[naoqi_driver_node-1] naoqi driver is shutting down..
[naoqi_driver_node-1] cannot publish data, at ./src/rmw_publish.cpp:62 during '__function__'
[naoqi_driver_node-1] [ERROR] [1712672996.880189984] [naoqi_driver.rclcpp]: Error in destruction of rcl subscription handle: Failed to delete datareader, at ./src/subscription.cpp:52, at ./src/rcl/subscription.c:184
[naoqi_driver_node-1] cannot publish data, at ./src/rmw_publish.cpp:62 during '__function__'
[naoqi_driver_node-1] Fail in delete datareader, at ./src/rmw_service.cpp:104 during '__function__'
[ERROR] [naoqi_driver_node-1]: process has died [pid 15992, exit code -11, cmd '/home/jetson/ros2_ws/install/naoqi_driver/lib/naoqi_driver/naoqi_driver_node --ros-args --params-file /tmp/launch_params_4p3b06oj'].

Can't find method: 'setEnableNotifications'

I'm running into the following error when running ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=192.168.137.180

Connecting to a real Nao6 robot.

On a fresh ROS2 foxy docker container, with the following packages installed:

ros-foxy-naoqi-libqi 
ros-foxy-naoqi-libqicore
ros-foxy-naoqi-bridge-msgs
ros-foxy-naoqi-driver

Output

julien@7143d4914d77:~/ws/src$ ros2 launch naoqi_driver naoqi_driver.launch.py nao_ip:=192.168.137.180
[INFO] [launch]: All log files can be found below /home/julien/.ros/log/2023-07-24-18-54-16-049886-7143d4914d77-1023
[INFO] [launch]: Default logging verbosity is set to INFO
/opt/ros/foxy/share/naoqi_driver/launch/naoqi_driver.launch.py:33: UserWarning: The parameter 'node_executable' is deprecated, use 'executable' instead
  launch_ros.actions.Node(
/opt/ros/foxy/share/naoqi_driver/launch/naoqi_driver.launch.py:33: UserWarning: The parameter 'node_name' is deprecated, use 'name' instead
  launch_ros.actions.Node(
[INFO] [naoqi_driver_node-1]: process started with pid [1028]
[naoqi_driver_node-1] Receiving information about robot model
[naoqi_driver_node-1] Robot detected/NAOqi version: NAO V6.0 / 2.8.7.4
[naoqi_driver_node-1] found a catkin prefix /opt/ros/foxy/share/naoqi_driver/share/boot_config.json
[naoqi_driver_node-1] load boot config from /opt/ros/foxy/share/naoqi_driver/share/boot_config.json
[naoqi_driver_node-1] terminate called after throwing an instance of 'qi::FutureUserException'
[naoqi_driver_node-1]   what():  Can't find method: setEnableNotifications (resolved to '(b)')
[naoqi_driver_node-1] 
[ERROR] [naoqi_driver_node-1]: process has died [pid 1028, exit code -6, cmd '/opt/ros/foxy/lib/naoqi_driver/naoqi_driver_node --ros-args -r __node:=naoqi_driver -r __node:=naoqi_driver --params-file /tmp/launch_params_ruajs2rd'].