import numpy as np
import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import scipy.misc
import os
def point_cloud_2_birdseye(points):
    x_points = points[:, 0]
    y_points = points[:, 1]
    z_points = points[:, 2]
    
    f_filt = np.logical_and((x_points > -50), (x_points < 50))
    # logical_and（逻辑与）
    s_filt = np.logical_and((y_points > -50), (y_points < 50))
    filter = np.logical_and(f_filt, s_filt)
    indices = np.argwhere(filter)	# 筛选符合范围的points
    # 返回符合filter条件的位置索引，即第几个位置
    
    x_points = x_points[indices]
    y_points = y_points[indices]
    z_points = z_points[indices]

    x_img = (-y_points*10).astype(np.int32)+500
    # 转换数组的数据类型
    # 点云数据通常是浮点数，而图像数据通常是整数，所以要float映射到int
    y_img = (-x_points *10).astype(np.int32)+500      

    pixel_values = np.clip(z_points,-2,2)
    # numpy.clip(a, a_min, a_max, out=None)
    # 将数组中的元素限制在-2和2之间，大于2的就使得它等于2，小于-2,的就使得它等于-2
    pixel_values = ((pixel_values +2) / 4.0) * 255

    im = np.zeros([1001, 1001], dtype=np.uint8)
    im[y_img, x_img] = pixel_values
    return im

def callback(lidar):
    lidar = pc2.read_points(lidar)
    # 函数 point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
    # 这个函数返回值是一个generator（python中的生成器，属于Iterator迭代器的一种）
    points = np.array(list(lidar))
    # 如果需要一次获得全部点，可以用list()转换为列表
    im = point_cloud_2_birdseye(points)
    scipy.misc.imsave('./lidar.png', im)	# 将数组保存成图像
    os._exit(0)		# python无错误退出程序

def cloud_subscribe():
    rospy.init_node('cloud_subscribe_node')
    rospy.Subscriber("/velodyne_points", PointCloud2, callback)
    rospy.spin()
cloud_subscribe()

?

?

?

?

?

四、Ros将回调函数写成类的形式：

wiki上介绍：http://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers (2.3.2) 在ROS中，想在回调函数中发布消息，有两个思路： （1）把函数写成类的形式，把需要的一些变量在类中声明为全局变量。【推荐，模块化好】 （2）在函数中，把回调函数需要调用的变量声明为全局变量。也可以解决这个问题。【不好，不符合面向对象的风格】 下面的例子是在同一个节点中实现订阅一个消息，然后在该消息的回调函数中处理一下这些数据后再发布到另一个topic上。

#include <ros/ros.h>  

class SubscribeAndPublish  
{  
    public:  
    SubscribeAndPublish()  
    {  
        //Topic you want to publish  
        pub_ = n_.advertise<PUBLISHED_MESSAGE_TYPE>("/published_topic", 1);  
        //PUBLISHED_MESSAGE_TYPE例如std_msgs::String

        //Topic you want to subscribe  
        sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this);  //注意这里，和wiki上不一样。&SubscribeAndPublish这个是类名
        //之所以用this，是因为第四个参数是一个指向【回调函数所在对象】的指针，官方文档例子里把sub定义在了类外面，我们把sub定义在了类的构造函数里面，所以this就是在实例化对象的时候指向对象的指针。（关于this：当调用成员函数a.volume 时，编译系统就把对象a的起始地址赋给this指针；构造函数：建立对象时自动执行。结合两者，在本例中建立类对象时，自动生成指向本对象的指针。）
    }  

    //SUBSCRIBED_MESSAGE_TYPE例如std_msgs::String,记得&要保留
    void callback(const SUBSCRIBED_MESSAGE_TYPE& input)  
    {  
        PUBLISHED_MESSAGE_TYPE output;  
        //.... do something with the input and generate the output...  
        //output = ...
        pub_.publish(output);  
    }  

    private:  
    ros::NodeHandle n_;   
    ros::Publisher pub_;  
    ros::Subscriber sub_;  

};//End of class SubscribeAndPublish  

int main(int argc, char **argv)  
{  
    //Initiate ROS  
    ros::init(argc, argv, "subscribe_and_publish");  

    //Create an object of class SubscribeAndPublish that will take care of everything  
    SubscribeAndPublish SAPObject;  

    ros::spin();  

    return 0;  
}