update sensor names & readme

luxonis · Sep 12, 2023 · 5335054 · 5335054
1 parent dbcadce
commit 5335054
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 11 deletions.
diff --git a/README.md b/README.md
@@ -406,6 +406,7 @@ For easier development inside isolated workspace, one can use Visual Studio Code
 - Open workspace directory in VSCode
 
 ### List of parameters:
+```yaml
 /oak:
   ros__parameters:
     camera:
@@ -611,4 +612,5 @@ For easier development inside isolated workspace, one can use Visual Studio Code
       i_subpixel: false
       i_update_ros_base_time_on_ros_msg: false
       i_width: 1280
-    use_sim_time: false
+    use_sim_time: false
+```
diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp
@@ -13,6 +13,7 @@
 #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
 #include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp"
 #include "depthai_ros_driver/param_handlers/stereo_param_handler.hpp"
+#include "depthai_ros_driver/utils.hpp"
 #include "image_transport/camera_publisher.hpp"
 #include "image_transport/image_transport.hpp"
 #include "rclcpp/node.hpp"
@@ -40,9 +41,13 @@ Stereo::Stereo(const std::string& daiNodeName,
             continue;
         }
     }
+    RCLCPP_DEBUG(node->get_logger(),
+                 "Creating stereo node with left sensor %s and right sensor %s",
+                 utils::getSocketName(leftSensInfo.socket).c_str(),
+                 utils::getSocketName(rightSensInfo.socket).c_str());
+    left = std::make_unique<SensorWrapper>(utils::getSocketName(leftSensInfo.socket), node, pipeline, device, leftSensInfo.socket, false);
+    right = std::make_unique<SensorWrapper>(utils::getSocketName(rightSensInfo.socket), node, pipeline, device, rightSensInfo.socket, false);
     stereoCamNode = pipeline->create<dai::node::StereoDepth>();
-    left = std::make_unique<SensorWrapper>(leftSensInfo.name, node, pipeline, device, leftSensInfo.socket, false);
-    right = std::make_unique<SensorWrapper>(rightSensInfo.name, node, pipeline, device, rightSensInfo.socket, false);
     ph->declareParams(stereoCamNode);
     setXinXout(pipeline);
     left->link(stereoCamNode->left);
@@ -118,7 +123,8 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
                             rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub,
                             image_transport::CameraPublisher& pubIT,
                             bool isLeft) {
-    auto tfPrefix = getTFPrefix(sensorInfo.name);
+    auto sensorName = getTFPrefix(utils::getSocketName(sensorInfo.socket));
+    auto tfPrefix = getTFPrefix(sensorName);
     conv = std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false, ph->getParam<bool>("i_get_base_device_timestamp"));
     conv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
     bool lowBandwidth = ph->getParam<bool>(isLeft ? "i_left_rect_low_bandwidth" : "i_right_rect_low_bandwidth");
@@ -131,16 +137,16 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
         conv->addExposureOffset(offset);
     }
     im = std::make_shared<camera_info_manager::CameraInfoManager>(
-        getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorInfo.name).get(), "/rect");
+        getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorName).get(), "/rect");
     if(ph->getParam<bool>("i_reverse_stereo_socket_order")) {
         conv->reverseStereoSocketOrder();
     }
     auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(),
                                              *conv,
                                              device,
                                              sensorInfo.socket,
-                                             ph->getOtherNodeParam<int>(sensorInfo.name, "i_width"),
-                                             ph->getOtherNodeParam<int>(sensorInfo.name, "i_height"));
+                                             ph->getOtherNodeParam<int>(sensorName, "i_width"),
+                                             ph->getOtherNodeParam<int>(sensorName, "i_height"));
     for(auto& d : info.d) {
         d = 0.0;
     }
@@ -152,13 +158,13 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
 
     im->setCameraInfo(info);
 
-    q = device->getOutputQueue(queueName, ph->getOtherNodeParam<int>(sensorInfo.name, "i_max_q_size"), false);
+    q = device->getOutputQueue(queueName, ph->getOtherNodeParam<int>(sensorName, "i_max_q_size"), false);
 
     // if publish synced pair is set to true then we skip individual publishing of left and right rectified frames
     bool addCallback = !ph->getParam<bool>("i_publish_synced_rect_pair");
 
     if(ipcEnabled()) {
-        pub = getROSNode()->create_publisher<sensor_msgs::msg::Image>("~/" + sensorInfo.name + "/image_rect", 10);
+        pub = getROSNode()->create_publisher<sensor_msgs::msg::Image>("~/" + sensorName + "/image_rect", 10);
         infoPub = getROSNode()->create_publisher<sensor_msgs::msg::CameraInfo>("~/" + getName() + "/camera_info", 10);
         if(addCallback) {
             q->addCallback(std::bind(sensor_helpers::splitPub,
@@ -171,7 +177,7 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
                                      ph->getParam<bool>("i_enable_lazy_publisher")));
         }
     } else {
-        pubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + sensorInfo.name + "/image_rect");
+        pubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + sensorName + "/image_rect");
         if(addCallback) {
             q->addCallback(std::bind(
                 sensor_helpers::cameraPub, std::placeholders::_1, std::placeholders::_2, *conv, pubIT, im, ph->getParam<bool>("i_enable_lazy_publisher")));
@@ -192,7 +198,7 @@ void Stereo::setupStereoQueue(std::shared_ptr<dai::Device> device) {
     if(ph->getParam<bool>("i_align_depth")) {
         tfPrefix = getTFPrefix(ph->getParam<std::string>("i_socket_name"));
     } else {
-        tfPrefix = getTFPrefix(rightSensInfo.name);
+        tfPrefix = getTFPrefix(utils::getSocketName(rightSensInfo.socket).c_str());
     }
     stereoConv = std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false, ph->getParam<bool>("i_get_base_device_timestamp"));
     stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));