Jazzy 2.10 (#588)

luxonis · Aug 29, 2024 · 9b59307 · 9b59307
1 parent 2e81008
commit 9b59307
Show file tree

Hide file tree

Showing 134 changed files with 5,031 additions and 1,493 deletions.
diff --git a/.github/workflows/main.workflow.yml b/.github/workflows/main.workflow.yml
@@ -30,7 +30,7 @@ jobs:
       fail-fast: false
     steps:
     - uses: actions/[email protected]
-    - uses: ros-tooling/setup-ros@0.6.2
+    - uses: ros-tooling/setup-ros@0.7.8
       with:
         required-ros-distributions: ${{ env.ROS_DISTRO }}
     - uses: ros-tooling/[email protected]
@@ -52,7 +52,7 @@ jobs:
         linter: [xmllint, pep257, lint_cmake]
     steps:
     - uses: actions/[email protected]
-    - uses: ros-tooling/setup-ros@0.6.2
+    - uses: ros-tooling/setup-ros@0.7.8
       with:
         required-ros-distributions: ${{ env.ROS_DISTRO }}
     - uses: ros-tooling/[email protected]
@@ -119,4 +119,4 @@ jobs:
           no-cache: true
           tags: | 
             luxonis/depthai-ros:${{ steps.vars.outputs.short_ref }}
-            luxonis/depthai-ros:${{ env.ROS_DISTRO }}-latest
+            luxonis/depthai-ros:${{ env.ROS_DISTRO }}-latest
diff --git a/README.md b/README.md
@@ -1,4 +1,4 @@
 # Depthai ROS Repository
 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/)
 
