pythonrosrealsensegazebo-simurviz

Displaying gazebo realsense image using ROS, depth image looks weird and having different size from the raw


Trying to display images captured by realsense in my gazebo simulation onto an opencv window. But the depth image is showing colorful dispite rviz showing black and white. And the raw and depth image from the same cam have different size despite not resizing. I want the simulation to have the same output as the real scene realsense cam do. How can I fix it? Down below is my image displaying python codes and the launch file and the picture of the output images. Just in case here's the git:

https://github.com/brian2lee/forklift_test/tree/main

The realsense d435 add-on used in gazebo:

https://github.com/issaiass/realsense2_description https://github.com/issaiass/realsense_gazebo_plugin

Edit: The colored depth map has been solved by @Christoph Rackwitz, updated the code, now showing normal depth map but the size problem remains.

images (from top left, 1.opencv raw 2. opencv depth 3. rviz depth): enter image description here im_show.py:

#!/usr/bin/env python3
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class ImageConverter:

    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback)

    def callback(self, data):
        try:
            # Convert the ROS Image message to a CV2 image
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)
            return

        # Display the image in an OpenCV window
        cv2.imshow("Camera Image", cv_image)
        cv2.waitKey(3)

def main():
    rospy.init_node('image_converter', anonymous=True)
    ic = ImageConverter()

    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

img_show_depth.py:

#!/usr/bin/env python3
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class DepthImageConverter:

    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.callback)

    def callback(self, data):
        try:
            # Convert the ROS Image message to a CV2 depth image
            cv_image = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
        except CvBridgeError as e:
            print(e)
            return

        # Normalize the depth image to fall within 0-255 and convert it to uint8
        cv_image_norm = cv2.normalize(cv_image, None, 0, 255, cv2.NORM_MINMAX)
        depth_map = cv_image_norm.astype(np.uint8)

        # Display the depth image in an OpenCV window
        cv2.imshow("Depth Image", depth_map)
        cv2.waitKey(3)

def main():
    rospy.init_node('depth_image_converter', anonymous=True)
    dic = DepthImageConverter()

    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

gazebo.launch:

<?xml version="1.0"?>
<launch>

    <param name="robot_description" command="xacro '$(find forklift)/urdf/forklift.urdf.xacro'"/>
    <param name="pallet_obj" command="xacro '$(find pallet)/urdf/pallet.urdf.xacro'"/>
    
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
    <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>

    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find env_world)/world/test.world"/>

        <arg name="paused" value="false"/>
        <arg name="use_sim_time" value="true"/>
        <arg name="gui" value="true"/>
        <arg name="headless" value="false"/>
        <arg name="debug" value="false"/>
        
    </include>

    <node name="spawning_forklift" pkg="gazebo_ros" type="spawn_model" args="-urdf -model forklift -param robot_description -z 0.160253"/> 
    <node name="spawning_pallet" pkg="gazebo_ros" type="spawn_model" args="-urdf -model pallet -param pallet_obj -x 5 -z 0.001500  "/> 
    
    
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find forklift)/rviz/cam.rviz" required="true" />
    <!--
    <?  default rviz   ?>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find realsense2_description)/rviz/urdf.rviz" required="true" />
-->

    <node name="img" pkg="img" type="img_show.py" output="screen" args="$(find img)/src/img_show.py"/>
    <node name="img_depth" pkg="img" type="img_show_depth.py" output="screen" args="$(find img)/src/img_show_depth.py"/>
    <!--
    <node name="img_both" pkg="img" type="img_show_both.py" output="screen" args="$(find img)/src/img_show_both.py"/>
-->
</launch>

Solution

  • The colorization happens because you call applyColorMap().

    Your code shows no resizing of the images, no setup to make the windows resizable, and no fixed sizes going into the creation of any images. From what you present so far, I cannot nobody can say what's going on there. You should look into your ROS and gazebo parts.