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.