Errors while trying to run nvblox with a Gemini335L

[nhathout]

So, i have been trying to get this example working with my Gemini 33L camera, and had to make quite a bit of edits to get everything looking correct in rqt node graph.

I remapped inputs to the nvblox and vslam nodes, as well as made sure my odometry_flattener package was being built. Everything looks fine now, but I noticed an issue when remapping the topic '/camera/depth/camera_info'. Whenever I correctly remap this topic, it breaks my program, ending in the error:

[component_container_mt-1] F0816 13:58:18.676508 11700 camera_cache.cpp:41] Check failed: camerasEqual(itr->second, camera)
[component_container_mt-1] *** Check failure stack trace: ***
[ERROR] [component_container_mt-1]: process has died [pid 11670, exit code -6, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=shared_nvblox_container'].

If I get rid of this remapping (which would render that topic an empty topic, or not published by the camera), the program runs but doesn't work as intended. It throws a bunch of messages in the terminal regarding to dropped frames and exceeding of frame thresholds.

Could I get some assistance with this? I will also insert my current rqt node graph (including the correct remapping for the above mentioned topic)

[jian-dong]

Hello, thank you for your feedback on this issue. I ran the code I pushed to GitHub yesterday, and I didn't notice that the topic mapping was incorrect. It should indeed map to /camera/depth/camera_info. I have already fixed it.

Please note that this is a test version, and our internal testing is also ongoing in parallel.

[nhathout]

Thanks for the reply.

I have a follow up issue though, I have connected all the topics to nodes correctly, and this now works for a colleague using a Gemini 335, but when I run the code I get a black screen for the color image in Rviz, as well as outputs in the terminal that look like the image pasted to this comment. Would you know any fix for this?

[jian-dong]

@nhathout Please try editing the file /sys/module/usbcore/parameters/usbfs_memory_mb using sudo vim and increase the value to 128. I believe this will resolve the issue.

[jian-dong]

Can you take a picture your Rviz2 UI

[nhathout]

Ok, let me try editing that file first and I will send the Rviz2 UI

[nhathout]

@jian-dong The issue is still not resolved. However, I did notice the vslam portion does work, meaning the camera knows when I move it, however I believe this issue arises due to the lack of color image.

when I run the orbbec camera alone using the package orbbec_camera and the launch file gemini_330_series.launch.py, the color image is able to show using rviz2, but when I run the whole orbbec/nvblox example, it doesnt seem to...

[jian-dong]

@nhathout I could run it on my computer, but the RGB image is black on your side. After carefully comparing the differences, the only discrepancy I noticed was the firmware version. Yours is 1.2.20, while my camera's firmware version is 1.3.25. Could you please upgrade your firmware and try again? Thank you!

[nhathout]

Yes, @jian-dong I fixed that last evening, which did fix the RGB image from showing, but I still get the same outputs in my terminal where the messages are dropped and delta between frames exceeds the threshold.. I will keep debugging.

[jian-dong]

Hi @nhathout

I don't know if the data lag or frame drops are caused by the large amount of data being transmitted by ROS2. Could you try adjusting the DDS settings according to this document ? I set these Fast DDS parameters on my computer and didn't experience any frame drops.

[nhathout]

Hi @jian-dong, that helped a lot thank you. Now I am not experiencing any frame drops.

By the way, I tried running this example completely ignoring the negotiated package and it worked perfectly fine, so I am pretty sure all of those steps are not necessary. I simply made sure all the required repositories are properly cloned, then made sure the relevant dependencies are all installed.

An important note is that I still have to comment out line 41 of the file /src/isaac_ros_nvblox/nvblox_ros/src/lib/camera_cache.cpp or else the program crashes at that line. Not sure why this happens but the example will crash at that line if not commented out.

After doing this I simply rebuilt all ros2 packages by running colcon build, then ran source install/setup.bash and then ros2 launch isaac_orbbec_launch orbbec_dynamics_example.launch.py and everything worked well without frame drops.

[jian-dong]

Hi @nhathout , I'm glad to hear everything is running now! Since I'm not familiar with NVBlox's internal logic, I need more time to figure out why that specific line is causing the crash. I appreciate your patience!

[nhathout]

No worries at all :)

[jian-dong]

Hi @nhathout, my colleague and I discussed it, and we believe that the line of code is checking whether the camera info of the depth and color cameras are the same. Could you please confirm if the depth_registration is set to true? Also, could you run the following commands to check if the camera info for the color and depth cameras are the same?