-You can find the newest documentation [here](https://docs-beta.luxonis.com/software/ros/depthai-ros/intro/).
+You can find the newest documentation [here](https://docs.luxonis.com/software/ros/depthai-ros/)
diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst
@@ -2,6 +2,31 @@
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.10.0 (2024-08-29)
+-------------------
+## What's Changed
+* Adding stl files for SR and LR models by @danilo-pejovic in https://github.com/luxonis/depthai-ros/pull/491
+* No imu fix Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/500
+* Tracking converter for ROS2 Humble by @daniqsilva25 in https://github.com/luxonis/depthai-ros/pull/505
+* Added Env Hooks so that depthai xacro can be used with gazebo sim by @r4hul77 in https://github.com/luxonis/depthai-ros/pull/507
+* Fix resource paths for Ignition Gazebo by @Nibanovic in https://github.com/luxonis/depthai-ros/pull/511
+* Use simulation flag to decide how to load meshes. by @destogl in https://github.com/luxonis/depthai-ros/pull/524
+* Add new launch file for starting multiple rgbd cameras on robots. by @destogl in https://github.com/luxonis/depthai-ros/pull/532
+* Missing fields in detection messages Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/574
+* Ip autodiscovery fix Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/561
+* RS Mode & Sync - Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/578
+* Compressed image publishers by @Serafadam in https://github.com/luxonis/depthai-ros/pull/580
+* ToF Support Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/581
+* WLS fix humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/582
+* Syncing & RS updates Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/586
+
+## New Contributors
+* @r4hul77 made their first contribution in https://github.com/luxonis/depthai-ros/pull/507
+* @Nibanovic made their first contribution in https://github.com/luxonis/depthai-ros/pull/511
+* @destogl made their first contribution in https://github.com/luxonis/depthai-ros/pull/524
+
+**Full Changelog**: https://github.com/luxonis/depthai-ros/compare/v2.9.0-humble...v2.10.0-humble
+
 2.9.0 (2024-01-24)
 -------------------
 

diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt
@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
 
-project(depthai-ros VERSION 2.9.0 LANGUAGES CXX C)
+project(depthai-ros VERSION 2.10.0 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 
 find_package(ament_cmake REQUIRED)
 
 
-ament_package()
+ament_package()
diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml
@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.9.0</version>
+  <version>2.10.0</version>
   <description>The depthai-ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
-  <maintainer email="sachin@luxonis.com">sachin</maintainer>
+  <maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
   <author>Sachin Guruswamy</author>
 
   <license>MIT</license>

diff --git 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.9.0 LANGUAGES CXX C)
+project(depthai_bridge VERSION 2.10.0 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -42,6 +42,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(composition_interfaces REQUIRED)
+find_package(ffmpeg_image_transport_msgs REQUIRED)
 
 set(dependencies
   ament_index_cpp
@@ -58,6 +59,7 @@ set(dependencies
   tf2_geometry_msgs
   tf2
   composition_interfaces
+  ffmpeg_image_transport_msgs
 )
 
 include_directories(
@@ -74,6 +76,8 @@ file(GLOB LIB_SRC
 "src/ImuConverter.cpp"
 "src/TFPublisher.cpp"
 "src/TrackedFeaturesConverter.cpp"
+"src/TrackDetectionConverter.cpp"
+"src/TrackSpatialDetectionConverter.cpp"
 )
 
 add_library(${PROJECT_NAME} SHARED ${LIB_SRC})

diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp
@@ -10,9 +10,12 @@
 #include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai-shared/common/Point2f.hpp"
 #include "depthai/device/CalibrationHandler.hpp"
+#include "depthai/pipeline/datatype/EncodedFrame.hpp"
 #include "depthai/pipeline/datatype/ImgFrame.hpp"
+#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
 #include "rclcpp/time.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "std_msgs/msg/header.hpp"
 
@@ -22,7 +25,10 @@ namespace ros {
 
 namespace StdMsgs = std_msgs::msg;
 namespace ImageMsgs = sensor_msgs::msg;
+namespace FFMPEGMsgs = ffmpeg_image_transport_msgs::msg;
 using ImagePtr = ImageMsgs::Image::SharedPtr;
+using FFMPEGImagePtr = FFMPEGMsgs::FFMPEGPacket::SharedPtr;
+using CompImagePtr = ImageMsgs::CompressedImage::SharedPtr;
 
 using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>;
 
@@ -47,7 +53,7 @@ class ImageConverter {
      * @param update: bool whether to automatically update the ROS base time on message conversion
      */
     void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
-        _updateRosBaseTimeOnToRosMsg = update;
+        updateRosBaseTimeOnToRosMsg = update;
     }
 
     /**
@@ -80,10 +86,20 @@ class ImageConverter {
      */
     void setAlphaScaling(double alphaScalingFactor = 0.0);
 
+    /**
+     * @brief Sets the encoding of the image when converting to FFMPEG message. Default is libx264.
+     * @param encoding: The encoding to be used.
+     */
+    void setFFMPEGEncoding(const std::string& encoding);
+
     void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs);
     ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info = sensor_msgs::msg::CameraInfo());
     ImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);
 
+    FFMPEGMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData);
+
+    ImageMsgs::CompressedImage toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData);
+
     void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData);
 
     /** TODO(sachin): Add support for ros msg to cv mat since we have some
@@ -99,36 +115,37 @@ class ImageConverter {
                                                   Point2f bottomRightPixelId = Point2f());
 
    private:
+    void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
+    void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
     static std::unordered_map<dai::RawImgFrame::Type, std::string> encodingEnumMap;
     static std::unordered_map<dai::RawImgFrame::Type, std::string> planarEncodingEnumMap;
 
     // dai::RawImgFrame::Type _srcType;
-    bool _daiInterleaved;
+    bool daiInterleaved;
     // bool c
-    const std::string _frameName = "";
-    void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
-    void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
-    std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
+    const std::string frameName = "";
+    std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime;
 
-    rclcpp::Time _rosBaseTime;
-    bool _getBaseDeviceTimestamp;
+    rclcpp::Time rosBaseTime;
+    bool getBaseDeviceTimestamp;
     // For handling ROS time shifts and debugging
-    int64_t _totalNsChange{0};
+    int64_t totalNsChange{0};
     // Whether to update the ROS base time on each message conversion
-    bool _updateRosBaseTimeOnToRosMsg{false};
-    dai::RawImgFrame::Type _srcType;
-    bool _fromBitstream = false;
-    bool _convertDispToDepth = false;
-    bool _addExpOffset = false;
-    bool _alphaScalingEnabled = false;
-    dai::CameraExposureOffset _expOffset;
-    bool _reverseStereoSocketOrder = false;
-    double _baseline;
-    double _alphaScalingFactor = 0.0;
+    bool updateRosBaseTimeOnToRosMsg{false};
+    dai::RawImgFrame::Type srcType;
+    bool fromBitstream = false;
+    bool dispToDepth = false;
+    bool addExpOffset = false;
+    bool alphaScalingEnabled = false;
+    dai::CameraExposureOffset expOffset;
+    bool reversedStereoSocketOrder = false;
+    double baseline;
+    double alphaScalingFactor = 0.0;
+    int camHeight = -1;
+    int camWidth = -1;
+    std::string ffmpegEncoding = "libx264";
 };
 
 }  // namespace ros
 
-namespace rosBridge = ros;
-
 }  // namespace dai
diff --git a/depthai_bridge/include/depthai_bridge/TFPublisher.hpp b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp
@@ -4,15 +4,19 @@
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "nlohmann/json.hpp"
-#include "rclcpp/node.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/parameter_client.hpp"
 #include "tf2_ros/static_transform_broadcaster.h"
 
+namespace rclcpp {
+class Node;
+}  // namespace rclcpp
+
 namespace dai {
 namespace ros {
 class TFPublisher {
    public:
-    explicit TFPublisher(rclcpp::Node* node,
+    explicit TFPublisher(std::shared_ptr<rclcpp::Node> node,
                          const dai::CalibrationHandler& calHandler,
                          const std::vector<dai::CameraFeatures>& camFeatures,
                          const std::string& camName,
@@ -27,7 +31,8 @@ class TFPublisher {
                          const std::string& camYaw,
                          const std::string& imuFromDescr,
                          const std::string& customURDFLocation,
-                         const std::string& customXacroArgs);
+                         const std::string& customXacroArgs,
+                         const bool rsCompatibilityMode);
     /**
      * @brief Obtain URDF description by running Xacro with provided arguments.
      */
