From f73904fdcbf385d700ac08c1cc6a435aefcdd00e Mon Sep 17 00:00:00 2001 From: saakgun Date: Thu, 13 Oct 2022 22:36:58 -0400 Subject: [PATCH 01/12] Stereo inertial publisher node now supports a way to connect to individual cameras with IP. In this way, we can connect to multiple cameras easier by specifying their IP adress. Device manager under depthai-python was used to assign static IP addresses to each camera --- .../launch/multi_stereo_inertial_node.launch | 220 ++++++++++++++++++ .../ros1_src/stereo_inertial_publisher.cpp | 64 +++-- 2 files changed, 263 insertions(+), 21 deletions(-) create mode 100644 depthai_examples/launch/multi_stereo_inertial_node.launch diff --git a/depthai_examples/launch/multi_stereo_inertial_node.launch b/depthai_examples/launch/multi_stereo_inertial_node.launch new file mode 100644 index 00000000..4357354a --- /dev/null +++ b/depthai_examples/launch/multi_stereo_inertial_node.launch @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 730916d9..92b5a43e 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -275,17 +275,19 @@ int main(int argc, char** argv) { ros::init(argc, argv, "stereo_inertial_node"); ros::NodeHandle pnh("~"); - std::string tfPrefix, mode, mxId, resourceBaseFolder, nnPath; + std::string tfPrefix, mode, mxId, resourceBaseFolder, nnPath, ipAddress; std::string monoResolution = "720p", rgbResolution = "1080p"; int badParams = 0, stereo_fps, confidence, LRchecktresh, imuModeParam, detectionClassesCount, expTime, sensIso; int rgbScaleNumerator, rgbScaleDinominator, previewWidth, previewHeight; bool lrcheck, extended, subpixel, enableDepth, rectify, depth_aligned, manualExposure; bool enableSpatialDetection, enableDotProjector, enableFloodLight; - bool usb2Mode, poeMode, syncNN; + bool usb2Mode, poeMode, syncNN, useWithIP; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; std::string nnName(BLOB_NAME); // Set your blob name for the model here + badParams += !pnh.getParam("useWithIP", useWithIP); + badParams += !pnh.getParam("ipAddress", ipAddress); badParams += !pnh.getParam("mxId", mxId); badParams += !pnh.getParam("usb2Mode", usb2Mode); badParams += !pnh.getParam("poeMode", poeMode); @@ -374,27 +376,47 @@ int main(int argc, char** argv) { nnPath); std::shared_ptr device; - std::vector availableDevices = dai::Device::getAllAvailableDevices(); - - std::cout << "Listing available devices..." << std::endl; - for(auto deviceInfo : availableDevices) { - std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - if(deviceInfo.getMxId() == mxId) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - if(poeMode) { - device = std::make_shared(pipeline, deviceInfo); - } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); + if (useWithIP) // Connecting to a camera with specific IP + { + auto deviceInfo = dai::DeviceInfo(ipAddress); + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + isDeviceFound = true; + std::cout << "Device found with IP Address: " << ipAddress << std::endl; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + } + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with ipAddress \"" + ipAddress + + "\" is already booted on different process. \""); + } else { + std::cout << "Device could NOT found with IP Address: " << ipAddress << std::endl; + } + } + else // List all available devices and connect one of them automatically + { + std::vector availableDevices = dai::Device::getAllAvailableDevices(); + std::cout << "Listing available devices..." << std::endl; + for(auto deviceInfo : availableDevices) { + std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; + if(deviceInfo.getMxId() == mxId) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + } + break; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); } - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); + } else if(mxId.empty()) { + isDeviceFound = true; + device = std::make_shared(pipeline); } - } else if(mxId.empty()) { - isDeviceFound = true; - device = std::make_shared(pipeline); } } From 20a584e1da10cd0766c035d7603334847445e4a0 Mon Sep 17 00:00:00 2001 From: saakgun Date: Thu, 3 Nov 2022 23:26:41 -0400 Subject: [PATCH 02/12] Added useWithMxId option as well an option to list all devices and connect one randomly. Also checked different possibilities to cover all conditional branches and added necessary error prints --- .../ros1_src/stereo_inertial_publisher.cpp | 59 ++++++++++--------- 1 file changed, 32 insertions(+), 27 deletions(-) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 92b5a43e..64559d51 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -281,13 +281,14 @@ int main(int argc, char** argv) { int rgbScaleNumerator, rgbScaleDinominator, previewWidth, previewHeight; bool lrcheck, extended, subpixel, enableDepth, rectify, depth_aligned, manualExposure; bool enableSpatialDetection, enableDotProjector, enableFloodLight; - bool usb2Mode, poeMode, syncNN, useWithIP; + bool usb2Mode, poeMode, syncNN, useWithIP, useWithMxId; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; std::string nnName(BLOB_NAME); // Set your blob name for the model here badParams += !pnh.getParam("useWithIP", useWithIP); badParams += !pnh.getParam("ipAddress", ipAddress); + badParams += !pnh.getParam("useWithMxId", useWithMxId); badParams += !pnh.getParam("mxId", mxId); badParams += !pnh.getParam("usb2Mode", usb2Mode); badParams += !pnh.getParam("poeMode", poeMode); @@ -354,7 +355,6 @@ int main(int argc, char** argv) { dai::Pipeline pipeline; int width, height; - bool isDeviceFound = false; std::tie(pipeline, width, height) = createPipeline(enableDepth, enableSpatialDetection, lrcheck, @@ -376,11 +376,10 @@ int main(int argc, char** argv) { nnPath); std::shared_ptr device; - if (useWithIP) // Connecting to a camera with specific IP + if(useWithIP) // Connecting to a camera with specific IP, requires static IP config beforehand { auto deviceInfo = dai::DeviceInfo(ipAddress); if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - isDeviceFound = true; std::cout << "Device found with IP Address: " << ipAddress << std::endl; if(poeMode) { device = std::make_shared(pipeline, deviceInfo); @@ -391,37 +390,43 @@ int main(int argc, char** argv) { throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with ipAddress \"" + ipAddress + "\" is already booted on different process. \""); } else { - std::cout << "Device could NOT found with IP Address: " << ipAddress << std::endl; + throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); } - } - else // List all available devices and connect one of them automatically + } + else if(useWithMxId) // Connecting to a camera with unique MxID + { + auto deviceInfo = dai::DeviceInfo(mxId); + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + std::cout << "Connecting to device with MxID : " << mxId << std::endl; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + } + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); + } + } + else if (!useWithIP && !useWithMxId) // List all available devices and connect one of them automatically { std::vector availableDevices = dai::Device::getAllAvailableDevices(); std::cout << "Listing available devices..." << std::endl; for(auto deviceInfo : availableDevices) { std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - if(deviceInfo.getMxId() == mxId) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - if(poeMode) { - device = std::make_shared(pipeline, deviceInfo); - } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - } - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } - } else if(mxId.empty()) { - isDeviceFound = true; - device = std::make_shared(pipeline); - } + device = std::make_shared(pipeline); + } + if (availableDevices.empty()) + { + throw std::runtime_error("Could NOT find any available device!"); } } - - if(!isDeviceFound) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" not found. \""); + else + { + throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" + ") or connect with IP or connect with MxId"); } if(!poeMode) { From e57960f51ae3fa52748b8616be2a998e9437e6f6 Mon Sep 17 00:00:00 2001 From: saakgun Date: Tue, 8 Nov 2022 10:43:30 -0500 Subject: [PATCH 03/12] useWithIP, useWithMxId, and none of them (connect automatically, default current method) options added to ROS2 --- .../launch/stereo_inertial_node.launch.py | 32 ++++++-- .../ros2_src/stereo_inertial_publisher.cpp | 81 +++++++++++++------ 2 files changed, 84 insertions(+), 29 deletions(-) diff --git a/depthai_examples/launch/stereo_inertial_node.launch.py b/depthai_examples/launch/stereo_inertial_node.launch.py index 3ddfc2ad..af04c680 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch.py +++ b/depthai_examples/launch/stereo_inertial_node.launch.py @@ -21,7 +21,9 @@ def generate_launch_description(): 'rviz', 'stereoInertial.rviz') default_resources_path = os.path.join(depthai_examples_path, 'resources') - + useWithIP = LaunchConfiguration('useWithIP', default = False) + ipAddress = LaunchConfiguration('ipAddress', default = 'x') + useWithMxId = LaunchConfiguration('useWithMxId', default = False) mxId = LaunchConfiguration('mxId', default = 'x') usb2Mode = LaunchConfiguration('usb2Mode', default = False) poeMode = LaunchConfiguration('poeMode', default = False) @@ -78,10 +80,25 @@ def generate_launch_description(): enableRviz = LaunchConfiguration('enableRviz', default = True) + declare_useWithIP_cmd = DeclareLaunchArgument( + 'useWithIP', + default_value=useWithIP, + description='Set this to true to enable connecting the device by specifying IP address.') + + declare_ipAddress_cmd = DeclareLaunchArgument( + 'ipAddress', + default_value=ipAddress, + description='select the device by passing the IP address of the device. It will give error if left empty and useWithIPAddress is enabled.') + + declare_useWithMxId_cmd = DeclareLaunchArgument( + 'useWithMxId', + default_value=useWithMxId, + description='Set this to true to enable connecting the device by specifying MxID.') + declare_mxId_cmd = DeclareLaunchArgument( 'mxId', default_value=mxId, - description='select the device by passing the MxID of the device. It will connect to first available device if left empty.') + description='select the device by passing the MxID of the device.') declare_usb2Mode_cmd = DeclareLaunchArgument( 'usb2Mode', @@ -318,7 +335,10 @@ def generate_launch_description(): stereo_node = launch_ros.actions.Node( package='depthai_examples', executable='stereo_inertial_node', output='screen', - parameters=[{'mxId': mxId}, + parameters=[{'useWithIP': useWithIP}, + {'ipAddress': ipAddress}, + {'useWithMxId': useWithMxId}, + {'mxId': mxId}, {'usb2Mode': usb2Mode}, {'poeMode': poeMode}, {'resourceBaseFolder': resourceBaseFolder}, @@ -422,6 +442,9 @@ def generate_launch_description(): ld = LaunchDescription() + ld.add_action(declare_useWithIP_cmd) + ld.add_action(declare_ipAddress_cmd) + ld.add_action(declare_useWithMxId_cmd) ld.add_action(declare_mxId_cmd) ld.add_action(declare_usb2Mode_cmd) ld.add_action(declare_poeMode_cmd) @@ -485,5 +508,4 @@ def generate_launch_description(): # ld.add_action(point_cloud_node) if LaunchConfigurationEquals('enableRviz', 'True') and rviz_node is not None: ld.add_action(rviz_node) - return ld - + return ld \ No newline at end of file diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index 26d85f4a..1ea790b0 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -274,17 +274,20 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("stereo_inertial_node"); - std::string tfPrefix, mode, mxId, resourceBaseFolder, nnPath; + std::string tfPrefix, mode, mxId, resourceBaseFolder, nnPath, ipAddress; std::string monoResolution = "720p", rgbResolution = "1080p"; int badParams = 0, stereo_fps, confidence, LRchecktresh, imuModeParam, detectionClassesCount, expTime, sensIso; int rgbScaleNumerator, rgbScaleDinominator, previewWidth, previewHeight; bool lrcheck, extended, subpixel, enableDepth, rectify, depth_aligned, manualExposure; bool enableSpatialDetection, enableDotProjector, enableFloodLight; - bool usb2Mode, poeMode, syncNN; + bool usb2Mode, poeMode, syncNN, useWithIP, useWithMxId;; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; std::string nnName(BLOB_NAME); // Set your blob name for the model here + node->declare_parameter("useWithIP", false); + node->declare_parameter("ipAddress", ""); + node->declare_parameter("useWithMxId", false); node->declare_parameter("mxId", ""); node->declare_parameter("usb2Mode", false); node->declare_parameter("poeMode", false); @@ -329,6 +332,9 @@ int main(int argc, char** argv) { // updating parameters if defined in launch file. + node->get_parameter("useWithIP", useWithIP); + node->get_parameter("ipAddress", ipAddress); + node->get_parameter("useWithMxId", useWithMxId); node->get_parameter("mxId", mxId); node->get_parameter("usb2Mode", usb2Mode); node->get_parameter("poeMode", poeMode); @@ -413,32 +419,59 @@ int main(int argc, char** argv) { nnPath); std::shared_ptr device; - std::vector availableDevices = dai::Device::getAllAvailableDevices(); - - std::cout << "Listing available devices..." << std::endl; - for(auto deviceInfo : availableDevices) { - std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - if(deviceInfo.getMxId() == mxId) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - if(poeMode) { - device = std::make_shared(pipeline, deviceInfo); - } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - } - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("\" DepthAI Device with MxId \"" + mxId + "\" is already booted on different process. \""); + if(useWithIP) // Connecting to a camera with specific IP, requires static IP config beforehand + { + auto deviceInfo = dai::DeviceInfo(ipAddress); + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + std::cout << "Device found with IP Address: " << ipAddress << std::endl; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + } + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("DepthAI Device with ipAddress \"" + ipAddress + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); + } + } + else if(useWithMxId) // Connecting to a camera with unique MxID + { + auto deviceInfo = dai::DeviceInfo(mxId); + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + std::cout << "Connecting to device with MxID : " << mxId << std::endl; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); } - } else if(mxId == "x") { - isDeviceFound = true; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); + } + } + else if (!useWithIP && !useWithMxId) // List all available devices and connect one of them automatically + { + std::vector availableDevices = dai::Device::getAllAvailableDevices(); + std::cout << "Listing available devices..." << std::endl; + for(auto deviceInfo : availableDevices) { + std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; device = std::make_shared(pipeline); } + if (availableDevices.empty()) + { + throw std::runtime_error("Could NOT find any available device!"); + } } - if(!isDeviceFound) { - throw std::runtime_error("\" DepthAI Device with MxId \"" + mxId + "\" not found. \""); + else + { + throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" + ") or connect with IP or connect with MxId"); } - + if(!poeMode) { std::cout << "Device USB status: " << usbStrings[static_cast(device->getUsbSpeed())] << std::endl; } @@ -676,4 +709,4 @@ int main(int argc, char** argv) { } return 0; -} +} \ No newline at end of file From 972091c4a30dbc13031f6b0633761b5dcdcb7396 Mon Sep 17 00:00:00 2001 From: saakgun Date: Wed, 9 Nov 2022 11:55:03 -0500 Subject: [PATCH 04/12] Stereo inertial node launch file updated to accommodate new parameters and tested in noetic. Multi node now is calling stereo inertial node multiple times to give an idea to users how they can use multiple cameras --- .../launch/multi_stereo_inertial_node.launch | 231 ++---------------- .../launch/stereo_inertial_node.launch | 6 + 2 files changed, 30 insertions(+), 207 deletions(-) diff --git a/depthai_examples/launch/multi_stereo_inertial_node.launch b/depthai_examples/launch/multi_stereo_inertial_node.launch index 4357354a..4e4ce1c3 100644 --- a/depthai_examples/launch/multi_stereo_inertial_node.launch +++ b/depthai_examples/launch/multi_stereo_inertial_node.launch @@ -3,218 +3,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + \ No newline at end of file diff --git a/depthai_examples/launch/stereo_inertial_node.launch b/depthai_examples/launch/stereo_inertial_node.launch index f2515cc0..abc42a0d 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch +++ b/depthai_examples/launch/stereo_inertial_node.launch @@ -1,6 +1,9 @@ + + + @@ -73,6 +76,9 @@ + + + From 2cfcb1ea5123b312ce373b3ae60bebe76642b4c9 Mon Sep 17 00:00:00 2001 From: saakgun Date: Fri, 11 Nov 2022 17:31:57 -0500 Subject: [PATCH 05/12] if condition is fixed to include any_state which corresponds to deviceInfo.state = 0 --- depthai_examples/ros1_src/stereo_inertial_publisher.cpp | 4 ++-- depthai_examples/ros2_src/stereo_inertial_publisher.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 64559d51..893b664b 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -379,7 +379,7 @@ int main(int argc, char** argv) { if(useWithIP) // Connecting to a camera with specific IP, requires static IP config beforehand { auto deviceInfo = dai::DeviceInfo(ipAddress); - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { std::cout << "Device found with IP Address: " << ipAddress << std::endl; if(poeMode) { device = std::make_shared(pipeline, deviceInfo); @@ -396,7 +396,7 @@ int main(int argc, char** argv) { else if(useWithMxId) // Connecting to a camera with unique MxID { auto deviceInfo = dai::DeviceInfo(mxId); - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { std::cout << "Connecting to device with MxID : " << mxId << std::endl; if(poeMode) { device = std::make_shared(pipeline, deviceInfo); diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index 1ea790b0..15fa29f2 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -422,7 +422,7 @@ int main(int argc, char** argv) { if(useWithIP) // Connecting to a camera with specific IP, requires static IP config beforehand { auto deviceInfo = dai::DeviceInfo(ipAddress); - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { std::cout << "Device found with IP Address: " << ipAddress << std::endl; if(poeMode) { device = std::make_shared(pipeline, deviceInfo); @@ -439,7 +439,7 @@ int main(int argc, char** argv) { else if(useWithMxId) // Connecting to a camera with unique MxID { auto deviceInfo = dai::DeviceInfo(mxId); - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { std::cout << "Connecting to device with MxID : " << mxId << std::endl; if(poeMode) { device = std::make_shared(pipeline, deviceInfo); From 4e9eea54500b8fafd6133f2a2b7afd5471624500 Mon Sep 17 00:00:00 2001 From: saakgun Date: Mon, 14 Nov 2022 22:32:44 -0500 Subject: [PATCH 06/12] Unnecessary condition under useWithIP conditinal branch removed. --- depthai_examples/ros1_src/stereo_inertial_publisher.cpp | 6 +----- depthai_examples/ros2_src/stereo_inertial_publisher.cpp | 6 +----- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 893b664b..23c0760a 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -381,11 +381,7 @@ int main(int argc, char** argv) { auto deviceInfo = dai::DeviceInfo(ipAddress); if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { std::cout << "Device found with IP Address: " << ipAddress << std::endl; - if(poeMode) { - device = std::make_shared(pipeline, deviceInfo); - } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - } + device = std::make_shared(pipeline, deviceInfo); } else if(deviceInfo.state == X_LINK_BOOTED) { throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with ipAddress \"" + ipAddress + "\" is already booted on different process. \""); diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index 15fa29f2..7c050d78 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -424,11 +424,7 @@ int main(int argc, char** argv) { auto deviceInfo = dai::DeviceInfo(ipAddress); if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { std::cout << "Device found with IP Address: " << ipAddress << std::endl; - if(poeMode) { - device = std::make_shared(pipeline, deviceInfo); - } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - } + device = std::make_shared(pipeline, deviceInfo); } else if(deviceInfo.state == X_LINK_BOOTED) { throw std::runtime_error("DepthAI Device with ipAddress \"" + ipAddress + "\" is already booted on different process. \""); From d31e32fe4994b9232e648a8f67049e1879779806 Mon Sep 17 00:00:00 2001 From: saakgun Date: Tue, 15 Nov 2022 20:42:23 -0500 Subject: [PATCH 07/12] Added conditions to ROS1 for the case when users select useWithIP or mxID but do not specify the addresses. --- .../ros1_src/stereo_inertial_publisher.cpp | 63 ++++++++++--------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 23c0760a..72789d66 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -376,38 +376,41 @@ int main(int argc, char** argv) { nnPath); std::shared_ptr device; - if(useWithIP) // Connecting to a camera with specific IP, requires static IP config beforehand - { - auto deviceInfo = dai::DeviceInfo(ipAddress); - if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - std::cout << "Device found with IP Address: " << ipAddress << std::endl; - device = std::make_shared(pipeline, deviceInfo); - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with ipAddress \"" + ipAddress - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); - } - } - else if(useWithMxId) // Connecting to a camera with unique MxID - { - auto deviceInfo = dai::DeviceInfo(mxId); - if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - std::cout << "Connecting to device with MxID : " << mxId << std::endl; - if(poeMode) { + if(useWithIP) { // Connecting to a camera with specific IP, requires static IP config beforehand + if(ipAddress.empty()) { + throw std::runtime_error("You have to specify MxID of the device!"); + } else { + auto deviceInfo = dai::DeviceInfo(ipAddress); + if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + std::cout << "Device found with IP Address: " << ipAddress << std::endl; device = std::make_shared(pipeline, deviceInfo); + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with ipAddress \"" + ipAddress + + "\" is already booted on different process. \""); } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); + throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); } - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); } - } - else if (!useWithIP && !useWithMxId) // List all available devices and connect one of them automatically - { + } else if(useWithMxId) { // Connecting to a camera with unique MxID + if(mxId.empty()) { + throw std::runtime_error("You have to specify MxID of the device!"); + } else { + auto deviceInfo = dai::DeviceInfo(mxId); + if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + std::cout << "Connecting to device with MxID : " << mxId << std::endl; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + } + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); + } + } + } else if(!useWithIP && !useWithMxId) {// List all available devices and connect one of them automatically std::vector availableDevices = dai::Device::getAllAvailableDevices(); std::cout << "Listing available devices..." << std::endl; for(auto deviceInfo : availableDevices) { @@ -418,9 +421,7 @@ int main(int argc, char** argv) { { throw std::runtime_error("Could NOT find any available device!"); } - } - else - { + } else { throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" ") or connect with IP or connect with MxId"); } From 9d3f9d38b51bca5b7cffa90c79012ef514e5ce61 Mon Sep 17 00:00:00 2001 From: saakgun Date: Mon, 28 Nov 2022 10:14:27 -0500 Subject: [PATCH 08/12] now using standard get all available devices function to get a vector of all devices then iterate through them. --- .../ros1_src/stereo_inertial_publisher.cpp | 85 ++++++++------- .../ros2_src/stereo_inertial_publisher.cpp | 100 ++++++++++-------- 2 files changed, 99 insertions(+), 86 deletions(-) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 72789d66..37cddee8 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -355,6 +355,7 @@ int main(int argc, char** argv) { dai::Pipeline pipeline; int width, height; + bool isDeviceFound = false; std::tie(pipeline, width, height) = createPipeline(enableDepth, enableSpatialDetection, lrcheck, @@ -376,54 +377,60 @@ int main(int argc, char** argv) { nnPath); std::shared_ptr device; - if(useWithIP) { // Connecting to a camera with specific IP, requires static IP config beforehand - if(ipAddress.empty()) { - throw std::runtime_error("You have to specify MxID of the device!"); - } else { - auto deviceInfo = dai::DeviceInfo(ipAddress); - if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - std::cout << "Device found with IP Address: " << ipAddress << std::endl; - device = std::make_shared(pipeline, deviceInfo); - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with ipAddress \"" + ipAddress - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); - } - } - } else if(useWithMxId) { // Connecting to a camera with unique MxID - if(mxId.empty()) { - throw std::runtime_error("You have to specify MxID of the device!"); - } else { - auto deviceInfo = dai::DeviceInfo(mxId); - if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - std::cout << "Connecting to device with MxID : " << mxId << std::endl; + std::vector availableDevices = dai::Device::getAllAvailableDevices(); + + std::cout << "Listing available devices..." << std::endl; + for(auto deviceInfo : availableDevices) { + std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; + if(useWithMxId) { + if(deviceInfo.getMxId() == mxId) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; + std::cout << "Connecting to device with MxID : " << mxId << std::endl; + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + break; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); + } + } + } else if(useWithIP) { + if(deviceInfo.name == ipAddress) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; + std::cout << "Connecting to device with IP Address: " << ipAddress << std::endl; + device = std::make_shared(pipeline, deviceInfo); + break; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); + } + } + } else if(!useWithIP && !useWithMxId) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; if(poeMode) { device = std::make_shared(pipeline, deviceInfo); } else { device = std::make_shared(pipeline, deviceInfo, usb2Mode); } + break; } else if(deviceInfo.state == X_LINK_BOOTED) { throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); - } - } - } else if(!useWithIP && !useWithMxId) {// List all available devices and connect one of them automatically - std::vector availableDevices = dai::Device::getAllAvailableDevices(); - std::cout << "Listing available devices..." << std::endl; - for(auto deviceInfo : availableDevices) { - std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - device = std::make_shared(pipeline); - } - if (availableDevices.empty()) - { - throw std::runtime_error("Could NOT find any available device!"); - } - } else { - throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" + } + } else { + throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" ") or connect with IP or connect with MxId"); + } + } + + if(!isDeviceFound) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" not found. \""); } if(!poeMode) { diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index 7c050d78..334c9139 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -419,60 +419,66 @@ int main(int argc, char** argv) { nnPath); std::shared_ptr device; - if(useWithIP) // Connecting to a camera with specific IP, requires static IP config beforehand - { - auto deviceInfo = dai::DeviceInfo(ipAddress); - if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - std::cout << "Device found with IP Address: " << ipAddress << std::endl; - device = std::make_shared(pipeline, deviceInfo); - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("DepthAI Device with ipAddress \"" + ipAddress - + "\" is already booted on different process. \""); + std::vector availableDevices = dai::Device::getAllAvailableDevices(); + + std::cout << "Listing available devices..." << std::endl; + for(auto deviceInfo : availableDevices) { + std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; + if(useWithMxId) { + if(deviceInfo.getMxId() == mxId) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; + std::cout << "Connecting to device with MxID : " << mxId << std::endl; + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + break; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); + } + } + } else if(useWithIP) { + if(deviceInfo.name == ipAddress) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; + std::cout << "Connecting to device with IP Address: " << ipAddress << std::endl; + device = std::make_shared(pipeline, deviceInfo); + break; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } else { + throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); + } + } + } else if(!useWithIP && !useWithMxId) { + if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { + isDeviceFound = true; + if(poeMode) { + device = std::make_shared(pipeline, deviceInfo); + } else { + device = std::make_shared(pipeline, deviceInfo, usb2Mode); + } + break; + } else if(deviceInfo.state == X_LINK_BOOTED) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + + "\" is already booted on different process. \""); + } } else { - throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); - } - } - else if(useWithMxId) // Connecting to a camera with unique MxID - { - auto deviceInfo = dai::DeviceInfo(mxId); - if(deviceInfo.state == X_LINK_ANY_STATE || deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { - std::cout << "Connecting to device with MxID : " << mxId << std::endl; - if(poeMode) { - device = std::make_shared(pipeline, deviceInfo); - } else { - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - } - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); - } - } - else if (!useWithIP && !useWithMxId) // List all available devices and connect one of them automatically - { - std::vector availableDevices = dai::Device::getAllAvailableDevices(); - std::cout << "Listing available devices..." << std::endl; - for(auto deviceInfo : availableDevices) { - std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - device = std::make_shared(pipeline); - } - if (availableDevices.empty()) - { - throw std::runtime_error("Could NOT find any available device!"); - } - } - else - { - throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" + throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" ") or connect with IP or connect with MxId"); + } } - + + if(!isDeviceFound) { + throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" not found. \""); + } + if(!poeMode) { std::cout << "Device USB status: " << usbStrings[static_cast(device->getUsbSpeed())] << std::endl; } - // Apply camera controls auto controlQueue = device->getInputQueue("control"); From c0b5e95dd29964743b36d101ac7d0decf6cce544 Mon Sep 17 00:00:00 2001 From: saakgun Date: Thu, 1 Dec 2022 18:12:16 -0500 Subject: [PATCH 09/12] simplifying the code. --- .../ros1_src/stereo_inertial_publisher.cpp | 40 +++--------------- .../ros2_src/stereo_inertial_publisher.cpp | 42 ++++--------------- 2 files changed, 13 insertions(+), 69 deletions(-) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 37cddee8..1aab4a39 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -382,35 +382,7 @@ int main(int argc, char** argv) { std::cout << "Listing available devices..." << std::endl; for(auto deviceInfo : availableDevices) { std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - if(useWithMxId) { - if(deviceInfo.getMxId() == mxId) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - std::cout << "Connecting to device with MxID : " << mxId << std::endl; - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); - } - } - } else if(useWithIP) { - if(deviceInfo.name == ipAddress) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - std::cout << "Connecting to device with IP Address: " << ipAddress << std::endl; - device = std::make_shared(pipeline, deviceInfo); - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); - } - } - } else if(!useWithIP && !useWithMxId) { + if(deviceInfo.getMxId() == mxId || deviceInfo.name == ipAddress) { if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { isDeviceFound = true; if(poeMode) { @@ -422,11 +394,11 @@ int main(int argc, char** argv) { } else if(deviceInfo.state == X_LINK_BOOTED) { throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" is already booted on different process. \""); - } - } else { - throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" - ") or connect with IP or connect with MxId"); - } + } + } else if(mxId.empty() && ipAddress.empty()) { + isDeviceFound = true; + device = std::make_shared(pipeline); + } } if(!isDeviceFound) { diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index 334c9139..ec25ba09 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -398,7 +398,7 @@ int main(int argc, char** argv) { dai::Pipeline pipeline; int width, height; bool isDeviceFound = false; - std::tie(pipeline, width, height) = createPipeline(enableDepth, + std::tie(pipeline, width, height) = createPipeline(enableDepth, enableSpatialDetection, lrcheck, extended, @@ -424,35 +424,7 @@ int main(int argc, char** argv) { std::cout << "Listing available devices..." << std::endl; for(auto deviceInfo : availableDevices) { std::cout << "Device Mx ID: " << deviceInfo.getMxId() << std::endl; - if(useWithMxId) { - if(deviceInfo.getMxId() == mxId) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - std::cout << "Connecting to device with MxID : " << mxId << std::endl; - device = std::make_shared(pipeline, deviceInfo, usb2Mode); - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with MxID: " + mxId); - } - } - } else if(useWithIP) { - if(deviceInfo.name == ipAddress) { - if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { - isDeviceFound = true; - std::cout << "Connecting to device with IP Address: " << ipAddress << std::endl; - device = std::make_shared(pipeline, deviceInfo); - break; - } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId - + "\" is already booted on different process. \""); - } else { - throw std::runtime_error("Could NOT connect to device with IP Address: " + ipAddress); - } - } - } else if(!useWithIP && !useWithMxId) { + if(deviceInfo.getMxId() == mxId || deviceInfo.name == ipAddress) { if(deviceInfo.state == X_LINK_UNBOOTED || deviceInfo.state == X_LINK_BOOTLOADER) { isDeviceFound = true; if(poeMode) { @@ -464,11 +436,11 @@ int main(int argc, char** argv) { } else if(deviceInfo.state == X_LINK_BOOTED) { throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" is already booted on different process. \""); - } - } else { - throw std::runtime_error("You need to specify to either connect automatically (no MxId or IP Address" - ") or connect with IP or connect with MxId"); - } + } + } else if(mxId.empty() && ipAddress.empty()) { + isDeviceFound = true; + device = std::make_shared(pipeline); + } } if(!isDeviceFound) { From 067604152313a28c82b755079ccc99d356f48913 Mon Sep 17 00:00:00 2001 From: saakgun Date: Thu, 1 Dec 2022 18:15:46 -0500 Subject: [PATCH 10/12] Clearing out unnecessary params from the previous commits. --- .../launch/stereo_inertial_node.launch | 6 +----- .../launch/stereo_inertial_node.launch.py | 20 ++----------------- .../ros1_src/stereo_inertial_publisher.cpp | 4 +--- .../ros2_src/stereo_inertial_publisher.cpp | 6 +----- 4 files changed, 5 insertions(+), 31 deletions(-) diff --git a/depthai_examples/launch/stereo_inertial_node.launch b/depthai_examples/launch/stereo_inertial_node.launch index abc42a0d..44931d95 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch +++ b/depthai_examples/launch/stereo_inertial_node.launch @@ -1,9 +1,7 @@ - - @@ -58,7 +56,7 @@ - + @@ -76,9 +74,7 @@ - - diff --git a/depthai_examples/launch/stereo_inertial_node.launch.py b/depthai_examples/launch/stereo_inertial_node.launch.py index af04c680..b6b54a35 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch.py +++ b/depthai_examples/launch/stereo_inertial_node.launch.py @@ -21,9 +21,7 @@ def generate_launch_description(): 'rviz', 'stereoInertial.rviz') default_resources_path = os.path.join(depthai_examples_path, 'resources') - useWithIP = LaunchConfiguration('useWithIP', default = False) ipAddress = LaunchConfiguration('ipAddress', default = 'x') - useWithMxId = LaunchConfiguration('useWithMxId', default = False) mxId = LaunchConfiguration('mxId', default = 'x') usb2Mode = LaunchConfiguration('usb2Mode', default = False) poeMode = LaunchConfiguration('poeMode', default = False) @@ -80,21 +78,11 @@ def generate_launch_description(): enableRviz = LaunchConfiguration('enableRviz', default = True) - declare_useWithIP_cmd = DeclareLaunchArgument( - 'useWithIP', - default_value=useWithIP, - description='Set this to true to enable connecting the device by specifying IP address.') - declare_ipAddress_cmd = DeclareLaunchArgument( 'ipAddress', default_value=ipAddress, - description='select the device by passing the IP address of the device. It will give error if left empty and useWithIPAddress is enabled.') + description='select the device by passing the IP address of the device.') - declare_useWithMxId_cmd = DeclareLaunchArgument( - 'useWithMxId', - default_value=useWithMxId, - description='Set this to true to enable connecting the device by specifying MxID.') - declare_mxId_cmd = DeclareLaunchArgument( 'mxId', default_value=mxId, @@ -335,9 +323,7 @@ def generate_launch_description(): stereo_node = launch_ros.actions.Node( package='depthai_examples', executable='stereo_inertial_node', output='screen', - parameters=[{'useWithIP': useWithIP}, - {'ipAddress': ipAddress}, - {'useWithMxId': useWithMxId}, + parameters=[{'ipAddress': ipAddress}, {'mxId': mxId}, {'usb2Mode': usb2Mode}, {'poeMode': poeMode}, @@ -442,9 +428,7 @@ def generate_launch_description(): ld = LaunchDescription() - ld.add_action(declare_useWithIP_cmd) ld.add_action(declare_ipAddress_cmd) - ld.add_action(declare_useWithMxId_cmd) ld.add_action(declare_mxId_cmd) ld.add_action(declare_usb2Mode_cmd) ld.add_action(declare_poeMode_cmd) diff --git a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp index 1aab4a39..0013c14b 100644 --- a/depthai_examples/ros1_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros1_src/stereo_inertial_publisher.cpp @@ -281,14 +281,12 @@ int main(int argc, char** argv) { int rgbScaleNumerator, rgbScaleDinominator, previewWidth, previewHeight; bool lrcheck, extended, subpixel, enableDepth, rectify, depth_aligned, manualExposure; bool enableSpatialDetection, enableDotProjector, enableFloodLight; - bool usb2Mode, poeMode, syncNN, useWithIP, useWithMxId; + bool usb2Mode, poeMode, syncNN; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; std::string nnName(BLOB_NAME); // Set your blob name for the model here - badParams += !pnh.getParam("useWithIP", useWithIP); badParams += !pnh.getParam("ipAddress", ipAddress); - badParams += !pnh.getParam("useWithMxId", useWithMxId); badParams += !pnh.getParam("mxId", mxId); badParams += !pnh.getParam("usb2Mode", usb2Mode); badParams += !pnh.getParam("poeMode", poeMode); diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index ec25ba09..5c8acde3 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -280,14 +280,12 @@ int main(int argc, char** argv) { int rgbScaleNumerator, rgbScaleDinominator, previewWidth, previewHeight; bool lrcheck, extended, subpixel, enableDepth, rectify, depth_aligned, manualExposure; bool enableSpatialDetection, enableDotProjector, enableFloodLight; - bool usb2Mode, poeMode, syncNN, useWithIP, useWithMxId;; + bool usb2Mode, poeMode, syncNN; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; std::string nnName(BLOB_NAME); // Set your blob name for the model here - node->declare_parameter("useWithIP", false); node->declare_parameter("ipAddress", ""); - node->declare_parameter("useWithMxId", false); node->declare_parameter("mxId", ""); node->declare_parameter("usb2Mode", false); node->declare_parameter("poeMode", false); @@ -332,9 +330,7 @@ int main(int argc, char** argv) { // updating parameters if defined in launch file. - node->get_parameter("useWithIP", useWithIP); node->get_parameter("ipAddress", ipAddress); - node->get_parameter("useWithMxId", useWithMxId); node->get_parameter("mxId", mxId); node->get_parameter("usb2Mode", usb2Mode); node->get_parameter("poeMode", poeMode); From d9b41bd894dd7e4292d9350343581cb2e485b127 Mon Sep 17 00:00:00 2001 From: saakgun Date: Mon, 2 Jan 2023 21:22:13 -0500 Subject: [PATCH 11/12] Fixed non empty ipaddress and mxId check for ROS2 launch file. --- depthai_examples/launch/stereo_inertial_node.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/depthai_examples/launch/stereo_inertial_node.launch.py b/depthai_examples/launch/stereo_inertial_node.launch.py index b6b54a35..a8162475 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch.py +++ b/depthai_examples/launch/stereo_inertial_node.launch.py @@ -21,8 +21,8 @@ def generate_launch_description(): 'rviz', 'stereoInertial.rviz') default_resources_path = os.path.join(depthai_examples_path, 'resources') - ipAddress = LaunchConfiguration('ipAddress', default = 'x') - mxId = LaunchConfiguration('mxId', default = 'x') + ipAddress = LaunchConfiguration('ipAddress', default = '') + mxId = LaunchConfiguration('mxId', default = '') usb2Mode = LaunchConfiguration('usb2Mode', default = False) poeMode = LaunchConfiguration('poeMode', default = False) From 9514c6e42be771b43bb5cab756ce9bbe607a23d8 Mon Sep 17 00:00:00 2001 From: Alp Akgun Date: Fri, 24 Feb 2023 22:18:32 -0500 Subject: [PATCH 12/12] fixing ros2 pnh error --- depthai_examples/ros2_src/stereo_inertial_publisher.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp index 5c8acde3..35d51d48 100644 --- a/depthai_examples/ros2_src/stereo_inertial_publisher.cpp +++ b/depthai_examples/ros2_src/stereo_inertial_publisher.cpp @@ -430,7 +430,7 @@ int main(int argc, char** argv) { } break; } else if(deviceInfo.state == X_LINK_BOOTED) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + throw std::runtime_error("\" DepthAI Device with MxId \"" + mxId + "\" is already booted on different process. \""); } } else if(mxId.empty() && ipAddress.empty()) { @@ -440,7 +440,7 @@ int main(int argc, char** argv) { } if(!isDeviceFound) { - throw std::runtime_error("ros::NodeHandle() from Node \"" + pnh.getNamespace() + "\" DepthAI Device with MxId \"" + mxId + "\" not found. \""); + throw std::runtime_error("\" DepthAI Device with MxId \"" + mxId + "\" not found. \""); } if(!poeMode) { @@ -679,4 +679,4 @@ int main(int argc, char** argv) { } return 0; -} \ No newline at end of file +}