def publish_pc2(pc, obj):
global rospy_node
import rospy
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
import std_msgs
if rospy_node is None:
rospy_node = rospy.init_node("pc2_publisher")
pub = rospy.Publisher("/points_raw", PointCloud2, queue_size=1000000)
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "velodyne"
points = pc2.create_cloud_xyz32(header, pc[:, :3])
pub2 = rospy.Publisher("/points_raw1", PointCloud2, queue_size=1000000)
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "velodyne"
points2 = pc2.create_cloud_xyz32(header, obj)
# r = rospy.Rate(0.1)
# while not rospy.is_shutdown():
pub.publish(points)
pub2.publish(points2)
def computeICPBetweenScans(no1,no2, init_x = 0 , init_y = 0, init_yaw = 0):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'ibeo'
example = cython_catkin_example.PyCCExample()
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no1)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no1)+'.txt')
if opts.use_accumulated:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no1)+'.txt'))
if opts.icp_use_filtered_data:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no1)+'.txt'))
else:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no1)+'.txt'))
H_points = [[p[0], p[1],1] for p in test_cloud]
# H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud]
pc_point_t1 = pc2.create_cloud_xyz32(header, H_points)
# print 'text1: ',len(test_cloud)
# print test_cloud
# example.load_2d_array('ref_map',np.array(test_cloud, np.float32))
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no2)+'.txt')
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no2)+'.txt')
# test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no2)+'.txt')
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no2)+'.txt')
# test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no2)+'.txt')
if opts.use_accumulated:
test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no2)+'.txt'))
if opts.icp_use_filtered_data:
test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no2)+'.txt'))
else:
test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no2)+'.txt'))
H_points = [[p[0], p[1],1] for p in test_cloud2]
# H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud]
pc_point_t2 = pc2.create_cloud_xyz32(header, H_points)
# print 'text2: ',len(test_cloud)
# example.load_2d_array('que_map',np.array(test_cloud, np.float32))
# names = example.get_point_xyz_clouds_names()
# print("Current point clouds: " + ",".join(names))
# icp_ros_result = g_processICP_srv(pc_point_t1, pc_point_t2)
# print icp_ros_result
# cython_icp_result = example.processICP(np.array(test_cloud, np.float32),np.array(test_cloud2, np.float32))
cython_icp_result = example.processICP(np.array([x[0] for x in test_cloud], np.float32),np.array([x[1] for x in test_cloud], np.float32),np.array([x[0] for x in test_cloud2], np.float32),np.array([x[1] for x in test_cloud2], np.float32), init_x, init_y, init_yaw)
# print cython_icp_result
return cython_icp_result
def est_sensory_function_cb(msg_var):
global scaled_polygon_pcl_est_lvl, est_sensory_level, header, plot_image_publisher, est_sensory_function_current, list_current_position
est_sensory_function_current = np.asarray(msg_var.data).reshape(
(len(par.Xaxis), len(par.Xaxis)))
est_points_lvl1 = []
est_points_lvl2 = []
est_points_lvl3 = []
est_points_lvl4 = []
est_points_lvl5 = []
maxSF = np.max(np.max(est_sensory_function_current))
minSF = np.min(np.min(est_sensory_function_current))
gapSF = (maxSF - minSF) / est_sensory_level
est_threshold_lvl1_2 = minSF + gapSF
est_threshold_lvl2_3 = minSF + 2 * gapSF
est_threshold_lvl3_4 = minSF + 3 * gapSF
est_threshold_lvl4_5 = minSF + 4 * gapSF
for h in range(len(par.Xaxis)):
for k in range(len(par.Yaxis)):
if (est_sensory_function_current[k, h] < est_threshold_lvl1_2):
est_points_lvl1.append([
par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale,
par.StaticAltitude
])
elif (est_sensory_function_current[k, h] >
est_threshold_lvl1_2) and (est_sensory_function_current[k, h]
< est_threshold_lvl2_3):
est_points_lvl2.append([
par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale,
par.StaticAltitude
])
elif (est_sensory_function_current[k, h] >
est_threshold_lvl2_3) and (est_sensory_function_current[k, h]
< est_threshold_lvl3_4):
est_points_lvl3.append([
par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale,
par.StaticAltitude
])
elif (est_sensory_function_current[k, h] >
est_threshold_lvl3_4) and (est_sensory_function_current[k, h]
< est_threshold_lvl4_5):
est_points_lvl4.append([
par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale,
par.StaticAltitude
])
else:
est_points_lvl5.append([
par.Xaxis[h] * par.RealScale, par.Yaxis[k] * par.RealScale,
par.StaticAltitude
])
scaled_polygon_pcl_est_lvl[0] = pcl2.create_cloud_xyz32(
header, est_points_lvl1)
scaled_polygon_pcl_est_lvl[1] = pcl2.create_cloud_xyz32(
header, est_points_lvl2)
scaled_polygon_pcl_est_lvl[2] = pcl2.create_cloud_xyz32(
header, est_points_lvl3)
scaled_polygon_pcl_est_lvl[3] = pcl2.create_cloud_xyz32(
header, est_points_lvl4)
scaled_polygon_pcl_est_lvl[4] = pcl2.create_cloud_xyz32(
header, est_points_lvl5)
def publish_three_pc(object1, object2): #, pc3):
"""publish two processed data at a time to the ROS node"""
pub = rospy.Publisher("/pointcloud1", PointCloud2, queue_size=1000000)
rospy.init_node("pc2_publisher")
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "velodyne"
points = pc2.create_cloud_xyz32(header, object1[:, :3])
pub_2 = rospy.Publisher("/pointcloud2", PointCloud2, queue_size=1000000)
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "velodyne"
points2 = pc2.create_cloud_xyz32(header, object2[:, :3])
# pub_3=rospy.Publisher("/pc3", PointCloud2, queue_size=1000000)
# header = std_msgs.msg.Header()
# header.stamp = rospy.Time.now()
# header.frame_id = "velodyne"
# points3 = pc2.create_cloud_xyz32(header, pc3[:, :3])
r = rospy.Rate(0.1)
while not rospy.is_shutdown():
pub.publish(points)
pub_2.publish(points2)
#pub_3.publish(points3)
r.sleep()
def talker():
rate = rospy.Rate(RATE_ROS_PUBLISHING)
counter_index = NO_START_FRAME # 1#21#30#x1
pcloud = PointCloud2()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "velodyne"
cloud = []
while not rospy.is_shutdown():
file_string = "scan" + format(counter_index, "03d")
cloud_new, cloud_raw = publish_xyz(file_string, counter_index, force_2d=FORCE_2D)
# cloud = cloud + cloud_new
cloud = cloud_new
print file_string
counter_index = counter_index + 1
# rate.sleep()
if not NO_ROS_PUBLISHING:
pcloud = PointCloud2()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "velodyne" #'odom' #'pose' #'frame' #'velodyne'
# header.frame_id = 'frame' #'odom' #'pose' #'frame' #'velodyne'
# header.frame_id = 'odom' #'odom' #'pose' #'frame' #'velodyne'
pcloud = pc2.create_cloud_xyz32(header, cloud)
pub_cloud.publish(pcloud)
header.frame_id = "odom" #'odom' #'pose' #'frame' #'velodyne'
pcloud = pc2.create_cloud_xyz32(header, cloud_raw)
pub_raw.publish(pcloud)
if counter_index > NO_LAST_FRAME: # 47:#91:#65:#19 :#65: #65: # 20 :#65 :# 468:
break
counter_index = 1
rate.sleep()
saveCloud("points.pts", cloud)
# pcloud = pc2.create_cloud_xyz32(header, cloud)
# print pcloud.header, pcloud.height, pcloud.width, pcloud.point_step, pcloud.row_step, pcloud.fields
# pub_cloud.publish(pcloud)
# rospy.spin()
# pos = np.random.random(size=(100,3))
# print cloud
pos = np.matrix(cloud)
# pos *= [10,-10,10]
# pos[0] = (0,0,0)
# color = np.ones((pos.shape[0], 4))
# d2 = (pos**2).sum(axis=1)**0.5
# size = np.random.random(size=pos.shape[0])*10
size = np.ones((1, pos.shape[0]))
sp2 = gl.GLScatterPlotItem(pos=pos, color=(1, 1, 1, 1), size=size)
w.addItem(sp2)
if (sys.flags.interactive != 1) or not hasattr(QtCore, "PYQT_VERSION"):
QtGui.QApplication.instance().exec_()
def FilterC(self):
pontos = pc2.read_points_list(self.old_cloud,
field_names=("x", "y", "z"),
skip_nans=True)
new_pontos = list()
d_max = 10
d_trig = 1.5
for k in range(270):
encontrou = False
for p in pontos:
if ((180 * atan2(p[1], p[0]) / pi) - (k - 135))**2 < 0.01:
Point = namedtuple("Point", ("x", "y", "z"))
l = (p[0], p[1], 0)
new_p = Point._make(l)
new_pontos.append(new_p)
encontrou = True
break
if not encontrou:
Point = namedtuple("Point", ("x", "y", "z"))
l = (d_max * cos(pi * (k - 135) / 180),
d_max * sin(pi * (k - 135) / 180), 0)
new_p = Point._make(l)
new_pontos.append(new_p)
disc = list()
# p = new_pontos[0]
# if sqrt(p[0]**2 + p[1]**2) < d_max:
# disc.append(p)
d_max = 9
for k1 in range(1, 269):
k2 = k1 + 1
p1 = new_pontos[k1]
p2 = new_pontos[k2]
d_p1 = sqrt(p1[0]**2 + p1[1]**2)
d_p2 = sqrt(p2[0]**2 + p2[1]**2)
if d_p1 == d_max and d_p2 < d_max:
disc.append(p2)
elif abs(d_p1 - d_p2) >= d_trig:
if d_p1 < d_p2:
disc.append(p1)
else:
disc.append(p2)
# p = new_pontos[269]
# if sqrt(p[0]**2 + p[1]**2) < d_max:
# disc.append(p)
print(len(disc))
return (pc2.create_cloud_xyz32(self.old_cloud.header, new_pontos),
pc2.create_cloud_xyz32(self.old_cloud.header, disc))
def processPose2d(msg):
global g_point_cloud_built
global g_pcloud
global g_pose
global g_dist_accum
global g_last_dist_accum
global g_last_x
global g_last_y
global g_last_captured_x
global g_last_captured_y
global g_last_captured_yaw
g_pose = msg
quat = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
euler = tf.transformations.euler_from_quaternion(quat)
yaw = -euler[2]
g_dist_accum = g_dist_accum + math.hypot(msg.pose.pose.position.x - g_last_x, msg.pose.pose.position.y - g_last_y)
g_last_x = msg.pose.pose.position.x
g_last_y = msg.pose.pose.position.y
if g_dist_accum > g_last_dist_accum + g_dist_thres:
g_last_dist_accum = g_last_dist_accum + g_dist_thres
tranformation_matrix = numpy.matrix([[math.cos(yaw),math.sin(yaw),msg.pose.pose.position.x],[-math.sin(yaw),math.cos(yaw),msg.pose.pose.position.y],[0,0,1]])
# tranformation_matrix = numpy.matrix([[math.cos(yaw),math.sin(yaw),0],[-math.sin(yaw),math.cos(yaw),0],[0,0,1]])
# tranformation_matrix = numpy.matrix([[1,0,-msg.pose.pose.position.x],[0,1,-msg.pose.pose.position.y],[0,0,1]])
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'ibeo'
H_points = [[p[0], p[1],1] for p in g_points_new_filtered]
M_points = numpy.matrix(H_points)
# print M_points
point_t = tranformation_matrix *M_points.getT()
point_t = point_t.getT().tolist()
g_point_cloud_built = g_point_cloud_built + point_t
# g_point_cloud_built = point_t
g_pcloud = pc2.create_cloud_xyz32(header, g_point_cloud_built)
pc_point_t = pc2.create_cloud_xyz32(header, point_t)
observed_x = -g_last_captured_x + msg.pose.pose.position.x
observed_y = -g_last_captured_y + msg.pose.pose.position.y
observed_x_local = math.cos(-yaw)*observed_x + math.sin(-yaw)*observed_y
observed_y_local = -math.sin(-yaw)*observed_x + math.cos(-yaw)*observed_y
print msg.pose.pose.position.x, msg.pose.pose.position.y, yaw, observed_x_local,observed_y_local,-g_last_captured_yaw +yaw, msg.pose.covariance[0], msg.pose.covariance[1],msg.pose.covariance[2],msg.pose.covariance[3],msg.pose.covariance[4], msg.pose.covariance[5]
g_pub_cloud.publish(g_pcloud)
g_pub_cloud_only_current_filtered.publish(pc_point_t)
H_points = [[p[0], p[1],1] for p in g_points_new]
M_points = numpy.matrix(H_points)
point_t = tranformation_matrix *M_points.getT()
point_t = point_t.getT().tolist()
pc_point_t = pc2.create_cloud_xyz32(header, point_t)
g_pub_cloud_only_current.publish(pc_point_t)
g_last_captured_x = msg.pose.pose.position.x
g_last_captured_y = msg.pose.pose.position.y
g_last_captured_yaw = yaw
pass
def publish_transformed_cloud(cloud_array, flag=True):
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
if not flag:
header.frame_id = 'sensor/side_camera_depth_optical_frame'
cloud = pcl2.create_cloud_xyz32(header, cloud_array)
pub2.publish(cloud)
else:
cloud = pcl2.create_cloud_xyz32(header, cloud_array)
pub1.publish(cloud)
return
def callback(self, msg):
cloud = pointcloud2_2_npxyz(msg)
if np.isnan(cloud).sum() > 0:
raise ValueError('Cloud should have no Nans')
ground, obst = self.compute(cloud.squeeze())
ground_msg = pc2.create_cloud_xyz32(msg.header, ground)
obst_msg = pc2.create_cloud_xyz32(msg.header, obst)
self.pub_ground.publish(ground_msg)
self.pub_obst.publish(obst_msg)
def cloud_callback(self, data):
"""process the cloud"""
# get points from the subscribed pointcloud
# for some reason storing pc into Pcam is working with point cloud that is voxel filtered. Investigate!!
pc = ros_numpy.numpify(data)
Pcam = np.zeros((pc.shape[0], 3))
Pcam[:, 0] = pc['x']
Pcam[:, 1] = pc['y']
Pcam[:, 2] = pc['z']
# make a cloud of points on the max z face of the camera
xx, yy = np.meshgrid(self.x, self.y)
xvalues = xx.ravel()
yvalues = yy.ravel()
zvalues = np.ones(len(xvalues)) * self.SensorRange
Pgrid = np.zeros((len(xvalues), 3))
Pgrid[:, 0] = xvalues
Pgrid[:, 1] = yvalues
Pgrid[:, 2] = zvalues
# project existing points to the plane containing new points
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
#header.frame_id = 'camera_color_optical_frame'
#total_points = np.concatenate((Pcam, Pgrid), 0)
#p = pc2.create_cloud_xyz32(header, total_points)
if len(Pcam) != 0:
Pprojected = self.camera_location + np.array(
[(Pcam[i] - self.camera_location) * self.SensorRange /
Pcam[i, 2] for i in range(len(Pcam))])
#projected_points = self.camera_location + map(lambda i, j : (i-self.camera_location)*self.SensorRange/j, points, points[:,2])
PCprojected = pcl_cloud(np.array(Pprojected, dtype=np.float32))
PCgrid = pcl_cloud(np.array(Pgrid, dtype=np.float32))
kdt = pcl.KdTreeFLANN(PCgrid)
indices, sqr_distances = kdt.nearest_k_search_for_cloud(
PCprojected, 10)
Pgrid_modified = np.delete(Pgrid, indices, axis=0)
total_points = np.concatenate((Pcam, Pgrid_modified), 0)
p = pc2.create_cloud_xyz32(header, total_points)
else:
#total_points = np.concatenate((Pcam, Pgrid), 0)
p = pc2.create_cloud_xyz32(header, Pgrid)
self.pcl_pub.publish(p)
def createImageEdges(self, image, drone3dPosInMap, stamp):
##Show image edges as 3D Points
imgHeight, imgWidth, channels = image.shape
# upperleft
ptul2D = [1, 1]
# upperleft
ptur2D = [1, imgHeight]
# upperleft
ptll2D = [imgWidth, 1]
# upperleft
ptlr2D = [imgWidth, imgHeight]
# Create Points which mark the edges of the camerapic in the world (RVIZ)
ptul3D = imagePointTransformations.getPixel3DPosOnWater(ptul2D, drone3dPosInMap, self.camModel, stamp)
# upperright
ptur3D = imagePointTransformations.getPixel3DPosOnWater(ptur2D, drone3dPosInMap, self.camModel, stamp)
# lowerleft
ptll3D = imagePointTransformations.getPixel3DPosOnWater(ptll2D, drone3dPosInMap, self.camModel, stamp)
# lowerright
ptlr3D = imagePointTransformations.getPixel3DPosOnWater(ptlr2D, drone3dPosInMap, self.camModel, stamp)
pixelProjection3DPoints = [ptul3D, ptur3D, ptll3D, ptlr3D]
header = Header()
header.frame_id = "map"
header.stamp = stamp
image3dProjection = PointCloud2()
image3dProjection = pc2.create_cloud_xyz32(header, pixelProjection3DPoints)
self.picProjPub.publish(image3dProjection)
def convert_to_pcl2(msg):
global pcl_points, listener, tot, bag
if msg.range < 0.01: # to filter out infinity
return
pts = PointStamped()
pts.header.frame_id = 'laser_frame'
pts.header.stamp = msg.header.stamp
# print msg.header.stamp.secs,
pts.point.x = msg.range
# print pts.header.frame_id, pts.point.x, pts.point.y, pts.point.z,
# print 'time:', pts.header.stamp.secs
# start = time.time()
listener.waitForTransform(
"laser_frame", "world", rospy.Time().now(), rospy.Duration(0.5))
pts2 = listener.transformPoint('world', pts)
# diff = time.time() - start
# tot += diff
# print diff, tot
# print pts2.header.frame_id, pts2.point.x, pts2.point.y, pts2.point.z
pcl_points.append([pts2.point.x, pts2.point.y, pts2.point.z])
# if len(pcl_points) > 200:
# pcl_points.pop(0)
pcl = pc2.create_cloud_xyz32(pts2.header, pcl_points[-201:-1])
pub.publish(pcl)
def goal_callback(self, status, result):
rospy.loginfo("Goal callback %s %s", status, result)
x, y, w = self.goal
if status == GoalStatus.SUCCEEDED:
self.measurements.append({
"depth":
self.depth,
"timestamp":
datetime.now().replace(microsecond=0).isoformat(),
"x":
x,
"y":
y
})
self.points.append([x, y, self.depth])
header = Header(frame_id="map", stamp=rospy.Time.now())
msg = point_cloud2.create_cloud_xyz32(header, self.points)
self.point_pub.publish(msg)
self.goal = self.next_goal(self.goal)
rospy.loginfo("Next goal %s", self.goal)
self.send_goal()
def publish_walls(): #build the point cloud imagining we are at 0,0,0. cloud_points = [] if(wall_left>0): cloud_points.append([0, 13.0/wall_left, 0.5]) if(wall_frontleft>0): cloud_points.append([13.0/wall_frontleft, 0.5, 0.5]) if(wall_frontright>0): cloud_points.append([13.0/wall_frontright, -0.5, 0.5]) if(wall_right>0): cloud_points.append([0, -13.0/wall_right, 0.5]) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' mypointcloud = pcl2.create_cloud_xyz32(header, cloud_points) #We need to build a transformation of our location. t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "world" t.child_frame_id = "zumo" t.transform.translation.x = transx t.transform.translation.y = transy t.transform.translation.z = transz t.transform.rotation.x = rotx t.transform.rotation.y = roty t.transform.rotation.z = rotz t.transform.rotation.w = rotw #convert the pointcloud to take into account our location. cloud_out = do_transform_cloud(mypointcloud, t) pcl_pub.publish(cloud_out)
def processing_xyz(self):
for i in range(self.len_files):
print("[+] {} th file name : {} ".format(
i, self.npy_files_list_xyz[i]))
bin_points = np.fromfile(os.path.join(self.npy_path,
self.npy_files_list_xyz[i]),
dtype=np.float32).reshape(-1, 4)
pc2_msg = PointCloud2()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'velodyne_link'
cloud_points = []
for j in range(len(bin_points)):
if (bin_points[j][0] >= 0.1 and bin_points[j][1] != 0
and bin_points[j][2] <= 5 and bin_points[j][3] < 10):
cloud_points.append(list(bin_points[j, :3]))
pc2_msg = pcl2.create_cloud_xyz32(header, cloud_points)
# ed: /velodyne_points_npy publish
self.velo_pub.publish(pc2_msg)
self.loop_rate.sleep()
if rospy.is_shutdown():
return
def generatePointCloud2(xyzreader, fileId):
cloud_stamp = 0
points = []
i = 0
for row in xyzreader:
if i == 0:
i += 1
continue
if i == 1:
cloud_stamp = row[0]
x = float(row[1])
y = float(row[2])
z = float(row[3])
p = [x, y, z]
points.append(p)
header = Header()
header.frame_id = "map"
if cloud_interval == 0:
times = cloud_stamp.split(".")
header.stamp.secs = int(times[0])
header.stamp.nsecs = int(times[1])
else:
header.stamp.secs = 1 + cloud_interval * fileId
header.stamp.nsecs = 0
pc = pc2.create_cloud_xyz32(header, points)
return pc
def pub_point_cloud(self, stamp, profile3d):
self.sequence = self.sequence + 1
self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d)
self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0",
stamp=stamp,
seq=self.sequence)
self.cloud_pub.publish(self.pcloud)
def pub_point_cloud(self, stamp, profile3d):
self.sequence = self.sequence + 1
self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d)
self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0",
stamp=stamp,
seq=self.sequence)
self.cloud_pub.publish(self.pcloud)
def Visualize(filename):
data_path = param.DATA_DIR + filename + ".pcd"
calib_path = param.CALIB_DIR + filename + ".txt"
label_path = param.LABEL_DIR + filename + ".txt"
print "data path : " + data_path
print "calib path : " + calib_path
print "label path : " + label_path
pc_pub = rospy.Publisher("cnn_3d_points_raw",
PointCloud2,
queue_size=100000)
rospy.init_node("cnn_3d")
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "cnn_3d"
marker_array_pub = rospy.Publisher("cnn_3d_anchor_obj",
MarkerArray,
queue_size=1000)
sleep_rate = rospy.Rate(1)
point_cloud, gt_objectness = Loader.get_visualize_input(
data_path, calib_path, label_path)
points = pc2.create_cloud_xyz32(header, point_cloud[:, :3])
marker_array = Utils.get_marker_array(gt_objectness, type="anchor")
while not rospy.is_shutdown():
pc_pub.publish(points)
marker_array_pub.publish(marker_array)
sleep_rate.sleep()
def downsample_and_publish(self):
while self.received_frames < self.max_frames: # wait until cloud_callback processes 8 messages
pass
downsampled_cloud = pcl.PointCloud()
downsampled_cloud.from_array(
np.asarray(self.input_cloud, dtype=np.float32))
self.input_cloud = [] # clean input cloud
self.received_frames = 0 # allow to copy next 8 msgs chunk
pass_fill = downsampled_cloud.make_passthrough_filter()
pass_fill.set_filter_field_name("z")
pass_fill.set_filter_limits(0, 1)
downsampled_cloud = pass_fill.filter()
sor = downsampled_cloud.make_voxel_grid_filter()
sor.set_leaf_size(0.005, 0.005, 0.005)
downsampled_cloud = sor.filter()
msg_header = Header()
msg_header.frame_id = "xtion_rgb_optical_frame"
self.pub.publish(
point_cloud2.create_cloud_xyz32(msg_header,
downsampled_cloud.to_list()))
def findMovement(self, data, publisher):
m = Munkres()
print "processing new scan"
convert_data = self.xyConvert(data)
if not (len(self.filter_scans) < self.filter_size):
print "starting filter"
self.update_filter(convert_data)
print "adding odom"
odom_scan = self.addOdom()
matches = [[] for ii in range(len(self.filter_scans[1]))]
for ii in range(1, self.filter_size - 1):
print "calculating cost mat"
cost_mat = self.makeCostMatrix(self.filter_scans[0],
self.filter_scans[ii])
print len(cost_mat)
print "doing matching"
#indexes = m.compute(cost_mat)
row_ind, col_ind = linear_sum_assignment(cost_mat)
print "done matching for scan: ", ii
#for item in indexes:
# matches[item[0]].append(self.filter_scans[ii][item[1]])
for jj, item in enumerate(row_ind):
matches[item].append(self.filter_scans[ii][jj])
matched_scans = []
print "processing"
for item in matches:
res = apply(zip, item)
matched_scans.append(res)
filtered_points = []
print "computing avg and std"
for item in matched_scans:
x_avg = np.mean(item[0])
x_std = np.std(item[0])
y_avg = np.mean(item[1])
y_std = np.std(item[1])
if x_std > self.max_var and y_std > self.max_var:
filtered_points.append([x_avg, y_avg, 0])
print filtered_points
pc_cloud = PointCloud2()
pc_cloud = pc2.create_cloud_xyz32(pc_cloud.header, filtered_points)
publisher.publish(pc_cloud)
print "ending filter"
else:
print "building filter"
self.filter_scans.append(convert_data)
def callback(msg):
angle_rad = msg.angle_min #assign first angle
angle_deg = angle_rad * 57.2957795 #angle in degrees
rangingsCount = len(msg.ranges) #number of rangings[usually 360]
print("[" + str(rangingsCount) + "] rangings\n")
pc_array = []
for r in range(rangingsCount):
#print("[" + str(r) + "] rad: " + str(angle_rad))
#print("deg: " + str(angle_deg))
#print("dist: " + str(msg.ranges[r]))
angle_rad += msg.angle_increment #new angle calculation
angle_deg = angle_rad * 57.2957795 #angle in deg
point_x = math.cos(angle_rad) * msg.ranges[r]
point_y = math.sin(angle_rad) * msg.ranges[r]
#print("[X;Y]: [" + str(point_x) + ";" + str(point_y) +"]\n")
pc_array.append((point_x, point_y, 0))
sem = pc2.create_cloud_xyz32(msg.header, pc_array)
#now we need to delete points from map where we aim
#rospy.wait_for_service('clear_bbx', 1)
#clear_bbx = rospy.ServiceProxy(SRV_NAME, SRV_INTERFACE)
#try:
# #resp1 = clear_bbx(bounding_box)
# resp1 = clear_bbx(min, max)
#except rospy.ServiceException as exc:
# print("Service did not process request: " + str(exc))
#send new points
pub.publish(sem)
def publish_point_cloud(pcl_pub, point_cloud):
rospy.loginfo("point cloud published")
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))
def PC_reduc_sep(self, bbox, CloudImage):
cv_image = np.array(
self.bridge.imgmsg_to_cv2(CloudImage),
dtype='float32') # Image with distance in channels x,y,z
pc_list = np.reshape(
np.reshape(cv_image, (CloudImage.height, CloudImage.width * 3)).T,
CloudImage.height * CloudImage.width * 3)
if bbox != []:
cx = bbox.bbox.center.x
sx = bbox.bbox.size_y
cy = bbox.bbox.center.y
sy = bbox.bbox.size_y
Start_x = int(cx - sx / 2)
End_x = int(cx + sx / 2)
Start_y = int(cy - sy / 2)
End_y = int(cy + sy / 2)
bbox_i = []
time1 = rospy.Time.now().to_sec()
for y in range(Start_y, End_y):
bbox_i += list(
range((y * 672 + Start_x) * 3, (y * 672 + End_x + 1) * 3))
pc_list = np.delete(pc_list, bbox_i)
time2 = rospy.Time.now().to_sec()
time3 = rospy.Time.now().to_sec()
pc_list = pc_list.reshape(int(len(pc_list) / 3), 3)
pc_list = pc_list[~np.isnan(pc_list).any(axis=1)]
pc_list = pc_list[~np.isinf(pc_list).any(axis=1)]
time4 = rospy.Time.now().to_sec()
print(pc_list.shape)
Reduced_PC2 = pc2.create_cloud_xyz32(CloudImage.header, pc_list)
return Reduced_PC2
def align_gt_object(gt_object_filepath, downsample_factor=10, rate=5):
ply = curvox.mesh_conversions.read_mesh_msg_from_ply_filepath(gt_object_filepath)
points = []
for i in range(len(ply.vertices)):
if i % downsample_factor == 0:
points.append((ply.vertices[i].x, ply.vertices[i].y, ply.vertices[i].z))
header = std_msgs.msg.Header()
header.frame_id = '/gt_object'
pcl_arr = pcl2.create_cloud_xyz32(header, np.array(points))
pc_pub = rospy.Publisher('/gt_overlay', sensor_msgs.msg.PointCloud2, queue_size=10)
r = rospy.Rate(rate)
tfl = tf.TransformListener()
for i in range(1000):
tfl.waitForTransform("/base_link", "/gt_object", rospy.Time(0), rospy.Duration(4))
tran, rot = tfl.lookupTransform("/base_link", "/gt_object", rospy.Time(0))
print '<node pkg="lcsr_tf_tools" type="interactive_transform_publisher" name="static_tf_world_to_gt" args="{} {} {} {} {} {} {} /base_link /gt_object 100"/>'.format(
tran[0], tran[1], tran[2], rot[0], rot[1], rot[2], rot[3])
print 'object_ps.pose = Pose(Point({}, {}, {}), Quaternion({}, {}, {}, {}))'.format(tran[0], tran[1], tran[2],
rot[0], rot[1], rot[2],
rot[3])
pc_pub.publish(pcl_arr)
r.sleep()
def camera_callback(self, pcd_msg):
# transform frame
trans = self.tf_buffer.lookup_transform('world', pcd_msg.header.frame_id, rospy.Time(0), rospy.Duration(1)) # ask for last known info
points = do_transform_cloud(pcd_msg, trans)
if self.filter_state == self.NO_FILTER:
print('##########NOT filtering point cloud########')
points.header = std_msgs.msg.Header()
points.header.stamp = rospy.Time.now()
points.header.frame_id = 'world'
self.pcd_pub.publish(points)
else:
print('##########filtering point cloud########')
pcd = list(pcl2.read_points(points, field_names = ("x", "y", "z"), skip_nans=True))
pcd = np.array(pcd)
pcd_add_axis = np.ones((len(pcd),4))
pcd_add_axis[:,:3] = pcd
pcd_transformed = self.filter_transform.dot(pcd_add_axis.T).T
pcd_transformed = pcd_transformed[:,:3]
box_mask_min = np.prod((pcd_transformed - self.filter_min_xyz) >= 0, axis=1) # AND across x-y-z axis
box_mask_max = np.prod((self.filter_max_xyz - pcd_transformed) >= 0, axis=1) # AND across x-y-z axis
box_mask = box_mask_min * box_mask_max # should satisfy both min and max constraints to be inside the box
# anything else stays
# filter by the mask
print('before filter: pcd length: %d' % (len(pcd)))
pcd = pcd[box_mask == 0] # anything not inside the box
print('after filter: pcd length: %d' % (len(pcd)))
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world'
#create pcl from points
filtered_pcd = pcl2.create_cloud_xyz32(header, pcd)
#publish
self.pcd_pub.publish(filtered_pcd)
def convertToPointCloud2(self, cloud, normals=None):
'''
Transform the Infinitam volume to a point cloud in the /base frame.
@type cloud:
@param cloud:
'''
header = HeaderMsg()
header.frame_id = "/base"
header.stamp = rospy.Time.now()
if normals is None:
return point_cloud2.create_cloud_xyz32(header, cloud)
# concatenate xyz and normals vertically
pts = numpy.zeros((cloud.shape[0],6))
pts[:,0:3] = cloud[:,0:3]
pts[:,3:6] = normals[:,0:3]
# create message
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('normal_x', 12, PointField.FLOAT32, 1),
PointField('normal_y', 16, PointField.FLOAT32, 1),
PointField('normal_z', 20, PointField.FLOAT32, 1)]
return point_cloud2.create_cloud(header, fields, pts)
def publishFrontiers(frontier):
delta = 0.25
size = 100
cloud = []
for frontierCell in frontier:
point = []
i = frontierCell[0]
j = frontierCell[1]
x = (-100 + j * delta) + delta / 2
y = (-100 + i * delta) + delta / 2
point.append(x)
point.append(y)
point.append(0.0)
cloud.append(point)
cloud_points = cloud
pcl_pub = rospy.Publisher("/frontiers", PointCloud2, queue_size=10)
rospy.sleep(1.)
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
#create pcl from points
scaled_polygon_pcl = pcl2.create_cloud_xyz32(header, cloud_points)
#publish
rospy.loginfo("Publishing Frontiers.. !")
pcl_pub.publish(scaled_polygon_pcl)
def callback(self, data):
if len(self.plane) != 0:
#define cloud in readable format and save to obejct field
self.cloud = []
for p in pc2.read_points(data,
field_names=("x", "y", "z"),
skip_nans=True):
#print "x : %f y : %f z : %f" %(p[0],p[1],p[2])
self.cloud.append(p)
#print len(self.cloud)
#filter using given intial rectangular region
#print "old array:"
l = len(data.data)
for i in range(len(self.cloud) - 1, -1, -1):
if not self.is_inside_region(self.cloud[i]):
del self.cloud[i]
print "new array:"
self.header.stamp = rospy.Time.now()
self.header.seq += 1
newCloud = pc2.create_cloud_xyz32(self.header, self.cloud)
print newCloud
pub.publish(newCloud)
#print newCloud.header
#print newCloud.fields
print "old legnth: " + str(l)
print "new length: " + str(len(self.cloud))
def process_radar_tracks(msg):
assert msg._md5sum == '6a2de2f790cb8bb0e149d45d297462f8'
tracks = RadarObservation.from_msg(msg)
num_tracks = len(tracks)
acc = []
cloud = np.zeros([num_tracks, 3], dtype=np.float32)
for i, track in enumerate(tracks):
cloud[i] = [track.x, track.y, track.z] - np.array(RADAR_TO_LIDAR)
if np.abs(track.y) < 2:
#acc.append(track.x)
#acc.append(track.power)
print track.vx * 3.7, track.vy * 3.7
#print x, y, z
#print acc #msg.header.stamp
header = Header()
header.stamp = msg.header.stamp
header.frame_id = 'velodyne'
cloud_msg = pc2.create_cloud_xyz32(header, cloud)
cloud_msg.width = 1
cloud_msg.height = num_tracks
rospy.Publisher('radar_points', PointCloud2,
queue_size=1).publish(cloud_msg)
def publish_static_robots(self, force_clear):
with self.neighbors_lock:
time = rospy.Time.now()
cloud_points = []
if self.me is None or (time - self.me.header.stamp
).to_sec() > last_seen_threshold:
return
my_pose = self.me.pose.pose
_, _, my_theta = tf.transformations.euler_from_quaternion(
quat_array_from_msg(my_pose.orientation))
change = False
for name in self.neighbors:
if (time - self.neighbors[name]['last_seen']).to_sec() < last_seen_threshold \
and ((abs(self.neighbors[name]['twist'].twist.linear.x) < 0.05 and abs(
self.neighbors[name]['twist'].twist.linear.y) < 0.05)):
if not self.neighbors[name]['stationary']:
change = True
self.neighbors[name]['stationary'] = True
cur_pose = self.neighbors[name]['position'].pose
_, _, cur_theta = tf.transformations.euler_from_quaternion(
quat_array_from_msg(cur_pose.orientation))
relative_pose_x = cur_pose.position.x - my_pose.position.x
relative_pose_y = cur_pose.position.y - my_pose.position.y
xform_foot = make_rotation_transformation(cur_theta -
my_theta)
xform_to_baselink = make_rotation_transformation(-my_theta)
pos_rel = xform_to_baselink(
(relative_pose_x, relative_pose_y))
for p in self.neighbors[name]['footprint'].polygon.points:
p_x = p.x
p_y = p.y
p_rotated = xform_foot((p_x, p_y))
p_x = STATIC_SCALE * p_rotated[0] + pos_rel[0]
p_y = STATIC_SCALE * p_rotated[1] + pos_rel[1]
cloud_points.append((p_x, p_y, Z_HEIGHT))
for _ in range(5):
scale = random.uniform(0.98, 1.02)
cloud_points.append(
(scale * p_x, scale * p_y, Z_HEIGHT))
else:
if self.neighbors[name]['stationary']:
self.neighbors[name]['stationary'] = False
change = True
if change or force_clear:
self.clearing_laser_scan.header.stamp = self.me.header.stamp
self.clearing_laser_pub.publish(self.clearing_laser_scan)
static_robots_cloud = pcl2.create_cloud_xyz32(
self.cloud_header, cloud_points)
static_robots_cloud.header.stamp = self.me.header.stamp
self.point_cloud_pub.publish(static_robots_cloud)
def PC_reduc_seg(bbox, Segmented_labels, pc_list, cloud):
if bbox == []:
reduced_PC2 = cloud
else:
"""
Start_x = Target[TargetOrder("Start_x")]
Start_y = Target[TargetOrder("Start_y")]
End_x = Target[TargetOrder("End_x")]
End_y = Target[TargetOrder("End_y")]
"""
Start_x = bbox[0]
End_x = bbox[2]
Start_y = bbox[1]
End_y = bbox[3]
bbox_i = []
Seg_index = 0
for y in range(Start_y, End_y):
for x in range(Start_x, End_x):
pc_index = (y * 672 + x) * 3
if Segmented_labels[Seg_index] == True:
for i in [0, 1, 2]:
bbox_i.append(pc_index + i)
Seg_index += 1
#print(pc_list.shape)
pc_list = np.delete(pc_list, bbox_i)
#print(pc_list.shape)
pc_list = pc_list.reshape(int(len(pc_list) / 3), 3)
pc_list = pc_list[~np.isnan(pc_list).any(axis=1)]
pc_list = pc_list[~np.isinf(pc_list).any(axis=1)]
header = cloud.header
Reduced_PC2 = pc2.create_cloud_xyz32(header, pc_list)
return Reduced_PC2
def publish_walls():
cloud_points=[]
if(wall_left>0):cloud_points.append([0, 13.0/wall_left, 0.5])
if(wall_frontleft>0):cloud_points.append([13.0/wall_frontleft, 0.5, 0.5])
if(wall_frontright>0):cloud_points.append([13.0/wall_frontright, -0.5, 0.5])
if(wall_right>0):cloud_points.append([0,-13.0/wall_right, 0.5])
header= std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world'
mypointcloud = pcl2.create_cloud_xyz32(header, cloud_points)
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = 'world'
t.child_frame_id = 'zumo'
t.transform.translation.x = transx
t.transform.translation.y = transy
t.transform.translation.z = transz
t.transform.rotation.x = rotx
t.transform.rotation.y = roty
t.transform.rotation.z = rotz
t.transform.rotation.w = rotw
cloud_out = do_transform_cloud(mypointcloud, t)
pcl_pub.publish(cloud_out)
def generatePointCloud2(points):
header = Header()
header.frame_id = "map"
header.stamp.secs = 1
header.stamp.nsecs = 0
pc = pc2.create_cloud_xyz32(header, points)
return pc
def talker(): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' g_pcloud = pc2.create_cloud_xyz32(header, wmap) g_pub_map.publish(g_pcloud) rospy.spin()
def publish_clusters(publisher, header, points, corners):
clusters = []
num_boxes = len(corners) / 8
init = False
for i in range(num_boxes):
corner = corners[i * 8:(i + 1) * 8]
# print corner
clusters_idx = np.logical_and((points[:, 0] > min(corner[:, 0])),
(points[:, 0] < max(corner[:, 0])))
clusters_tmp = points[clusters_idx]
clusters_idx = np.logical_and(
(clusters_tmp[:, 1] > min(corner[:, 1])),
(clusters_tmp[:, 1] < max(corner[:, 1])))
clusters_tmp = clusters_tmp[clusters_idx]
clusters_idx = np.logical_and(
(clusters_tmp[:, 2] > min(corner[:, 2])),
(clusters_tmp[:, 2] < max(corner[:, 2])))
clusters_tmp = clusters_tmp[clusters_idx]
if not init:
clusters = clusters_tmp
init = True
else:
# np.append ==> 1D
clusters = np.append(clusters, clusters_tmp).reshape(-1, 4)
# print clusters
msg_clusters = pc2.create_cloud_xyz32(header, clusters[:, :3])
publisher.publish(msg_clusters)
def get_pointcloud2(self, grid, position, target_frame, transform, range=12.0):
header = std_msg.Header(0, rospy.Time.now(), target_frame)
grid_np = np.array(grid.data, dtype='u1').reshape((grid.info.height,grid.info.width))
origin = np.array((grid.info.origin.position.x,
grid.info.origin.position.y,
grid.info.origin.position.z))
world2map = lambda x:np.clip(trunc((x-origin)/grid.info.resolution),
zeros((3,)),
np.r_[grid.info.width-1, grid.info.height-1,0])
ll = world2map(np.r_[position.x-range, position.y-range, 0])
ur = world2map(np.r_[position.x+range, position.y+range, 0])
submap = grid_np[ll[1]:ur[1],ll[0]:ur[0]]
mappts = np.c_[np.where(submap==100)][:,::-1] + ll[:2]
map2world = lambda x:(x+0.5)*grid.info.resolution+origin[:2]
wpts = np.array([map2world(x) for x in mappts])
pcpts=[]
if len(wpts>0):
for z in np.linspace(0,1,11):
pcpts.append(np.c_[wpts,z*ones_like(wpts[:,0])])
pcpts = np.vstack(pcpts)
pcpts = np.einsum("ki,...ji", transform, np.c_[pcpts,np.ones((len(pcpts),1))])[:,:3]
else:
pcpts = []
pc = pc2.create_cloud_xyz32(header, pcpts)
return pc
def handle_pointcloud(msg):
results = []
points = pc2.read_points(msg)
for x, y, z in points:
results.extend([[x + a, y + b, z + c] for a, b, c in deltas])
pub.publish(pc2.create_cloud_xyz32(msg.header, results))
def publishScan(no):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'ibeo'
test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no)+'.txt')
H_points = [[p[0], p[1],1] for p in test_cloud]
pc_point_t = pc2.create_cloud_xyz32(header, H_points)
g_pub_ros.publish(pc_point_t)
def talker():
header = Header()
header.stamp = rospy.Time.now()
# header.frame_id = 'velodyne' #'map'
header.frame_id = "world" #'map'
wmap = getCloudFromFile(opts.filename)
g_pcloud = pc2.create_cloud_xyz32(header, wmap)
g_pub_map.publish(g_pcloud)
rospy.spin()
def pub_point_cloud(self, stamp, profile3d):
# ERROR: Calibration done in meters
#cloud = profile3d * 0.001 # Conversion from milimeters to meters
self.sequence = self.sequence + 1
self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, profile3d)
self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0",
stamp=stamp,
seq=self.sequence)
self.cloud_pub.publish(self.pcloud)
def publish_obstacles(self, data):
for obs in data['obstacles']:
pose = self.create_pose(obs[0], obs[1], obs[2])
self.publishers['obstacle'].publish(pose)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
cloud = pcl2.create_cloud_xyz32(header, data['obstacles'])
self.publishers['obstacle_points'].publish(cloud)
def mat2pointcloud(filename): m = scipy.io.loadmat(filename) scan = numpy.transpose(m['SCAN']['XYZ'][0][0]).astype(numpy.float32) stamp = m['SCAN']['timestamp_laser'][0][0][0][0] * 1e-6 cloud = PointCloud2() cloud.header.stamp = rospy.Time.from_sec(stamp) cloud.header.frame_id = 'velodyne' cloud = pc2.create_cloud_xyz32(cloud.header, scan) return cloud
def visualize_pc(pc):
pcl_pub = rospy.Publisher("/custom_pc", PointCloud2, queue_size=10)
points = pc.tolist()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
header = msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
point_cloud = pc2.create_cloud_xyz32(header, points)
pcl_pub.publish(point_cloud)
rate.sleep()
def mouseMoveEvent(self, e):
pos = np.array(e.pos().toTuple())
pos[1] = self.size().height() - pos[1] # make +y up
pos_world = pos * self.scale + self.offset
cloud = create_cloud_xyz32(
rospy.Header(frame_id=self.frame_id, stamp=rospy.Time.now()),
np.array([[
pos_world[0],
pos_world[1],
0
]]))
self.point_pub.publish(cloud)
def _compute_sensor_msg(self, sensor_data, cur_time):
header = Header()
header.frame_id = self.name
header.stamp = cur_time
# we take the oposite of y axis (as lidar point are express in left handed coordinate system, and ros need right handed)
# we need a copy here, because the data are read only in carla numpy array
new_sensor_data = sensor_data.data.copy()
new_sensor_data = -new_sensor_data
# we also need to permute x and y
new_sensor_data = new_sensor_data[..., [1, 0, 2]]
point_cloud_msg = create_cloud_xyz32(header, new_sensor_data)
topic = self.name
self.process_msg_fun(topic, point_cloud_msg)
def callback(msg):
pcloud = PointCloud2()
pcloud.header.frame_id = '/radar'
cloud = []
for ii in range(0,64):
x = msg.range[ii] * cos(deg2rad(msg.angle[ii]))
y = msg.range[ii] * sin(deg2rad(msg.angle[ii]))
z = 0.0
cloud.append([x,y,z])
pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
pc_pub.publish(pcloud)
def pub_point_cloud(self):
stamp = rospy.Time.now()
points3d = np.random.random((100, 3))
points3d[:,0] = 0.01 * points3d[:,1]
points3d[:,1] = np.linspace(-0.1, 0.1, 100)
points3d[:,2] = 0.05 * points3d[:,2] + 0.25
#rospy.loginfo(points3d)
# ERROR: Calibration done in meters
#cloud = profile3d * 0.001 # Conversion from milimeters to meters
self.sequence = self.sequence + 1
self.pcloud = pc2.create_cloud_xyz32(self.pcloud.header, points3d)
self.pcloud.header = std_msgs.msg.Header(frame_id="/camera0",
stamp=stamp,
seq=self.sequence)
self.cloud_pub.publish(self.pcloud)
def process(self):
if self.last_scan is None or self.last_image is None:
return
last_image = self.last_image
last_scan = self.last_scan
scan_pc = pc2.read_points(last_scan)
image_pc = pc2.read_points(last_image)
scan_obstacles = []
tmp = Obstacle()
# Enumerate the point cloud from the laser scan to group points that belongs
# to the same obstacle.
for x, y, _ in scan_pc:
dist = x ** 2 + y ** 2
# If it's in range, the point is considered to be part of the current obstacle
if dist < self.max_range:
tmp.add_point(Point(x, y, dist))
else:
if not tmp.is_empty():
scan_obstacles.append(tmp)
tmp = Obstacle()
if not tmp.is_empty():
scan_obstacles.append(tmp)
if len(scan_obstacles) == 0:
self.cloud_in.publish(last_image)
else:
image_points = []
for x, y, z, _ in image_pc:
point = Point(x, y)
for obs in scan_obstacles:
if not obs.is_behind(point):
image_points.append((x, y, z))
msg = pc2.create_cloud_xyz32(last_image.header, image_points)
self.cloud_in.publish(msg)
self.cloud_in.publish(last_scan)
self.last_scan = None
self.last_image = None
def __init__(self):
rospy.init_node('pointcloud_generator')
pub_cloud = rospy.Publisher("~/cloud", PointCloud2, queue_size=10)
r = rospy.Rate(10)
pcloud = PointCloud2()
cloud = [[1.0,-0.5,0],[2.0,-0.5,0],[3.0,-0.5,0],
[1.0,0.5, 0],[2.0,0.5, 0],[3.0, 0.5,0]]
while not rospy.is_shutdown():
pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
pcloud.header.frame_id = "base_footprint"
pub_cloud.publish(pcloud)
r.sleep()
def __init__(self):
rospy.init_node('pointcloud_generator')
pub_cloud = rospy.Publisher('/cloud_in', PointCloud2, queue_size=10)
s = rospy.get_param('~size', 26)
r = rospy.Rate(10)
pcloud = PointCloud2()
cloud = [[s, s, 0], [-s, -s, 0]]
print cloud
while not rospy.is_shutdown():
pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
pcloud.header.frame_id = 'odom'
pub_cloud.publish(pcloud)
r.sleep()
def geometry_to_cloud2(self, fore, left, right, chin): fore_cloud = [(fore.translation.x)/1000, (fore.translation.y)/1000, (fore.translation.z)/1000] left_cloud = [(left.translation.x)/1000, (left.translation.y)/1000, (left.translation.z)/1000] right_cloud = [(right.translation.x)/1000, (right.translation.y)/1000, (right.translation.z)/1000] chin_cloud = [(chin.translation.x)/1000, (chin.translation.y)/1000, (chin.translation.z)/1000] #cloud_header header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'cloud_map' cloud = [fore_cloud, left_cloud, right_cloud, chin_cloud] #create pcl2 clouds from points scaled_pcl = pcl2.create_cloud_xyz32(header, cloud) transformed_cloud = do_transform_cloud(scaled_pcl, self.transformer) self.handle_head_pose(self.supername) self.pcl_pub.publish(transformed_cloud)
def callback(self, raw_data, republisher):
current_time = time.time()
if self.__period < current_time - self.__previous_time:
self.__previous_time += (1+int((current_time - self.__previous_time)/self.__period)) * self.__period
points = []
for point in pc2.read_points(raw_data):
points.append(list(point[0:3]))
points_map_pcl = pcl.PointCloud(points)
pc_filter = points_map_pcl.make_voxel_grid_filter()
pc_filter.set_leaf_size(*self.__leaf_size)
points_map_pcl = pc_filter.filter()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.__frame_id
downsampled_data = pc2.create_cloud_xyz32(header, points_map_pcl.to_list())
republisher.publish(downsampled_data)
def send_cloud(self, cloud, duration):
ref_frame = '/base_footprint'
self.obs_index += 1
obstacle_frame = "/obs{}".format(self.obs_index)
(trans, rotQ) = self.tf_listener.lookupTransform("/odom", ref_frame, rospy.Time(0))
pcloud = PointCloud2()
rospy.loginfo("Publishing obstacle on frame {} for {}s and exiting".format(obstacle_frame, duration))
r = rospy.Rate(10)
start_time = rospy.get_time()
while not rospy.is_shutdown() and (duration == -1 or (rospy.get_time() - start_time) <= duration):
self.br.sendTransform(trans,rotQ,rospy.Time.now(), obstacle_frame, "/odom")
pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
pcloud.header.frame_id = obstacle_frame
self.pub_cloud.publish(pcloud)
r.sleep()
def publish_points(self, data, mask):
print "I should be publishing points!"
mybridge = CvBridge()
img = mybridge.imgmsg_to_cv(mask)
# TODO: I dislike having this dependency here on the cv encodings ... the binary transform should happen in grabcut3d_segmentation
# TODO: depend on the read_points that should be in common_msgs?
# TODO: why didn't the uvs=idxs code work? x
pt_gen = pts.read_points(data.points)#, uvs=idxs, skip_nans=True) # this didn't work!!
out_pts = []
for jj in range(data.points.height):
for ii in range(data.points.width):
pt = pt_gen.next()
if not math.isnan(pt[0]):
if (img[jj,ii] == 1 or img[jj,ii]==3):
out_pts.append(pt[0:3])
print "done creating output point cloud"
out_cloud = pts.create_cloud_xyz32(data.points.header, out_pts)
self.point_publisher.publish(out_cloud)
def occupancy2pointcloud(grid, position, range=10.0):
grid_np = np.array(grid.data, dtype='u1').reshape((grid.info.height,grid.info.width))
origin = np.array((grid.info.origin.position.x,
grid.info.origin.position.y,
grid.info.origin.position.z))
world2map = lambda x:trunc((x-origin)/grid.info.resolution)
ll = world2map(np.r_[position[0]-range, position[1]-range, 0])
ll = np.clip(ll, zeros((3,)), np.r_[grid.info.height-1,
grid.info.width-1,0])
ur = world2map(np.r_[position[0]+range, position[1]+range, 0])
ur = np.clip(ur, zeros((3,)), np.r_[grid.info.height-1,
grid.info.width-1,0])
submap = grid_np[ll[1]:ur[1],ll[0]:ur[0]]
mappts = np.c_[np.where(submap==100)]
map2world = lambda x:(x+0.5)*og.info.resolution+origin[:2]
wpts = np.array([map2world(x) for x in mappts])
pcpts=[]
for z in linspace(0,1,11):
pcpts.append(np.c_[wpts,z*ones_like(wpts[:,0])])
pcpts = np.vstack(pcpts)
hdr = grid.header
pc = pc2.create_cloud_xyz32(hdr, pcpts)
return pc
def create_point_cloud_message(self, pts):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
cloud_message = pcl2.create_cloud_xyz32(header, pts)
return cloud_message
def create_point_cloud(points, frame_id='/'):
_points_check(points)
header = rospy.Header(frame_id=frame_id)
return create_cloud_xyz32(header, points)
print(i)
pl = np.where(pc[:,i] >= bounds[0][0])[0]
ph = np.where(pc[:,i] <= bounds[0][1])[0]
p = np.intersect1d(pl, ph)
potentials.append(p)
p = np.intersect1d(potentials[0], potentials[1])
p = np.intersect1d(p, potentials[2])
return pc[p]
def merge_dimg_to_pcs(dimg1, dimg2, trans1, rot1, trans2, rot2):
h1, w1 = dimg1.shape
h2, w2 = dimg2.shape
K1 = np.array([[525,0,w1/2],[0,525,h1/2],[0,0,1]], dtype=np.uint16)
K2 = np.array([[525,0,w2/2],[0,525,h2/2],[0,0,1]], dtype=np.uint16) # K1 should equal K2 when the depth images are of the same size
pc1 = dimg_to_pc(dimg1, K1, trans1, rot1)
pc2 = dimg_to_pc(dimg2, K2, trans2, rot2)
return np.array(pc1 + pc2)
def rviz_pc_visualizer(pc):
rospy.init_node('pc_publisher', anonymous=True)
pcl_pub = rospy.Publisher("/custom_pc", PointCloud2, queue_size=10)
points = pc.tolist()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
header = msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
point_cloud = pc2.create_cloud_xyz32(header, points)
pcl_pub.publish(point_cloud)
rate.sleep()
if __name__ == "__main__":
rospy.init_node("sample_simulate_tabletop_cloud")
pub = rospy.Publisher("~output", PointCloud2)
pub_polygon = rospy.Publisher("~output/polygon", PolygonArray)
pub_coef = rospy.Publisher("~output/coef", ModelCoefficientsArray)
pub_boxes = rospy.Publisher("~output/candidate_boxes", BoundingBoxArray)
r = rospy.Rate(10)
counter = 0
model_index = 4
reset = False
while not rospy.is_shutdown():
points = generatePoints(model_index)
header = Header()
header.frame_id = "odom"
header.stamp = rospy.Time.now()
msg = create_cloud_xyz32(header, points)
pub.publish(msg)
(polygon, coef) = generatePolygons(header, model_index)
pub_polygon.publish(polygon)
pub_coef.publish(coef)
counter = counter + 1
pub_boxes.publish(candidateBoxes(header, model_index))
if reset and counter > 1.0 / r.sleep_dur.to_sec() * 10:
reset = rospy.ServiceProxy("/plane_supported_cuboid_estimator/reset", Empty)
counter = 0
model_index = model_index + 1
if model_index >= 5:
model_index = 0
reset()
r.sleep()
