sebastiangrans / ros2-point-cloud-demo Goto Github PK

Hi,
Thank you for this great demo.
I have used your subscriber and publisher and have successfully subscribed and published PointCloud2 mgs. However, I am unable to visualise them in RVIZ. Is it because theres no rgba channel ?

here is my code, thank you

pt_cloud_image = np.array(list(read_points(pt_cloud_msg)))
pt_cloud_image = np.reshape(pt_cloud_image,
        (rgb_image.shape[0],rgb_image.shape[1],pt_cloud_image.shape[1]))

resized_pt_cloud = cv2.resize(pt_cloud_image, self.dim, interpolation=cv2.INTER_AREA)

for mask in masks:
            x = np.dstack([pt_cloud[:,:,0]])
            y = np.dstack([pt_cloud[:,:,1]])
            z = np.dstack([pt_cloud[:,:,2]])

            mask = mask.detach().cpu().numpy()
            x_new = np.multiply(x[:,:,0],mask)
            y_new = np.multiply(y[:,:,0],mask)
            z_new = np.multiply(z[:,:,0],mask)
            pt_cloud = np.dstack([x_new, y_new, z_new])
            pt_cloud_msg = point_cloud(pt_cloud,'zed2_base_link')
            self.object_point_pub_.publish(pt_cloud_msg)

What I am trying to do is apply a mask to the point cloud to only get the points within that mask. Thank you for any support.

I have been trying to display in rviz2 Oak-D-Light stereo depth camera pointcloud2 topics to no avail so when I saw your github.io post I got excited to give it a try as an alternate pointcloud2 source.

I had to pip3 install opencd and use the command:

pi@WaLiPi5:~/wali_pi5/testws $ ros2 run pcd_demo pcd_publisher_node ~/wali_pi5/testws/install/pcd_demo/share/pcd_demo/resource/teapot.ply 

to run the publisher.

(I can't use the launch file as I am running ROS 2 inside a docker container on a Raspberry Pi 5 and the wayland - qt5 issues and my inexperience with docker, I have not even tried to run rviz in the container.

pi@WaLiPi5:~/wali_pi5/testws $ ros2 launch pcd_demo pcd_publisher_demo.launch.py 
[INFO] [launch]: All log files can be found below /home/pi/wali_pi5/c3ws/roslogs/2024-01-25-12-56-45-458225-WaLiPi5-767
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rviz2-1]: process started with pid [768]
[INFO] [pcd_publisher_node-2]: process started with pid [770]
[rviz2-1] qt.qpa.xcb: could not connect to display 
[rviz2-1] qt.qpa.plugin: Could not load the Qt platform plugin "xcb" in "" even though it was found.
[rviz2-1] This application failed to start because no Qt platform plugin could be initialized. Reinstalling the application may fix this problem.
[rviz2-1] 
[rviz2-1] Available platform plugins are: eglfs, linuxfb, minimal, minimalegl, offscreen, vnc, xcb.
[rviz2-1] 

I have rviz2 (on a separate host) for visualization, but I could not get the /pcd pointcloud2 to display.

If I set global options to map as shown in your post, I see:

[INFO] [1706207398.140837740] [rviz]: Message Filter dropping message: frame 'map' at time 0.000 for reason 'discarding message because the queue is full'

I'm not going to pursue this, just wanted to vent a little. You put together a great tutorial for which I thank you.

Regards,

slowrunner and "Wali" (Wallfollower Looking for Intelligence)

When I run ros2 run pcd_demo pcd_publisher_node /home/ubuntu/map.pcd
I got this error:

Traceback (most recent call last):
File "/home/ubuntu/ros2_ws/install/pcd_demo/lib/pcd_demo/pcd_publisher_node", line 11, in
load_entry_point('pcd-demo==0.0.1', 'console_scripts', 'pcd_publisher_node')()
File "/usr/lib/python3/dist-packages/pkg_resources/init.py", line 490, in load_entry_point
return get_distribution(dist).load_entry_point(group, name)
File "/usr/lib/python3/dist-packages/pkg_resources/init.py", line 2854, in load_entry_point
return ep.load()
File "/usr/lib/python3/dist-packages/pkg_resources/init.py", line 2445, in load
return self.resolve()
File "/usr/lib/python3/dist-packages/pkg_resources/init.py", line 2451, in resolve
module = import(self.module_name, fromlist=['name'], level=0)
ModuleNotFoundError: No module named 'pcd_demo.pcd_publisher'

The o3d.utility.Vector3dVector() line throws a runtime error without any description.

I'm guessing it's something to do with the conversion in pcd_as_numpy_array . The line doesn't throw an error though.

This alternative works though.

        gen = read_points(msg, skip_nans=True)
        int_data = list(gen)

        xyz = np.empty((len(int_data), 3))
        rgb = np.empty((len(int_data), 3))
        idx = 0
        for x in int_data:
            test = x[3] 
            # cast float32 to int so that bitwise operations are possible
            s = struct.pack('>f' ,test)
            i = struct.unpack('>l',s)[0]
            # you can get back the float value by the inverse operations
            pack = ctypes.c_uint32(i).value
            r = (pack & 0x00FF0000)>> 16
            g = (pack & 0x0000FF00)>> 8
            b = (pack & 0x000000FF)
            # prints r,g,b values in the 0-255 range
                        # x,y,z can be retrieved from the x[0],x[1],x[2]
            # xyz = np.append(xyz,[[x[0],x[1],x[2]]], axis = 0)
            # rgb = np.append(rgb,[[r,g,b]], axis = 0)
            xyz[idx] = [x[0],x[1],x[2]]
            rgb[idx] = [r,g,b]
            idx = idx + 1

        out_pcd = o3d.geometry.PointCloud()    
        out_pcd.points = o3d.utility.Vector3dVector(xyz)
        out_pcd.colors = o3d.utility.Vector3dVector(rgb)

Hey

thank you for the tutorial for point cloud use within ROS2 foxy. I have a few questions about pcd_demo. I have followed the steps in the readme file, and if I want to launch automatic mode I get errors:

LookupError: Could not find the resource 'pcd_publisher' of type 'packages'

And if I want to do manual start using ros2 run I get problem with dependency 'open3d', even though I have open3d installed with pip for python 3. I get the error for missing open3d:

ModuleNotFoundError: No module named 'open3d'

So I have question about the problem, is it due to colcon build or something else?
Thank you.

Hello! I am just writing to report that I get the following warning when trying to build in ros2 humble on ubuntu jammy.

➜  pcd_workspace source /opt/ros/humble/setup.zsh 
➜  pcd_workspace colcon build
Starting >>> pcd_demo
/usr/lib/python3/dist-packages/setuptools/dist.py:723: UserWarning: Usage of dash-separated 'script-dir' will not be supported in future versions. Please use the underscore name 'script_dir' instead
  warnings.warn(
/usr/lib/python3/dist-packages/setuptools/dist.py:723: UserWarning: Usage of dash-separated 'install-scripts' will not be supported in future versions. Please use the underscore name 'install_scripts' instead
  warnings.warn(
--- stderr: pcd_demo                   
/usr/lib/python3/dist-packages/setuptools/dist.py:723: UserWarning: Usage of dash-separated 'script-dir' will not be supported in future versions. Please use the underscore name 'script_dir' instead
  warnings.warn(
/usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
  warnings.warn(
---
Finished <<< pcd_demo [1.00s]

I am not sure when this deprecation will happen, but I just wanted to share in case someone has a version of setuptools where this does break