Usb port humble

README.md

@@ -126,7 +126,7 @@ As for the parameters themselves, there are a few crucial ones that decide on ho
 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` are for connecting to a specific camera. If not set, it automatically connects to the next available device.
+* `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/CHANGELOG.rst

@@ -5,6 +5,7 @@ Changelog for package depthai-ros
 2.6.3 (2023-02-10)
 ___________
 * Camera calibration updates
+* Option to connect to the device via USB port id
 
 2.6.2 (2023-02-01)
 ___________

depthai_ros_driver/config/camera.yaml

@@ -9,6 +9,7 @@
       i_mx_id: ''
       i_nn_type: spatial
       i_pipeline_type: RGBD
+      i_usb_port_id: ''
       i_usb_speed: SUPER_PLUS
     left:
       i_board_socket_id: 1

depthai_ros_driver/include/depthai_ros_driver/camera.hpp

@@ -32,5 +32,6 @@ class Camera : public rclcpp::Node {
     std::shared_ptr<dai::Pipeline> pipeline;
     std::shared_ptr<dai::Device> device;
     std::vector<std::unique_ptr<dai_nodes::BaseNode>> daiNodes;
+    bool camRunning = false;
 };
 }  // namespace depthai_ros_driver

depthai_ros_driver/src/camera.cpp

@@ -70,49 +70,60 @@ void Camera::setupQueues() {
 }
 
 void Camera::startDevice() {
-    dai::DeviceInfo info;
-    auto mxid = ph->getParam<std::string>(this, "i_mx_id");
-    auto ip = ph->getParam<std::string>(this, "i_ip");
-    if(!mxid.empty()) {
-    } else if(!ip.empty()) {
-    }
     rclcpp::Rate r(1.0);
-    bool cam_running = false;
-    std::vector<dai::DeviceInfo> availableDevices = dai::Device::getAllAvailableDevices();
-    while(!cam_running) {
+    while(!camRunning) {
+        auto mxid = ph->getParam<std::string>(this, "i_mx_id");
+        auto ip = ph->getParam<std::string>(this, "i_ip");
+        auto usb_id = ph->getParam<std::string>(this, "i_usb_port_id");
         try {
-            dai::UsbSpeed speed = ph->getUSBSpeed(this);
-            if(mxid.empty() && ip.empty()) {
+            if(mxid.empty() && ip.empty() && usb_id.empty()) {
                 RCLCPP_INFO(this->get_logger(), "No ip/mxid specified, connecting to the next available device.");
                 device = std::make_shared<dai::Device>();
+                camRunning = true;
             } else {
+                std::vector<dai::DeviceInfo> availableDevices = dai::Device::getAllAvailableDevices();
+                if(availableDevices.size() == 0) {
+                    throw std::runtime_error("No devices detected!");
+                }
+                dai::UsbSpeed speed = ph->getUSBSpeed(this);
                 for(const auto& info : availableDevices) {
                     if(!mxid.empty() && info.getMxId() == mxid) {
                         RCLCPP_INFO(this->get_logger(), "Connecting to the camera using mxid: %s", mxid.c_str());
                         if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) {
                             device = std::make_shared<dai::Device>(info, speed);
+                            camRunning = true;
                         } else if(info.state == X_LINK_BOOTED) {
-                            throw std::runtime_error("Device with mxid %s is already booted in different process.");
+                            throw std::runtime_error("Device is already booted in different process.");
                         }
                     } else if(!ip.empty() && info.name == ip) {
                         RCLCPP_INFO(this->get_logger(), "Connecting to the camera using ip: %s", ip.c_str());
                         if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) {
                             device = std::make_shared<dai::Device>(info);
+                            camRunning = true;
+                        } else if(info.state == X_LINK_BOOTED) {
+                            throw std::runtime_error("Device is already booted in different process.");
+                        }
+                    } else if(!usb_id.empty() && info.name == usb_id) {
+                        RCLCPP_INFO(this->get_logger(), "Connecting to the camera using USB ID: %s", usb_id.c_str());
+                        if(info.state == X_LINK_UNBOOTED || info.state == X_LINK_BOOTLOADER) {
+                            device = std::make_shared<dai::Device>(info);
+                            camRunning = true;
                         } else if(info.state == X_LINK_BOOTED) {
-                            throw std::runtime_error("Device with ip %s is already booted in different process.");
+                            throw std::runtime_error("Device is already booted in different process.");
                         }
+                    } else {
+                        RCLCPP_INFO(this->get_logger(), "Device info: MXID: %s, Name: %s", info.getMxId().c_str(), info.name.c_str());
+                        throw std::runtime_error("Unable to connect to the device, check if parameters match with given info.");
                     }
                 }
             }
-            cam_running = true;
         } catch(const std::runtime_error& e) {
             RCLCPP_ERROR(this->get_logger(), "%s", e.what());
         }
         r.sleep();
     }
 
-    auto deviceName = device->getMxId();
-    RCLCPP_INFO(this->get_logger(), "Camera %s connected!", deviceName.c_str());
+    RCLCPP_INFO(this->get_logger(), "Camera with MXID: %s and Name: %s connected!", device->getMxId().c_str(), device->getDeviceInfo().name.c_str());
     auto protocol = device->getDeviceInfo().getXLinkDeviceDesc().protocol;
 
     if(protocol != XLinkProtocol_t::X_LINK_TCP_IP) {

depthai_ros_driver/src/param_handlers/camera_param_handler.cpp

@@ -27,6 +27,7 @@ void CameraParamHandler::declareParams(rclcpp::Node* node) {
     declareAndLogParam<std::string>(node, "i_usb_speed", "SUPER_PLUS");
     declareAndLogParam<std::string>(node, "i_mx_id", "");
     declareAndLogParam<std::string>(node, "i_ip", "");
+    declareAndLogParam<std::string>(node, "i_usb_port_id", "");
     declareAndLogParam<int>(node, "i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200));
     declareAndLogParam<int>(node, "i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500));
 }