Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Connect to a Specific Device using IP address as input #153

Open
wants to merge 12 commits into
base: ros-release
Choose a base branch
from
+65 −16
Open

Connect to a Specific Device using IP address as input #153

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
f73904f
Stereo inertial publisher node now supports a way to connect to indiv…
Oct 14, 2022
20a584e
Added useWithMxId option as well an option to list all devices and co…
Nov 4, 2022
e57960f
useWithIP, useWithMxId, and none of them (connect automatically, defa…
Nov 8, 2022
972091c
Stereo inertial node launch file updated to accommodate new parameter…
Nov 9, 2022
2cfcb1e
if condition is fixed to include any_state which corresponds to devic…
Nov 11, 2022
4e9eea5
Unnecessary condition under useWithIP conditinal branch removed.
Nov 15, 2022
d31e32f
Added conditions to ROS1 for the case when users select useWithIP or …
Nov 16, 2022
9d3f9d3
now using standard get all available devices function to get a vector…
Nov 28, 2022
c0b5e95
simplifying the code.
Dec 1, 2022
0676041
Clearing out unnecessary params from the previous commits.
Dec 1, 2022
d9b41bd
Fixed non empty ipaddress and mxId check for ROS2 launch file.
Jan 3, 2023
9514c6e
fixing ros2 pnh error
Feb 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 37 additions & 0 deletions depthai_examples/launch/multi_stereo_inertial_node.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
<?xml version="1.0"?>
<launch>
<!-- Multiple camera args -->
<arg name="enable_camera1" default="true"/>
<arg name="enable_camera2" default="false"/>
<arg name="useWithIP_camera1" default="false"/>
<arg name="useWithIP_camera2" default="false"/>
<arg name="ip_address_camera1" default=""/>
<arg name="ip_address_camera2" default=""/>
<arg name="useWithMxId_camera1" default="false"/>
<arg name="useWithMxId_camera2" default="false"/>
<arg name="mxId_camera1" default=""/>
<arg name="mxId_camera2" default=""/>
<arg name="tf_prefix_camera1" default="oak_front"/>
<arg name="tf_prefix_camera2" default="oak_back"/>

<!-- First Camera -->
<include if="$(eval arg('enable_camera1') == true)" file="$(find depthai_examples)/launch/stereo_inertial_node.launch">
<arg name="tf_prefix" value="$(arg tf_prefix_camera1)" />
<arg name="useWithIP" value="$(arg useWithIP_camera1)"/>
<arg name="ipAddress" value="$(arg ip_address_camera1)"/>
<arg name="useWithMxId" value="$(arg useWithMxId_camera1)"/>
<arg name="mxId" value="$(arg mxId_camera1)"/>
</include>

<!-- Second Camera -->
<include if="$(eval arg('enable_camera2') == true)" file="$(find depthai_examples)/launch/stereo_inertial_node.launch">
<arg name="tf_prefix" value="$(arg tf_prefix_camera2)" />
<arg name="useWithIP" value="$(arg useWithIP_camera2)"/>
<arg name="ipAddress" value="$(arg ip_address_camera2)"/>
<arg name="useWithMxId" value="$(arg useWithMxId_camera2)"/>
<arg name="mxId" value="$(arg mxId_camera2)"/>
</include>

<!-- More cameras can be added in the same fashion -->

</launch>
6 changes: 6 additions & 0 deletions depthai_examples/launch/stereo_inertial_node.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@
<?xml version="1.0"?>
<launch>
<!-- <args for urdf/> -->
<arg name="useWithIP" default="false"/>
<arg name="ipAddress" default=""/>
<arg name="useWithMxId" default="false"/>
<arg name="mxId" default=""/>
<arg name="usb2Mode" default="false"/>
<arg name="poeMode" default="false"/>
Expand Down Expand Up @@ -73,6 +76,9 @@

<!-- launch-prefix="xterm -e gdb (add [- - args] without space) -->
<node name="stereo_inertial_publisher" pkg="depthai_examples" type="stereo_inertial_node" output="screen" required="true">
<param name="useWithIP" value="$(arg useWithIP)"/>
<param name="ipAddress" value="$(arg ipAddress)"/>
<param name="useWithMxId" value="$(arg useWithMxId)"/>
<param name="mxId" value="$(arg mxId)"/>
<param name="usb2Mode" value="$(arg usb2Mode)"/>
<param name="poeMode" value="$(arg poeMode)"/>
Expand Down
32 changes: 27 additions & 5 deletions depthai_examples/launch/stereo_inertial_node.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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',
Expand Down Expand Up @@ -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},
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
77 changes: 52 additions & 25 deletions depthai_examples/ros1_src/stereo_inertial_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -275,17 +275,20 @@ 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, 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);
Expand Down Expand Up @@ -352,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,
Expand All @@ -374,32 +376,57 @@ int main(int argc, char** argv) {
nnPath);

