RS Mode & Sync - Humble (#578)

.github/workflows/main.workflow.yml

@@ -28,7 +28,7 @@ jobs:
       fail-fast: false
     steps:
     - uses: actions/[email protected]
-    - uses: ros-tooling/setup-ros@0.5.0
+    - uses: ros-tooling/setup-ros@0.7.8
       with:
         required-ros-distributions: ${{ env.ROS_DISTRO }}
     - uses: ros-tooling/[email protected]
@@ -50,7 +50,7 @@ jobs:
         linter: [xmllint, pep257, lint_cmake]
     steps:
     - uses: actions/[email protected]
-    - uses: ros-tooling/setup-ros@0.5.0
+    - uses: ros-tooling/setup-ros@0.7.8
       with:
         required-ros-distributions: ${{ env.ROS_DISTRO }}
     - uses: ros-tooling/[email protected]
@@ -117,4 +117,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

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

depthai_bridge/src/SpatialDetectionConverter.cpp

@@ -61,8 +61,8 @@ void SpatialDetectionConverter::toRosMsg(std::shared_ptr<dai::SpatialImgDetectio
 
         opDetectionMsg.detections[i].results[0].class_id = std::to_string(inNetData->detections[i].label);
         opDetectionMsg.detections[i].results[0].score = inNetData->detections[i].confidence;
- 
-		opDetectionMsg.detections[i].bbox.center.position.x = xCenter;
+
+        opDetectionMsg.detections[i].bbox.center.position.x = xCenter;
         opDetectionMsg.detections[i].bbox.center.position.y = yCenter;
         opDetectionMsg.detections[i].bbox.size_x = xSize;
         opDetectionMsg.detections[i].bbox.size_y = ySize;