Skip to content

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

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

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

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

Show file tree
Hide file tree
Changes from 1 commit
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
220 changes: 220 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,220 @@
<?xml version="1.0"?>
<launch>
<!-- Multiple camera args -->
<arg name="enable_camera1" default="true"/>
<arg name="enable_camera2" default="false"/>
<arg name="enable_camera3" default="false"/>
<arg name="useWithIP_camera1" default="false"/>
<arg name="useWithIP_camera2" default="false"/>
<arg name="useWithIP_camera3" default="false"/>
<arg name="ip_address_camera1" default=""/>
<arg name="ip_address_camera2" default=""/>
<arg name="ip_address_camera3" default=""/>
<arg name="tf_prefix_camera1" default="front"/>
<arg name="tf_prefix_camera2" default="back_left"/>
<arg name="tf_prefix_camera3" default="back_right"/>

<!-- <args for urdf/> -->
<arg name="mxId" default=""/>
<arg name="usb2Mode" default="false"/>
<arg name="poeMode" default="false"/>

<arg name="camera_model" default="OAK-D" /> <!-- 'zed' or 'zedm' -->
<arg name="tf_prefix" default="oak" />
<arg name="mode" default="depth" />
<arg name="base_frame" default="oak-d_frame" />
<arg name="parent_frame" default="oak-d-base-frame" />
<arg name="imuMode" default="1" /> <!-- 0 -> COPY, 1 -> LINEAR_INTERPOLATE_GYRO, 2 -> LINEAR_INTERPOLATE_ACCEL -->

<arg name="cam_pos_x" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_y" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_z" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_roll" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_pitch" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_yaw" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->

<arg name="lrcheck" default="true" />
<arg name="extended" default="false"/>
<arg name="subpixel" default="true" />
<arg name="rectify" default="true" />
<arg name="depth_aligned" default="false" />

<arg name="enableSpatialDetection" default="false" />
<arg name="syncNN" default="false" />
<arg name="detectionClassesCount" default="80" />
<arg name="nnName" default="x" />
<arg name="resourceBaseFolder" default="$(find depthai_examples)/resources" />
<arg name="nnPath" default="" />

<!-- see https://docs.luxonis.com/projects/api/en/latest/components/nodes/stereo_depth/#currently-configurable-blocks for possible combination of optionf for higher fps-->
<arg name="stereo_fps" default="40" />
<arg name="confidence" default="90" />
<arg name="LRchecktresh" default="1" />
<arg name="monoResolution" default="720p"/>
<arg name="angularVelCovariance" default="0" />
<arg name="linearAccelCovariance" default="0" />

<arg name="enableDotProjector" default="true"/>
<arg name="enableFloodLight" default="true"/>
<arg name="dotProjectormA" default="400.0"/>
<arg name="floodLightmA" default="400.0"/>
<arg name="enableRviz" default="false"/>
<arg name="enableMarkerPublish" default="false" />

<include file="$(find depthai_bridge)/launch/urdf.launch">
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="parent_frame" value="$(arg parent_frame)"/>
<arg name="camera_model" value="$(arg camera_model)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)" />
<arg name="cam_pos_x" value="$(arg cam_pos_x)" />
<arg name="cam_pos_y" value="$(arg cam_pos_y)" />
<arg name="cam_pos_z" value="$(arg cam_pos_z)" />
<arg name="cam_roll" value="$(arg cam_roll)" />
<arg name="cam_pitch" value="$(arg cam_pitch)" />
<arg name="cam_yaw" value="$(arg cam_yaw)" />
</include>

<!-- launch-prefix="xterm -e gdb (add [- - args] without space) -->
<node if="$(eval arg('enable_camera1') == true)" name="stereo_inertial_publisher_front" pkg="depthai_examples" type="cassie_oak" output="screen" required="true">
<param name="ipAddress" value="$(arg ip_address_camera1)"/>
<param name="useWithIP" value="$(arg useWithIP_camera1)"/>

<param name="mxId" value="$(arg mxId)"/>
<param name="usb2Mode" value="$(arg usb2Mode)"/>
<param name="poeMode" value="$(arg poeMode)"/>

