search

luxonis / depthai-ros

Official ROS Driver for DepthAI Sensors.
MIT License
260 stars 191 forks source link

stereo_inertial_node.launch multi camera #569

Closed khuechuong closed 3 months ago

khuechuong commented 4 months ago

Start with the why:

I'm trying to do multiple oak-d pro poe. Problem is that I'm not exactly sure since there's no example of stereo_inertial_node. There's the example_multicam.launch example but that's uses a different node. I tried putting a group in, but it did keeps shutting down. Any advice?

Serafadam commented 4 months ago

Hi, stereo_inertial_node serves more as a code example, the driver node in depthai_ros_driver provides the same functionality but with much more options for configuring and setting up so I would suggest using that one. Is there a specific thing that you need from stereo_inertial_node?

khuechuong commented 4 months ago

It just looks better when I use stereo_inertial_node.

<?xml version="1.0"?>
<launch>
    <!-- <args for urdf/> -->
    <arg name="mxId"                 default=""/>
    <arg name="mxId_0"                 default="1844301041E8E9F400"/>
    <arg name="mxId_1"                 default="1844301091CBE2F400"/>
    <!-- 1844301041E8E9F400 -->
    <!-- 1844301091CBE2F400 -->
    <arg name="usb2Mode"             default="false"/>
    <arg name="poeMode"              default="true"/>

    <arg name="camera_model"         default="OAK-D-PRO-POE"            /> <!-- 'zed' or 'zedm' -->
    <arg name="tf_prefix"            default="oak"              />
    <arg name="tf_prefix_0"            default="oak_0"              />
    <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="true" />
    <arg name="manualExposure"        default="false"/>
    <arg name="expTime"               default="20000"/>
    <arg name="sensIso"               default="800" />

    <arg name="enableSpatialDetection" default="true" />
    <arg name="syncNN"                 default="true" />
    <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="15"  />
    <arg name="confidence"            default="200" />
    <arg name="LRchecktresh"          default="5"   />
    <arg name="monoResolution"        default="720p"/>

    <arg name="rgbResolution"           default="1080p"/>
    <arg name="rgbScaleNumerator"       default="2"/>
    <arg name="rgbScaleDinominator"     default="3"/>
    <arg name="previewWidth"            default="416"/>
    <arg name="previewHeight"           default="416"/>

    <arg name="angularVelCovariance"  default="0"   />
    <arg name="linearAccelCovariance" default="0"   />

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

    <arg name="enableRosBaseTimeUpdate" default="false" />  <!-- Used to handle when ROS time shifts and steady_clock does not -->

        <include file="$(find depthai_descriptions)/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>
    <!-- <group ns="oak_0"> -->
        <!-- 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="mxId"                  value="$(arg mxId)"/>
            <param name="usb2Mode"              value="$(arg usb2Mode)"/>
            <param name="poeMode"               value="$(arg poeMode)"/>

            <param name="tf_prefix"             value="$(arg tf_prefix)"/>
            <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="manualExposure"        value="$(arg manualExposure)" />
            <param name="expTime"               value="$(arg expTime)" />
            <param name="sensIso"               value="$(arg sensIso)" />

            <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="rgbResolution"         value="$(arg rgbResolution)" />
            <param name="rgbScaleNumerator"     value="$(arg rgbScaleNumerator)" />
            <param name="rgbScaleDinominator"   value="$(arg rgbScaleDinominator)" />
            <param name="previewWidth"          value="$(arg previewWidth)" />
            <param name="previewHeight"         value="$(arg previewHeight)" />

            <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)" />
            <param name="enableRosBaseTimeUpdate" value="$(arg enableRosBaseTimeUpdate)" />
        </node>  
    <!-- </group>           -->

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

    <node pkg="nodelet" type="nodelet" name="depth_image_convertion_nodelet"
        args="load depth_image_proc/convert_metric nodelet_manager">
        <remap from="image_raw" to="/stereo_inertial_publisher/stereo/depth"/>    
        <remap from="image" to="/stereo_inertial_publisher/stereo/image"/>
    </node>

    <node if="$(eval arg('depth_aligned') == true)" pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud"
        args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager">
        <param name="queue_size"          value="10"/>

        <remap from="rgb/camera_info" to="stereo_inertial_publisher/color/camera_info"/>
        <remap from="rgb/image_rect_color" to="stereo_inertial_publisher/color/image"/>
        <remap from="depth_registered/image_rect" to="stereo_inertial_publisher/stereo/image"/>    
        <remap from="depth_registered/points" to="stereo_inertial_publisher/stereo/points"/>
    </node>

    <node if="$(eval arg('depth_aligned') == false)" pkg="nodelet" type="nodelet" name="depth_image_to_pointcloud"
        args="load depth_image_proc/point_cloud_xyz nodelet_manager">
        <param name="queue_size"          value="10"/>
        <remap from="camera_info" to="stereo_inertial_publisher/stereo/camera_info"/>
        <remap from="image_rect" to="stereo_inertial_publisher/stereo/image"/>    
        <remap from="points" to="stereo_inertial_publisher/stereo/points"/>
    </node>

    <node if="$(eval arg('enableMarkerPublish') == true)" type="markerPublisher.py" name="markerPublisher" pkg="depthai_examples">
        <remap from="spatialDetections" to="stereo_inertial_publisher/color/yolov4_Spatial_detections"/>
        <remap from="spatialDetectionMarkers" to="stereo_inertial_publisher/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>

the pointcloud seems to be clearer. than when I ran rgbd_pcl.launch

khuechuong commented 4 months ago

here's rgbd_pcl.launch pcl and here's stereo_inertial_node.launch stereo

khuechuong commented 3 months ago

@Serafadam any updates?

Serafadam commented 3 months ago

Hi @khuechuong, do you have a comparison where the two pointclouds overlap? These pointclouds should be virtually the same as the mechanisms they use are the same

khuechuong commented 3 months ago

I'm not sure what you mean by overlap? I haven't had any success running 2 oak-d-pro with ROS. basically my problem is I'm not sure how to use multiple oak-d-pro with stereo_inertial_node.launch.

I prefer stereo_inertial_node.launch because it is more clear as you can see in the picture where the face pointcloud generated by stereo_inertial_node.launch is clearer than rgbd_pcl.launch. The pictures are separated times I ran the 2 node with the same environment fov.

khuechuong commented 3 months ago

@Serafadam

Serafadam commented 3 months ago

By above comment I meant displaying point clouds both from stereo_inertial_node and rgbd_pcl side-by-side, preferably in same rviz window to see the differences between your clouds as they shouldn't be that significant, maybe perspective change or some parameter affects these results.

khuechuong commented 3 months ago

I'm not sure how I can run them at the same time, I've already given you the result from picture above. I did change parameters in the stereo_inertial_node.launch be enabling features like dot projection and stuff.

Anyways, we are straying from the question. Can you provide me a launch file that can run multiple stereo_inertial_node? I tried using the group tag but it didn't work.

khuechuong commented 3 months ago

@Serafadam okay, I found the problem. the rviz in stereo_inertial_node.launch uses depthcloud which for some reason, is much better than pointcloud2.

Serafadam commented 3 months ago

Hi, thanks for clarifying, I think the "quality" of the pointclouds is just related to visualization, as DepthCloud has "Autosize" parameter which seems to make futher pixels larger, whereas for PointCloud the size is fixed, when I set DepthCloud's size to the same number, the pointclouds seemed the same image