def pcl_callback(ros_point_cloud_msg):
# TODO: Convert ROS msg of type PointCloud2 to PCL data PointXYZRGB format
pcl_data = pclh.ros_to_pcl(ros_point_cloud_msg)
# TODO: Statistical filter
stat_inlier, stat_outlier = flt.statistical_outlier_filter(pcl_data)
# TODO: Voxel Grid Downsampling
vox_filt = flt.voxel_downsampling(stat_inlier)
# TODO: PassThrough Filter
pass_filt = flt.pass_through_filter(vox_filt)
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers and outliers
cloud_table, cloud_objects = flt.ransac(pass_filt)
# TODO: Euclidean Clustering
cluster_indices = flt.euclidean_clustering(cloud_objects)
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_cloud = flt.visualize_clusters(cloud_objects, cluster_indices)
# TODO: Convert PCL data to ROS messages
ros_table_cloud = pclh.pcl_to_ros(cloud_table)
ros_objects_cloud = pclh.pcl_to_ros(cloud_objects)
ros_cluster_cloud = pclh.pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_table_pub.publish(ros_table_cloud)
pcl_objects_pub.publish(ros_objects_cloud)
pcl_cluster_pub.publish(ros_cluster_cloud)
def pointcloud_cb(self, ros_cloud):
ransac_start = time.clock()
filtered_cloud = ground_filter(ros_cloud)
ransac_end = time.clock()
pc_msg = pcl_to_ros(filtered_cloud, ros_cloud.header)
self.objects_pub.publish(pc_msg)
ece_start = time.clock()
cluster_indices = get_clusters(filtered_cloud)
ece_end = time.clock()
tracking_start = time.clock()
objects_loc = []
objects = []
#for all j clusters
print("# clusters ", len(cluster_indices))
for j, indices in enumerate(cluster_indices):
print("Cluster", j, ": # points ", len(indices))
sum_x = 0
sum_y = 0
sum_z = 0
#for all points in the cluster j
for i, indice in enumerate(indices):
sum_x = sum_x + filtered_cloud[indice][0]
sum_y = sum_y + filtered_cloud[indice][1]
sum_z = sum_z + filtered_cloud[indice][2]
x = sum_x / len(indices)
y = sum_y / len(indices)
z = sum_z / len(indices)
objects_loc.append([x, y, z])
obj = filtered_cloud.extract(indices, negative=False)
objects.append(obj)
pc_msg = pcl_to_ros(obj, ros_cloud.header)
self.object_pub.publish(pc_msg)
marker_array = MarkerArray()
for i, obj in enumerate(objects_loc):
marker_array.markers.append(
get_marker(i, ros_cloud.header.frame_id, obj))
self.track(marker_array)
tracking_end = time.clock()
print("RANSAC: ", ransac_end - ransac_start)
print("ECE: ", ece_end - ece_start)
print("Tracking: ", tracking_end - tracking_start)
print("Total: ", tracking_end - ransac_start)
def CB_msgPCL(msgPCL):
"""
ROS "/sensor_stick/point_cloud" subscription Callback handler
Handle the PointCloud ROS msg received by the "/sensor_stick/point_cloud"
This function is almost entirely unpacking/packing ROS messages and publishing.
The the unpacked input pcl is processed by Process_rawPCL(pclpcRawIn)
which returns the values that need to be packed and published
:param msgPCL:
:return:
"""
global g_callBackCount
g_callBackCount += 1
if (g_callBackCount % g_callBackSkip != 0):
return;
print "\rCBCount= {:05d}".format(g_callBackCount),
sys.stdout.flush()
DebugDumpMsgPCL(msgPCL)
# Extract pcl Raw from Ros msgPCL
pclpcRawIn = pcl_helper.ros_to_pcl(msgPCL)
#------- PROCESS RAW PCL-------------------------
labelRecs, pclpcObjects, pclpcTable, pclpcClusters = Process_rawPCL(pclpcRawIn)
detected_objects_labels = [] # For ros loginfo only
detected_objects = [] # For publish - for PROJ3!
for (labelText, labelPos, labelIndex) in labelRecs:
detected_objects_labels.append(labelText)
g_object_markers_pub.publish(marker_tools.make_label(labelText, labelPos, labelIndex ))
# Add detected object to the list of detected objects.
do = DetectedObject()
do.label = labelText
do.cloud = pclpcClusters
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Package Processed pcls into Ros msgPCL
msgPCLObjects = pcl_helper.pcl_to_ros(pclpcObjects)
msgPCLTable = pcl_helper.pcl_to_ros(pclpcTable)
msgPCLClusters = pcl_helper.pcl_to_ros(pclpcClusters)
# Publish everything
# This is the output you'll need to complete the upcoming project!
g_detected_objects_pub.publish(detected_objects) # THIS IS THE CRUCIAL STEP FOR PROJ3
g_pcl_objects_pub.publish(msgPCLObjects)
g_pcl_table_pub.publish(msgPCLTable)
g_pcl_cluster_pub.publish(msgPCLClusters)
def Process_msgPCL(msgPCL):
# Extract pcl Raw from Ros msgPCL
pclpcRawIn = pcl_helper.ros_to_pcl(msgPCL)
#------- PROCESS RAW PC-------------------------
pclpcObjects, pclpcTable, pclpcClusters = ProcessPCL(pclpcRawIn)
# Package Processed pcls into Ros msgPCL
msgPCLObjects = pcl_helper.pcl_to_ros(pclpcObjects)
msgPCLTable = pcl_helper.pcl_to_ros(pclpcTable)
msgPCLClusters = pcl_helper.pcl_to_ros(pclpcClusters)
return msgPCLObjects, msgPCLTable, msgPCLClusters
def do_euclidian_clustering(cloud):
# Euclidean Clustering
white_cloud = pcl_helper.XYZRGB_to_XYZ(cloud) # <type 'pcl._pcl.PointCloud'>
tree = white_cloud.make_kdtree() # <type 'pcl._pcl.KdTree'>
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02) # for hammer
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(250)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract() # indices for each cluster (a list of lists)
# Assign a color to each cluster
cluster_color = pcl_helper.random_color_gen()
#cluster_color = pcl_helper.get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], pcl_helper.rgb_to_float(cluster_color)])
# Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# publish to cloud
ros_cluster_cloud = pcl_helper.pcl_to_ros(cluster_cloud)
# save to local
pcl.save(cluster_cloud, 'cluster.pcd')
return cluster_cloud
def do_euclidian_clustering(cloud):
# Euclidean Clustering
white_cloud = pcl_helper.XYZRGB_to_XYZ(cloud)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(5)
ec.set_MinClusterSize(1)
ec.set_MaxClusterSize(3500)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
cluster_color = pcl_helper.random_color_gen()
#cluster_color = pcl_helper.get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
pcl_helper.rgb_to_float(cluster_color)
])
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
ros_cluster_cloud = pcl_helper.pcl_to_ros(cluster_cloud)
return cluster_cloud
def callback(input_ros_msg):
cloud = pcl_helper.ros_to_pcl(input_ros_msg)
#print("INPUT: ", cloud)
cloud = do_passthrough(cloud, "x", 0.0, 20.0)
cloud = do_passthrough(cloud, "y", -5.0, 5.0)
_, _, cloud = do_ransac_plane_normal_segmentation(cloud, 0.14)
#cloud = do_euclidian_clustering(cloud)
cloud_new = pcl_helper.pcl_to_ros(cloud)
pub.publish(cloud_new)
def callback(input_ros_msg):
cloud = pcl_helper.ros_to_pcl(input_ros_msg)
# print("INPUT: ", cloud)
LEAF_SIZE = 0.1
cloud = do_voxel_grid_downsampling(cloud, LEAF_SIZE)
# print("OUTPUT: ", cloud)
# print("")
cloud_new = pcl_helper.pcl_to_ros(cloud)
pub.publish(cloud_new)
def callback(input_ros_msg):
cloud = pcl_helper.ros_to_pcl(input_ros_msg)
print("INPUT: ", cloud, type(cloud))
cloud = pcl_helper.XYZRGB_to_XYZ(cloud)
mean_k = 10
thresh = 0.001
cloud = do_statistical_outlier_filtering(cloud, mean_k, thresh)
color = pcl_helper.random_color_gen()
cloud = pcl_helper.XYZ_to_XYZRGB(cloud, color)
cloud_new = pcl_helper.pcl_to_ros(cloud)
print("OUTPUT: ", cloud, type(cloud))
pub.publish(cloud_new)
def get_clusters(self, cloud):
#print("point cloud size = ",len(cloud.data), " bytes")
marker_array = MarkerArray()
pcl_cloud = ros_to_pcl(cloud)
#convert pointcloud_xyzrgb to pointcloud_xyz
# since pointcloud_xyzrgb does not support make_EuclideanClusterExtraction
xyz_pc = pcl.PointCloud()
xyz_pc.from_list( [ x[:3] for x in pcl_cloud.to_list() ] )
tree = xyz_pc.make_kdtree()
t1 = time.clock()
ec = xyz_pc.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance (0.005)
ec.set_MinClusterSize (200)
ec.set_MaxClusterSize (5000)
ec.set_SearchMethod (tree)
cluster_indices = ec.Extract()
t2 = time.clock()
#print("ECE took ", t2-t1)
#for all j clusters
print("# clusters ", len(cluster_indices))
for j, indices in enumerate(cluster_indices):
print("Cluster", j ,": # points ", len(indices))
sum_x = 0
sum_y = 0
sum_z = 0
#for all points in the cluster j
for i, indice in enumerate(indices):
sum_x = sum_x + xyz_pc[indice][0]
sum_y = sum_y + xyz_pc[indice][1]
sum_z = sum_z + xyz_pc[indice][2]
x = sum_x/len(indices)
y = sum_y/len(indices)
z = sum_z/len(indices)
label = "obj_" + str(j)
marker_array.markers.append(get_marker(j, label, cloud.header.frame_id, x, y, z))
obj = pcl_cloud.extract(indices, negative=False)
pc_msg = pcl_to_ros(obj, cloud.header)
self.pub.publish(pc_msg)
self.track(marker_array)
def callback(input_ros_msg):
cloud = pcl_helper.ros_to_pcl(input_ros_msg)
filter_axis = 'x'
axis_min = 0.0
axis_max = 20.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'y'
axis_min = -5.0
axis_max = 5.0
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
cloud_new = pcl_helper.pcl_to_ros(cloud)
pub.publish(cloud_new)
def Process_rawPCL(pclpcRawIn):
DebugDumpMsgPCL(pclpcRawIn)
pclRecs = [] # For dev/debug display. Container for point cloud records: tuple (pclObj, pclName# )
pclRecs.append((pclpcRawIn, "pclpcRawIn"))
pclRecsDownSampled = pclproc.PCLProc_DownSampleVoxels(pclpcRawIn)
pclRecs += pclRecsDownSampled
pclpcDownSampled, pclpcDownSampledName = pclRecsDownSampled[0]
# PassThrough Filter
pclRecsRansac = pclproc.PCLProc_Ransac(pclpcDownSampled)
pclRecs += pclRecsRansac
# Extract inliers and outliers
pclpcPassZ, pclpcPassZIn, pclpcPassZOut = pclRecsRansac[0][0], pclRecsRansac[1][0], pclRecsRansac[2][0]
pclpcTable, pclpcObjects = pclpcPassZIn, pclpcPassZOut # Rename for clarity
# Euclidean Clustering
pclpObjectsNoColor = pcl_helper.XYZRGB_to_XYZ(pclpcObjects)
clusterIndices, pclpcClusters = pclproc.PCLProc_ExtractClusters(pclpObjectsNoColor)
labelRecs = []
for index, pts_list in enumerate(clusterIndices):
# Get points for a single object in the overall cluster
pcl_cluster = pclpcObjects.extract(pts_list)
msgPCL_cluster = pcl_helper.pcl_to_ros(pcl_cluster) # Needed for histograms... would refactor
# Extract histogram features
chists = pclproc.compute_color_histograms(msgPCL_cluster, doConvertToHSV=True)
normals = get_normals(msgPCL_cluster)
nhists = pclproc.compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# CLASSIFY, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = g_clf.predict(g_scaler.transform(feature.reshape(1, -1)))
label = g_encoder.inverse_transform(prediction)[0]
# Accumulate label records for publishing (and labeling detected objects)
label_pos = list(pclpcObjects[pts_list[0]])
label_pos[2] += 0.3
labelRecs.append((label, label_pos, index))
return labelRecs, pclpcObjects, pclpcTable, pclpcClusters
def detect_objects(cloud_objects, cluster_indices):
# TODO: Classify the clusters!
detected_objects_labels = []
detected_objects = []
labeled_features = []
for index, pts_list in enumerate(cluster_indices):
# TODO: Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: Convert the cluster from pcl to ROS using helper function
ros_cluster = pclh.pcl_to_ros(pcl_cluster)
# TODO: Extract histogram features
hists_color = ft.compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
hists_normals = ft.compute_normal_histograms(normals)
feature = np.concatenate((hists_color, hists_normals))
#labeled_features.append([feature, model_name])
# TODO: Make the prediction
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
# TODO: Retrieve the label for the result
label = encoder.inverse_transform(prediction)[0]
# TODO: Add it to detected_objects_labels list
detected_objects_labels.append(label)
# TODO: Publish a label into RViz
white_cloud = pclh.XYZRGB_to_XYZ(cloud_objects)
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .2
object_markers_pub.publish(make_label(label, label_pos, index))
# TODO: Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
return detected_objects, detected_objects_labels
def callback(input_ros_msg):
cloud = pcl_helper.ros_to_pcl(input_ros_msg)
#cloud = pcl_helper.XYZRGB_to_XYZ(cloud)
#cloud_denoised = do_statistical_outlier_filtering(cloud)
#cloud_downsampled = do_voxel_grid_downsampling(cloud)
cloud_roi_x = do_passthrough(cloud, 'x', 0.0, 20.0)
cloud_roi_y = do_passthrough(cloud_roi_x, 'y', -5.0, 5.0)
_, _, cloud_ransaced = do_ransac_plane_normal_segmentation(
cloud_roi_y, 0.007)
cloud_clustered = do_euclidian_clustering(cloud_ransaced)
cloud_new = pcl_helper.pcl_to_ros(cloud_clustered)
pub.publish(cloud_new)
def pc2CB(self, msg):
# Convert ROS PointCloud2 msg to PCL data
cloud = pcl_helper.ros_to_pcl(msg)
# Filter the Cloud according limitation on x-axis and y-axis
fil_cloud = filtering_helper.do_passthrough_filter(
cloud, 'x', -0.5, 0.0)
fil_cloud = filtering_helper.do_passthrough_filter(
fil_cloud, 'y', -0.5, 0.5)
print fil_cloud.size
# Visualize the filtered cloud
outmsg = pcl_helper.pcl_to_ros(fil_cloud, "base_link")
outmsg.header = msg.header
self.pc2_pub.publish(outmsg)
# Save the cloud
if self.init:
pcl.save(fil_cloud, "/home/samuel/charger_unit.pcd", None, False)
self.init = False
print "record finish"
def parsePoint(data, tlvLength): # Type : x06
point_data = np.zeros([tlvLength/struct.calcsize('4f'),3],dtype=np.float32)
for i in range(tlvLength/struct.calcsize('4f')):
if(struct.calcsize('4f') == len(data[16*i:16*i+16])):
parse_range, parse_angle, parse_doppler, parse_snr = struct.unpack('4f', data[16*i:16*i+16]) #struct.unpack('3H3h', data[4+12*i:4+12*i+12]) # heder 4, payload 12
#print("Point : {}, {}".format(parse_range, parse_angle))
x,y = rect(parse_range, parse_angle)
datas = np.array([x,y,0])
point_data[i,]=datas
#np.append(point_data, datas)
#point_data = np.vstack((point_data,datas))
#plt.scatter(x, y)
#plt.show()
#plt.pause(0.0001) #Note this correction
else:
#print("** parsePoint Size [{}]**".format(len(data[16*i:16*i+16])))
pass
pc = pcl.PointCloud(point_data)
pcl_xyzrgb = pcl_helper.XYZ_to_XYZRGB(pc, [255,255,255])
out_ros_msg = pcl_helper.pcl_to_ros(pcl_xyzrgb)
pub_point.publish(out_ros_msg)
def parseTargetList(data, tlvLength): # Type : x07
target_data = np.zeros([tlvLength/struct.calcsize('I16f'),3],dtype=np.float32) # 'I16f' for little endian , '>I16f' for big endian
for i in range(tlvLength/struct.calcsize('I16f')):
if(struct.calcsize('I16f') == len(data[68*i:68*i+68])):
tid, posX, posY, velX, velY, accX, accY, EC1, EC2,EC3,EC4,EC5,EC6,EC7,EC8,EC9, g = struct.unpack('I16f', data[68*i:68*i+68]) #76, 144, 212, 280
#print("Detect[{}] : {}, {}".format(tid, posX, posY))
print("Detect[{}] : {}, {}".format(tid, abs((int(posX*100))), abs(int(posY*100))))
datas = np.array([posX,posY,0])
target_data[i,]=datas
#np.append(taget_data, datas)
#point_data = np.vstack((taget_data,datas))
#plt.scatter(posX, posY)
#plt.show()
#plt.pause(0.0001) #Note this correction
else:
#print("** parseTargetList Size[{}]**".format(len(data[68*i:68*i+68])))
pass
pc = pcl.PointCloud(target_data)
pcl_xyzrgb = pcl_helper.XYZ_to_XYZRGB(pc, [255,255,255])
out_ros_msg = pcl_helper.pcl_to_ros(pcl_xyzrgb)
pub_track.publish(out_ros_msg)
def pc2CB(self, msg):
if self.init:
self.start_time = rospy.Time.now().to_sec()
print "-----------------"
# Convert ROS PointCloud2 msg to PCL data
cloud = pcl_helper.ros_to_pcl(msg)
# Get transform of "base_link" relative to "map"
trans = self.getTransform("map", "base_link")
# Filter the Cloud
fil_cloud = filtering_helper.do_passthrough_filter(
cloud, 'x', self.x_min, self.x_max)
fil_cloud = filtering_helper.do_passthrough_filter(
fil_cloud, 'y', self.y_min, self.y_max)
fil_cloud = filtering_helper.do_passthrough_filter(
fil_cloud, 'intensity', 450, 4096)
# Statistical outlier filter
fil = pcl_helper.XYZI_to_XYZ(
fil_cloud).make_statistical_outlier_filter()
fil.set_mean_k(10)
fil.set_std_dev_mul_thresh(0.05) #1.0 / 0.1
filtered_cloud = fil.filter()
pc2_msg = pcl_helper.pcl_to_ros(filtered_cloud, "base_link")
self.pc2_pub.publish(pc2_msg)
# Get groups of cluster of points
clusters = self.getClusters(filtered_cloud, 0.05, 10, 800)
print "len(clusters)", len(clusters)
for i in range(len(clusters)):
print "clusters[" + str(i) + "]", len(clusters[i])
# Not enough clusters to be identified as charger unit
if (len(clusters) < 3):
return
# self.middle_time_1 = rospy.Time.now().to_sec()
# Extract charger_wall & charger_pillars clusters
# - use for compute gradient of Line of Best Fit
# - use for compute middle_pt
wall_cluster, pillar_clusters = self.getChargerClusters(
clusters, filtered_cloud)
# self.middle_time_2 = rospy.Time.now().to_sec()
# Cannot find the charger_wall and charger_pillars
if (wall_cluster == None or pillar_clusters == []):
print "wall cluster or pillar clusters not found"
return
print "wall_cluster", len(wall_cluster)
self.vizPoint(
0, self.getMeanPointFromCluster(wall_cluster, filtered_cloud),
ColorRGBA(0, 1, 0, 1), "base_link")
for i in range(len(pillar_clusters)):
print "pillar_clusters[" + str(i) + "]", len(
pillar_clusters[i])
self.vizPoint(
i + 1,
self.getMeanPointFromCluster(pillar_clusters[i],
filtered_cloud),
ColorRGBA(0, 1, 0, 1), "base_link")
# Get Points of wall_cluster
wall_pts = self.getPointsFromCluster(wall_cluster, filtered_cloud)
# Turn wall_pts into map frame
for i in range(len(wall_cluster)):
wall_pts[i] = tf2_geometry_msgs.do_transform_pose(
self.toPoseStamped(wall_pts[i], Quaternion(0, 0, 0, 1)),
trans).pose.position
# Get gradient of the Line of Best Fit
m1 = self.getGradientLineOfBestFit(wall_pts)
print "gradient", m1
pillar_pts = []
# Get mean points from pillar_clusters
for cluster in pillar_clusters:
pillar_pts.append(
self.getMeanPointFromCluster(cluster, filtered_cloud))
# Finding 1 middle_pt from 2 charger_pillars and use it as path_pts
print pillar_pts
if (len(pillar_pts) == 2):
middle_pt = None
for p1 in pillar_pts:
for p2 in pillar_pts:
if (p1 == p2):
break
# Only register middle_pt of 2 charger_pillars points
if (0.5 < self.getDistance(p1, p2) < 0.65):
middle_pt = self.getMiddlePoint(p1, p2)
break
if (middle_pt == None):
"middle_pt not found"
return
# Turn middle_pt into map frame
middle_pt = tf2_geometry_msgs.do_transform_pose(
self.toPoseStamped(middle_pt, Quaternion(0, 0, 0, 1)),
trans).pose.position
# Register the middle_pt as one of the path_pts
path_pts = []
path_pts.append(middle_pt)
# Record the starting point and heading once
if self.record_once:
self.start_pt = trans.transform.translation
self.record_once = False
# Insert another point on the normal line with d[m] away from middle_pt
path_pts.insert(
0,
self.getPointOnNormalLine(m1, 1.5, middle_pt,
self.start_pt))
# Duplicate the path points to prevent from being edited
charger_pathpts = path_pts[:]
# Insert another point on the normal line with d[m] away from middle_pt
path_pts.insert(0,
self.getPointOnNormalLine(
m1, 0.29, middle_pt, self.start_pt)) #0.27
# Create a list with distance information between init_pt and path_pts
distance_list = [
self.getDistance(p, self.start_pt) for p in path_pts
]
# Rearrange the path_pts to be in ascending order
path_pts = self.getAscend(path_pts, distance_list)
# Add the AGV start_pt to the begin of path_pts
# path_pts.insert(0, self.start_pt)
# Remove last point of path_pts (middle pt of 2 charger feature pts)
path_pts = path_pts[:len(path_pts) - 1]
print "len(path_pts)", len(path_pts)
# # Visualize the path_pts
self.vizPoint(0, path_pts[0], ColorRGBA(1, 0, 0, 1),
"map") # Red
self.vizPoint(1, path_pts[1], ColorRGBA(1, 1, 0, 1),
"map") # Yellow
# self.vizPoint(2, path_pts[2], ColorRGBA(0,0,1,1), "map") # Blue
# Find the heading of the charger (last 2 path_pts)
heading_q = self.getOrientation(charger_pathpts[0],
charger_pathpts[1])
# Publish the path
self.route_pub.publish(
self.getPath2Charger(path_pts, heading_q))
print "start_time", self.start_time
# print "middle_time_1", self.middle_time_1
# print "middle_time_2", self.middle_time_2
print "end_time", rospy.Time.now().to_sec()
def pc2CB(self, msg):
if self.init:
self.start_time = rospy.Time.now().to_sec()
print "---------------------"
# Convert ROS PointCloud2 msg to PCL data
cloud = pcl_helper.ros_to_pcl(msg)
# Get transform of "base_link" relative to "map"
trans = self.getTransform("map", "base_link")
# Filter the Cloud
fil_cloud = filtering_helper.do_passthrough_filter(cloud, 'x', self.x_min, self.x_max)
fil_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'y', self.y_min, self.y_max)
fil_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'intensity', 330, 4096)
# Turn XYZI cloud into XYZ cloud
XYZ_cloud = pcl_helper.XYZI_to_XYZ(fil_cloud)
# Statistical outlier filter
fil = XYZ_cloud.make_statistical_outlier_filter()
fil.set_mean_k(5)
fil.set_std_dev_mul_thresh(0.2)
filtered_cloud = fil.filter()
pc2_msg = pcl_helper.pcl_to_ros(filtered_cloud, "base_link")
self.pc2_pub.publish(pc2_msg)
# Get groups of cluster of points
# clusters = self.getClusters(colorless_cloud, 0.05, 20, 250) # Table on Top AGV - 60~230
clusters = self.getClusters(filtered_cloud, 0.05, 5, 350)
print "len(clusters)", len(clusters)
for i in range(0,len(clusters)):
print "len(clusters[", i, "]", len(clusters[i])
# Not enough clusters to be identified as table unit
if(len(clusters)<4):
return
# Get mean points from clusters
mean_pts = []
for cluster in clusters:
mean_pts.append(self.getMeanPoint(cluster, filtered_cloud))
print "len(mean_pts)", len(mean_pts)
for i in range(0,len(mean_pts)):
print "mean_pts[", i, "]", mean_pts[i]
# Remove other points, leave the table points only
table_pts = []
table_pts = self.getTablePoints(mean_pts)
# print "len(table_pts)", len(table_pts)
# print table_pts
for i in range(len(table_pts)):
self.vizPoint(i, table_pts[i], ColorRGBA(1,1,1,1), "base_link")
# Finding 2 middle points from 4 table legs and use them as path_pts
path_pts = []
if(len(table_pts)==4):
for p1 in table_pts:
for p2 in table_pts:
if(p1==p2):
break
# Register 2 middle_pts of 4 table legs
if(0.73 < self.getDistance(p1, p2) < 0.83):
path_pts.append(self.getMiddlePoint(p1, p2))
break
# print "len(path_pts)", len(path_pts)
# print path_pts
# Path_pts that generated from between 2 legs of table must be fulfill below requirement, else return
if(len(path_pts)==2):
if not (0.33 < self.getDistance(path_pts[0], path_pts[1]) < 0.48):
return
# Turn path_pts into map frame
for i in range(len(path_pts)):
path_pts[i] = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(path_pts[i], Quaternion(0,0,0,1)), trans).pose.position
# Record the starting point once
if self.record_once:
self.start_pt = trans.transform.translation
self.record_once = False
# Create a list with distance information between initial_pt and path_pts
distance_list = [self.getDistance(p, self.start_pt) for p in path_pts]
# Rearrange the path_pts to be in ascending order
path_pts = self.getAscend(path_pts, distance_list)
# Duplicate the path_pts to prevent from being edited
table_pathpts = path_pts[:]
if(len(table_pathpts)==2):
# Get gradient of the Line of Best Fit
m1 = self.getGradientLineOfBestFit(table_pathpts)
# Insert another point on the line with d[m] away from the 1st table_pathpts
path_pts.insert(0, self.getPointOnLine(m1, 1.5, table_pathpts[0], self.start_pt))
# Insert a point ahead d[m] of table to the begin of path_pts
# ahead_pt = self.getPointAheadTable(table_pathpts, 1.0)
# path_pts.insert(0, ahead_pt)
# Insert a point ahead d[m] of table to the middle of table_pathpts
# midway_pt = self.getPointAheadTable(table_pathpts, -0.4)
# path_pts.insert(2, midway_pt)
# Insert another point on the line with d[m] away from the 1st table_pathpts
# path_pts.insert(2, self.getPointOnLine(m1, 0.05, table_pathpts[1], self.start_pt))
# Remove the last point of path_pts
# path_pts = path_pts[:len(path_pts)-1]
# Add the AGV start_pt to the begin of path_pts
# path_pts.insert(0, self.start_pt)
# Visualize the path_pts
self.vizPoint(10, path_pts[0], ColorRGBA(1,0,0,1), "map") # Red
self.vizPoint(11, path_pts[1], ColorRGBA(1,1,0,1), "map") # Yellow
self.vizPoint(12, path_pts[2], ColorRGBA(1,0,1,1), "map") # Magenta
# Find the heading of the table (last 2 path_pts)
heading_q = [self.getOrientation(path_pts[i], path_pts[i+1]) for i in range(len(path_pts)-1)]
heading_q.append(self.getOrientation(table_pathpts[0], table_pathpts[1]))
# print heading_q
# Publish the path
self.route_pub.publish(self.getPath2Table(path_pts, heading_q))
print "start_time", self.start_time
print "end_time", rospy.Time.now().to_sec()
def Process_rawPCL(pclpcRawIn):
DebugDumpMsgPCL(pclpcRawIn)
pclRecs = [
] # For dev/debug display. Container for point cloud records: tuple (pclObj, pclName# )
pclRecs.append((pclpcRawIn, "pclpcRawIn"))
pclRecsDownSampled = pclproc.PCLProc_DownSampleVoxels(pclpcRawIn)
pclRecs += pclRecsDownSampled
pclpcDownSampled, pclpcDownSampledName = pclRecsDownSampled[0]
# Region of Interest: PassThrough Filter
recsPassThruZ = pclproc.PCLProc_PassThrough(pclpcDownSampled, 'z', 0.6,
1.1)
pclRecs += recsPassThruZ
pclpcPassZ = recsPassThruZ[0][0]
recsPassThruY = pclproc.PCLProc_PassThrough(pclpcPassZ, 'y', -0.5, 0.5)
pclRecs += recsPassThruY
pclpcPassY = recsPassThruY[0][0]
# RANSAC Table/Object separation
pclRecsRansac = pclproc.PCLProc_Ransac(pclpcPassY)
pclRecs += pclRecsRansac
pclpcPassZIn, pclpcPassZOut = pclRecsRansac[0][0], pclRecsRansac[1][0]
pclpcTable, pclpcObjects = pclpcPassZIn, pclpcPassZOut # Rename for clarity
# Noise reduction
pclpRecsNoNoise = pclproc.PCLProc_Noise(pclpcObjects)
pclRecs += pclpRecsNoNoise
pclpNoColorNoNoise = pclpRecsNoNoise[0][0]
Debug_Publish(pclpNoColorNoNoise)
# Euclidean Clustering
pclpObjectsNoColor = pcl_helper.XYZRGB_to_XYZ(pclpcObjects)
clusterIndices, pclpClusters = pclproc.PCLProc_ExtractClusters(
pclpObjectsNoColor)
#clusterIndices, pclpClusters = pclproc.PCLProc_ExtractClusters(pclpNoColorNoNoise)
labelRecs = []
for index, pts_list in enumerate(clusterIndices):
# Get points for a single object in the overall cluster
pcl_cluster = pclpcObjects.extract(pts_list)
msgPCL_cluster = pcl_helper.pcl_to_ros(
pcl_cluster) # Needed for histograms... would refactor
# Extract histogram features
chists = pclproc.compute_color_histograms(msgPCL_cluster,
g_numHistBinsHSV,
doConvertToHSV=True)
normals = get_normals(msgPCL_cluster)
nhists = pclproc.compute_normal_histograms(normals,
g_numHistBinsNormals)
feature = np.concatenate((chists, nhists))
# CLASSIFY, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = g_clf.predict(g_scaler.transform(feature.reshape(1, -1)))
label = g_encoder.inverse_transform(prediction)[0]
# Accumulate label records for publishing (and labeling detected objects)
label_pos = list(pclpcObjects[pts_list[0]])
label_pos[0] -= 0.1
label_pos[2] += 0.2
labelRecs.append(
(label, label_pos, index,
pcl_cluster)) # Don't like passing pcl_cluster in this Records...
return labelRecs, pclpcObjects, pclpcTable, pclpClusters
def pcl_callback(ros_pcl_msg):
# Exercise-1 TODOs:
# TODO: Load point cloud data
# TODO: Voxel Grid Downsampling
# TODO: PassThrough Filter
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers and outliers
# TODO: Save the inliers in .pcd file
# TODO: Run the inliers.pcd file on terminal
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
# TODO: Voxel Grid Downsampling
# TODO: PassThrough Filter
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers and outliers
# TODO: Euclidean Clustering
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# TODO: Convert PCL data to ROS messages
# TODO: Publish ROS messages
# Exercise-3 TODOs:
# TODO: Classify the clusters! (loop through each detected cluster one at a time)
# TODO: Grab the points for the cluster
# TODO: Compute the associated feature vector
# TODO: Make the prediction
# TODO: Publish a label into RViz
# TODO: Add the detected object to the list of detected objects.
# TODO: Publish the list of detected objects
# TODO: Convert ROS msg of type PointCloud2 to PCL data PointXYZRGB format
pcl_data = pclh.ros_to_pcl(ros_pcl_msg)
# TODO: Statistical filter
stat_inlier, stat_outlier = flt.statistical_outlier_filter(pcl_data)
pcl_data = stat_inlier
# TODO: Voxel Grid Downsampling
vox_filt = flt.voxel_downsampling(pcl_data)
# TODO: PassThrough Filter
pass_filt = flt.pass_through_filter(vox_filt)
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers (objects) and outliers (table)
cloud_table, cloud_objects = flt.ransac(pass_filt)
# TODO: Euclidean Clustering
cluster_indices = flt.euclidean_clustering(cloud_objects)
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_cloud = flt.visualize_clusters(cloud_objects, cluster_indices)
# TODO: Convert PCL data to ROS messages
ros_table_cloud = pclh.pcl_to_ros(cloud_table)
ros_objects_cloud = pclh.pcl_to_ros(cloud_objects)
ros_cluster_cloud = pclh.pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_table_pub.publish(ros_table_cloud)
pcl_objects_pub.publish(ros_objects_cloud)
pcl_cluster_pub.publish(ros_cluster_cloud)
# TODO: Classify the clusters!
detected_objects_labels = []
detected_objects = []
labeled_features = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: convert the cluster from pcl to ROS using helper function
ros_cluster = pclh.pcl_to_ros(pcl_cluster)
# Extract histogram features
# TODO: complete this step just as is covered in capture_features.py
# Extract histogram features
hists_color = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
hists_normals = compute_normal_histograms(normals)
feature = np.concatenate((hists_color, hists_normals))
#labeled_features.append([feature, model_name])
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(feature.reshape(1,-1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
white_cloud = pclh.XYZRGB_to_XYZ(cloud_objects)
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label,label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)
def do_euclidian_clustering(self, cloud):
# Euclidean Clustering
white_cloud = pcl_helper.XYZRGB_to_XYZ(cloud)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.3) #0.14, 0.08
ec.set_MinClusterSize(1)
ec.set_MaxClusterSize(3500)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
cluster_color = pcl_helper.random_color_gen()
#cluster_color = pcl_helper.get_color_list(len(cluster_indices))
#print("No. of cones visible at this moment ", len(cluster_indices))
#print(cluster_indices)
cluster_coords = Track()
#cone = TrackCone()
#cluster_coords.cones = []
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
x, y, z = 0, 0, 0
cone_clusters = []
for i, indice in enumerate(indices):
color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], pcl_helper.rgb_to_float(cluster_color)])
x = x + white_cloud[indice][0]
y = y + white_cloud[indice][1]
z = z + white_cloud[indice][2]
cone_clusters.append(x/len(indices) + self.carPosX)
cone_clusters.append(y/len(indices) + self.carPosY)
print(cone_clusters)
#print(color_cluster_point_list)
#cone.x = x/len(indices)
#cone.y = y/len(indices)
#cone.type = f'cone{j+1}'
#cluster_coords.cones = cone_clusters
#cluster_coords.data.append(TrackCone(data = cone_clusters))
cluster_coords.data.append(TrackCone(x = x/len(indices) + self.carPosX,
y = y/len(indices) + self.carPosY))
#cluster_coords.cones[j].x = x/len(indices)
#cluster_coords.cones[j].y = y/len(indices)
#cluster_coords.cones[j].type = 'cone{k}'
#print(len(cluster_coords.cones))
#print(len(cluster_coords.cones[0]))
#print("No. of cones visible at this moment ", len(color_cluster_point_list))
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
ros_cluster_cloud = pcl_helper.pcl_to_ros(cluster_cloud)
return cluster_cloud, cluster_coords
def Debug_Publish(pclIn):
pclpcIn = pcl_helper.XYZ_to_XYZRGB(pclIn, (0, 255, 0))
msgPCL = pcl_helper.pcl_to_ros(pclpcIn)
g_pcl_debug_pub.publish(msgPCL)
def pc2CB(self, msg):
if self.init:
# Convert ROS PointCloud2 msg to PCL data
cloud = pcl_helper.ros_to_pcl(msg)
# Get transform of "base_link" relative to "map"
trans = self.getTransform("map", "base_link")
# Filter the Cloud
fil_cloud = filtering_helper.do_passthrough_filter(
cloud, 'x', -3.0, 3.0)
fil_cloud = filtering_helper.do_passthrough_filter(
fil_cloud, 'y', -3.0, 3.0)
# Turn cloud into colorless cloud
colorless_cloud = pcl_helper.XYZRGB_to_XYZ(fil_cloud)
# source_cloud = colorless_cloud
# Get groups of cluster of points
clusters = self.getClusters(colorless_cloud, 0.05, 100, 450)
# print "len(clusters)", len(clusters)
source_cloud = self.getCloudFromCluster(clusters[0],
colorless_cloud)
middle_pt = self.getMeanPoint(clusters[0], colorless_cloud)
self.vizPoint(0, middle_pt, ColorRGBA(1, 1, 1, 1), "base_link")
icp = source_cloud.make_IterativeClosestPoint()
converged, transform, estimate, fitness = icp.icp(
source_cloud, self.target_cloud, 100)
# gicp = colorless_cloud.make_GeneralizedIterativeClosestPoint()
# converged, transform, estimate, fitness = gicp.gicp(colorless_cloud, self.target_cloud, 100)
# icp_nl = colorless_cloud.make_IterativeClosestPointNonLinear()
# converged, transform, estimate, fitness = icp_nl.icp_nl(self.target_cloud, colorless_cloud, 100)
# if not converged:
print "converged", converged
print "transform", transform
print "estimate", estimate
print "fitness", fitness
# new_transform = transform.inverse()
# print new_transform
translation = Vector3(transform[0, 3], transform[1, 3],
transform[2, 3])
q = tf.transformations.quaternion_from_matrix(transform)
rotation = Quaternion(q[0], q[1], q[2], q[3])
roll, pitch, yaw = tf.transformations.euler_from_matrix(transform)
print np.rad2deg(yaw)
# print "translation\n", translation
# print "rotation\n", rotation
# transformation = self.toTransformStamped(translation, rotation, "map", "base_link")
print "------------------------------------------------"
# Visualize the cloud
outmsg = pcl_helper.pcl_to_ros(estimate, "map")
outmsg.header = msg.header
self.pc2_pub.publish(outmsg)
def pcl_callback(ros_pcl_msg):
# Exercise-1 TODOs:
# TODO: Load point cloud data
# TODO: Voxel Grid Downsampling
# TODO: PassThrough Filter
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers and outliers
# TODO: Save the inliers in .pcd file
# TODO: Run the inliers.pcd file on terminal
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
# TODO: Voxel Grid Downsampling
# TODO: PassThrough Filter
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers and outliers
# TODO: Euclidean Clustering
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# TODO: Convert PCL data to ROS messages
# TODO: Publish ROS messages
# Exercise-3 TODOs:
# TODO: Classify the clusters! (loop through each detected cluster one at a time)
# TODO: Grab the points for the cluster
# TODO: Compute the associated feature vector
# TODO: Make the prediction
# TODO: Publish a label into RViz
# TODO: Add the detected object to the list of detected objects.
# TODO: Publish the list of detected objects
# TODO: Convert ROS msg of type PointCloud2 to PCL data PointXYZRGB format
pcl_data = pclh.ros_to_pcl(ros_pcl_msg)
# TODO: Statistical filter
stat_inlier, stat_outlier = flt.statistical_outlier_filter(pcl_data)
# TODO: Voxel Grid Downsampling
vox_filt = flt.voxel_downsampling(stat_inlier)
# TODO: PassThrough Filter
pass_filt = flt.pass_through_filter(vox_filt)
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers (objects) and outliers (table)
cloud_table, cloud_objects = flt.ransac(pass_filt)
# TODO: Euclidean Clustering
cluster_indices = flt.euclidean_clustering(cloud_objects)
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_cloud = flt.visualize_clusters(cloud_objects, cluster_indices)
# TODO: Classify the clusters and create list of detected objects
detected_objects, detected_objects_labels = detect_objects(cloud_objects, cluster_indices)
# TODO: Convert PCL data to ROS messages
ros_table_cloud = pclh.pcl_to_ros(cloud_table)
ros_objects_cloud = pclh.pcl_to_ros(cloud_objects)
ros_cluster_cloud = pclh.pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_table_pub.publish(ros_table_cloud)
pcl_objects_pub.publish(ros_objects_cloud)
pcl_cluster_pub.publish(ros_cluster_cloud)
detected_objects_pub.publish(detected_objects)
def get_clusters(self, cloud):
print("point cloud size = ",len(cloud.data), " bytes")
marker_array = MarkerArray()
pcl_cloud = ros_to_pcl(cloud)
#convert pointcloud_xyzrgb to pointcloud_xyz
# since pointcloud_xyzrgb does not support make_EuclideanClusterExtraction
xyz_pc = pcl.PointCloud()
xyz_pc.from_list( [ x[:3] for x in pcl_cloud.to_list() ] )
tree = xyz_pc.make_kdtree()
ec = xyz_pc.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance (0.01)
ec.set_MinClusterSize (100)
ec.set_MaxClusterSize (25000)
ec.set_SearchMethod (tree)
cluster_indices = ec.Extract()
indx = []
#for all j clusters
print("# clusters ", len(cluster_indices))
for j, indices in enumerate(cluster_indices):
tmp_indx = []
print("# points ", len(indices))
min_x = 999.9
max_x = -999.9
min_y = 999.9
max_y = -999.9
min_z = 999.9
max_z = -999.9
#for all points in the cluster j
for i, indice in enumerate(indices):
tmp_indx.append(indice)
if xyz_pc[indice][0] < min_x:
min_x = xyz_pc[indice][0]
elif xyz_pc[indice][0] > max_x:
max_x = xyz_pc[indice][0]
if xyz_pc[indice][1] < min_y:
min_y = xyz_pc[indice][1]
elif xyz_pc[indice][1] > max_y:
max_y = xyz_pc[indice][1]
if xyz_pc[indice][2] < min_z:
min_z = xyz_pc[indice][2]
elif xyz_pc[indice][2] > max_z:
max_z = xyz_pc[indice][2]
x = (min_x + max_x)/2
y = (min_y + max_y)/2
z = (min_z + max_z)/2
p = get_stamped_point(x, y, z, cloud.header.frame_id)
label = "obj_" + str(j)
marker_array.markers.append(get_marker(j, label, cloud.header.frame_id, x, y, z))
obj = pcl_cloud.extract(indices, negative=False)
pc_msg = pcl_to_ros(obj, stamp=cloud.header.stamp, frame_id=cloud.header.frame_id, seq=cloud.header.seq)
converter = PointCloudToImages('/home/phiggin1/deep_rgbd_v01.2/data/')
converter.convert(obj)
fuse_color_depth()
features = get_features()
self.pub.publish(pc_msg)
print("object at ", p, "\nwith features: ",features)
self.obj_markers_pub.publish(marker_array)
def pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# create a voxelgrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
LEAF_SIZE = 0.01
# set the voxel (or leaf)size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
#filename = 'voxel_downsampled.pcd'
#pcl.save(cloud_filtered, filename)
# PassThrough filter
# create pass through filter object
passthrough = cloud_filtered.make_passthrough_filter()
# assign axis and ranve to the passthrough filter object
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
# generate the resultant point cloud
cloud_filtered = passthrough.filter()
# RANSAC plane segmentation
# create the segmentation object
seg = cloud_filtered.make_segmenter()
# set the model to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
# Extract table
cloud_table = cloud_filtered.extract(inliers, negative=False)
#filename = 'cloud_table.pcd'
#pcl.save(cloud_table, filename)
# Extract objects
#cloud_objects = cloud_filtered.extract(inliers, negative=True)
# Extract outliers
# create the filter objectd
objects = cloud_filtered.extract(inliers, negative=True)
outlier_filter = objects.make_statistical_outlier_filter()
# set the number of points to analyze for any given point
outlier_filter.set_mean_k(50)
# set threshold scale factor
x = 1.0
# any point with a mena distance large than the global (mean distance + x * std_dev)
# will be considered outlier
outlier_filter.set_std_dev_mul_thresh(x)
# call the filter function
cloud_objects = outlier_filter.filter()
filename = 'cloud_objects.pcd'
pcl.save(cloud_objects, filename)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# create cluster extraction
ec = white_cloud.make_EuclideanClusterExtraction()
# set the tolerances for
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(2000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# asign colors to the individual point cloud indices for visualization
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
rgb_to_float(cluster_color[j])
])
# create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
def pc2CB(self, msg):
if self.init:
print "---------------------"
# Convert ROS PointCloud2 msg to PCL data
cloud = pcl_helper.ros_to_pcl(msg)
# Get transform of "base_link" relative to "map"
trans = self.getTransform("map", "base_link")
os1_trans = self.getTransform("map", "os1_sensor")
# Filter the Cloud
fil_cloud = filtering_helper.do_passthrough_filter(cloud, 'x', self.x_min, self.x_max)
fil_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'y', self.y_min, self.y_max)
fil_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'intensity', 330, 4096)
# Turn XYZI cloud into XYZ cloud
XYZ_cloud = pcl_helper.XYZI_to_XYZ(fil_cloud)
# Statistical outlier filter
fil = XYZ_cloud.make_statistical_outlier_filter()
fil.set_mean_k(10)
fil.set_std_dev_mul_thresh(0.1)
filtered_cloud = fil.filter()
pc2_msg = pcl_helper.pcl_to_ros(filtered_cloud, "base_link")
self.pc2_pub.publish(pc2_msg)
# Get groups of cluster of points
clusters = self.getClusters(filtered_cloud, 0.05, 5, 350)
print "len(clusters)", len(clusters)
for i in range(0,len(clusters)):
print "len(clusters[", i, "]", len(clusters[i])
# Get mean points from clusters
mean_pts = []
for cluster in clusters:
mean_pts.append(self.getMeanPoint(cluster, filtered_cloud))
print "len(mean_pts)", len(mean_pts)
# for i in range(0,len(mean_pts)):
# print "mean_pts[", i, "]", mean_pts[i]
# Remove other points, leave the table points only
table_pts = []
table_pts = self.getTablePoints(mean_pts)
print "len(table_pts)", len(table_pts)
# print table_pts
for i in range(len(table_pts)):
self.vizPoint(i, table_pts[i], ColorRGBA(1,1,1,1), "base_link")
# Finding 2 middle points from 4 table legs and use them as path_pts
path_pts = []
if(len(table_pts)==4):
for p1 in table_pts:
for p2 in table_pts:
if(p1==p2):
break
# Register 2 middle_pts of 4 table legs
if(0.73 < self.getDistance(p1, p2) < 0.83):
path_pts.append(self.getMiddlePoint(p1, p2))
break
# print "len(path_pts)", len(path_pts)
# print path_pts
# Turn path_pts into map frame
for i in range(len(path_pts)):
path_pts[i] = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(path_pts[i], Quaternion(0,0,0,1)), trans).pose.position
path_pts[i] = Point(path_pts[i].x, path_pts[i].y, 0)
# Record the starting point and heading once
if self.record_once:
# self.start_pt = trans.transform.translation
self.start_pt = os1_trans.transform.translation
self.record_once = False
# Create a list with distance information between initial_pt and path_pts
distance_list = [self.getDistance(p, self.start_pt) for p in path_pts]
# Rearrange the path_pts to be in descending order
path_pts = self.getDescend(path_pts, distance_list)
# Duplicate the path_pts to prevent from being edited
table_pathpts = path_pts[:]
# print "table_pathpts: ", len(table_pathpts)
if(len(table_pathpts)==2):
# Get gradient of the Line of Best Fit
m1 = self.getGradientLineOfBestFit(table_pathpts)
# Insert another point on the line with d[m] away from the 2nd table_pathpts
path_pts.append(self.getPointOnLine(m1, 1.0, table_pathpts[1], self.start_pt))
# Visualize the path_pts
self.vizPoint(10, path_pts[0], ColorRGBA(1,0,0,1), "map") # Red
self.vizPoint(11, path_pts[1], ColorRGBA(1,1,0,1), "map") # Yellow
self.vizPoint(12, path_pts[2], ColorRGBA(1,0,1,1), "map") # Magenta
# print len(path_pts)
# Find the heading of the table (last 2 path_pts)
heading_q = self.getOrientation(table_pathpts[0], table_pathpts[1])
# Publish the path
self.route_pub.publish(self.getPath2Table(path_pts, heading_q))
pc_xyzrgb_list = []
for line in lines:
pc_xyzrgb = line.strip().split(',')
pc_xyz = [float(pc_xyzrgb[0]), float(pc_xyzrgb[1]), float(pc_xyzrgb[2])]
color = [int(pc_xyzrgb[3]), int(pc_xyzrgb[4]), int(pc_xyzrgb[5])]
#pc_list.append(pc_xyz)
#color_list.append(color)
float_color = pcl_helper.rgb_to_float(color)
pc_xyzrgb_list.append([pc_xyz[0], pc_xyz[1], pc_xyz[2], float_color])
#np_pc_list = np.asarray(pc_list)
#np_color = np.asarray(color_list)
#msg_pc = pcl_helper.xyzrgb_array_to_pointcloud2(points=np_pc_list, colors=np_color, stamp=time, frame_id='map')
XYZRGB_cloud = pcl.PointCloud_PointXYZRGB()
XYZRGB_cloud.from_list(pc_xyzrgb_list)
msg_pc = pcl_helper.pcl_to_ros(XYZRGB_cloud)
time = rospy.Time.now()
write_rosbag.write(topic='/soslab_sl/pointcloud', msg=msg_pc, t=time)
r.sleep()
write_rosbag.close()
def pointcloud_cb(self, cloud):
filtered_cloud = self.ground_filter(cloud)
pc_msg = pcl_to_ros(filtered_cloud, cloud.header)
self.pub.publish(pc_msg)