<param name="tf_prefix" value="$(arg tf_prefix_camera1)"/>
<param name="mode" value="$(arg mode)"/>
<param name="imuMode" value="$(arg imuMode)"/>

<param name="lrcheck" value="$(arg lrcheck)"/>
<param name="extended" value="$(arg extended)"/>
<param name="subpixel" value="$(arg subpixel)"/>
<param name="rectify" value="$(arg rectify)" />
<param name="depth_aligned" value="$(arg depth_aligned)" />

<param name="enableSpatialDetection" value="$(arg enableSpatialDetection)" />
<param name="syncNN" value="$(arg syncNN)" />
<param name="detectionClassesCount" value="$(arg detectionClassesCount)" />
<param name="nnName" value="$(arg nnName)"/>
<param name="resourceBaseFolder" value="$(arg resourceBaseFolder)"/>

<param name="stereo_fps" value="$(arg stereo_fps)" />
<param name="confidence" value="$(arg confidence)" />
<param name="LRchecktresh" value="$(arg LRchecktresh)" />
<param name="monoResolution" value="$(arg monoResolution)" />
<param name="angularVelCovariance" value="$(arg angularVelCovariance)" />
<param name="linearAccelCovariance" value="$(arg linearAccelCovariance)" />

<param name="enableDotProjector" value="$(arg enableDotProjector)" />
<param name="enableFloodLight" value="$(arg enableFloodLight)" />
<param name="dotProjectormA" value="$(arg dotProjectormA)" />
<param name="floodLightmA" value="$(arg floodLightmA)" />
</node>

<node if="$(eval arg('enable_camera2') == true)" name="stereo_inertial_publisher_back_left" pkg="depthai_examples" type="cassie_oak" output="screen" required="true">
<param name="ipAddress" value="$(arg ip_address_camera2)"/>
<param name="useWithIP" value="$(arg useWithIP_camera2)"/>

<param name="mxId" value="$(arg mxId)"/>
<param name="usb2Mode" value="$(arg usb2Mode)"/>
<param name="poeMode" value="$(arg poeMode)"/>

<param name="tf_prefix" value="$(arg tf_prefix_camera2)"/>
<param name="mode" value="$(arg mode)"/>
<param name="imuMode" value="$(arg imuMode)"/>

<param name="lrcheck" value="$(arg lrcheck)"/>
<param name="extended" value="$(arg extended)"/>
<param name="subpixel" value="$(arg subpixel)"/>
<param name="rectify" value="$(arg rectify)" />
<param name="depth_aligned" value="$(arg depth_aligned)" />

<param name="enableSpatialDetection" value="$(arg enableSpatialDetection)" />
<param name="syncNN" value="$(arg syncNN)" />
<param name="detectionClassesCount" value="$(arg detectionClassesCount)" />
<param name="nnName" value="$(arg nnName)"/>
<param name="resourceBaseFolder" value="$(arg resourceBaseFolder)"/>

<param name="stereo_fps" value="$(arg stereo_fps)" />
<param name="confidence" value="$(arg confidence)" />
<param name="LRchecktresh" value="$(arg LRchecktresh)" />
<param name="monoResolution" value="$(arg monoResolution)" />
<param name="angularVelCovariance" value="$(arg angularVelCovariance)" />
<param name="linearAccelCovariance" value="$(arg linearAccelCovariance)" />

<param name="enableDotProjector" value="$(arg enableDotProjector)" />
<param name="enableFloodLight" value="$(arg enableFloodLight)" />
<param name="dotProjectormA" value="$(arg dotProjectormA)" />
<param name="floodLightmA" value="$(arg floodLightmA)" />
</node>

<node if="$(eval arg('enable_camera3') == true)" name="stereo_inertial_publisher_back_right" pkg="depthai_examples" type="cassie_oak" output="screen" required="true">
<param name="ipAddress" value="$(arg ip_address_camera3)"/>
<param name="useWithIP" value="$(arg useWithIP_camera3)"/>

<param name="mxId" value="$(arg mxId)"/>
<param name="usb2Mode" value="$(arg usb2Mode)"/>
<param name="poeMode" value="$(arg poeMode)"/>

