diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..ebe8113 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,121 @@ +ARG BASE_IMAGE=ros:humble-perception + +# The following steps are based on the offical multi-stage build: https://github.com/IntelRealSense/librealsense/blob/master/scripts/Docker/Dockerfile +################################# +# Librealsense Builder Stage # +################################# +FROM $BASE_IMAGE as librealsense-builder + +SHELL ["/bin/bash", "-c"] + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -qq -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + libssl-dev \ + libusb-1.0-0-dev \ + pkg-config \ + libgtk-3-dev \ + libglfw3-dev \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + curl \ + python3 \ + python3-dev \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /usr/src +# Get the latest tag of remote repository: https://stackoverflow.com/a/12704727 +# Needs to be a single command as ENV can't be set from Bash command: https://stackoverflow.com/questions/34911622/dockerfile-set-env-to-result-of-command +RUN export LIBRS_VERSION=2.53.1; \ + curl https://codeload.github.com/IntelRealSense/librealsense/tar.gz/refs/tags/v${LIBRS_VERSION} -o librealsense.tar.gz; \ + tar -zxf librealsense.tar.gz; \ + rm librealsense.tar.gz; \ + ln -s /usr/src/librealsense-${LIBRS_VERSION} /usr/src/librealsense + +RUN cd /usr/src/librealsense \ + && mkdir build && cd build \ + && cmake \ + -DCMAKE_C_FLAGS_RELEASE="${CMAKE_C_FLAGS_RELEASE} -s" \ + -DCMAKE_CXX_FLAGS_RELEASE="${CMAKE_CXX_FLAGS_RELEASE} -s" \ + -DCMAKE_INSTALL_PREFIX=/opt/librealsense \ + -DBUILD_GRAPHICAL_EXAMPLES=OFF \ + -DBUILD_PYTHON_BINDINGS:bool=true \ + -DCMAKE_BUILD_TYPE=Release ../ \ + && make -j$(($(nproc)-1)) all \ + && make install + + ENV DEBIAN_FRONTEND=dialog + +###################################### +# librealsense Base Image Stage # +###################################### +FROM ${BASE_IMAGE} as librealsense + +SHELL ["/bin/bash", "-c"] + +COPY --from=librealsense-builder /opt/librealsense /usr/local/ +COPY --from=librealsense-builder /usr/lib/python3/dist-packages/pyrealsense2 /usr/lib/python3/dist-packages/pyrealsense2 +COPY --from=librealsense-builder /usr/src/librealsense/config/99-realsense-libusb.rules /etc/udev/rules.d/ +ENV PYTHONPATH=${PYTHONPATH}:/usr/local/lib + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libusb-1.0-0 \ + udev \ + apt-transport-https \ + ca-certificates \ + curl \ + software-properties-common \ + && rm -rf /var/lib/apt/lists/* +# The following steps are based on: https://github.com/IntelRealSense/realsense-ros/tree/ros2-development +# ENV WS_DIR="/ros2_ws" +# WORKDIR ${WS_DIR} +RUN apt-get update -y \ + && apt-get install -y \ + ros-${ROS_DISTRO}-rviz2 +# && mkdir src + +RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +RUN sudo apt-get install ros-humble-image-transport-plugins -y + +# COPY docker/realsense-ros ./src +# COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch +# COPY docker/depth_handler /ros2_ws/src/depth_handler +# COPY docker/proximity_monitor /ros2_ws/src/proximity_monitor +RUN apt-get install -y python3-rosdep \ + && apt-get update \ + && source /opt/ros/${ROS_DISTRO}/setup.bash \ + && rm /etc/ros/rosdep/sources.list.d/20-default.list \ + && rosdep init \ + && rosdep update \ + && rosdep install -i --from-path src --rosdistro ${ROS_DISTRO} --skip-keys=librealsense2 -y \ + && colcon build +RUN sudo apt-get install python3-opencv +# RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc +# COPY docker/launch.sh . + +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Create a non-root user +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ + # Add sudo support for the non-root user + && apt-get update \ + && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* + +# RUN chmod +x /ros2_ws/launch.sh +ENV DEBIAN_FRONTEND=dialog +# ENTRYPOINT ["/ros2_ws/launch.sh"] diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index f36b7b6..47ae70b 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,11 +1,53 @@ +// See https://aka.ms/vscode-remote/devcontainer.json for format details. { - "name": "Docker for Intel Realsense with ROS 2", - "dockerComposeFile": [ - "../docker/docker-compose-gui.yml" // Alternatives: "../docker/docker-compose-gui.yml", "../docker/docker-compose-gui-nvidia.yml" - ], - "service": "realsense_ros2", - "workspaceFolder": "/ros2_ws", - "shutdownAction": "stopCompose", - "extensions": [ - ] + "dockerFile": "Dockerfile", + "context": "..", + "build": { + "args": { + "WORKSPACE": "${containerWorkspaceFolder}" + } + }, + "remoteUser": "ros", + "runArgs": [ + "--network=host", + "--cap-add=SYS_PTRACE", + "--security-opt=seccomp:unconfined", + "--security-opt=apparmor:unconfined", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix", + "--volume=/mnt/wslg:/mnt/wslg", + "--ipc=host", + "--volume=/dev:/dev", + "--device-cgroup-rule=c 81:* rmw", + "--device-cgroup-rule=c 189:* rmw" + // uncomment to use intel iGPU + // "--device=/dev/dri" + ], + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}", // Needed for GUI try ":0" for windows + "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", + "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", + "PULSE_SERVER": "${localEnv:PULSE_SERVER}", + "LIBGL_ALWAYS_SOFTWARE": "1", // Needed for software rendering of opengl + "RMW_IMPLEMENTATION": "rmw_cyclonedds_cpp" + }, + // Set *default* container specific settings.json values on container create. + "customizations": { + "vscode": { + "extensions": [ + "althack.ament-task-provider", + "betwo.b2-catkin-tools", + "DotJoshJohnson.xml", + "ms-azuretools.vscode-docker", + "ms-iot.vscode-ros", + "ms-python.python", + "ms-vscode.cpptools", + "redhat.vscode-yaml", + "smilerobotics.urdf", + "streetsidesoftware.code-spell-checker", + "twxs.cmake", + "yzhang.markdown-all-in-one", + "zachflower.uncrustify" + ] + } + } } diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d02872e --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +*/__pycache__ +build/* +install/* +log/* +site/* \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index bb4df47..afe87e9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "docker/realsense-ros"] - path = docker/realsense-ros +[submodule "src/realsense-ros"] + path = src/realsense-ros url = https://github.com/juandelos/realsense-ros.git diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..40d2c8e --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/humble/include/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "compileCommands": "${workspaceFolder}/build/compile_commands.json", + "cStandard": "c99", + "cppStandard": "c++17", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..6dc8dd6 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,90 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + // Example launch of a python file + { + "name": "Python: Current File", + "type": "python", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal", + }, + // Example gdb launch of a ros executable + { + "name": "(gdb) Launch (merge-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "(gdb) Launch (isolated-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/${input:package}/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + //Example of a ROS Launch file + { + "name": "ROS: Launch File (merge-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/share/${input:package}/launch/${input:ros_launch}", + }, + { + "name": "ROS: Launch File (isolated-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/${input:package}/share/${input:package}/launch/${input:ros_launch}", + }, + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name", + "default": "examples_rclcpp_minimal_publisher" + }, + { + "id": "program", + "type": "promptString", + "description": "Program name", + "default": "publisher_member_function" + }, + { + "id": "ros_launch", + "type": "promptString", + "description": "ROS launch name", + "default": "file_name_launch.py" + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..a132292 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,74 @@ +{ + "editor.tabSize": 8, + "editor.rulers": [ + 100 + ], + "files.associations": { + "*.repos": "yaml", + "*.world": "xml", + "*.xacro": "xml" + }, + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages/" + ], + // Autocomplete from ros python packages + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages/" + ], + // Environment file lets vscode find python files within workspace + "python.envFile": "${workspaceFolder}/.env", + // Use the system installed version of autopep8 + "python.formatting.autopep8Path": "/usr/bin/autopep8", + "python.formatting.autopep8Args": [ + "--max-line-length=100" + ], + "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", + "C_Cpp.formatting": "disabled", + "uncrustify.configPath.linux": "/opt/ros/humble/lib/python3.10/site-packages/ament_uncrustify/configuration/ament_code_style.cfg", + "[cpp]": { + "editor.defaultFormatter": "zachflower.uncrustify" + }, + "search.exclude": { + "**/node_modules": true, + "**/bower_components": true, + "**/*.code-search": true, + "**/build": true, + "**/install": true, + "**/log": true + }, + "cSpell.words": [ + "RTPS", + "athackst", + "autopep", + "cmake", + "cppcheck", + "cpplint", + "DCMAKE", + "deque", + "devcontainer", + "ints", + "noqa", + "pytest", + "rclcpp", + "rclpy", + "repos", + "rosdep", + "rosdistro", + "rosidl", + "RTPS", + "uncrustify", + "Wextra", + "Wpedantic", + "xmllint" + ], + "cSpell.allowCompoundWords": true, + "cSpell.ignorePaths": [ + "**/package-lock.json", + "**/node_modules/**", + "**/vscode-extension/**", + "**/.git/objects/**", + ".vscode", + ".vscode-insiders", + ".devcontainer/devcontainer.json" + ] +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..d1ed3ee --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,247 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + // Build tasks + { + "label": "build", + "detail": "Build workspace (default)", + "type": "shell", + "command": "./build.sh", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": "$gcc" + }, + { + "label": "debug", + "detail": "Build workspace (debug)", + "type": "shell", + "command": "./build.sh", + "options": { + "env": { + "BUILD_TYPE": "Debug" + } + }, + "group": "build", + "problemMatcher": "$gcc" + }, + // Test tasks + { + "label": "test", + "detail": "Run all unit tests and show results.", + "type": "shell", + "command": "./test.sh", + "group": { + "kind": "test", + "isDefault": true + } + }, + // Clean + { + "label": "clean", + "detail": "Run the clean target", + "type": "shell", + "command": "colcon build --cmake-target clean", + "problemMatcher": "$gcc" + }, + { + "label": "purge", + "detail": "Purge workspace by deleting all generated files.", + "type": "shell", + "command": "sudo rm -fr build install log; sudo py3clean .", + "problemMatcher": [] + }, + // Linting and static code analysis tasks + { + "label": "fix", + "detail": "Reformat files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify --reformat src/", + "problemMatcher": [] + }, + { + "label": "uncrustify", + "detail": "Lint files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify src/", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "problemMatcher": [ + { + "owner": "uncrustify", + "source": "uncrustify", + "fileLocation": "relative", + "pattern": [ + // just the file name message + { + "regexp": "^(.*)'(.*)':", + "kind": "file", + "file": 2, + "message": 1 + } + ] + } + ] + }, + { + "label": "cpplint", + "detail": "Lint files with cpplint.", + "type": "ament", + "task": "cpplint", + "path": "src/", + "problemMatcher": "$ament_cpplint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + }, + { + "label": "cppcheck", + "detail": "Run static code checker cppcheck.", + "type": "ament", + "task": "cppcheck", + "path": "src/", + "problemMatcher": "$ament_cppcheck", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "options": { + "env": { + "AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS": "1" + } + } + }, + { + "label": "lint_cmake", + "detail": "Run lint on cmake files.", + "type": "ament", + "task": "lint_cmake", + "path": "src/", + "problemMatcher": "$ament_lint_cmake", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "flake8", + "detail": "Run flake8 on python files.", + "type": "ament", + "task": "flake8", + "path": "src/", + "problemMatcher": "$ament_flake8", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "pep257", + "detail": "Run pep257 on python files.", + "type": "ament", + "task": "pep257", + "path": "src/", + "problemMatcher": "$ament_pep257", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "xmllint", + "detail": "Run xmllint on xml files.", + "type": "ament", + "task": "xmllint", + "path": "src/", + "problemMatcher": "$ament_xmllint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "lint all", + "detail": "Run all linters.", + "dependsOn": [ + "cppcheck", + "cpplint", + "flake8", + "lint_cmake", + "pep257", + "xmllint", + "uncrustify" + ], + "problemMatcher": [] + }, + // Workspace editing tasks + { + "label": "new ament_cmake package", + "detail": "Create a new ROS cpp package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory src --build-type ament_cmake ${input:package}", + "problemMatcher": [] + }, + { + "label": "new ament_python package", + "detail": "Create a new ROS python package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory src --build-type ament_python ${input:package}", + "problemMatcher": [] + }, + { + "label": "import from workspace file", + "detail": "Use vcs to import modules specified by a workspace/rosinstall file.", + "type": "shell", + "command": "vcs import < src/ros2.repos src", + "problemMatcher": [] + }, + { + "label": "update workspace file", + "detail": "Use vcs to update repositories in src to workspace file.", + "type": "shell", + "command": "vcs export src > src/ros2.repos", + "problemMatcher": [] + }, + { + "label": "install dependencies", + "detail": "Install all dependencies specified in the workspaces package.xml files.", + "type": "shell", + "command": "sudo apt-get update && rosdep update && rosdep install --from-paths src --ignore-src -y", + "problemMatcher": [] + }, + { + "label": "setup", + "detail": "Set up the workspace", + "type": "shell", + "command": "./setup.sh", + "problemMatcher": [] + }, + { + "label": "add submodules from .repos", + "detail": "Create a git submodule for all repositories in your .repos file", + "type": "shell", + "command": "python3 .devcontainer/repos_to_submodules.py", + "problemMatcher": [] + } + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name" + } + ] +} diff --git a/ReadMe.md b/ReadMe.md index b76be14..7334379 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,7 +1,9 @@ # Docker for Intel Realsense cameras on ROS 2 -Author: Juan de los Rios(March 2023 - July 2023) +Author: Juan de los Rios (March 2023 - July 2023) + +Author \[proximity-monitor\]: Arkadiy Telegin (October 2023 - April 2024) Fork from [https://github.com/2b-t](https://github.com/2b-t) @@ -16,6 +18,14 @@ In order to make it work with Nvidia Jetson Orin/Nano, librealsense has to be bu On the robot everything is installed and a server can be run to start on boot up under the name of peripheral.service +### 0.1 Proximity monitor +The latest addition to the repository is the `proximity_monitor` package. The ROS2 node defined in `proximity_monitor.py` reads the IntelRealsense camera feed, calculates the proximity of the closest object to the camera, and publishes a message to `/roboy/pinky/sensing//proximity_warning` topic at 40Hz. The published message is a [Float32MultiArray](https://github.com/ros2/common_interfaces/blob/rolling/std_msgs/msg/Float32MultiArray.msg) that contains: +- Warning: `1.0` if the proximity < `proximity_warning_threshold` and `0.0` otherwise +- Proximity: distance to the nearest point seen by the camera (meters) +- Current value of `proximity_warning_threshold` (meters) + +For configuration options of the proximity monitor please see section ["Proximity monitor usage"](#4-proximity-monitor-usage). + ## 1. Creating a Docker There are two different approaches for creating a Docker for a Realsense camera, one uses existing **Debian packages** while the other performs a full **compilation from source**. It is then important to mount `/dev` as a volume so that the Docker can access the hardware. @@ -29,6 +39,13 @@ In the `docker-compose.yml` this is done with the options: - 'c 189:* rmw' ``` +#### 1.1 Development Docker container +For the ease of the development using VSCode, a similar Dockerfile is provided along `devcontainer.json` launching instruction. The difference with the main Dockerfile consists of mounting all the source files instead of copying them into the container image. Using devconainer allows code-completion within VSCode. Additionally all the changes reflected in the source code will be immediately available for building and running the code inside of the container without the need of rebuilding the docker image. This allows for much faster development and testing iterations. + +To build and launch the container development environment, please follow [these instructions (subsection "Open it in vscode")](https://github.com/athackst/vscode_ros2_workspace#open-it-in-vscode). After this, you'll have access to the development environment and terminal executing inside of the running container. + +To learn more about VSCode's devcontainers, please consult the [official documentation](https://code.visualstudio.com/docs/devcontainers/containers) + ## 2. Launching Allow the container to display contents on your host machine by typing @@ -148,7 +165,38 @@ def generate_launch_description(): ]) ``` -## 4. Debugging +## 4. Proximity monitor usage +The included `proximity-monitor` package declares the following configuration options: +- `camera_name` \[Default: `camera`\] +- `proximity_warning_threshold` \[Default: `0.2`\] + +Additionally, the `proximity_warning_threshold` parameter can be reconfigured dynamically during the runtime. For instructions how to dynamically reconfiure ROS2 parameters, please consult [the official documentation](https://docs.ros.org/en/humble/How-To-Guides/Using-ros2-param.html) + +The launch file `multi_camera_launch.py` provided by the package features a working launch configuration for two RealSense cameras. + +The following is a sample of ROS2 `LaunchDescription` using a camera named `camera1` with a warning threshold of 0.2 meters. You can adjust it according to your setup and complete it with the launch boilerplate as seen in `multi_camera_launch.py` to create your own ROS2 launch file that will start a RealSense ROS2 node along with the proximity monitor: +```python +LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(realsense_launch_file), + launch_arguments={ + 'camera_name': 'camera1', + "serial_no":"_829212071824", # CHANGE THIS! + 'depth_module.profile': '424x240x6', + 'rgb_camera.profile': '320x240x6', + }.items(), + ), + launch_ros.actions.Node( + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_1', + parameters = [{'camera_name': 'camera1'}, + {'proximity_warning_threshold': 0.2}] + ), +]) +``` + +## 5. Debugging The Intel Realsense driver has several serious flaws/bugs. Probably the main one is that it is **closely connected to the [kernel version of the Linux operating system](https://github.com/IntelRealSense/librealsense/issues/9360)**. If the Dockerfile above do not work then you are likely unlucky and it is an incompatible version of the kernel of your host system and you will either have to [downgrade your kernel](https://linuxhint.com/install-linux-kernel-ubuntu/) or switch to another Ubuntu version that is officially supported. Furthermore from time to time the software will give you cryptic error messages. For some restarting the corresponding software component might help, for others you will find a fix googling and with others you will have to learn to live. The Realsense is **pretty [picky about USB 3.x cables](https://github.com/IntelRealSense/librealsense/issues/2045)**. If your camera is detected via `rs-enumerate-devices`, you can see it `realsense-viewer` but can't output its video stream, then it might be that your cable lacks the bandwidth. Either you can try to turn down the resolution of the camera in the `realsense-viewer` or switch cable (preferably to one that is [already known to work](https://community.intel.com/t5/Items-with-no-label/long-USB-cable-for-realsense-D435i/m-p/694963)). diff --git a/docker/launch.sh b/docker/launch.sh deleted file mode 100644 index 6319698..0000000 --- a/docker/launch.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash -source install/setup.bash -ros2 launch realsense2_camera multi_camera_launch.py diff --git a/setup.sh b/setup.sh new file mode 100755 index 0000000..beab00e --- /dev/null +++ b/setup.sh @@ -0,0 +1,7 @@ +#!/bin/bash +set -e + +vcs import < src/ros2.repos src +sudo apt-get update +rosdep update --rosdistro=$ROS_DISTRO +rosdep install --from-paths src --ignore-src -y --rosdistro=$ROS_DISTRO diff --git a/docker/Dockerfile b/src/Dockerfile similarity index 88% rename from docker/Dockerfile rename to src/Dockerfile index 06dc605..fadc33d 100644 --- a/docker/Dockerfile +++ b/src/Dockerfile @@ -81,9 +81,15 @@ RUN apt-get update -y \ && apt-get install -y \ ros-${ROS_DISTRO}-rviz2 \ && mkdir src -COPY docker/realsense-ros ./src -COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch -COPY docker/depth_handler /ros2_ws/src/depth_handler + +RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +RUN sudo apt-get install ros-humble-image-transport-plugins -y + +COPY src/realsense-ros ./src +COPY src/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch +COPY src/depth_handler /ros2_ws/src/depth_handler +COPY src/proximity_monitor /ros2_ws/src/proximity_monitor RUN apt-get install -y python3-rosdep \ && apt-get update \ && source /opt/ros/${ROS_DISTRO}/setup.bash \ @@ -94,7 +100,7 @@ RUN apt-get install -y python3-rosdep \ && colcon build RUN sudo apt-get install python3-opencv RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc -COPY docker/launch.sh . +COPY src/launch.sh . RUN chmod +x /ros2_ws/launch.sh ENV DEBIAN_FRONTEND=dialog -ENTRYPOINT ["/ros2_ws/launch.sh"] \ No newline at end of file +ENTRYPOINT ["/ros2_ws/launch.sh"] diff --git a/docker/depth_handler/depth_handler/__init__.py b/src/depth_handler/depth_handler/__init__.py similarity index 100% rename from docker/depth_handler/depth_handler/__init__.py rename to src/depth_handler/depth_handler/__init__.py diff --git a/docker/depth_handler/depth_handler/depth_subscriber.py b/src/depth_handler/depth_handler/depth_subscriber.py similarity index 100% rename from docker/depth_handler/depth_handler/depth_subscriber.py rename to src/depth_handler/depth_handler/depth_subscriber.py diff --git a/docker/depth_handler/depth_handler/joint_plotter.py b/src/depth_handler/depth_handler/joint_plotter.py similarity index 100% rename from docker/depth_handler/depth_handler/joint_plotter.py rename to src/depth_handler/depth_handler/joint_plotter.py diff --git a/docker/depth_handler/depth_handler/pcd_subscriber.py b/src/depth_handler/depth_handler/pcd_subscriber.py similarity index 100% rename from docker/depth_handler/depth_handler/pcd_subscriber.py rename to src/depth_handler/depth_handler/pcd_subscriber.py diff --git a/docker/depth_handler/package.xml b/src/depth_handler/package.xml similarity index 100% rename from docker/depth_handler/package.xml rename to src/depth_handler/package.xml diff --git a/docker/depth_handler/resource/depth_handler b/src/depth_handler/resource/depth_handler similarity index 100% rename from docker/depth_handler/resource/depth_handler rename to src/depth_handler/resource/depth_handler diff --git a/docker/depth_handler/setup.cfg b/src/depth_handler/setup.cfg similarity index 100% rename from docker/depth_handler/setup.cfg rename to src/depth_handler/setup.cfg diff --git a/docker/depth_handler/setup.py b/src/depth_handler/setup.py similarity index 100% rename from docker/depth_handler/setup.py rename to src/depth_handler/setup.py diff --git a/docker/depth_handler/test/test_copyright.py b/src/depth_handler/test/test_copyright.py similarity index 100% rename from docker/depth_handler/test/test_copyright.py rename to src/depth_handler/test/test_copyright.py diff --git a/docker/depth_handler/test/test_flake8.py b/src/depth_handler/test/test_flake8.py similarity index 100% rename from docker/depth_handler/test/test_flake8.py rename to src/depth_handler/test/test_flake8.py diff --git a/docker/depth_handler/test/test_pep257.py b/src/depth_handler/test/test_pep257.py similarity index 100% rename from docker/depth_handler/test/test_pep257.py rename to src/depth_handler/test/test_pep257.py diff --git a/docker/docker-compose-gui-nvidia.yml b/src/docker-compose-gui-nvidia.yml similarity index 100% rename from docker/docker-compose-gui-nvidia.yml rename to src/docker-compose-gui-nvidia.yml diff --git a/docker/docker-compose-gui.yml b/src/docker-compose-gui.yml similarity index 100% rename from docker/docker-compose-gui.yml rename to src/docker-compose-gui.yml diff --git a/docker/docker-compose-nvidia.yml b/src/docker-compose-nvidia.yml similarity index 100% rename from docker/docker-compose-nvidia.yml rename to src/docker-compose-nvidia.yml diff --git a/docker/docker-compose-unity.yml b/src/docker-compose-unity.yml similarity index 100% rename from docker/docker-compose-unity.yml rename to src/docker-compose-unity.yml diff --git a/docker/docker-compose.yml b/src/docker-compose.yml similarity index 86% rename from docker/docker-compose.yml rename to src/docker-compose.yml index b28db05..dd10db6 100644 --- a/docker/docker-compose.yml +++ b/src/docker-compose.yml @@ -3,7 +3,7 @@ services: realsense_ros2: build: context: .. - dockerfile: docker/Dockerfile + dockerfile: src/Dockerfile #target: librealsense tty: true volumes: diff --git a/src/launch.sh b/src/launch.sh new file mode 100644 index 0000000..1df99ff --- /dev/null +++ b/src/launch.sh @@ -0,0 +1,3 @@ +#!/bin/bash +source install/setup.bash +ros2 launch proximity_monitor multi_camera_launch.py diff --git a/src/proximity_monitor/CMakeLists.txt b/src/proximity_monitor/CMakeLists.txt new file mode 100644 index 0000000..d75d9f6 --- /dev/null +++ b/src/proximity_monitor/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +project(your_package_name) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/ProximityWarning.msg" +# ) +# We use built-in messages to avoid the need to compile for Unity + +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} + ) + +ament_package() diff --git a/docker/multi_camera_launch.py b/src/proximity_monitor/launch/multi_camera_launch.py similarity index 53% rename from docker/multi_camera_launch.py rename to src/proximity_monitor/launch/multi_camera_launch.py index 930a548..5eda34b 100644 --- a/docker/multi_camera_launch.py +++ b/src/proximity_monitor/launch/multi_camera_launch.py @@ -22,20 +22,18 @@ # ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4.. """Launch realsense2_camera node.""" +import os import copy from launch import LaunchDescription import launch_ros.actions from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory import sys import pathlib sys.path.append(str(pathlib.Path(__file__).parent.absolute())) -import rs_launch -local_parameters = [{'name': 'camera_name1', 'default': 'camera_back', 'description': 'camera unique name'}, - {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'}, - ] def set_configurable_parameters(local_params): return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params]) @@ -47,31 +45,53 @@ def duplicate_params(general_params, posix): param['name'] += posix return local_params - def generate_launch_description(): - params1 = duplicate_params(rs_launch.configurable_parameters, '1') - params2 = duplicate_params(rs_launch.configurable_parameters, '2') - return LaunchDescription( - rs_launch.declare_configurable_parameters(local_parameters) + - rs_launch.declare_configurable_parameters(params1) + - rs_launch.declare_configurable_parameters(params2) + - [ + realsense_launch_file = os.path.join( + get_package_share_directory('realsense2_camera'), + 'launch', + 'rs_launch.py' + ) + + return LaunchDescription([ IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - launch_arguments={'camera_name': 'camera_shoulder_left', "serial_no":"_829212071824"}.items(), + PythonLaunchDescriptionSource(realsense_launch_file), + launch_arguments={ + 'camera_name': 'camera_back_top', + "serial_no":"_829212071824", + 'depth_module.profile': '424x240x6', + 'rgb_camera.profile': '320x240x6', + }.items(), ), IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - launch_arguments={'camera_name': 'camera_back', "serial_no":"_829212071844", "pointcloud.enable":"false"}.items(), + PythonLaunchDescriptionSource(realsense_launch_file), + launch_arguments={ + 'camera_name': 'camera_back_bottom', + "serial_no":"_829212071844", + 'depth_module.profile': '424x240x6 ', + 'rgb_camera.profile': '320x240x6',}.items(), ), + # launch_ros.actions.Node( + # package = "depth_handler", + # executable = "depth_subscriber" + # ), + # launch_ros.actions.Node( + # package = "depth_handler", + # executable = "pcd_subscriber", + # name = 'pointcloud_shoulder_left', + # parameters = [{'camera_side': 'left'}] + # ), launch_ros.actions.Node( - package = "depth_handler", - executable = "depth_subscriber" + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_top', + parameters = [{'camera_name': 'camera_back_top'}, + {'proximity_warning_threshold': 0.2}] ), - launch_ros.actions.Node( - package = "depth_handler", - executable = "pcd_subscriber", - name = 'pointcloud_shoulder_left', - parameters = [{'camera_side': 'left'}] + launch_ros.actions.Node ( + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_bottom', + parameters = [{'camera_name': 'camera_back_bottom'}, + {'proximity_warning_threshold': 0.2}] ), - ]) \ No newline at end of file + ]) diff --git a/src/proximity_monitor/launch/proximity_monitor_launch.py b/src/proximity_monitor/launch/proximity_monitor_launch.py new file mode 100644 index 0000000..de6add5 --- /dev/null +++ b/src/proximity_monitor/launch/proximity_monitor_launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import Node, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +configurable_parameters = [ + {'name': 'first_camera_name', 'default': 'camera1', 'description': 'name of the first camera'}, + {'name': 'second_camera_name', 'default': 'camera2', 'description': 'name of the second camera'}, + {'name': 'threshold', 'default': 1.0, 'description': 'distance threshold for proximity warning in meters'}, +] + +def declare_configurable_parameters(parameters): + return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] + +def set_configurable_parameters(parameters): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) + +def generate_launch_description(): + return LaunchDescription( + declare_configurable_parameters(configurable_parameters) + [ + Node(package='proximity_monitor', + executable='proximity_monitor', + name='proximity_monitor', + parameters=[set_configurable_parameters(configurable_parameters)], + ), + ]) \ No newline at end of file diff --git a/src/proximity_monitor/package.xml b/src/proximity_monitor/package.xml new file mode 100644 index 0000000..f1bbb88 --- /dev/null +++ b/src/proximity_monitor/package.xml @@ -0,0 +1,31 @@ + + + + proximity_monitor + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + rclpy + sensor_msgs + std_msgs + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + realsense2_camera + realsense2_description + + cv_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/proximity_monitor/proximity_monitor/__init__.py b/src/proximity_monitor/proximity_monitor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/proximity_monitor/proximity_monitor/proximity_monitor.py b/src/proximity_monitor/proximity_monitor/proximity_monitor.py new file mode 100644 index 0000000..650f105 --- /dev/null +++ b/src/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -0,0 +1,100 @@ +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import SetParametersResult +from std_msgs.msg import Float32MultiArray +from sensor_msgs.msg import Image + +from rcl_interfaces.srv import SetParameters + +from cv_bridge import CvBridge, CvBridgeError +import numpy as np + +bridge = CvBridge() + +# ProximityWarning message +# Float32MultiArray: [warning, proximity, threshold] +# warning: 1 if the distance is less than the threshold +# proximity: distance to the nearest point (meters) +# threshold: distance threshold (meters) + +class ProximityMonitorNode(Node): + def __init__(self): + super().__init__('proximity_monitor') + + # Declare parameters and initialize them + self.declare_parameter('camera_name', 'camera') + self.declare_parameter('proximity_warning_threshold', 0.2) + + # Get parameters + self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value + self.threshold = self.get_parameter('proximity_warning_threshold').get_parameter_value().double_value + + # Publishers and Subscribers + self.warning_topic = f'/roboy/pinky/sensing/{self.camera_name}/proximity_warning' + self.warning_publisher = self.create_publisher(Float32MultiArray, self.warning_topic, 10) + + self.camera_subscriber = self.create_subscription(Image, f'/{self.camera_name}/depth/image_rect_raw', self.camera_callback, 10) + self.parameter_client = self.create_client(SetParameters, + f'/{self.camera_name}/set_parameters') + + self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') + + self.last_warning = 0.0 + self.last_proximity = None + + self.timer = self.create_timer(0.025, self.publish_warning) # 40 Hz + + # Dynamic reconfiguration callback + self.add_on_set_parameters_callback(self.parameters_callback) + + def process_camera_data(self, data): + proximity = self.nearest_point_distance(data) + warning = 1.0 if proximity < self.threshold else 0.0 + return warning, proximity + + def camera_callback(self, data): + self.last_warning, self.last_proximity = self.process_camera_data(data) + + def publish_warning(self): + warning_msg = Float32MultiArray() + + if self.last_warning: + self.get_logger().info(f'Publishing warning: {self.last_proximity}', throttle_duration_sec=1.0) + else: + self.get_logger().debug(f'Publishing NO warning', throttle_duration_sec=1.0) + + warning_msg.data = [ + self.last_warning, + self.last_proximity or float('NaN'), + self.threshold + ] + self.warning_publisher.publish(warning_msg) + + def parameters_callback(self, params): + for param in params: + if param.name == 'proximity_warning_threshold' and param.type_ == param.PARAMETER_DOUBLE: + self.threshold = param.value + self.get_logger().info(f'New proximity_warning_threshold set: {self.threshold}') + return SetParametersResult(successful=True) + return SetParametersResult(successful=False) + + def nearest_point_distance(self, ros_image): + try: + depth_image = bridge.imgmsg_to_cv2(ros_image, desired_encoding='passthrough') + depth_array = np.array(depth_image, dtype=np.float64) + min_distance = float(np.min(depth_array[depth_array!=0])) + return min_distance * 0.001 # Convert to meters + except CvBridgeError as e: + self.get_logger().error(e) + + +def main(args=None): + rclpy.init(args=args) + proximity_monitor_node = ProximityMonitorNode() + proximity_monitor_node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) + proximity_monitor_node.get_logger().info('Proximity Monitor Node started') + rclpy.spin(proximity_monitor_node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/proximity_monitor/resource/proximity_monitor b/src/proximity_monitor/resource/proximity_monitor new file mode 100644 index 0000000..e69de29 diff --git a/src/proximity_monitor/setup.cfg b/src/proximity_monitor/setup.cfg new file mode 100644 index 0000000..48ed68f --- /dev/null +++ b/src/proximity_monitor/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/proximity_monitor +[install] +install_scripts=$base/lib/proximity_monitor diff --git a/src/proximity_monitor/setup.py b/src/proximity_monitor/setup.py new file mode 100644 index 0000000..d65d3d0 --- /dev/null +++ b/src/proximity_monitor/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'proximity_monitor' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['launch/multi_camera_launch.py']) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ros', + maintainer_email='ros@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'proximity_monitor = proximity_monitor.proximity_monitor:main' + ], + }, +) diff --git a/src/proximity_monitor/test/test_copyright.py b/src/proximity_monitor/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/proximity_monitor/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/proximity_monitor/test/test_flake8.py b/src/proximity_monitor/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/proximity_monitor/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/proximity_monitor/test/test_pep257.py b/src/proximity_monitor/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/proximity_monitor/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/docker/realsense-ros b/src/realsense-ros similarity index 100% rename from docker/realsense-ros rename to src/realsense-ros