NVIDIA-ISAAC-ROS / isaac_ros_visual_slam

Visual SLAM/odometry package based on NVIDIA-accelerated cuVSLAM
https://developer.nvidia.com/isaac-ros-gems
Apache License 2.0
862 stars 139 forks source link

cuVSLAM not publishing any points or point cloud #162

Open Shivam7Sharma opened 4 months ago

Shivam7Sharma commented 4 months ago

I will get straight to the point. The following are the topics:

shivam@shivam-ROG-Zephyrus-M16-GU604VI-GU604VI:~/turtlebot3_ws$ ros2 topic list
/camera_info_left
/camera_info_right
/clicked_point
/clock
/cmd_vel
/goal_pose
/imu
/initialpose
/joint_states
/odom
/parameter_events
/performance_metrics
/robot_description
/rosout
/scan
/stereo_camera/left/camera_info
/stereo_camera/left/image
/stereo_camera/right/camera_info
/stereo_camera/right/image
/tf
/tf_static
/visual_slam/imu
/visual_slam/status
/visual_slam/tracking/odometry
/visual_slam/tracking/slam_path
/visual_slam/tracking/vo_path
/visual_slam/tracking/vo_pose
/visual_slam/tracking/vo_pose_covariance
/visual_slam/vis/gravity
/visual_slam/vis/gravity_array
/visual_slam/vis/landmarks_cloud
/visual_slam/vis/localizer
/visual_slam/vis/localizer_loop_closure_cloud
/visual_slam/vis/localizer_map_cloud
/visual_slam/vis/localizer_observations_cloud
/visual_slam/vis/loop_closure_cloud
/visual_slam/vis/observations_cloud
/visual_slam/vis/pose_graph_edges
/visual_slam/vis/pose_graph_edges2
/visual_slam/vis/pose_graph_edges2_array
/visual_slam/vis/pose_graph_edges_array
/visual_slam/vis/pose_graph_nodes
/visual_slam/vis/slam_odometry
/visual_slam/vis/velocity

I am using two RGB cameras with a 48-mm baseline on a robot in Gazebo. Not that it matters, but I am running the simulation on the host and the slam on Docker. I can see the images in RVIZ inside the container. The following is the TF tree: Screenshot from 2024-05-28 19-28-58

What is the slam not publishing any points? I edited the frame names to the right frame name in the slam code. It looks like the following:

// Node parameters.
      denoise_input_images_(
          declare_parameter<bool>("denoise_input_images", false)),
      rectified_images_(declare_parameter<bool>("rectified_images", true)),
      enable_imu_fusion_(declare_parameter<bool>("enable_imu_fusion", false)),
      imu_params_{declare_parameter<double>("gyro_noise_density", 0.000244),
                  declare_parameter<double>("gyro_random_walk", 0.000019393),
                  declare_parameter<double>("accel_noise_density", 0.001862),
                  declare_parameter<double>("accel_random_walk", 0.003),
                  declare_parameter<double>("calibration_frequency", 200.0)},
      img_jitter_threshold_ms_(
          declare_parameter<double>("img_jitter_threshold_ms", 33.33)),
      imu_jitter_threshold_ms_(
          declare_parameter<double>("imu_jitter_threshold_ms", 10.0)),
      enable_verbosity_(declare_parameter<bool>("enable_verbosity", false)),
      force_planar_mode_(declare_parameter<bool>("force_planar_mode", false)),
      enable_observations_view_(
          declare_parameter<bool>("enable_observations_view", false)),
      enable_landmarks_view_(
          declare_parameter<bool>("enable_landmarks_view", false)),
      enable_debug_mode_(declare_parameter<bool>("enable_debug_mode", false)),
      debug_dump_path_(
          declare_parameter<std::string>("debug_dump_path", "/tmp/cuvslam")),
      enable_localization_n_mapping_(
          declare_parameter<bool>("enable_localization_n_mapping", true)),
      enable_slam_visualization_(
          declare_parameter<bool>("enable_slam_visualization", false)),
      input_base_frame_(
          declare_parameter<std::string>("input_base_frame", "base_link")),
      input_left_camera_frame_(declare_parameter<std::string>(
          "input_left_camera_frame", "left_camera_link")),
      input_right_camera_frame_(declare_parameter<std::string>(
          "input_right_camera_frame", "right_camera_link")),
      input_imu_frame_(
          declare_parameter<std::string>("input_imu_frame", "imu")),
      // frames hierarchy:
      // map - odom - base_link - ... - camera
      publish_odom_to_base_tf_(
          declare_parameter<bool>("publish_odom_to_base_tf", true)),
      publish_map_to_odom_tf_(
          declare_parameter<bool>("publish_map_to_odom_tf", true)),
      invert_odom_to_base_tf_(
          declare_parameter<bool>("invert_odom_to_base_tf", false)),
      invert_map_to_odom_tf_(
          declare_parameter<bool>("invert_map_to_odom_tf", false)),
      map_frame_(declare_parameter<std::string>("map_frame", "map")),
      odom_frame_(declare_parameter<std::string>("odom_frame", "odom")),
      base_frame_(declare_parameter<std::string>("base_frame", "base_link")),

      override_publishing_stamp_(
          declare_parameter<bool>("override_publishing_stamp", false)),

      path_max_size_(declare_parameter<int>("path_max_size", 1024)),

      msg_filter_queue_size_(
          declare_parameter<int>("msg_filter_queue_size", 100)),
      image_qos_(parseQosString(
          declare_parameter<std::string>("image_qos", "SENSOR_DATA"))),
      // Subscribers
      left_image_sub_(message_filters::Subscriber<ImageType>(
          this, "stereo_camera/left/image", image_qos_))
      left_camera_info_sub_(message_filters::Subscriber<CameraInfoType>(
          this, "stereo_camera/left/camera_info")),
      right_image_sub_(message_filters::Subscriber<ImageType>(
          this, "stereo_camera/right/image", image_qos_))
      right_camera_info_sub_(message_filters::Subscriber<CameraInfoType>(
          this, "stereo_camera/right/camera_info")),
      imu_sub_(
          create_subscription<ImuType>("visual_slam/imu", rclcpp::QoS(100),
                                       std::bind(&VisualSlamNode::ReadImuData,
                                                 this, std::placeholders::_1))), 

I have an RTX 4070 GPU. I created a static transform of map to odom because my simulation didn't have a map frame. Is the slam supposed to create the map frame?

Edit 1: Please look at the latest update on this issue on the Isaac Ros forum. I have posted this in the thread.

@hemalshahNV @jaiveersinghNV