diff --git a/README.md b/README.md index c9ea0a71..590972ac 100644 --- a/README.md +++ b/README.md @@ -9,56 +9,22 @@ Main features: 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 & Noetic) to get default experience (see details below on how) + * Use `depthai_ros_driver` package (currently available on ROS2 Humble and ROS Noetic) to get default experience (see details below on how) ![](docs/multicam.gif) Supported ROS versions: - Noetic -- Galactic - Humble 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): -``` -docker build --build-arg USE_RVIZ=1 -t depthai-ros . -``` -If you find out that you run out of RAM during building, you can also set `BUILD_SEQUENTIAL=1` to build packages one at a time, it should take longer, but use less RAM. - -`RUN_RVIZ` arg means rviz will be installed inside docker. If you want to run it you need to also execute following command (you'll have to do it again after restarting your PC): -``` -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 -``` -will run an interactive docker session. You can also try: - -``` -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.py -``` -to run a launch file of your choice. - -**NOTE** ROS2 Humble docker image uses Cyclone as RMW implementation. -### Running docker iamge 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 -``` -Will only start `stereo_inertial_node` launch file (you can try different commands). -### Running docker iamge on ROS2 -``` -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.py -``` ### Install from ros binaries Add USB rules to your system -``` +``` bash 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 ``` @@ -72,7 +38,7 @@ Install depthai-ros. (Available for Noetic, foxy, galactic and humble) The following script will install depthai-core and update usb rules and install depthai devices -``` +``` bash sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/main/install_dependencies.sh | sudo bash ``` if you don't have opencv installed then try `sudo apt install libopencv-dev` @@ -88,16 +54,84 @@ The following setup procedure assumes you have cmake version >= 3.10.2 and OpenC 1. `mkdir -p dai_ws/src` 2. `cd dai_ws/src` -3. `git clone https://github.com/luxonis/depthai-ros.git` -4. `cd ../..` +3. `git clone --branch https://github.com/luxonis/depthai-ros.git` +4. `cd ..` 5. `rosdep install --from-paths src --ignore-src -r -y` 6. `source /opt/ros//setup.bash` -7. `catkin_make_isolated -l1 -j1` (For ROS1) `MAKEFLAGS="-j1 -l1" colcon build` (for ROS2) +7. `catkin_make_isolated` (For ROS1) `MAKEFLAGS="-j1 -l1" colcon build` (for ROS2) 8. `source devel/setup.bash` (For ROS1) & `source install/setup.bash` (for ROS2) **Note** If you are using a lower end PC or RPi, standard building may take a lot of RAM and clog your PC. To avoid that, you can use `build.sh` command from your workspace (it just wraps colcon commands): `./src/depthai-ros/build.sh` +## Docker +You can additionally build and run docker images on your local machine. To do that, **add USB rules as in above step**. + +### Running prebuilt images + +Each tagged version has it's own prebuilt docker image. To download and run it: + +``` +xhost +local:docker +``` +to enable GUI tools such as rviz or rqt. + +Then +``` bash +docker run -it -v /dev/:/dev/ --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix luxonis/depthai-ros:noetic-latest [CMD] +``` + +Where [CMD] is what's going to be executed when container is initially run and could be for example: +- bash (it will allow you to browse files and make modifications to the code and rebuild it) +- zsh (other shell that is installed that has some advantages over bash) +- roslaunch depthai_ros_driver camera.launch (this is just an example, any launch file will work here) +A side note here, launch files in depthai_ros_driver have some default parameters set by .yaml files inside the driver. You can either edit them inside the container itself, or you can make a .yaml file on your host (let's say `/home/YOUR_USERNAME_HERE/params/example_config.yaml`) and pass it as an argument to the executable, as follows: +``` bash +docker run -it -v /dev/:/dev/ -v /home/YOUR_USERNAME_HERE/params:/params --network host --privileged -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix luxonis/depthai-ros:noetic-latest roslaunch depthai_ros_driver camera.launch params_file:=/params/example_config.yaml +``` +Note, to make images more compact only some external dependencies are installed, for example if you want to try out RTABMap example in docker, you have to: +- Install it by running the container in bash/zsh mode +- Modify the Dockerfile so that it's installed during building - you'll have to rebuild the container after that. +- Run base camera node in our container and RTABMap separately on host/ in a separate container (see the launch file on what parameters/topic names need to be changed when running separately). + + +### Building + +Clone the repository and inside it run (it matters on which branch you are on): +``` bash +docker build --build-arg USE_RVIZ=1 -t depthai-ros . +``` +If you find out that you run out of RAM during building, you can also set `BUILD_SEQUENTIAL=1` to build packages one at a time, it should take longer, but use less RAM. + +`RUN_RVIZ` arg means rviz will be installed inside docker. If you want to run it you need to also execute following command (you'll have to do it again after restarting your PC): +``` bash +xhost +local:docker +``` + +Then you can run your image in following way: +``` bash +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: + +``` bash +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.py +``` +to run a launch file of your choice. + +**NOTE** ROS2 Humble docker image uses Cyclone as RMW implementation. +### Running docker image on ROS1 + +``` bash +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 +``` +Will only start `stereo_inertial_node` launch file (you can try different commands). +### Running docker image on ROS2 +``` bash +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.py +``` + + ### Depthai ROS Driver Currently, recommended way to launch cameras is to use executables from depthai_ros_driver package. @@ -106,14 +140,37 @@ This runs your camera as a ROS2 Component and gives you the ability to customize 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: -### **Note** There is a difference in parameter naming with ROS1 & ROS2, an example: -In ROS1 - `rgb_i_fps`, ROS2, `rgb.i_fps` -This is due to change in parameter design in ROS versions (dynamic reconfigure doesn't support parameter namespacing) - #### Setting RGB parameters + +By default RGB camera outputs `ISP` frame. To set custom width and height of output image, you can set `i_isp_num` and `i_isp_den` which scale image dimensions (2 and 3 by default, so from 1920x1080 to 1280x720), note for RGBD alignment to work resulting width and height must be divisible by 16. + +Additionally you can set `i.output_isp: false` to use `video` output and set custom size using `i_width` and `i_height` parameters. ![](docs/param_rgb.gif) #### Setting Stereo parameters ![](docs/param_stereo.gif) + +#### Setting IMU parameters +Parameters: +* `i_acc_freq: 400` - Accelerometer sensor frequency +* `i_acc_cov: 0.0` - Accelerometer covariance +* `i_batch_report_threshold: 1` - Batch report size +* `i_enable_rotation: false` - Whether to enable rotation vector & magnetometer data (available when using IMU_WITH_MAGN/ IMU_WITH_MAGN_SPLIT message type) +* `i_gyro_cov: 0.0` - Gyroscope covariance +* `i_gyro_freq: 400` - Gyroscope frequency +* `i_mag_cov: 0.0` - Magnetometer covariance +* `i_mag_freq: 100` - Magnetometer frequency +* `i_max_batch_reports: 10` - Max reports per batch +* `i_message_type: IMU` - ROS publisher type: + * `IMU` - sensor_msgs/Imu + * `IMU_WITH_MAG` - depthai_ros_msgs/ImuWithMagneticField + * `IMU_WITH_MAG_SPLIT` - two publishers - sensor_msgs/Imu & sensor_msgs/MagneticField +* `i_rot_cov: -1.0` - Rotation covariance +* `i_rot_freq: 100` - Rotation frequency +* `i_sync_method: LINEAR_INTERPOLATE_ACCEL` - sync method. Available options: + * `COPY` + * `LINEAR_INTERPOLATE_GYRO` + * `LINEAR_INTERPOLATE_ACCEL` + #### Stopping/starting camera for power saving/reconfiguration ![](docs/start_stop.gif) @@ -125,13 +182,13 @@ As for the parameters themselves, there are a few crucial ones that decide on ho * `RGBD` - Publishes RGB + Depth streams (set `i_publish_topic` for left and right cameras to enable them), NN & Spatial NN available * `Stereo` - Publishes streams from left and right sensors, NN not available * `RGBStereo` - Publishes RGB + Left + Right streams, only RGB NN available - * `Depth` - Publishes only depth stream, NN not available - * `CamArray` - Publishes streams for all detected sensors, NN not available + * `Depth` - Publishes only depth stream, no NN available + * `CamArray` - Publishes streams for all detected sensors, no NN available 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`/`camera_i_usb_port_id` are for connecting to a specific camera. If not set, it automatically connects to the next available device. You can get those parameters from logs by running the default launch file. -* `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` +* `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`/`camera.i_usb_port_id` are for connecting to a specific camera. If not set, it automatically connects to the next available device. You can get those parameters from logs by running the default launch file. +* `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` @@ -142,23 +199,23 @@ All available camera-specific parameters and their default values can be seen in Currently, we provide few examples: -* `camera.launch` launches camera in RGBD, and NN in spatial (Mobilenet) mode. -* `rgbd_pcl.launch` launches camera in basic RGBD configuration, doesn't load any NNs. Also loads ROS depth processing nodes for RGBD pointcloud. -* `example_multicam.launch` launches several cameras at once, each one in different container. Edit the `multicam_example.yaml` config file in `config` directory to change parameters -* `example_segmentation.launch` launches camera in RGBD + semantic segmentation (pipeline type=RGBD, nn_type=rgb) -* `pointcloud.launch` - similar to `rgbd_pcl.launch.py`, but doesn't use RGB component for pointcloud -* `example_marker_publish.launch` launches `camera.launch.py` + small python node that publishes detected objects as markers/tfs -* `rtabmap.launch` 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. +* `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 +* `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) #### Specific camera configurations: ##### **PoE Cameras** Since PoE cameras use protocol that has lower throughput than USB, running default camera launch can result in lags depending on chosen resolution/fps. To combat this issue, you can use encoded frames, which let you keep desired resolution/fps at the cost of image quality reduction due to compression. One additional difference is that `subpixel` depth filtering is disabled in this mode. To enable low_bandwidth, for example for rgb camera, change parameters: -* `rgb_i_low_bandwidth` - `true` to enable -* `rgb_i_low_bandwidth_quality` - desired quality % (default-50) +* `rgb.i_low_bandwidth` - `true` to enable +* `rgb.i_low_bandwidth_quality` - desired quality % (default-50) See `low_bandwidth.yaml` file for example parameters for all streams ##### **OAK D PRO W** -To properly align with depth, you need to set `rgb_i_resolution` parameter to `720` (see `config/oak_d_w_pro.yaml`). +To properly align with depth, you need to set `rgb.i_resolution` parameter to `720` (see `config/oak_d_w_pro.yaml`). #### Recalibration If you want to use other calibration values than the ones provided by the device, you can do it in following ways: @@ -171,6 +228,36 @@ Calibration file syntax (from `camera_info_manager`): - package://camera_info_manager/tests/test_calibration.yaml - package://ros_package_name/calibrations/camera3.yaml ``` + +### Depthai filters + +`depthai_filters` contains small composable node examples that show how to work with data from multiple topics. +Available filters: +- Detection2DOverlay - subscribes to `/nn/detections` and `rgb/preview/image_raw` topics. To see it in action, run +`ros2 launch depthai_filters example_det2d_overla.launch.py`. Note here - If you see that detections misalign in the overlay, adjust `rgb.i_preview_size` parameter. +- SegmentationOverlay, overlays semantic segmentation from `/nn/image_raw` on top of image from `rgb/preview/image_raw`, to see it in action, run +`ros2 launch depthai_filters example_seg_overlay.launch.py` +- WLS filter - stereo depth filter that smooths out overall depth image based on disparity data. It subscribes to `stereo/image_raw` and `left/image raw` topics. Parameters needed to enable it - `left.i_publish_topic`, `stereo.i_output_disparity` +an example can be seen by running `ros2 launch depthai_filters example_wls_filter.launch.py` + + +### Using external sources for NN inference or Stereo Depth + +There is a possibility of using external image topics as input for NNs or Depth calculation. + +Example scenarios: + +1. We want to get segmentation or 2D detection output based on images published by usb camera node. +This can be achieved by setting `rgb.i_simulate_from_topic` parameter to `true`. This creates `sensor_msgs/Image` subscriber listening by default on `//rgb/input` topic that passes data into the pipeline on each callback. Topic names can be changed either by classic ROS topic remapping or setting `rgb.i_simulated_topic_name` to a desired name. +By default, original sensor node still runs and publishes data. Setting `rgb.i_disable_node` to true will prevent it from spawning. Check `det2d_usb_cam_overlay.launch.py` in `depthai_filters to see it in action. + +2. Calculating depth - both `left` and `right` sensor nodes can be setup as in the example above to calculate calculate depth/disparity from external topics. Note that for this to work properly you need specify: +- `left.i_board_socket_id: 1` +- `right.i_board_socket_id: 2` +- Default stereo input size is set to 1280x720, in case of different image size, To set custom ones, set `stereo.i_set_input_size: true` and adjust `stereo.i_input_width` and `stereo.i_input_height` accordingly. +- external calibration file path using `camera.i_external_calibration_path` parameter. To get calibration from the device you can either set `camera.i_calibration_dump` to true or call `save_calibration` service. Calibration will be saved to `/tmp/_calibration.json`. +An example can be seen in `stereo_from_rosbag.launch.py` in `depthai_ros_driver` + ## Executing an example ### ROS1 @@ -189,20 +276,6 @@ For more examples please check the launch files. ## Running Examples -### Depthai Ros Driver: -#### Default camera: -```roslaunch depthai_ros_driver camera.launch` -#### RGBD camera: -```roslaunch depthai_ros_driver rgbd_pcl.launch` -#### Segmentation: -```roslaunch depthai_ros_driver example_segmentation.launch` -#### Multi-camera example: -First, add mx_ids,IPs to the config file (see config/multicam.yaml for reference) -```roslaunch depthai_ros_driver example_multicam.launch` -#### Basic Pointcloud (no RGB) -```roslaunch depthai_ros_driver pointcloud.launch``` - - ### Mobilenet Publisher: #### ROS1: ##### OAK-D @@ -220,20 +293,6 @@ roslaunch depthai_examples mobile_publisher.launch | rqt_image_view -t /mobilene #### ROS2: -### Depthai Ros Driver: -#### Default camera: -```ros2 launch depthai_ros_driver camera.launch.py` -#### RGBD camera: -```ros2 launch depthai_ros_driver rgbd_pcl.launch.py` -#### Segmentation: -```ros2 launch depthai_ros_driver example_segmentation.launch.py` -#### Multi-camera example: -First, add mx_ids,IPs to the config file (see config/multicam.yaml for reference) -```ros2 launch depthai_ros_driver example_multicam.launch.py` -#### Basic Pointcloud (no RGB) -```ros2 launch depthai_ros_driver pointcloud.launch.py``` - - ##### OAK-D ``` ros2 launch depthai_examples mobile_publisher.launch.py camera_model:=OAK-D @@ -245,6 +304,12 @@ ros2 launch depthai_examples mobile_publisher.launch.py camera_model:=OAK-D-LITE ``` +### Testing results +- ImageConverter - Tested using `roslaunch depthai_examples stereo_inertial_node.launch` && `roslaunch depthai_examples rgb_publisher.launch`' +- ImgDetectionCnverter - tested using `roslaunch depthai_examples mobile_publisher.launch` +- SpatialImgDetectionConverter - Ntested using `roslaunch depthai_examples stereo_inertial_node.launch` + + ### 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. diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 6250cc01..d7a61e1b 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.2 (20230-5-08) +* IMU improvements + 2.7.1 (2023-03-29) ------------------- * Add custom output size option for streams diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 4b94f178..0fe2ed1a 100644 --- a/depthai-ros/CMakeLists.txt +++ b/depthai-ros/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai-ros VERSION 2.7.1 LANGUAGES CXX C) +project(depthai-ros VERSION 2.7.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 87a3c8f0..0fe54e22 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.1 + 2.7.2 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index a37b2463..9d5a11f3 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS set (CMAKE_POSITION_INDEPENDENT_CODE ON) -project(depthai_bridge VERSION 2.7.1 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.7.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index c367b645..9d8eaac5 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -7,8 +7,11 @@ #include "depthai-shared/datatype/RawIMUData.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" +#include "depthaiUtility.hpp" +#include "depthai_ros_msgs/ImuWithMagneticField.h" #include "ros/time.h" #include "sensor_msgs/Imu.h" +#include "sensor_msgs/MagneticField.h" namespace dai { @@ -24,21 +27,165 @@ class ImuConverter { ImuConverter(const std::string& frameName, ImuSyncMethod syncMode = ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL, double linear_accel_cov = 0.0, - double angular_velocity_cov = 0.0); + double angular_velocity_cov = 0.0, + double rotation_cov = 0.0, + double magnetic_field_cov = 0.0, + bool enable_rotation = false); + ~ImuConverter(); + void toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs); + void toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs); - void toRosMsg(std::shared_ptr inData, std::deque& outImuMsg); + template + T lerp(const T& a, const T& b, const double t) { + return a * (1.0 - t) + b * t; + } + + template + T lerpImu(const T& a, const T& b, const double t) { + T res; + res.x = lerp(a.x, b.x, t); + res.y = lerp(a.y, b.y, t); + res.z = lerp(a.z, b.z, t); + return res; + } private: - void FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs); - ImuMsgs::Imu CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro); - ImuMsgs::Imu CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro, dai::IMUReportRotationVectorWAcc rot); + template + void FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs) { + static std::deque accelHist; + static std::deque gyroHist; + static std::deque rotationHist; + static std::deque magnHist; + + for(int i = 0; i < imuPackets.size(); ++i) { + if(accelHist.size() == 0) { + accelHist.push_back(imuPackets[i].acceleroMeter); + } else if(accelHist.back().sequence != imuPackets[i].acceleroMeter.sequence) { + accelHist.push_back(imuPackets[i].acceleroMeter); + } + + if(gyroHist.size() == 0) { + gyroHist.push_back(imuPackets[i].gyroscope); + } else if(gyroHist.back().sequence != imuPackets[i].gyroscope.sequence) { + gyroHist.push_back(imuPackets[i].gyroscope); + } + + if(_enable_rotation && rotationHist.size() == 0) { + rotationHist.push_back(imuPackets[i].rotationVector); + } else if(_enable_rotation && rotationHist.back().sequence != imuPackets[i].rotationVector.sequence) { + rotationHist.push_back(imuPackets[i].rotationVector); + } else { + rotationHist.resize(accelHist.size()); + } + + if(_enable_rotation && magnHist.size() == 0) { + magnHist.push_back(imuPackets[i].magneticField); + } else if(_enable_rotation && magnHist.back().sequence != imuPackets[i].magneticField.sequence) { + magnHist.push_back(imuPackets[i].magneticField); + } else { + magnHist.resize(accelHist.size()); + } + + if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { + if(accelHist.size() < 3) { + continue; + } else { + interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs); + } + + } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { + if(gyroHist.size() < 3) { + continue; + } else { + interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs); + } + } + } + } uint32_t _sequenceNum; - double _linear_accel_cov, _angular_velocity_cov; + double _linear_accel_cov, _angular_velocity_cov, _rotation_cov, _magnetic_field_cov; + bool _enable_rotation; const std::string _frameName = ""; ImuSyncMethod _syncMode; std::chrono::time_point _steadyBaseTime; ::ros::Time _rosBaseTime; + + void fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg); + void fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg); + void fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::Imu& msg); + void fillImuMsg(dai::IMUReportMagneticField report, ImuMsgs::Imu& msg); + + void fillImuMsg(dai::IMUReportAccelerometer report, depthai_ros_msgs::ImuWithMagneticField& msg); + void fillImuMsg(dai::IMUReportGyroscope report, depthai_ros_msgs::ImuWithMagneticField& msg); + void fillImuMsg(dai::IMUReportRotationVectorWAcc report, depthai_ros_msgs::ImuWithMagneticField& msg); + void fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::ImuWithMagneticField& msg); + + template + void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, dai::Timestamp timestamp) { + fillImuMsg(first, msg); + fillImuMsg(second, msg); + fillImuMsg(third, msg); + fillImuMsg(fourth, msg); + + msg.header.frame_id = _frameName; + + msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp.get()); + } + + template + void interpolate(std::deque& interpolated, std::deque& second, std::deque& third, std::deque& fourth, std::deque& imuMsgs) { + I interp0, interp1; + S currSecond; + T currThird; + F currFourth; + interp0.sequence = -1; + while(interpolated.size()) { + if(interp0.sequence == -1) { + interp0 = interpolated.front(); + interpolated.pop_front(); + } else { + interp1 = interpolated.front(); + interpolated.pop_front(); + // remove std::milli to get in seconds + std::chrono::duration duration_ms = interp1.timestamp.get() - interp0.timestamp.get(); + double dt = duration_ms.count(); + while(second.size()) { + currSecond = second.front(); + currThird = third.front(); + currFourth = fourth.front(); + if(currSecond.timestamp.get() > interp0.timestamp.get() && currSecond.timestamp.get() <= interp1.timestamp.get()) { + // remove std::milli to get in seconds + std::chrono::duration diff = currSecond.timestamp.get() - interp0.timestamp.get(); + const double alpha = diff.count() / dt; + I interp = lerpImu(interp0, interp1, alpha); + M msg; + CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, currSecond.timestamp); + imuMsgs.push_back(msg); + second.pop_front(); + third.pop_front(); + fourth.pop_front(); + } else if(currSecond.timestamp.get() > interp1.timestamp.get()) { + interp0 = interp1; + if(interpolated.size()) { + interp1 = interpolated.front(); + interpolated.pop_front(); + duration_ms = interp1.timestamp.get() - interp0.timestamp.get(); + dt = duration_ms.count(); + } else { + break; + } + } else { + second.pop_front(); + third.pop_front(); + fourth.pop_front(); + } + } + interp0 = interp1; + } + } + interpolated.push_back(interp0); + } }; } // namespace ros diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index ef21bb66..64ee9730 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.1 + 2.7.2 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index ffc16862..78890aa0 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -7,231 +7,78 @@ namespace dai { namespace ros { -ImuConverter::ImuConverter(const std::string& frameName, ImuSyncMethod syncMode, double linear_accel_cov, double angular_velocity_cov) +ImuConverter::ImuConverter(const std::string& frameName, + ImuSyncMethod syncMode, + double linear_accel_cov, + double angular_velocity_cov, + double rotation_cov, + double magnetic_field_cov, + bool enable_rotation) : _frameName(frameName), _syncMode(syncMode), _linear_accel_cov(linear_accel_cov), _angular_velocity_cov(angular_velocity_cov), + _rotation_cov(rotation_cov), + _magnetic_field_cov(magnetic_field_cov), + _enable_rotation(enable_rotation), _sequenceNum(0), _steadyBaseTime(std::chrono::steady_clock::now()) { _rosBaseTime = ::ros::Time::now(); } -void ImuConverter::FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs) { - static std::deque accelHist; - static std::deque gyroHist; +ImuConverter::~ImuConverter() = default; - for(int i = 0; i < imuPackets.size(); ++i) { - if(accelHist.size() == 0) { - accelHist.push_back(imuPackets[i].acceleroMeter); - } else if(accelHist.back().sequence != imuPackets[i].acceleroMeter.sequence) { - accelHist.push_back(imuPackets[i].acceleroMeter); - } - - if(gyroHist.size() == 0) { - gyroHist.push_back(imuPackets[i].gyroscope); - } else if(gyroHist.back().sequence != imuPackets[i].gyroscope.sequence) { - gyroHist.push_back(imuPackets[i].gyroscope); - } +void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg) { + msg.linear_acceleration.x = report.x; + msg.linear_acceleration.y = report.y; + msg.linear_acceleration.z = report.z; + msg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov}; +} - if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { - if(accelHist.size() < 3) { - continue; - } else { - dai::IMUReportAccelerometer accel0, accel1; - dai::IMUReportGyroscope currGyro; - accel0.sequence = -1; - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", " Interpolating LINEAR_INTERPOLATE_ACCEL mode " << std::endl); - while(accelHist.size()) { - if(accel0.sequence == -1) { - accel0 = accelHist.front(); - accelHist.pop_front(); - } else { - accel1 = accelHist.front(); - accelHist.pop_front(); - - // remove std::milli to get in seconds - std::chrono::duration duration_ms = accel1.timestamp.get() - accel0.timestamp.get(); - double dt = duration_ms.count(); - - if(!gyroHist.size()) { - DEPTHAI_ROS_WARN_STREAM("IMU INTERPOLATION: ", "Gyro data not found. Dropping accel data points"); - } - while(gyroHist.size()) { - currGyro = gyroHist.front(); - - DEPTHAI_ROS_DEBUG_STREAM( - "IMU INTERPOLATION: ", - "Accel 0: Seq => " << accel0.sequence << " timeStamp => " << (accel0.timestamp.get() - _steadyBaseTime).count() << std::endl); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "currGyro 0: Seq => " << currGyro.sequence << "timeStamp => " - << (currGyro.timestamp.get() - _steadyBaseTime).count() << std::endl); - DEPTHAI_ROS_DEBUG_STREAM( - "IMU INTERPOLATION: ", - "Accel 1: Seq => " << accel1.sequence << " timeStamp => " << (accel1.timestamp.get() - _steadyBaseTime).count() << std::endl); - if(currGyro.timestamp.get() > accel0.timestamp.get() && currGyro.timestamp.get() <= accel1.timestamp.get()) { - // remove std::milli to get in seconds - std::chrono::duration diff = currGyro.timestamp.get() - accel0.timestamp.get(); - const double alpha = diff.count() / dt; - dai::IMUReportAccelerometer interpAccel = lerpImu(accel0, accel1, alpha); - imuMsgs.push_back(CreateUnitMessage(interpAccel, currGyro)); - gyroHist.pop_front(); - } else if(currGyro.timestamp.get() > accel1.timestamp.get()) { - accel0 = accel1; - if(accelHist.size()) { - accel1 = accelHist.front(); - accelHist.pop_front(); - duration_ms = accel1.timestamp.get() - accel0.timestamp.get(); - dt = duration_ms.count(); - } else { - break; - } - } else { - gyroHist.pop_front(); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Droppinh GYRO with old timestamps which are below accel10 \n"); - } - } - // gyroHist.push_back(currGyro); // Undecided whether this is necessary - accel0 = accel1; - } - } - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Count ->" << i << " Placing Accel 0 Seq Number :" << accel0.sequence << std::endl); - - accelHist.push_back(accel0); - } - } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { - if(gyroHist.size() < 3) { - continue; - } else { - dai::IMUReportGyroscope gyro0, gyro1; - dai::IMUReportAccelerometer currAccel; - gyro0.sequence = -1; - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", " Interpolating LINEAR_INTERPOLATE_GYRO mode " << std::endl); - while(gyroHist.size()) { - if(gyro0.sequence == -1) { - gyro0 = gyroHist.front(); - gyroHist.pop_front(); - } else { - gyro1 = gyroHist.front(); - gyroHist.pop_front(); - // remove std::milli to get in seconds - std::chrono::duration duration_ms = gyro1.timestamp.get() - gyro0.timestamp.get(); - double dt = duration_ms.count(); - - if(!accelHist.size()) { - DEPTHAI_ROS_WARN_STREAM("IMU INTERPOLATION: ", "Accel data not found. Dropping data"); - } - while(accelHist.size()) { - currAccel = accelHist.front(); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "gyro 0: Seq => " << gyro0.sequence << std::endl - << " timeStamp => " << (gyro0.timestamp.get() - _steadyBaseTime).count() - << std::endl); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "currAccel 0: Seq => " << currAccel.sequence << std::endl - << " timeStamp => " << (currAccel.timestamp.get() - _steadyBaseTime).count() - << std::endl); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "gyro 1: Seq => " << gyro1.sequence << std::endl - << " timeStamp => " << (gyro1.timestamp.get() - _steadyBaseTime).count() - << std::endl); - if(currAccel.timestamp.get() > gyro0.timestamp.get() && currAccel.timestamp.get() <= gyro1.timestamp.get()) { - // remove std::milli to get in seconds - std::chrono::duration diff = currAccel.timestamp.get() - gyro0.timestamp.get(); - const double alpha = diff.count() / dt; - dai::IMUReportGyroscope interpGyro = lerpImu(gyro0, gyro1, alpha); - imuMsgs.push_back(CreateUnitMessage(currAccel, interpGyro)); - accelHist.pop_front(); - } else if(currAccel.timestamp.get() > gyro1.timestamp.get()) { - gyro0 = gyro1; - if(gyroHist.size()) { - gyro1 = gyroHist.front(); - gyroHist.pop_front(); - duration_ms = gyro1.timestamp.get() - gyro0.timestamp.get(); - dt = duration_ms.count(); - } else { - break; - } - } else { - accelHist.pop_front(); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Droppinh ACCEL with old timestamps which are below accel10 \n"); - } - } - gyro0 = gyro1; - } - } - gyroHist.push_back(gyro0); - } - } - } +void ImuConverter::fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg) { + msg.angular_velocity.x = report.x; + msg.angular_velocity.y = report.y; + msg.angular_velocity.z = report.z; + msg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; } -ImuMsgs::Imu ImuConverter::CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro) { - ImuMsgs::Imu interpMsg; - interpMsg.linear_acceleration.x = accel.x; - interpMsg.linear_acceleration.y = accel.y; - interpMsg.linear_acceleration.z = accel.z; - - interpMsg.angular_velocity.x = gyro.x; - interpMsg.angular_velocity.y = gyro.y; - interpMsg.angular_velocity.z = gyro.z; - - interpMsg.orientation.x = 0.0; - interpMsg.orientation.y = 0.0; - interpMsg.orientation.z = 0.0; - interpMsg.orientation.w = 1.0; - - interpMsg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - interpMsg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov}; - interpMsg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; - - interpMsg.header.frame_id = _frameName; - interpMsg.header.seq = _sequenceNum; - _sequenceNum++; - - if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, gyro.timestamp.get()); - } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get()); +void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::Imu& msg) { + if(_enable_rotation) { + msg.orientation.x = report.i; + msg.orientation.y = report.j; + msg.orientation.z = report.k; + msg.orientation.w = report.real; + msg.orientation_covariance = {_rotation_cov, 0.0, 0.0, 0.0, _rotation_cov, 0.0, 0.0, 0.0, _rotation_cov}; } else { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get()); + msg.orientation.x = 0.0; + msg.orientation.y = 0.0; + msg.orientation.z = 0.0; + msg.orientation.w = 1.0; + msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } +} - return interpMsg; +void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, ImuMsgs::Imu& msg) { + return; } -ImuMsgs::Imu ImuConverter::CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro, dai::IMUReportRotationVectorWAcc rot) { - ImuMsgs::Imu interpMsg; - interpMsg.linear_acceleration.x = accel.x; - interpMsg.linear_acceleration.y = accel.y; - interpMsg.linear_acceleration.z = accel.z; - - interpMsg.angular_velocity.x = gyro.x; - interpMsg.angular_velocity.y = gyro.y; - interpMsg.angular_velocity.z = gyro.z; - - interpMsg.orientation.x = rot.i; - interpMsg.orientation.y = rot.j; - interpMsg.orientation.z = rot.k; - interpMsg.orientation.w = rot.real; - - interpMsg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - interpMsg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov}; - interpMsg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; - - interpMsg.header.frame_id = _frameName; - interpMsg.header.seq = _sequenceNum; - _sequenceNum++; - - if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, gyro.timestamp.get()); - } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get()); - } else { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get()); - } +void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, depthai_ros_msgs::ImuWithMagneticField& msg) { + fillImuMsg(report, msg.imu); +} - return interpMsg; +void ImuConverter::fillImuMsg(dai::IMUReportGyroscope report, depthai_ros_msgs::ImuWithMagneticField& msg) { + fillImuMsg(report, msg.imu); +} + +void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, depthai_ros_msgs::ImuWithMagneticField& msg) { + fillImuMsg(report, msg.imu); +} + +void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::ImuWithMagneticField& msg) { + msg.field.magnetic_field.x = report.x; + msg.field.magnetic_field.y = report.y; + msg.field.magnetic_field.z = report.z; + msg.field.magnetic_field_covariance = {_magnetic_field_cov, 0.0, 0.0, 0.0, _magnetic_field_cov, 0.0, 0.0, 0.0, _magnetic_field_cov}; } void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs) { @@ -242,7 +89,26 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::dequepackets[i].acceleroMeter; auto gyro = inData->packets[i].gyroscope; auto rot = inData->packets[i].rotationVector; - outImuMsgs.push_back(CreateUnitMessage(accel, gyro, rot)); + auto magn = inData->packets[i].magneticField; + ImuMsgs::Imu msg; + CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + outImuMsgs.push_back(msg); + } + } +} + +void ImuConverter::toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs) { + if(_syncMode != ImuSyncMethod::COPY) { + FillImuData_LinearInterpolation(inData->packets, outImuMsgs); + } else { + for(int i = 0; i < inData->packets.size(); ++i) { + auto accel = inData->packets[i].acceleroMeter; + auto gyro = inData->packets[i].gyroscope; + auto rot = inData->packets[i].rotationVector; + auto magn = inData->packets[i].magneticField; + depthai_ros_msgs::ImuWithMagneticField msg; + CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + outImuMsgs.push_back(msg); } } } diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 02446bf8..4bb72d8a 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_descriptions VERSION 2.7.1 LANGUAGES CXX C) +project(depthai_descriptions VERSION 2.7.2 LANGUAGES CXX C) find_package(catkin REQUIRED diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index ad774769..ddd99add 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.1 + 2.7.2 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index bd9538c5..214fb9cf 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai_examples VERSION 2.7.1 LANGUAGES CXX C) +project(depthai_examples VERSION 2.7.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 28baf260..e41e0750 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.1 + 2.7.2 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 14ac771c..ae017a47 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_filters VERSION 2.7.1 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 3037be23..ba789611 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -1,7 +1,7 @@ depthai_filters - 2.7.1 + 2.7.2 The depthai_filters package Adam Serafin diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 607355cb..87f40dab 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10.2) +cmake_minimum_required(VERSION 3.16.3) project(depthai_ros_driver) if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) @@ -15,12 +15,17 @@ set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) add_compile_options(-g) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) +set(BUILD_SHARED_LIBS ON) +set(THREADS_PREFER_PTHREAD_FLAG ON) ## is used, also find other catkin packages if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) endif() - +set(CMAKE_LINKER_FLAGS "-pthread" CACHE STRING "Linker Flags" FORCE) +set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINKER_FLAGS}" CACHE STRING "" FORCE) +set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS}" CACHE STRING "" FORCE) +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS}" CACHE STRING "" FORCE) set(opencv_version 4) find_package(OpenCV ${opencv_version} QUIET COMPONENTS imgproc highgui) @@ -45,6 +50,8 @@ find_package(catkin REQUIRED COMPONENTS image_transport nodelet ) +find_package(Threads REQUIRED) + find_package(depthai CONFIG REQUIRED) generate_dynamic_reconfigure_options( @@ -63,7 +70,9 @@ include_directories( include_directories(include) catkin_package( - LIBRARIES ${PROJECT_NAME} ${SENSOR_LIB_NAME} + LIBRARIES + ${PROJECT_NAME} + ${SENSOR_LIB_NAME} ${NN_LIB_NAME} ${COMMON_LIB_NAME} CATKIN_DEPENDS roscpp sensor_msgs std_msgs camera_info_manager depthai_bridge vision_msgs cv_bridge message_filters image_transport @@ -149,7 +158,10 @@ target_link_libraries(${PROJECT_NAME} opencv_highgui ) install(TARGETS -${PROJECT_NAME} +${PROJECT_NAME} +${SENSOR_LIB_NAME} +${NN_LIB_NAME} +${COMMON_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/depthai_ros_driver/cfg/parameters.cfg b/depthai_ros_driver/cfg/parameters.cfg index 8dbdfbdf..dd663981 100755 --- a/depthai_ros_driver/cfg/parameters.cfg +++ b/depthai_ros_driver/cfg/parameters.cfg @@ -34,7 +34,7 @@ camera.add("left_i_max_q_size", int_t, 0, "Internal queue size", 30) camera.add("left_i_preview_size", int_t, 0, "Preview size", 416) camera.add("left_i_publish_topic", bool_t, 0, "Enable ROS topic", False) camera.add("left_i_resolution", str_t, 0, "Sensor resolution", "720") -camera.add("left_i_set_isp_scale", bool_t, 0, "Set ISP scale", True) +camera.add("left_i_set_isp_scale", bool_t, 0, "Set ISP scale", False) camera.add("left_i_width", int_t, 0, "Image height", 1280) camera.add("left_r_exposure", int_t, 0, "Sensor exposure", 1000, 1, 33000) camera.add("left_r_focus", int_t, 0, "Focus", 1, 0, 255) @@ -61,7 +61,7 @@ camera.add("right_i_max_q_size", int_t, 0, "Internal queue size", 30) camera.add("right_i_preview_size", int_t, 0, "Preview size", 416) camera.add("right_i_publish_topic", bool_t, 0, "Enable ROS topic", False) camera.add("right_i_resolution", str_t, 0, "Sensor resolution", "720") -camera.add("right_i_set_isp_scale", bool_t, 0, "Set ISP scale", True) +camera.add("right_i_set_isp_scale", bool_t, 0, "Set ISP scale", False) camera.add("right_i_width", int_t, 0, "Image height", 1280) camera.add("right_r_exposure", int_t, 0, "Sensor exposure", 1000, 1, 33000) camera.add("right_r_focus", int_t, 0, "Focus", 1, 0, 255) @@ -87,7 +87,7 @@ camera.add("rgb_r_keep_preview_aspect_ratio", bool_t, 0, "Keep preview aspect ra camera.add("rgb_i_low_bandwidth", bool_t, 0, "Use encoding for data", False) camera.add("rgb_i_low_bandwidth_quality", int_t, 0, "Quality when using low-bandwidth mode", 50, 1, 100) camera.add("rgb_i_max_q_size", int_t, 0, "Internal queue size", 30) -camera.add("rgb_i_preview_size", int_t, 0, "Preview size", 416) +camera.add("rgb_i_preview_size", int_t, 0, "Preview size", 300) camera.add("rgb_i_publish_topic", bool_t, 0, "Enable ROS topic", True) camera.add("rgb_i_enable_preview", bool_t, 0, "Enable ROS topic", False) camera.add("rgb_i_resolution", str_t, 0, "Sensor resolution", "1080") @@ -150,11 +150,25 @@ camera.add("stereo_i_input_width", int_t, 0, "Input image width", 1280) camera.add("stereo_i_input_height", int_t, 0, "Input image height", 720) camera.add("nn_i_nn_config_path", str_t, 0, "NN JSON Config path", "depthai_ros_driver/mobilenet") -camera.add("nn_i_disable_resize", bool_t, 0, "Disable ImageManip input", False) +camera.add("nn_i_disable_resize", bool_t, 0, "Disable ImageManip input", True) camera.add("nn_i_enable_passthrough", bool_t, 0, "Publish passthrough", False) camera.add("nn_i_enable_passthrough_depth", bool_t, 0, "Publish passthrough depth", False) camera.add("imu_i_max_q_size", int_t, 0, "Internal queue size", 30) +camera.add("imu_i_acc_freq", int_t, 0, "Accelerometer frequency", 500) +camera.add("imu_i_acc_cov", double_t, 0, "Accelerometer covariance", 0.0) +camera.add("imu_i_batch_report_threshold", int_t, 0, "Batch report size", 5) +camera.add("imu_i_enable_rotation", bool_t, 0, "Whether to enable rotation vector & magnetometer data (available when using IMU_WITH_MAGN/ IMU_WITH_MAGN_SPLIT message type)", False) +camera.add("imu_i_gyro_cov", double_t, 0, "Gyroscope covariance", 0.0) +camera.add("imu_i_gyro_freq", int_t, 0, "Gyroscope frequency", 400) +camera.add("imu_i_mag_cov", double_t, 0, "Magnetometer covariance", 0.0) +camera.add("imu_i_mag_freq", int_t, 0, "Magnetometer frequency", 100) +camera.add("imu_i_max_batch_reports", int_t, 0, "Max reports per batch", 20) +camera.add("imu_i_message_type", str_t, 0, "ROS Publisher type", "IMU") +camera.add("imu_i_rot_cov", double_t, 0, "Rotation covariance", -1.0) +camera.add("imu_i_rot_freq", int_t, 0, "Rotation frequency", 100) +camera.add("imu_i_sync_method", str_t, 0, "Imu sync method", "COPY") + exit(gen.generate(PACKAGE, "depthai_ros_driver", "parameters")) \ No newline at end of file diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 2e50a03f..e39d657a 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -20,7 +20,7 @@ left_i_preview_size: 300 left_i_publish_topic: false left_i_resolution: '720' - left_i_set_isp_scale: true + left_i_set_isp_scale: false left_i_width: 1280 left_r_exposure: 1000 left_r_focus: 1 @@ -36,7 +36,7 @@ rgb_i_enable_preview: false rgb_i_fps: 60.0 rgb_i_height: 720 - rgb_i_interleaved: true + rgb_i_interleaved: false rgb_i_keep_preview_aspect_ratio: true rgb_i_low_bandwidth: false rgb_i_low_bandwidth_quality: 50 @@ -67,7 +67,7 @@ right_i_preview_size: 300 right_i_publish_topic: false right_i_resolution: '720' - right_i_set_isp_scale: true + right_i_set_isp_scale: false right_i_width: 1280 right_r_exposure: 1000 right_r_focus: 1 diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp index fb37531d..4338bb07 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp @@ -31,7 +31,7 @@ namespace dai_nodes { class Imu : public BaseNode { public: - explicit Imu(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline); + explicit Imu(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline, std::shared_ptr device); ~Imu(); void updateParams(parametersConfig& config) override; void setupQueues(std::shared_ptr device) override; @@ -42,8 +42,10 @@ class Imu : public BaseNode { private: std::unique_ptr imuConverter; - void imuQCB(const std::string& name, const std::shared_ptr& data); - ros::Publisher imuPub; + void imuRosQCB(const std::string& name, const std::shared_ptr& data); + void imuDaiRosQCB(const std::string& name, const std::shared_ptr& data); + void imuMagQCB(const std::string& name, const std::shared_ptr& data); + ros::Publisher rosImuPub, daiImuPub, magPub; std::shared_ptr imuNode; std::unique_ptr ph; std::shared_ptr imuQ; diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp index 7cbd5df7..8654b3e4 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp @@ -5,6 +5,7 @@ #include #include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai_bridge/ImuConverter.hpp" #include "depthai_ros_driver/param_handlers/base_param_handler.hpp" #include "depthai_ros_driver/parametersConfig.h" @@ -20,12 +21,19 @@ class NodeHandle; namespace depthai_ros_driver { namespace param_handlers { +namespace imu { +enum class ImuMsgType { IMU, IMU_WITH_MAG, IMU_WITH_MAG_SPLIT }; +} class ImuParamHandler : public BaseParamHandler { public: explicit ImuParamHandler(ros::NodeHandle node, const std::string& name); ~ImuParamHandler(); - void declareParams(std::shared_ptr imu); + void declareParams(std::shared_ptr imu, const std::string& imuType); dai::CameraControl setRuntimeParams(parametersConfig& config) override; + std::unordered_map imuSyncMethodMap; + std::unordered_map imuMessagetTypeMap; + imu::ImuMsgType getMsgType(); + dai::ros::ImuSyncMethod getSyncMethod(); }; } // namespace param_handlers } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/launch/pointcloud.launch b/depthai_ros_driver/launch/pointcloud.launch index fcdc2ae3..dc6238eb 100644 --- a/depthai_ros_driver/launch/pointcloud.launch +++ b/depthai_ros_driver/launch/pointcloud.launch @@ -20,14 +20,24 @@ - + + + - + + + + + + + + + - @@ -36,25 +46,5 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index caf9410c..e6b62ea5 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.1 + 2.7.2 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -29,6 +29,8 @@ depthai depthai_bridge depthai_descriptions + depthai_ros_msgs + depthai_examples nodelet diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index 48aaa5fe..1e0af79a 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -7,16 +7,20 @@ #include "depthai/pipeline/node/XLinkOut.hpp" #include "depthai_bridge/ImuConverter.hpp" #include "depthai_ros_driver/param_handlers/imu_param_handler.hpp" +#include "depthai_ros_msgs/ImuWithMagneticField.h" #include "ros/node_handle.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/MagneticField.h" namespace depthai_ros_driver { namespace dai_nodes { -Imu::Imu(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline) { +Imu::Imu(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline, std::shared_ptr device) + : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); imuNode = pipeline->create(); ph = std::make_unique(node, daiNodeName); - ph->declareParams(imuNode); + ph->declareParams(imuNode, device->getConnectedIMU()); setXinXout(pipeline); ROS_DEBUG("Node %s created", daiNodeName.c_str()); } @@ -35,23 +39,77 @@ void Imu::setupQueues(std::shared_ptr device) { imuQ = device->getOutputQueue(imuQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = std::string(getROSNode().getNamespace()) + "_" + getName(); tfPrefix.erase(0, 1); - auto imuMode = static_cast(0); - imuConverter = std::make_unique(tfPrefix + "_frame", imuMode, 0.0, 0.0); - imuQ->addCallback(std::bind(&Imu::imuQCB, this, std::placeholders::_1, std::placeholders::_2)); - imuPub = getROSNode().advertise(getName() + "/data", 10); + auto imuMode = ph->getSyncMethod(); + imuConverter = std::make_unique(tfPrefix + "_frame", + imuMode, + ph->getParam("i_acc_cov"), + ph->getParam("i_gyro_cov"), + ph->getParam("i_rot_cov"), + ph->getParam("i_mag_cov"), + ph->getParam("i_enable_rotation")); + + param_handlers::imu::ImuMsgType msgType = ph->getMsgType(); + switch(msgType) { + case param_handlers::imu::ImuMsgType::IMU: { + rosImuPub = getROSNode().advertise(getName() + "/data", 10); + imuQ->addCallback(std::bind(&Imu::imuRosQCB, this, std::placeholders::_1, std::placeholders::_2)); + break; + } + case param_handlers::imu::ImuMsgType::IMU_WITH_MAG: { + daiImuPub = getROSNode().advertise(getName() + "/data", 10); + imuQ->addCallback(std::bind(&Imu::imuDaiRosQCB, this, std::placeholders::_1, std::placeholders::_2)); + break; + } + case param_handlers::imu::ImuMsgType::IMU_WITH_MAG_SPLIT: { + rosImuPub = getROSNode().advertise(getName() + "/data", 10); + magPub = getROSNode().advertise(getName() + "/mag", 10); + imuQ->addCallback(std::bind(&Imu::imuMagQCB, this, std::placeholders::_1, std::placeholders::_2)); + break; + } + default: { + break; + } + } } void Imu::closeQueues() { imuQ->close(); } -void Imu::imuQCB(const std::string& /*name*/, const std::shared_ptr& data) { +void Imu::imuRosQCB(const std::string& /*name*/, const std::shared_ptr& data) { auto imuData = std::dynamic_pointer_cast(data); std::deque deq; imuConverter->toRosMsg(imuData, deq); while(deq.size() > 0) { auto currMsg = deq.front(); - imuPub.publish(currMsg); + rosImuPub.publish(currMsg); + deq.pop_front(); + } +} + +void Imu::imuDaiRosQCB(const std::string& /*name*/, const std::shared_ptr& data) { + auto imuData = std::dynamic_pointer_cast(data); + std::deque deq; + imuConverter->toRosDaiMsg(imuData, deq); + while(deq.size() > 0) { + auto currMsg = deq.front(); + daiImuPub.publish(currMsg); + deq.pop_front(); + } +} + +void Imu::imuMagQCB(const std::string& /*name*/, const std::shared_ptr& data) { + auto imuData = std::dynamic_pointer_cast(data); + std::deque deq; + imuConverter->toRosDaiMsg(imuData, deq); + while(deq.size() > 0) { + auto currMsg = deq.front(); + sensor_msgs::Imu imu = currMsg.imu; + sensor_msgs::MagneticField field = currMsg.field; + imu.header = currMsg.header; + field.header = currMsg.header; + rosImuPub.publish(imu); + magPub.publish(field); deq.pop_front(); } } diff --git a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp index 03851f31..e38a1797 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -1,19 +1,45 @@ #include "depthai_ros_driver/param_handlers/imu_param_handler.hpp" #include "depthai/pipeline/node/IMU.hpp" +#include "depthai_ros_driver/utils.hpp" #include "ros/node_handle.h" namespace depthai_ros_driver { namespace param_handlers { ImuParamHandler::ImuParamHandler(ros::NodeHandle node, const std::string& name) : BaseParamHandler(node, name) {} ImuParamHandler::~ImuParamHandler() = default; -void ImuParamHandler::declareParams(std::shared_ptr imu) { - imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 400); - imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400); - // imu->enableIMUSensor(dai::IMUSensor::ROTATION_VECTOR, 400); - imu->setBatchReportThreshold(1); - imu->setMaxBatchReports(10); +void ImuParamHandler::declareParams(std::shared_ptr imu, const std::string& imuType) { + imuSyncMethodMap = { + {"COPY", dai::ros::ImuSyncMethod::COPY}, + {"LINEAR_INTERPOLATE_GYRO", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_GYRO}, + {"LINEAR_INTERPOLATE_ACCEL", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL}, + }; + imuMessagetTypeMap = { + {"IMU", imu::ImuMsgType::IMU}, {"IMU_WITH_MAG", imu::ImuMsgType::IMU_WITH_MAG}, {"IMU_WITH_MAG_SPLIT", imu::ImuMsgType::IMU_WITH_MAG_SPLIT}}; + bool rotationAvailable = imuType == "BNO086"; + if(getParam("i_enable_rotation")) { + if(rotationAvailable) { + imu->enableIMUSensor(dai::IMUSensor::ROTATION_VECTOR, getParam("i_rotation_vec_freq", 400)); + imu->enableIMUSensor(dai::IMUSensor::MAGNETOMETER_CALIBRATED, getParam("i_magnetometer_freq", 100)); + } else { + ROS_ERROR("Rotation enabled but not available with current sensor"); + setParam("i_enable_rotation", false); + } + } + imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, getParam("i_acc_freq", 500)); + imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, getParam("i_gyro_freq", 400)); + imu->setBatchReportThreshold(getParam("i_batch_report_threshold", 5)); + imu->setMaxBatchReports(getParam("i_max_batch_reports", 20)); } + +dai::ros::ImuSyncMethod ImuParamHandler::getSyncMethod() { + return utils::getValFromMap(utils::getUpperCaseStr(getParam("i_sync_method")), imuSyncMethodMap); +} + +imu::ImuMsgType ImuParamHandler::getMsgType() { + return utils::getValFromMap(utils::getUpperCaseStr(getParam("i_message_type")), imuMessagetTypeMap); +} + dai::CameraControl ImuParamHandler::setRuntimeParams(parametersConfig& /*config*/) { dai::CameraControl ctrl; return ctrl; diff --git a/depthai_ros_driver/src/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline_generator.cpp index 064ec74e..a65e4c18 100644 --- a/depthai_ros_driver/src/pipeline_generator.cpp +++ b/depthai_ros_driver/src/pipeline_generator.cpp @@ -127,7 +127,7 @@ std::vector> PipelineGenerator::createPipel } } if(enableImu) { - auto imu = std::make_unique("imu", node, pipeline); + auto imu = std::make_unique("imu", node, pipeline, device); daiNodes.push_back(std::move(imu)); } ROS_INFO("Finished setting up pipeline."); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index f2c9576d..d0338bdf 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required (VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project (depthai_ros_msgs VERSION 2.7.1) +project (depthai_ros_msgs VERSION 2.7.2) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) @@ -30,6 +30,7 @@ add_message_files ( # ImageMarkerArray.msg SpatialDetection.msg SpatialDetectionArray.msg + ImuWithMagneticField.msg ) ## Generate services in the 'srv' folder diff --git a/depthai_ros_msgs/msg/ImuWithMagneticField.msg b/depthai_ros_msgs/msg/ImuWithMagneticField.msg new file mode 100644 index 00000000..6c41b315 --- /dev/null +++ b/depthai_ros_msgs/msg/ImuWithMagneticField.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +sensor_msgs/Imu imu +sensor_msgs/MagneticField field \ No newline at end of file diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 629e9bc7..f8f0dc43 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.1 + 2.7.2 Package to keep interface independent of the driver Sachin Guruswamy