简体   繁体   中英

Is there a method I can utilize to visualize my pointcloud data I converted from PC2 in Rviz?

import hdbscan
import rospy
from rospy.core import rospyinfo
from sensor_msgs.msg import PointCloud, PointCloud2
import numpy as np
import pcl
import ros_numpy
import time
from hdbscan_py.msg import cluster_list 
from rospy.numpy_msg import numpy_msg

   
rospy.init_node('hdbscan')

#This is utilizing the data to cluster
def callback(data):
    tstart = time.time()
    pc = ros_numpy.numpify(data)
    points=np.zeros((pc.shape[0], 3))
    points[:,0]=pc['x']
    points[:,1]=pc['y']
    points[:,2]=pc['z']
    p = pcl.PointCloud(np.array(points, dtype=np.float32))
    

    #Utilizes PointCloud points generated to passthrough HDBSCAN
    clusterer = hdbscan.HDBSCAN(min_cluster_size=15, min_samples=1, alpha=1.3).fit(p)
    cluster_labels = clusterer.fit_predict(p)
    p = pcl.PointCloud(np.array(points, dtype=np.float32))
    cluster_list
    #cluster_cloud = pcl.PointCloud_PointXYZRGB()

    #cloudpoints = 
    #rospy.loginfo(cluster_labels)
    tstop = time.time()
    
    talker(cluster_labels, p)
    rospy.loginfo(tstop-tstart)

   

def talker(cluster_labels, p):
    pubClusters = rospy.Publisher('cluster', numpy_msg(p), queue_size=1)
    cluster_list_msg=cluster_list()
    #cluster=[]
    for cluster_index in range(np.size(p)):
        cluster_list_msg.cluster.append(p[cluster_index])
        #cluster_list_msg[cluster_labels[cluster_index]].append(p[cluster_index])
    
    rospy.loginfo('Test')
    pubClusters.publish(cluster_list_msg)
    #pub.publish(clusters_msg)

#This is subscribing to the sensor data
def listener():
    rospy.Subscriber('/wamv/sensors/lidars/lidar_wamv/points', PointCloud2, callback)
    rospy.spin()

if __name__=='__main__':
    listener()

===================================================== I am currently using custom msgs and truthfully. I don't even know if this is a good approach as I have been mostly using Ros wiki and other input from research online. There seems to be a lot of ways to do what I am doing, but ultimately I am struggling implementing them to my code. I read on a good method pcl::toROSmsg(), but that seems to not be compatable with my python code. I ultimately would like to verify my lidar code is coming in and that the standard hdbscan is working and clustering the data. I am using a rosbag of bouncing a single buoy and I get an error which i will paste below:

[ERROR] [1638228118.356593]: bad callback: <function callback at 0x7f1940b711f0>
Traceback (most recent call last):
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "dbscan_lidar.py", line 37, in callback
    talker(cluster_labels, p)
  File "dbscan_lidar.py", line 43, in talker
    pubClusters = rospy.Publisher('cluster', numpy_msg(p), queue_size=1)
  File "/usr/local/lib/python3.8/dist-packages/ros_numpy/numpy_msg.py", line 13, in numpy_msg
    res = _numpy_msg(cls)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/numpy_msg.py", line 87, in numpy_msg
    classdict = { '__slots__': msg_type.__slots__, '_slot_types': msg_type._slot_types,
AttributeError: 'pcl._pcl.PointCloud' object has no attribute '__slots__'

===========================================

I have 2 custom msgs as well which are cluster_list.msg and cluster.msg under cluster_list.msg I have cluster[] cluster_list and for cluster.msg I defined it as float32 cluster. I don't know if this is a great approach, but if any advice could be spared on how I can accomplish my goal. I am thankful.

When creating the publisher numpy_msg is expecting a type and you're giving it an instance of a type. If you would like to publish PointCloud data over that topic it should look like this:

pubClusters = rospy.Publisher('cluster', numpy_msg(PointCloud), queue_size=1)

Another note, since talker() is being called multiple times you should not be creating a publisher inside of it. Instead you should make pubClusters a global variable.

The technical post webpages of this site follow the CC BY-SA 4.0 protocol. If you need to reprint, please indicate the site URL or the original address.Any question please contact:yoyou2525@163.com.

 
粤ICP备18138465号  © 2020-2024 STACKOOM.COM