microsoft / Azure_Kinect_ROS_Driver

A ROS sensor driver for the Azure Kinect Developer Kit.
MIT License
302 stars 222 forks source link

No map received on RViz (rtabmap) #174

Closed vinesmsuic closed 3 years ago

vinesmsuic commented 3 years ago

Describe the bug I am trying to see the map generated with rtabmap in RViz.

To Reproduce Steps to reproduce the behavior:

  1. Installed rtabmap_ros via apt sudo apt install ros-melodic-rtabmap-ros
  2. Entered the commend roslaunch azure_kinect_ros_driver slam_rtabmap.launch
  3. RViz shows up and Log info appears in terminal
  4. Tick the Map option
  5. See error (No map received)
  6. I used rostopic echo /topic_name to see the topics (/rtabmap/octomap_grid, /rtabmap/proj_map, /rtabmap/grid_map, /rtabmap/grid_prob_map), nothing inside.

The launch file I used (the updated slam_rtabmap.launch with some extra args)

<!--
Copyright (c) Microsoft Corporation. All rights reserved.
Licensed under the MIT License.
-->

<launch>

  <!-- If color_enabled is false, IR camera is used (may work better in dark areas). -->
  <!-- When using color camera, to avoid black borders in point clouds 
       in rtabmapviz, set ROI ratios in Preferences->3D rendering 
       to "0.05 0.05 0.05 0.05" under Map and Odom columns. -->
  <arg name="color_enabled" default="true"/> 

  <param name="robot_description"
    command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- Start the K4A sensor driver -->
  <group ns="k4a" >

    <!-- Spawn a nodelet manager -->
    <node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen">
      <param name="num_worker_threads" value="16" />
    </node>

    <!-- Spawn an image_proc/rectify nodelet to rectify the RGB image -->
    <node pkg="nodelet" type="nodelet" name="rectify_rgb"
          args="load image_proc/rectify manager --no-bond"
          respawn="true">
      <remap from="image_mono"  to="rgb/image_raw" />
      <remap from="image_rect"  to="rgb/image_rect" />
    </node>

    <!-- Spawn an image_proc/rectify nodelet to rectify the depth image -->
    <node unless="$(arg color_enabled)" pkg="nodelet" type="nodelet" name="rectify_depth"
          args="load image_proc/rectify manager --no-bond"
          respawn="true">
      <param name="interpolation"  value="0" />
      <remap from="image_mono"  to="depth/image_raw" />
      <remap from="image_rect"  to="depth/image_rect" />
    </node>

    <!-- Spawn an image_proc/rectify nodelet to rectify the ir image -->
    <node unless="$(arg color_enabled)" pkg="nodelet" type="nodelet" name="rectify_ir"
          args="load image_proc/rectify manager --no-bond"
          respawn="true">
      <remap from="image_mono"  to="ir/image_raw" />
      <remap from="image_rect"  to="ir/image_rect" />
    </node>

    <node pkg="nodelet" type="nodelet" name="k4a_ros_bridge"
          args="load Azure_Kinect_ROS_Driver/K4AROSBridgeNodelet manager --no-bond"
          respawn="true">
      <param name="depth_enabled"     value="true" />
      <param name="depth_mode"        value="WFOV_2X2BINNED" />
      <param name="color_enabled"     value="$(arg color_enabled)" />
      <param name="color_resolution"  value="720P" />
      <param name="fps"               value="30" />
      <param name="point_cloud"       value="false" />
      <param name="rgb_point_cloud"   value="false" />
      <param name="required"          value="true" />
      <param name="imu_rate_target"   value="100" />
      <param name="rescale_ir_to_mono8"   value="true" />
    </node>

  </group>

  <!-- compute IMU quaternion -->
  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
    <param name="use_mag" value="false" />
    <param name="publish_tf" value="false" />
    <param name="world_frame" value="enu" />
    <remap from="/imu/data_raw" to="/k4a/imu" />
  </node>

  <!-- Start rtabmap_ros node -->
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg     if="$(arg color_enabled)" name="rgb_topic"          value="/k4a/rgb/image_rect" />
    <arg     if="$(arg color_enabled)" name="depth_topic"        value="/k4a/depth_to_rgb/image_raw" />
    <arg     if="$(arg color_enabled)" name="camera_info_topic"  value="/k4a/rgb/camera_info" />
    <arg unless="$(arg color_enabled)" name="rgb_topic"          value="/k4a/ir/image_rect" />
    <arg unless="$(arg color_enabled)" name="depth_topic"        value="/k4a/depth/image_rect" />
    <arg unless="$(arg color_enabled)" name="camera_info_topic"  value="/k4a/ir/camera_info" />
    <arg name="approx_sync" value="true" />
    <arg name="frame_id"    value="camera_base" />
    <arg      if="$(arg color_enabled)" name="args" value="--delete_db_on_start --GFTT/MinDistance 7 --Vis/CorGuessWinSize 40 --Optimizer/GravitySigma 0.3 --Odom/ResetCountdown 1" />
    <arg unless ="$(arg color_enabled)" name="args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/ResetCountdown 1" />
    <arg name="wait_imu_to_init"   value="true"/>

    <arg name="rtabmap_args"       value="-d --Rtabmap/DetectionRate 2"/>
    <arg name="localization"       value="true"/>
    <arg name="queue_size"         value="10000"/>
    <arg name="rtabmapviz"         value="false" /> 
    <arg name="rviz"               value="true" />
  </include>

</launch>

Expected behavior A Map in RViz.

Logs

Screenshots

Desktop (please complete the following information):

Additional context I have tried to purge the rtabmap_ros via apt and build it from source, still yield the same error.

ooeygui commented 3 years ago

Typically when we see this, the computer is processing the data coming off the Kinect. This can be for many reasons - including an incompatible USB controller, low end CPU, low end GPU, or other background processing which is affecting the utilization, causing timing gaps. I see in the output that the kinect is being reset, which is an indicator of unusually high latency on the USB bus.

vinesmsuic commented 3 years ago

hi, just managed to get myself a new laptop but ubuntu 18.04 cant run probably on that laptop (due to kernel version), so I had to switch to ubuntu 20.04.

I looked up https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1263 and people there figured out a way to install k4atools on ubuntu 20.04. But after I installed k4atools and try to use the Azure_Kinect_ROS_Driver, I get ERROR: cannot launch node of type [robot_state_publisher/state_publisher]: on whatever script I run.

Then I realized Azure_Kinect_ROS_Driver is only available up to melodic. I guess the error is due to the program is finding robot_state_publisher package of melodic instead of noetic. Please correct me if I made a wrong guess.

Will there be a noetic support on Azure_Kinect_ROS_Driver soon?

RoseFlunder commented 3 years ago

I guess noetic support would only make sense if there is official support for the SDK on Ububtu 20.04. but there isnt. Its only tested to work on 18.04. by Microsoft.