Пример #1
0
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)
Пример #4
0
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()
Пример #5
0
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_()
Пример #6
0
    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
Пример #8
0
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
Пример #9
0
    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)
Пример #10
0
    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)
Пример #11
0
 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)
Пример #13
0
    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()
Пример #14
0
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)
Пример #15
0
    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
Пример #17
0
 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)
Пример #18
0
 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)
Пример #19
0
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()))
Пример #21
0
    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)
Пример #22
0
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]))
Пример #24
0
    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
Пример #25
0
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)
Пример #27
0
 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)
Пример #29
0
    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))
Пример #30
0
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)
Пример #32
0
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
Пример #33
0
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)
Пример #34
0
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
Пример #35
0
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()
Пример #36
0
    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)
Пример #37
0
 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
Пример #38
0
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))
Пример #39
0
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)
Пример #40
0
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()
Пример #41
0
 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)
Пример #42
0
 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)
Пример #43
0
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
Пример #44
0
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()
Пример #45
0
    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)
Пример #46
0
 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)
Пример #47
0
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)
Пример #48
0
 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)
Пример #49
0
    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
Пример #50
0
    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()
Пример #51
0
    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()
Пример #52
0
	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)
Пример #53
0
    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)
Пример #54
0
    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)
Пример #56
0
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
Пример #57
0
 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
Пример #58
0
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()