No pointcloud data after "Semantic point cloud ready!"

[yashmewada9618]

System

• [x] Ubuntu 20.04.6 LTS
• [x] ROS Noetic
• [x] Python 3.8.10

I have been trying to use this repo to create a similar 3D Metric Semantic Map from the depth_image,color_image, and the segmented_image. The code has no errors, and it runs without any exit codes. But somehow it is not publishing the point cloud.

I am using a bag file to run the frames recorded using Intel Realsense D455. The semantic Segmented image is generated using the Segformer model and below is my launch file that publishes the Segmented image. To keep the frames the same I had to run a tf static transform publisher to publish the camera_link to the camera_depth_frame transform.

Also the segmented_image has 24 classes that were trained on RUGD Dataset.

<launch>
    <arg name="model" default="nvidia/segformer-b0-finetuned-ade-512-512"/>
    <arg name="ckpt" default="$(find segformer_custom)/checkpoints/lightning_logs/version_35/checkpoints/epoch=40-step=1927.ckpt"/>
    <arg name="annotations" default="$(find segformer_custom)/Annotations/RUGD_annotation-colormap.txt"/>
    <arg name="viz" default="true" doc="whether to run a rviz"/>

    <node pkg="tf" type="static_transform_publisher" name="color_broadcaster" args="0 0 0 0 0 0 camera camera_color_optical_frame 300"/>
    <node pkg="tf" type="static_transform_publisher" name="depth_broadcaster" args="0 0 0 0 0 0 camera camera_depth_optical_frame 300"/>

    <node name="semantic_seg" pkg="segformer_custom" type="semsegPublish.py" output="screen">
        <param name="model" value="$(arg model)"/>
        <param name="ckpt" value="$(arg ckpt)"/>
        <param name="annotations" value="$(arg annotations)"/>
    </node>
    <node pkg="rosbag" type="play" name="player" args="-l $(find segformer_custom)/Test_Bags/Thu_Jun-08-2023_11-56-40_EDT_0.bag"/>
    <node if="$(arg viz)"
        type="rviz" name="rviz" pkg="rviz"
        output="screen" required="false"
        launch-prefix="bash -c 'sleep 5; $0 $@' " 
        args="-d $(find segformer_custom)/rviz/display.rviz" />
</launch>

The published topics are as follows:

/camera/align_to_color/parameter_descriptions
/camera/align_to_color/parameter_updates
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/color/camera_info
/camera/color/image_raw
/camera/color/metadata
/camera/depth/camera_info
/camera/depth/color/points
/camera/depth/image_rect_raw
/camera/depth/metadata
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/extrinsics/depth_to_infra2
/camera/image_semseg
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra1/metadata
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/infra2/metadata
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/pointcloud/parameter_descriptions
/camera/pointcloud/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/clock
/diagnostics
/occupancy_map_2D
/octomap_full
/ouster/imu
/ouster/imu_packets
/ouster/lidar_packets
/ouster/nearir_image
/ouster/os_nodelet_mgr/bond
/ouster/points
/ouster/range_image
/ouster/reflec_image
/ouster/signal_image
/rosout
/rosout_agg
/semantic_pcl/semantic_pcl
/tf
/tf_static

I also changed the semantic_cloud.yaml (just changed the camera topics and not the Camera Intrinsics).

# Camera intrinsic matrix parameters
camera:
  fx: 195.74853990717293
  fy: 195.74853990717293
  cx: 240.0
  cy: 180.0

  width: 480
  height: 360

semantic_pcl:
  color_image_topic: "/camera/color/image_raw"
  semantic_image_topic: "/camera/image_semseg"
  depth_image_topic: "/camera/depth/image_rect_raw"
  point_type: 0
  frame_id: "camera"
  depth_noise_std: 0.0
  true_class_prob: 1.0

But after all this the output stucks at the below snippet.

Generate semantic point cloud.
Subscribed to color image topic: /camera/color/image_raw
Subscribed to semantic image topic: /camera/image_semseg
Subscribed to depth image topic: /camera/depth/image_rect_raw
Warning: TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained) for frame camera_depth_optical_frame at time 1686239823.184390 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained) for frame camera_color_optical_frame at time 1686239823.184488 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
720 720 480
Semantic point cloud ready!
^C[semantic_octomap-3] killing on exit
[semantic_cloud-2] killing on exit
Could not open /home/catkin_ws/src/SSMI/SSMI-Mapping/ for writing
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Am I missing something here !? Thanks.

[yashmewada9618]

After changing the static_transform_publisher for different topics it worked. Also, a weird thing I encountered was initially when the topics were given as per their names in the semantic_cloud.yaml file the point cloud that was publishing was the colored point cloud directly from the real sense frames and when I swapped the topics of semantic_image and color_image it worked perfectly.