@@ -53,43 +58,50 @@ class TFPublisher {
      * Frame names are based on socket names and use following convention: [base_frame]_[socket_name]_camera_frame and
      * [base_frame]_[socket_name]_camera_optical_frame
      */
-    void publishCamTransforms(nlohmann::json camData, rclcpp::Node* node);
+    void publishCamTransforms(nlohmann::json camData, std::shared_ptr<rclcpp::Node> node);
     /**
      * @brief Publish IMU transform based on calibration data.
      * Frame name is based on IMU name and uses following convention: [base_frame]_imu_frame.
      * If IMU extrinsics are not set, warning is printed out and imu frame is published with zero translation and rotation.
      */
-    void publishImuTransform(nlohmann::json json, rclcpp::Node* node);
+    void publishImuTransform(nlohmann::json json, std::shared_ptr<rclcpp::Node> node);
     /**
      * @brief Check if model STL file is available in depthai_descriptions package.
      */
     bool modelNameAvailable();
     std::string getCamSocketName(int socketNum);
-    std::unique_ptr<rclcpp::AsyncParametersClient> _paramClient;
-    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tfPub;
-    std::string _camName;
-    std::string _camModel;
-    std::string _baseFrame;
-    std::string _parentFrame;
-    std::string _camPosX;
-    std::string _camPosY;
-    std::string _camPosZ;
-    std::string _camRoll;
-    std::string _camPitch;
-    std::string _camYaw;
-    std::string _imuFromDescr;
-    std::string _customURDFLocation;
-    std::string _customXacroArgs;
-    std::vector<dai::CameraFeatures> _camFeatures;
-    rclcpp::Logger _logger;
-    const std::unordered_map<dai::CameraBoardSocket, std::string> _socketNameMap = {
+    std::unique_ptr<rclcpp::AsyncParametersClient> paramClient;
+    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfPub;
+    std::string camName;
+    std::string camModel;
+    std::string baseFrame;
+    std::string parentFrame;
+    std::string camPosX;
+    std::string camPosY;
+    std::string camPosZ;
+    std::string camRoll;
+    std::string camPitch;
+    std::string camYaw;
+    std::string imuFromDescr;
+    std::string customURDFLocation;
+    std::string customXacroArgs;
+    std::vector<dai::CameraFeatures> camFeatures;
+    bool rsCompatibilityMode;
+    rclcpp::Logger logger;
+    const std::unordered_map<dai::CameraBoardSocket, std::string> socketNameMap = {
         {dai::CameraBoardSocket::AUTO, "rgb"},
         {dai::CameraBoardSocket::CAM_A, "rgb"},
         {dai::CameraBoardSocket::CAM_B, "left"},
         {dai::CameraBoardSocket::CAM_C, "right"},
         {dai::CameraBoardSocket::CAM_D, "left_back"},
         {dai::CameraBoardSocket::CAM_E, "right_back"},
     };
+    const std::unordered_map<dai::CameraBoardSocket, std::string> rsSocketNameMap = {
+        {dai::CameraBoardSocket::AUTO, "color"},
+        {dai::CameraBoardSocket::CAM_A, "color"},
+        {dai::CameraBoardSocket::CAM_B, "infra2"},
+        {dai::CameraBoardSocket::CAM_C, "infra1"},
+    };
 };
 }  // namespace ros
-}  // namespace dai
+}  // namespace dai