<param name="tf_prefix" value="$(arg tf_prefix_camera3)"/>
<param name="mode" value="$(arg mode)"/>
<param name="imuMode" value="$(arg imuMode)"/>

<param name="lrcheck" value="$(arg lrcheck)"/>
<param name="extended" value="$(arg extended)"/>
<param name="subpixel" value="$(arg subpixel)"/>
<param name="rectify" value="$(arg rectify)" />
<param name="depth_aligned" value="$(arg depth_aligned)" />

<param name="enableSpatialDetection" value="$(arg enableSpatialDetection)" />
<param name="syncNN" value="$(arg syncNN)" />
<param name="detectionClassesCount" value="$(arg detectionClassesCount)" />
<param name="nnName" value="$(arg nnName)"/>
<param name="resourceBaseFolder" value="$(arg resourceBaseFolder)"/>

<param name="stereo_fps" value="$(arg stereo_fps)" />
<param name="confidence" value="$(arg confidence)" />
<param name="LRchecktresh" value="$(arg LRchecktresh)" />
<param name="monoResolution" value="$(arg monoResolution)" />
<param name="angularVelCovariance" value="$(arg angularVelCovariance)" />
<param name="linearAccelCovariance" value="$(arg linearAccelCovariance)" />

<param name="enableDotProjector" value="$(arg enableDotProjector)" />
<param name="enableFloodLight" value="$(arg enableFloodLight)" />
<param name="dotProjectormA" value="$(arg dotProjectormA)" />
<param name="floodLightmA" value="$(arg floodLightmA)" />
</node>

<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen"/>

<node if="$(eval arg('enable_camera1') == true)" pkg="nodelet" type="nodelet" name="depth_image_convertion_nodelet_front"
args="load depth_image_proc/convert_metric nodelet_manager">
<remap from="image_raw" to="/stereo_inertial_publisher_front/stereo/depth"/>
<remap from="image" to="/stereo_inertial_publisher_front/stereo/image"/>
</node>

<node if="$(eval arg('enable_camera2') == true)" pkg="nodelet" type="nodelet" name="depth_image_convertion_nodelet_back_left"
args="load depth_image_proc/convert_metric nodelet_manager">
<remap from="image_raw" to="/stereo_inertial_publisher_back_left/stereo/depth"/>
<remap from="image" to="/stereo_inertial_publisher_back_left/stereo/image"/>
</node>

<node if="$(eval arg('enable_camera3') == true)" pkg="nodelet" type="nodelet" name="depth_image_convertion_nodelet_back_right"
args="load depth_image_proc/convert_metric nodelet_manager">
<remap from="image_raw" to="/stereo_inertial_publisher_back_right/stereo/depth"/>
<remap from="image" to="/stereo_inertial_publisher_back_right/stereo/image"/>
</node>

<!-- Visualizations for only the first camera -->
<node if="$(eval arg('enableMarkerPublish') == true)" type="markerPublisher.py" name="markerPublisher" pkg="depthai_examples">
<remap from="spatialDetections" to="/stereo_inertial_publisher_front/color/yolov4_Spatial_detections"/>
<remap from="spatialDetectionMarkers" to="/stereo_inertial_publisher_front/color/spatialDetectionMarkers"/>
</node>

<group if="$(eval arg('enableRviz') == true)">
<node if="$(eval arg('depth_aligned') == true)" type="rviz" name="rviz" pkg="rviz" args="-d $(find depthai_examples)/rviz/stereoInertialDepthAlign.rviz" />
<node if="$(eval arg('depth_aligned') == false)" type="rviz" name="rviz" pkg="rviz" args="-d $(find depthai_examples)/rviz/stereoInertialROS1.rviz" />
</group>

</launch>
64 changes: 43 additions & 21 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,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);
Expand Down Expand Up @@ -374,27 +376,47 @@ 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);
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) {
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 ?

isDeviceFound = true;
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 {
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<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. \"");
}
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<dai::Device>(pipeline);
}
} else if(mxId.empty()) {
isDeviceFound = true;
device = std::make_shared<dai::Device>(pipeline);
}
}

Expand Down