1. ros2 topic echo /camera/color/camera_info
2. ros2 topic echo /camera/depth/camera_info

Thanks!

[nhathout]

Hi @jian-dong,

Yes, the depth_registration is indeed set to true. Below I will paste screenshots of both echoed topics, however I am not quite sure at what specifically I should be looking for/at. But from what I can see they look like they are the same.

The left is the color topic, the right is the depth topic

[jian-dong]

Hi @nhathout,

Thank you very much for your information. I noticed that the two camera info topics are indeed the same. I reviewed the Nvblox code, and it seems that the topic subscription is initialized in src/isaac_ros_nvblox/nvblox_ros/src/lib/nvblox_human_node.cpp as follows:

void NvbloxNode::subscribeToTopics() {
  RCLCPP_INFO_STREAM(get_logger(), "NvbloxNode::subscribeToTopics()");

  // other initialization code...

  if (params_.use_depth) {
    for (int i = camera_index; i < params_.num_cameras; ++i) {
      // Subscription logic for depth camera
    }
  }
  if (params_.use_color) {
    for (int i = camera_index; i < params_.num_cameras; ++i) {
      // Subscription logic for color camera
    }
  }
}

In the file src/isaac_ros_nvblox/nvblox_ros/src/lib/camera_cache.cpp, line 41 checks if the current camera info matches the existing one. It compares parameters like u, v, fx, fy, width, height, corresponding to the k array in the CameraInfo message definition. According to your information, these values should be identical.

Here is the relevant comparison function:

bool camerasEqual(const nvblox::Camera & c1, const nvblox::Camera & c2) {
  bool equal = (c1.fu() == c2.fu()) && (c1.fv() == c2.fv()) && (c1.cu() == c2.cu()) &&
    (c1.cv() == c2.cv()) && (c1.width() == c2.width()) && (c1.height() == c2.height());

  return equal;
}

My option is to print the data to see where the discrepancy might be. I appreciate your help.

[nhathout]

Ok, thank you. So you're saying that I should just add prints within that function and find out where the parameters are initialized, and fix the initialization where the discrepancy is evident?

Also, I have a bit of an unrelated question, but I thought it would be worth asking you; do you happen to know how to display nvblox voxels instead of the nvblox mesh in rviz2? When I got this example to work with a realsense camera, the topic was just there, so I tried to check any differences in the visualization files (.rviz), but I don't seem to find any. I am not quite sure how to get the voxels to show.

I appreciate your help too :)

[jian-dong]

Yes, I mean in the CameraCache::update function, I suggest adding print statements to display the camera info during the CHECK process:

void CameraCache::update(const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg) {
  const Camera camera = conversions::cameraFromMessage(*camera_info_msg);

  // If the camera already exists in the cache, we check that it hasn't changed
  const auto itr = cache_.find(camera_info_msg->header.frame_id);
  if (itr != cache_.end()) {
    // CHECK(camerasEqual(itr->second, camera));
    if (!camerasEqual(itr->second, camera)) {
      auto old_camera = itr->second;
      auto new_camera = camera;
      std::cout << "Old camera info "
        << "fu: " << old_camera.fu() << " fv: " << old_camera.fv()
        << " cu: " << old_camera.cu() << " cv: " << old_camera.cv()
        << " width: " << old_camera.width() << " height: " << old_camera.height() << std::endl;
      std::cout << "New camera info "
        << "fu: " << new_camera.fu() << " fv: " << new_camera.fv()
        << " cu: " << new_camera.cu() << " cv: " << new_camera.cv()
        << " width: " << new_camera.width() << " height: " << new_camera.height() << std::endl;
    }
  } else {
    cache_[camera_info_msg->header.frame_id] = conversions::cameraFromMessage(*camera_info_msg);
  }
}

Also, in the initialization section (src/isaac_ros_nvblox/nvblox_ros/src/lib/camera_cache.cpp), add print statements to log the subscribed topic names:

