Monolithic node (#176)

monolithic node integration

.devcontainer/devcontainer.json

@@ -0,0 +1,46 @@
+// See https://aka.ms/vscode-remote/devcontainer.json for format details.
+{
+	"dockerFile": "../src/depthai-ros/Dockerfile",
+	"build": {
+		"args": {"BUILD_SEQUENTIAL": "1",
+				 "USE_RVIZ": "1"}
+	},
+	"remoteUser": "root",
+	"runArgs": [
+		"--device=/dev/ttyUSB0",
+		"--privileged",
+		"--network=host",
+		"--cap-add=SYS_PTRACE",
+		"--security-opt=seccomp:unconfined",
+		"--security-opt=apparmor:unconfined",
+		"--volume=/dev:/dev",
+		"--volume=/tmp/.X11-unix:/tmp/.X11-unix"
+	],
+	"containerEnv": { "DISPLAY": "${localEnv:DISPLAY}" },
+	// Set *default* container specific settings.json values on container create.
+	"settings": {
+		"terminal.integrated.profiles.linux": {
+			"zsh": {
+				"path": "zsh"
+			},
+			"bash": {
+				"path": "bash"
+			}
+
+		},
+		"terminal.integrated.defaultProfile.linux": "zsh"
+	},
+	"extensions": [
+		"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",
+		"augustocdias.tasks-shell-input"
+	]
+}

.gitignore

@@ -1,4 +1,3 @@
-*.json
 .vscode
 devel
 __pycache__

Dockerfile

@@ -6,18 +6,32 @@ ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
    && apt-get -y install --no-install-recommends software-properties-common git libusb-1.0-0-dev wget zsh python3-colcon-common-extensions
 
+# using cyclone since there are some problems with discovery using fastrps & docker
+RUN apt install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp
+
 ENV DEBIAN_FRONTEND=dialog
 RUN sh -c "$(wget https://raw.github.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)"
 
+# temporary depthai install until next sync
+
 ENV WS=/ws
 RUN mkdir -p $WS/src
-COPY ./ .$WS/src/depthai_ros
-RUN cd .$WS/ && rosdep install --from-paths src --ignore-src -y
-RUN if [ "$BUILD_SEQUENTIAL" = "1" ] ; then cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && colcon build --executor sequential --cmake-args -DCMAKE_BUILD_TYPE=Release ; else cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release; fi
+COPY ./ .$WS/src/depthai-ros
+RUN cd .$WS/ && rosdep install --from-paths src --ignore-src --skip-keys depthai -y
+RUN cd /tmp && \
+   git clone --recursive https://github.com/luxonis/depthai-core.git --branch main && \
+   cmake -Hdepthai-core -Bdepthai-core/build -DBUILD_SHARED_LIBS=ON -DCMAKE_INSTALL_PREFIX=/usr/local && \
+   cmake --build depthai-core/build --target install && \
+   cd /tmp && \
+   rm -r depthai-core
+
+RUN cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && ./src/depthai-ros/build.sh -s $BUILD_SEQUENTIAL -r 1 -m 1 
 RUN if [ "$USE_RVIZ" = "1" ] ; then echo "RVIZ ENABLED" && sudo apt install -y ros-${ROS_DISTRO}-rviz2 ros-${ROS_DISTRO}-rviz-imu-plugin ; else echo "RVIZ NOT ENABLED"; fi
 RUN echo "if [ -f ${WS}/install/setup.zsh ]; then source ${WS}/install/setup.zsh; fi" >> $HOME/.zshrc
 RUN echo 'eval "$(register-python-argcomplete3 ros2)"' >> $HOME/.zshrc
 RUN echo 'eval "$(register-python-argcomplete3 colcon)"' >> $HOME/.zshrc
+RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> $HOME/.zshrc
 RUN echo "if [ -f ${WS}/install/setup.bash ]; then source ${WS}/install/setup.bash; fi" >> $HOME/.bashrc
+RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> $HOME/.bashrc
 ENTRYPOINT [ "/ws/src/depthai_ros/entrypoint.sh" ]
 CMD ["zsh"]

README.md

@@ -1,22 +1,25 @@
 # Depthai ROS Repository
-Hi and welcome to the main depthai-ros respository!
+Hi and welcome to the main depthai-ros respository! Here you can find ROS related code for OAK cameras from Luxonis. Don't have one? You can get them [here!](https://shop.luxonis.com/)
+
+Main features:
+
+* You can use the cameras as classic RGBD sensors for your 3D vision needs.
+* You can also load Neural Networks and get the inference results straight from camera!
+
+You can develop your ROS applications in following ways:
+
+  * Use classes provided in `depthai_bridge` to construct your own driver (see `stereo_inertial_node` example on how to do that)
+  * Use `depthai_ros_driver` class (currently available on ROS2 Humble) to get default experience (see details below on how) 
+
+![](docs/segmentation.gif)
+
 
 Supported ROS versions:
 - Noetic
 - Galactic
 - Humble
 
-For development check out respective git branches.
-
-### Install from ros binaries
-
-Add USB rules to your system
-```
-echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="03e7", MODE="0666"' | sudo tee /etc/udev/rules.d/80-movidius.rules
-sudo udevadm control --reload-rules && sudo udevadm trigger
-```
-Install depthai-ros. (Available for Noetic, foxy, galactic and humble)
-`sudo apt install ros-<distro>-depthai-ros`
+For usage check out respective git branches.
 
 ## Docker
 You can additionally build and run docker images on your local machine. To do that, add USB rules as in above step, clone the repository and inside it run (it matters on which branch you are on):
@@ -32,9 +35,28 @@ xhost +local:docker
 
 Then you can run your image in following way:
 ```
-docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai_ros
+docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai=ros
+```
+will run an interactive docker session. You can also try:
+
 ```
-will run an interactive docker session.
+docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai-ros:humble roslaunch depthai_examples stereo_inertial_node.launch.py
+```
+to run a launch file of your choice.
+
+**NOTE** ROS2 Humble docker image uses Cyclone as RMW implementation. 
+
+
+### Install from ros binaries
+
+Add USB rules to your system
+```
+echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="03e7", MODE="0666"' | sudo tee /etc/udev/rules.d/80-movidius.rules
+sudo udevadm control --reload-rules && sudo udevadm trigger
+```
+Install depthai-ros. (Available for Noetic, foxy, galactic and humble)
+`sudo apt install ros-<distro>-depthai-ros`
+
 ### Running on ROS1
 ```
 docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix depthai_ros roslaunch depthai_examples stereo_inertial_node.launch
@@ -73,6 +95,49 @@ The following setup procedure assumes you have cmake version >= 3.10.2 and OpenC
 7. `catkin_make` (For ROS1) `colcon build` (for ROS2)
 8. `source devel/setup.bash` (For ROS1) & `source install/setup.bash` (for ROS2) 
 
+
+### Depthai ROS Driver
+
+Currently, recommended way to launch cameras is to use executables from depthai_ros_driver package. 
+
+This runs your camera as a ROS2 Component and gives you the ability to customize your camera using ROS parameters. 
+Paramerers that begin with `r_` can be freely modified during runtime, for example with rqt. 
+Parameters that begin with `i_` are set when camera is initializing, to change them you have to call `stop` and `start` services. This can be used to hot swap NNs during runtime, changing resolutions, etc. Below you can see some examples:
+
+#### Setting RGB parameters
+![](docs/param_rgb.gif)
+#### Setting Stereo parameters
+![](docs/param_stereo.gif)
+#### Stopping/starting camera for power saving/reconfiguration
+![](docs/start_stop.gif)
+
+Stopping camera also can be used for power saving, as pipeline is removed from the device. Topics are also removed when camera is stopped.
+
+As for the parameters themselves, there are a few crucial ones that decide on how camera behaves.
+* `camera.i_pipeline_type` can be either `RGB` or `RGBD`. This tells the camera whether it should load stereo components. Default set to `RGBD`
+* `camera.i_nn_type` can be either `none`, `rgb` or `spatial`. This is responsible for whether the NN that we load should also take depth information (and for example provide detections in 3D format). Default set to `spatial`
+* `camera.i_mx_id`/`camera.i_ip` are for connecting to a specific camera. If not set, it automatically connects to the next available device.
+* `nn.i_nn_config_path` represents path to JSON that contains information on what type of NN to load, and what parameters to use. Currently we provide options to load MobileNet, Yolo and Segmentation (not in spatial) models. To see their example configs, navigate to `depthai_ros_driver/config/nn`. Defaults to `mobilenet.json` from `depthai_ros_driver`
+
+To use provided example NN's, you can set the path to:
+* `depthai_ros_driver/segmentation`
+* `depthai_ros_driver/mobilenet`
+* `depthai_ros_driver/yolo`
+
+All available camera-specific parameters and their default values can be seen in `depthai_ros_driver/config/camera.yaml`.
+
+Currently, we provide few examples:
+
+* `camera.launch.py` launches camera in RGBD, and NN in spatial (Mobilenet) mode.
+* `rgbd_pcl.launch.py` launches camera in basic RGBD configuration, doesn't load any NNs. Also loads ROS depth processing nodes for RGBD pointcloud.
+* `example_multicam.launch.py` launches several cameras at once, each one in different container. Edit the `multicam_example.yaml` config file in `config` directory to change parameters
+
+![](docs/multicam.gif)
+* `example_segmentation.launch.py` launches camera in RGBD + semantic segmentation (pipeline type=RGBD, nn_type=rgb)
+* `pointcloud.launch.py` - similar to `rgbd_pcl.launch.py`, but doesn't use RGB component for pointcloud
+* `example_marker_publish.launch.py` launches `camera.launch.py` + small python node that publishes detected objects as markers/tfs
+* `rtabmap.launch.py` launches camera and RTAB-MAP RGBD SLAM (you need to install it first - `sudo apt install ros-$ROS_DISTRO-rtabmap-ros`). You might need to set manual focus via parameters here.
+![](docs/rtabmap.gif)
 ## Executing an example
 
 ### ROS1
@@ -127,4 +192,12 @@ ros2 launch depthai_examples mobile_publisher.launch.py camera_model:=OAK-D-LITE
 
 ### Users can write Custom converters and plug them in for bridge Publisher. 
 If there a standard Message or usecase for which we have not provided a ros msg or
- converter feel free to create a issue or reach out to us on our discord community. We would be happy to add more. 
+ converter feel free to create a issue or reach out to us on our discord community. We would be happy to add more. 
+
+### Developers guide
+
+For easier development inside isolated workspace, one can use Visual Studio Code with DevContainers plugin, to do that:
+- Create separate workspace
+- Clone repository into src
+- Copy `.devcontainer` directory into main workspace directory
+- Open workspace directory in VSCode

build.sh

@@ -0,0 +1,62 @@
+#!/bin/bash
+Help()
+{
+    echo "Build workspace"
+    echo
+    echo "Build options:"
+    echo "-s [1]   Set to 1 to build sequentially (longer, but saves RAM & CPU)"
+    echo "-r [0]  Set to 1 to build in Debug mode. (RelWithDebInfo)"
+    echo "-m [0]   Set to 1 to build with --merge-install option."
+    echo
+}
+
+sequential=1
+release=0
+merge=0
+build_type=Release
+install_type=symlink-install
+while getopts ":h:s:r:m:" option; do
+   case $option in
+      h) # display Help
+         Help
+         exit;;
+      s) # Sequential executor
+         sequential=$OPTARG;;
+      r) # Build type
+         release=$OPTARG;;
+      m) # Install type
+         merge=$OPTARG;;
+     \?) # Invalid option
+         echo "Error: Invalid option"
+         exit;;
+   esac
+done
+
+if [ "$release" == 0 ]
+then
+    build_type="RelWithDebInfo"
+fi
+
+if [ "$merge" == 1 ]
+then
+    install_type="merge-install"
+fi
+echo "Build type: $build_type, Install_type: $install_type"
+if [ "$sequential" == 1 ]
+then
+    echo "Sequential build" && \
+    MAKEFLAGS="-j1 -l1" colcon build \
+        --$install_type \
+        --executor sequential \
+        --cmake-args -DCMAKE_BUILD_TYPE=$build_type \
+        --cmake-args -DBUILD_TESTING=OFF \
+        --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \
+        --cmake-args -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
+        --cmake-args -DBUILD_SHARED_LIBS=ON
+else
+    echo "Parallel build" && \
+    colcon build \
+    --$install_type \
+    --cmake-args -DCMAKE_BUILD_TYPE=$build_type \
+    --cmake-args -DBUILD_TESTING=OFF --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --cmake-args -DCMAKE_POSITION_INDEPENDENT_CODE=ON --cmake-args -DBUILD_SHARED_LIBS=ON
+fi

depthai_bridge/include/depthai_bridge/ImageConverter.hpp

@@ -26,7 +26,12 @@ namespace ImageMsgs = sensor_msgs::msg;
 using ImagePtr = ImageMsgs::Image::SharedPtr;
 
 using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>;
-
+ImageMsgs::CameraInfo calibrationToCameraInfo(dai::CalibrationHandler calibHandler,
+                                              dai::CameraBoardSocket cameraId,
+                                              int width = -1,
+                                              int height = -1,
+                                              Point2f topLeftPixelId = Point2f(),
+                                              Point2f bottomRightPixelId = Point2f());
 class ImageConverter {
    public:
     // ImageConverter() = default;

depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp

@@ -6,6 +6,7 @@
 
 #include "depthai/depthai.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "vision_msgs/msg/detection3_d_array.hpp"
 
 namespace dai {
 
@@ -20,6 +21,7 @@ class SpatialDetectionConverter {
     SpatialDetectionConverter(std::string frameName, int width, int height, bool normalized = false);
 
     void toRosMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<SpatialMessages::SpatialDetectionArray>& opDetectionMsg);
+    void toRosVisionMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<vision_msgs::msg::Detection3DArray>& opDetectionMsg);
 
     SpatialDetectionArrayPtr toRosMsgPtr(std::shared_ptr<dai::SpatialImgDetections> inNetData);