std::shared_ptr<dai::Device> device;
std::vector<dai::DeviceInfo> 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<dai::Device>(pipeline, deviceInfo);
} else {
device = std::make_shared<dai::Device>(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. \"");
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) {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The value of deviceInfo.state is always 0 here so the exception "Could NOT connect to device with IP Address" is always thrown. If we want to check if another process already has a connection, maybe we should do something like this:

    if(useWithIP) {
        auto connectionInfo = dai::Device::getAllConnectedDevices();
        for (auto const& info : connectionInfo)
        {
            if (info.name == ipAddress && info.state == X_LINK_BOOTED) {
                // Throw exception here....
            }
        }
        auto deviceInfo = dai::DeviceInfo(ipAddress);
        if(poeMode) {
            device = std::make_shared<dai::Device>(pipeline, deviceInfo);
        } else {
            device = std::make_shared<dai::Device>(pipeline, deviceInfo, usb2Mode);
        }
        if(device.getDeviceInfo().state == X_LINK_BOOTED) {
            std::cout << "Device found with IP Address: " << ipAddress <<  std::endl;
        }
    else if(useWithMxId) {
        // ...
    }

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@zflat Did you assign a static ip to your camera using depthai device manager? I tested it with my camera and it seems to be working. deviceInfo.state is actually an enum, so if it returns 1, it can take the following values
image
so 1 probably corresponds to X_LINK_BOOTED, which kinda tells me that you have not used device manager, because once you use device manager to assign static IP, deviceInfo.state becomes X_LINK_FLASH_BOOTED.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, I did assign a static IP.
I tested this code on an OAK-1 (actually copy and pasted the logic to rgb_publisher.cpp because it can't use a pipeline that is set up for steroe) and also an OAK-D.

OAK-1
Screenshot from 2022-11-09 16-29-06

OAK-D
Screenshot from 2022-11-11 12-08-53

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@zflat Could you try to flash the newest bootloader? I remember I had a problem connecting to my device with IP address before updating the bootloader. If it doesn't solve the issue, I should have OAK (Series 1) POE lying somewhere. I will test it with that guy.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have tried flashing a new bootloader and I just get an error popup and then a message to the terminal that it was cancelled. Sorry I can't be of more help.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@zflat No problem! Lemme give it a try.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@zflat You are right. Somehow, deviceInfo.state returns 0 once it is not created by get all available devices. 0 means X_LINK_ANY_STATE, so I added that one to if condition and tested it with two cameras at the same time. It should be working fine now.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@zflat what was your execution command here ?

std::cout << "Device found with IP Address: " << ipAddress << std::endl;
if(poeMode) {
device = std::make_shared<dai::Device>(pipeline, deviceInfo);
} else {
device = std::make_shared<dai::Device>(pipeline, deviceInfo, usb2Mode);
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if(useWithIP)
section is already PoE region. you don't need to check for if PoE and else here. Do it only under MxID condition

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Makes a lot of sense.

} 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_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<dai::Device>(pipeline, deviceInfo);
} else {
device = std::make_shared<dai::Device>(pipeline, deviceInfo, usb2Mode);
}
} else if(mxId.empty()) {
isDeviceFound = true;
} 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<dai::DeviceInfo> 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<dai::Device>(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) {
Expand Down
81 changes: 57 additions & 24 deletions depthai_examples/ros2_src/stereo_inertial_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -413,32 +419,59 @@ int main(int argc, char** argv) {
nnPath);

std::shared_ptr<dai::Device> device;
std::vector<dai::DeviceInfo> 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<dai::Device>(pipeline, deviceInfo);
} else {
device = std::make_shared<dai::Device>(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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What will happen if useWithIp is set but ipAddress is not set ?
Same for useWithMxID

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good condition to check. will test and update it.

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<dai::Device>(pipeline, deviceInfo);
} else {
device = std::make_shared<dai::Device>(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<dai::Device>(pipeline, deviceInfo);
} else {
device = std::make_shared<dai::Device>(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<dai::DeviceInfo> 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<dai::Device>(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<int32_t>(device->getUsbSpeed())] << std::endl;
}
Expand Down Expand Up @@ -676,4 +709,4 @@ int main(int argc, char** argv) {
}

return 0;
}
}