void NvbloxNode::subscribeToTopics() {
  RCLCPP_INFO_STREAM(get_logger(), "NvbloxNode::subscribeToTopics()");

  constexpr int kQueueSize = 10;

  if (!params_.use_depth && !params_.use_lidar) {
    RCLCPP_WARN(
      get_logger(), "Nvblox is running without depth or lidar input, the cost maps and"
      " reconstructions will not update");
  }

  size_t camera_index = 0;
  if (isHumanMapping(params_.mapping_type)) {
    camera_index = 1;
  }

  const rclcpp::QoS input_qos =
    isaac_ros::common::AddQosParameter(*this, kDefaultInputQos_, "input_qos");
  std::string input_qos_str = kDefaultInputQos_;
  get_parameter("input_qos", input_qos_str);
  RCLCPP_INFO_STREAM(get_logger(), "Subscribing to input topics with QoS: " << input_qos_str);

  if (params_.use_depth) {
    for (int i = camera_index; i < params_.num_cameras; ++i) {
      const std::string base_name(kDepthTopicBaseNames[i]);
      std::cout << "Depth info topic: " << base_name + "/camera_info" << std::endl;
      camera_info_subs_.emplace_back(
        create_subscription<sensor_msgs::msg::CameraInfo>(
          base_name + "/camera_info", input_qos,
          std::bind(&CameraCache::update, &depth_camera_cache_, std::placeholders::_1)));

      nitros_image_subs_.emplace_back(
        std::make_shared<NitrosViewSubscriber>(
          this, base_name + "/image",
          nvidia::isaac_ros::nitros::nitros_image_32FC1_t::supported_type_name,
          std::bind(&NvbloxNode::depthImageCallback, this, std::placeholders::_1),
          nvidia::isaac_ros::nitros::NitrosStatisticsConfig(), input_qos));
    }
  }

  if (params_.use_color) {
    for (int i = camera_index; i < params_.num_cameras; ++i) {
      const std::string base_name(kColorTopicBaseNames[i]);
      std::cout << "Color info topic: " << base_name + "/camera_info" << std::endl;
      camera_info_subs_.emplace_back(
        create_subscription<sensor_msgs::msg::CameraInfo>(
          base_name + "/camera_info", input_qos,
          std::bind(&CameraCache::update, &color_camera_cache_, std::placeholders::_1)));

      nitros_image_subs_.emplace_back(
        std::make_shared<NitrosViewSubscriber>(
          this, base_name + "/image",
          nvidia::isaac_ros::nitros::nitros_image_rgb8_t::supported_type_name,
          std::bind(&NvbloxNode::colorImageCallback, this, std::placeholders::_1),
          nvidia::isaac_ros::nitros::NitrosStatisticsConfig(), input_qos));
    }
  }

  if (params_.use_lidar) {
    pointcloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
      "pointcloud", input_qos,
      std::bind(&NvbloxNode::pointcloudCallback, this, std::placeholders::_1));
  }
}

As for the second issue, I'm not sure at the moment, but I'll take a look at it tomorrow morning (UTC+8).

[jian-dong]

@jjiszjj 你明天也这样打印看一下。

[nhathout]

@jian-dong , I have now added the print statements (the first code block you sent me), and I can now see where the discrepancies are:

When I tried to edit the subscribeToTopics() function as you did, it actually didn't build correctly because of an unused variable kQueueSize, but it's ok I will now try to figure out how to fix the differences between the cameras' variables.

And no worries about the second issue, take your time.

[jian-dong]

Hi @nhathout,

I believe this issue may be due to a timing mismatch, where the camera driver fails to capture both the depth and color frames simultaneously, leading to the depth not being aligned with the color. I think this design might be flawed, and I've already informed my colleagues who are working on the SDK design. In the ROS camera driver, I discarded the depth frame when the color frame wasn't available, which might solve the problem. Sorry for any inconvenience this may have caused.

[jian-dong]

Hi @nhathout,

I believe this issue may be due to a timing mismatch, where the camera driver fails to capture both the depth and color frames simultaneously, leading to the depth not being aligned with the color. I think this design might be flawed, and I've already informed my colleagues who are working on the SDK design. In the ROS camera driver, I discarded the depth frame when the color frame wasn't available, which might solve the problem. Sorry for any inconvenience this may have caused.
@zhonghong322

[jian-dong]

Hi @nhathout, I updated the rviz2 config; I think the red box above is the voxels topic you mentioned.

[nhathout]

Hi @jian-dong , thank you so much for all your help, that is exactly what I was trying to figure out. I will let you know if I come across any other problems or have any questions.

And also let me know if you were able to correct the timing mismatch.

Again, really appreciate all the help.