Sr lr update noetic (#481)

luxonis · Jan 23, 2024 · a8a692e · a8a692e
1 parent abb8948
commit a8a692e
Show file tree

Hide file tree

Showing 26 changed files with 276 additions and 75 deletions.
diff --git a/depthai_descriptions/launch/urdf.launch b/depthai_descriptions/launch/urdf.launch
@@ -15,7 +15,7 @@
     <arg name="cam_yaw"               default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
 
 
-    <param name="robot_description"
+    <param name="$(arg tf_prefix)/robot_description"
                command="$(find xacro)/xacro '$(find depthai_descriptions)/urdf/depthai_descr.urdf.xacro'
                         camera_name:=$(arg tf_prefix)
                         camera_model:=$(arg camera_model)
@@ -29,5 +29,7 @@
                         cam_yaw:=$(arg cam_yaw)"/>
 
 
-    <node name="$(arg tf_prefix)_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" required="true"/>
+    <node name="$(arg tf_prefix)_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" required="true">
+        <remap from="robot_description" to="$(arg tf_prefix)/robot_description"/>
+    </node>
 </launch>
diff --git a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro
@@ -12,35 +12,82 @@
         <xacro:property name="has_imu" value="false" />
         <xacro:property name="baseline" value="0.075" />
 
+        <xacro:if value="${model in ['OAK-D-SR']}">
+            <xacro:property name="baseline" value="0.02" />
+        </xacro:if>
+
         <xacro:if value="${model in ['OAK-D', 'OAK-D-PRO', 'OAK-D-POE']}">
             <xacro:property name="has_imu" value="true" />
         </xacro:if>
 
         <xacro:base camera_name="${camera_name}" parent="${parent}" camera_model="${camera_model}" base_frame="${base_frame}" cam_pos_x="${cam_pos_x}" cam_pos_y="${cam_pos_y}" cam_pos_z="${cam_pos_z}" cam_roll="${cam_roll}" cam_pitch="${cam_pitch}" cam_yaw="${cam_yaw}" has_imu="${has_imu}"/>
+
         <!-- RGB Camera -->
-        <link name="${camera_name}_rgb_camera_frame" />
+        <xacro:unless value="${model in ['OAK-D-SR']}">
+            <link name="${camera_name}_rgb_camera_frame" />
+
+            <joint name="${camera_name}_rgb_camera_joint" type="fixed">
+                <parent link="${base_frame}"/>
+                <child link="${camera_name}_rgb_camera_frame"/>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+            </joint>
+
+            <link name="${camera_name}_rgb_camera_optical_frame"/>
+
+            <joint name="${camera_name}_rgb_camera_optical_joint" type="fixed">
+                <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
+                <parent link="${camera_name}_rgb_camera_frame"/>
+                <child link="${camera_name}_rgb_camera_optical_frame"/>
+            </joint>
+        </xacro:unless>
+        <xacro:unless value="${model in ['OAK-D-LR']}">
+
+        <!-- Left Camera -->
+        <link name="${camera_name}_left_camera_frame" />
 
-        <joint name="${camera_name}_rgb_camera_joint" type="fixed">
+        <joint name="${camera_name}_left_camera_joint" type="fixed">
             <parent link="${base_frame}"/>
-            <child link="${camera_name}_rgb_camera_frame"/>
-            <origin xyz="0 0 0" rpy="0 0 0" />
+            <child link="${camera_name}_left_camera_frame"/>
+            <origin xyz="0 ${baseline/2} 0" rpy="0 0 0" />
         </joint>
 
-        <link name="${camera_name}_rgb_camera_optical_frame"/>
+        <link name="${camera_name}_left_camera_optical_frame"/>
 
-        <joint name="${camera_name}_rgb_camera_optical_joint" type="fixed">
+        <joint name="${camera_name}_left_camera_optical_joint" type="fixed">
             <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
-            <parent link="${camera_name}_rgb_camera_frame"/>
-            <child link="${camera_name}_rgb_camera_optical_frame"/>
+            <parent link="${camera_name}_left_camera_frame"/>
+            <child link="${camera_name}_left_camera_optical_frame"/>
         </joint>
 
-        <!-- Left Camera -->
+
+        <!-- right Camera -->
+        <link name="${camera_name}_right_camera_frame" />
+
+        <joint name="${camera_name}_right_camera_joint" type="fixed">
+            <parent link="${base_frame}"/>
+            <child link="${camera_name}_right_camera_frame"/>
+            <origin xyz="0 -${baseline/2} 0" rpy="0 0 0" />
+        </joint>
+
+        <link name="${camera_name}_right_camera_optical_frame"/>
+
+        <joint name="${camera_name}_right_camera_optical_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
+            <parent link="${camera_name}_right_camera_frame"/>
+            <child link="${camera_name}_right_camera_optical_frame"/>
+        </joint>
+
+        </xacro:unless>
+
+        <xacro:if value="${model in ['OAK-D-LR']}">
+
+        <!-- left Camera -->
         <link name="${camera_name}_left_camera_frame" />
 
         <joint name="${camera_name}_left_camera_joint" type="fixed">
             <parent link="${base_frame}"/>
             <child link="${camera_name}_left_camera_frame"/>
-            <origin xyz="0 ${baseline/2} 0" rpy="0 0 0" />
+            <origin xyz="0 0.1 0" rpy="0 0 0" />
         </joint>
 
         <link name="${camera_name}_left_camera_optical_frame"/>
@@ -51,14 +98,13 @@
             <child link="${camera_name}_left_camera_optical_frame"/>
         </joint>
 
-
-        <!-- right Camera -->
+         <!-- right Camera -->
         <link name="${camera_name}_right_camera_frame" />
 
         <joint name="${camera_name}_right_camera_joint" type="fixed">
             <parent link="${base_frame}"/>
             <child link="${camera_name}_right_camera_frame"/>
-            <origin xyz="0 -${baseline/2} 0" rpy="0 0 0" />
+            <origin xyz="0 -0.05 0" rpy="0 0 0" />
         </joint>
 
         <link name="${camera_name}_right_camera_optical_frame"/>
@@ -68,8 +114,7 @@
             <parent link="${camera_name}_right_camera_frame"/>
             <child link="${camera_name}_right_camera_optical_frame"/>
         </joint>
-
-
+    </xacro:if>
     </xacro:macro>
 
 </robot>
diff --git a/depthai_ros_driver/config/sr_rgbd.yaml b/depthai_ros_driver/config/sr_rgbd.yaml
@@ -0,0 +1,5 @@
+/oak:
+    camera_i_nn_type: none
+    camera_i_pipeline_type: 'Depth'
+    right_i_publish_topic: true
+    stereo_i_extended_disp: true
diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp
@@ -5,6 +5,7 @@
 #include <vector>
 
 #include "camera_info_manager/camera_info_manager.h"
+#include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai/device/DataQueue.hpp"
 #include "depthai/device/Device.hpp"
 #include "depthai/pipeline/Pipeline.hpp"
@@ -17,6 +18,7 @@
 #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
 #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp"
 #include "depthai_ros_driver/parametersConfig.h"
+#include "depthai_ros_driver/utils.hpp"
 #include "image_transport/camera_publisher.h"
 #include "image_transport/image_transport.h"
 #include "ros/node_handle.h"
@@ -36,12 +38,16 @@ class Detection : public BaseNode {
      * @param      node         The node
      * @param      pipeline     The pipeline
      */
-    Detection(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline) : BaseNode(daiNodeName, node, pipeline), it(node) {
+    Detection(const std::string& daiNodeName,
+              ros::NodeHandle node,
+              std::shared_ptr<dai::Pipeline> pipeline,
+              const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A)
+        : BaseNode(daiNodeName, node, pipeline), it(node) {
         ROS_DEBUG("Creating node %s", daiNodeName.c_str());
         setNames();
         detectionNode = pipeline->create<T>();
         imageManip = pipeline->create<dai::node::ImageManip>();
-        ph = std::make_unique<param_handlers::NNParamHandler>(node, daiNodeName);
+        ph = std::make_unique<param_handlers::NNParamHandler>(node, daiNodeName, socket);
         ph->declareParams(detectionNode, imageManip);
         ROS_DEBUG("Node %s created", daiNodeName.c_str());
         imageManip->out.link(detectionNode->input);
@@ -56,12 +62,13 @@ class Detection : public BaseNode {
      */
     void setupQueues(std::shared_ptr<dai::Device> device) override {
         nnQ = device->getOutputQueue(nnQName, ph->getParam<int>("i_max_q_size"), false);
-        auto tfPrefix = getTFPrefix("rgb");
+        std::string socketName = utils::getSocketName(static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id")));
+        auto tfPrefix = getTFPrefix(socketName);
         int width;
         int height;
         if(ph->getParam<bool>("i_disable_resize")) {
-            width = ph->getOtherNodeParam<int>("rgb", "i_preview_size");
-            height = ph->getOtherNodeParam<int>("rgb", "i_preview_size");
+            width = ph->getOtherNodeParam<int>(socketName, "i_preview_width");
+            height = ph->getOtherNodeParam<int>(socketName, "i_preview_height");
         } else {
             width = imageManip->initialConfig.getResizeConfig().width;
             height = imageManip->initialConfig.getResizeConfig().height;

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp
@@ -4,6 +4,7 @@
 #include <string>
 #include <vector>
 
+#include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai_ros_driver/dai_nodes/base_node.hpp"
 #include "depthai_ros_driver/parametersConfig.h"
 
@@ -25,7 +26,10 @@ namespace dai_nodes {
 
 class NNWrapper : public BaseNode {
    public:
-    explicit NNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline);
+    explicit NNWrapper(const std::string& daiNodeName,
+                       ros::NodeHandle node,
+                       std::shared_ptr<dai::Pipeline> pipeline,
+                       const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A);
     ~NNWrapper();
     void updateParams(parametersConfig& config) override;
     void setupQueues(std::shared_ptr<dai::Device> device) override;

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp
@@ -5,6 +5,7 @@
 #include <vector>
 
 #include "cv_bridge/cv_bridge.h"
+#include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai_ros_driver/dai_nodes/base_node.hpp"
 #include "depthai_ros_driver/parametersConfig.h"
 #include "image_transport/camera_publisher.h"
@@ -39,7 +40,10 @@ namespace dai_nodes {
 namespace nn {
 class Segmentation : public BaseNode {
    public:
-    Segmentation(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline);
+    Segmentation(const std::string& daiNodeName,
+                 ros::NodeHandle node,
+                 std::shared_ptr<dai::Pipeline> pipeline,
+                 const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A);
     ~Segmentation();
     void updateParams(parametersConfig& config) override;
     void setupQueues(std::shared_ptr<dai::Device> device) override;

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp
@@ -5,6 +5,7 @@
 #include <vector>
 
 #include "camera_info_manager/camera_info_manager.h"
+#include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai/device/DataQueue.hpp"
 #include "depthai/device/Device.hpp"
 #include "depthai/pipeline/Pipeline.hpp"
@@ -18,6 +19,7 @@
 #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
 #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp"
 #include "depthai_ros_driver/parametersConfig.h"
+#include "depthai_ros_driver/utils.hpp"
 #include "image_transport/camera_publisher.h"
 #include "image_transport/image_transport.h"
 #include "ros/node_handle.h"
@@ -28,13 +30,16 @@ namespace nn {
 template <typename T>
 class SpatialDetection : public BaseNode {
    public:
-    SpatialDetection(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline)
+    SpatialDetection(const std::string& daiNodeName,
+                     ros::NodeHandle node,
+                     std::shared_ptr<dai::Pipeline> pipeline,
+                     const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A)
         : BaseNode(daiNodeName, node, pipeline), it(node) {
         ROS_DEBUG("Creating node %s", daiNodeName.c_str());
         setNames();
         spatialNode = pipeline->create<T>();
         imageManip = pipeline->create<dai::node::ImageManip>();
-        ph = std::make_unique<param_handlers::NNParamHandler>(node, daiNodeName);
+        ph = std::make_unique<param_handlers::NNParamHandler>(node, daiNodeName, socket);
         ph->declareParams(spatialNode, imageManip);
         ROS_DEBUG("Node %s created", daiNodeName.c_str());
         imageManip->out.link(spatialNode->input);
@@ -46,12 +51,14 @@ class SpatialDetection : public BaseNode {
     };
     void setupQueues(std::shared_ptr<dai::Device> device) override {
         nnQ = device->getOutputQueue(nnQName, ph->getParam<int>("i_max_q_size"), false);
-        auto tfPrefix = getTFPrefix("rgb");
+        std::string socketName = utils::getSocketName(static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id")));
+
+        auto tfPrefix = getTFPrefix(socketName);
         int width;
         int height;
         if(ph->getParam<bool>("i_disable_resize")) {
-            width = ph->getOtherNodeParam<int>("rgb", "i_preview_size");
-            height = ph->getOtherNodeParam<int>("rgb", "i_preview_size");
+            width = ph->getOtherNodeParam<int>(socketName, "i_preview_width");
+            height = ph->getOtherNodeParam<int>(socketName, "i_preview_height");
         } else {
             width = imageManip->initialConfig.getResizeConfig().width;
             height = imageManip->initialConfig.getResizeConfig().height;
@@ -74,10 +81,8 @@ class SpatialDetection : public BaseNode {
         }
 
         if(ph->getParam<bool>("i_enable_passthrough_depth")) {
-            dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A;
-            bool align;
-            getROSNode().getParam("stereo_i_align_depth", align);
-            if(!align) {
+            dai::CameraBoardSocket socket = static_cast<dai::CameraBoardSocket>(ph->getOtherNodeParam<int>("stereo", "i_board_socket_id"));
+            if(!ph->getOtherNodeParam<bool>("stereo", "i_align_depth")) {
                 tfPrefix = getTFPrefix("right");
                 socket = dai::CameraBoardSocket::CAM_C;
             };

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp
@@ -4,6 +4,7 @@
 #include <string>
 #include <vector>
 
+#include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai_ros_driver/dai_nodes/base_node.hpp"
 #include "depthai_ros_driver/parametersConfig.h"
 
@@ -26,7 +27,10 @@ namespace dai_nodes {
 
 class SpatialNNWrapper : public BaseNode {
    public:
-    explicit SpatialNNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline);
+    explicit SpatialNNWrapper(const std::string& daiNodeName,
+                              ros::NodeHandle node,
+                              std::shared_ptr<dai::Pipeline> pipeline,
+                              const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A);
     ~SpatialNNWrapper();
     void updateParams(parametersConfig& config) override;
     void setupQueues(std::shared_ptr<dai::Device> device) override;

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp
@@ -52,7 +52,7 @@ class SensorWrapper : public BaseNode {
 
    private:
     void subCB(const sensor_msgs::Image::ConstPtr& img);
-    std::unique_ptr<BaseNode> sensorNode, featureTrackerNode;
+    std::unique_ptr<BaseNode> sensorNode, featureTrackerNode, nnNode;
     std::unique_ptr<param_handlers::SensorParamHandler> ph;
     std::unique_ptr<dai::ros::ImageConverter> converter;
     ros::Subscriber sub;

diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp
@@ -84,7 +84,7 @@ class Stereo : public BaseNode {
     std::shared_ptr<dai::node::VideoEncoder> stereoEnc, leftRectEnc, rightRectEnc;
     std::unique_ptr<SensorWrapper> left;
     std::unique_ptr<SensorWrapper> right;
-    std::unique_ptr<BaseNode> featureTrackerLeftR, featureTrackerRightR;
+    std::unique_ptr<BaseNode> featureTrackerLeftR, featureTrackerRightR, nnNode;
     std::unique_ptr<param_handlers::StereoParamHandler> ph;
     std::shared_ptr<dai::DataOutputQueue> stereoQ, leftRectQ, rightRectQ;
     std::shared_ptr<dai::node::XLinkOut> xoutStereo, xoutLeftRect, xoutRightRect;

diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp
@@ -6,6 +6,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai/pipeline/datatype/CameraControl.hpp"
 #include "depthai_ros_driver/param_handlers/base_param_handler.hpp"
 #include "nlohmann/json.hpp"
@@ -32,7 +33,7 @@ enum class NNFamily { Segmentation, Mobilenet, Yolo };
 }
 class NNParamHandler : public BaseParamHandler {
    public:
-    explicit NNParamHandler(ros::NodeHandle node, const std::string& name);
+    explicit NNParamHandler(ros::NodeHandle node, const std::string& name, const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A);
     ~NNParamHandler();
     nn::NNFamily getNNFamily();
     std::string getConfigPath();

diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp
@@ -25,13 +25,14 @@ class StereoParamHandler : public BaseParamHandler {
     ~StereoParamHandler();
     void declareParams(std::shared_ptr<dai::node::StereoDepth> stereo);
     dai::CameraControl setRuntimeParams(parametersConfig& config) override;
-    void updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right);
+    void updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right, dai::CameraBoardSocket& align);
 
    private:
     std::unordered_map<std::string, dai::node::StereoDepth::PresetMode> depthPresetMap;
     std::unordered_map<std::string, dai::StereoDepthConfig::CostMatching::DisparityWidth> disparityWidthMap;
     std::unordered_map<std::string, dai::StereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode> decimationModeMap;
     std::unordered_map<std::string, dai::StereoDepthConfig::PostProcessing::TemporalFilter::PersistencyMode> temporalPersistencyMap;
+    dai::CameraBoardSocket alignSocket;
 };
 }  // namespace param_handlers
 }  // namespace depthai_ros_driver